Morikazu Takegaki and Suguru Arimoto, A New Feedback Method for Dynamic Control of Manipulators, Journal of Dynamic Systems, Measurement, and Control, Vol. 102, pp. 119-125, 1981.
元論文へのリンク
マニピュレータは非線形で複雑なダイナミクスを持っている．非線形項を補償して制御を簡単にする方法[1-3]（訳注※いわゆる厳密な線形化）が開発されてきたが，計算が複雑でマイコンに実装するのが難しい上に，モデル誤差やパラメータ変動に弱い．逆にほとんどの産業用ロボットは，関節ごとに独立に線形フィードバック制御されているが，それでなぜ目標値に収束するかはよく判っていない．
本稿では，新たなマニピュレータ運動制御を提案する．まず，一般化力を入力とする線形フィードバック制御によって一般的な機械システムに任意の姿勢をとらせることが出来ること，かつ，この方法がある意味で最適であることを示す．
次に，これを作業座標系での制御に拡張する．従来は，作業座標を（訳注※逆運動学で）関節座標に変換し個々の関節サーボ系に送るが，逆運動学を実時間で解くのは困難である．分解速度制御はこれを線形な問題に変換する方法だが，ヤコビ行列の逆行列計算が必要な点は問題である．
提案する制御系では，マニピュレータを作業座標系で（逆運動学なしで）直接制御する．安定性も保証する．さらに特異点や冗長性があっても，また作業に拘束条件が伴っても適用可能である．ヤコビ行列の計算が必要だが，これは効率の良いアルゴリズムが提案されているのでマイコンにも実装できる．有効性を計算機シミュレーションで確認する．
一般化座標{\bm{x}=(x_{1}, \cdots, x_{n})^{\mathrm{T}}}（マニピュレータならば{x_{i}}は{i}番目関節角度）で記述される，{n}自由度の機械システムの運動エネルギーと一般化運動量はそれぞれ，
\begin{align} T&=\frac{1}{2}\dot{\bm{x}}^{\mathrm{T}}\bm{R}(\bm{x})\dot{\bm{x}} \tag{1} \\ \bm{p}&=\left( \frac{\partial T}{\partial\dot{x}_{1}},\cdots,\frac{\partial T}{\partial\dot{x}_{n}} \right)^{\mathrm{T}} =\left(\frac{\partial T}{\partial\dot{\bm{x}}}\right)^{\mathrm{T}} =\bm{R}(\bm{x})\dot{\bm{x}} \tag{2} \end{align}と書ける．ただし{\bm{R}(\bm{x})}は正定値対称行列（訳注※慣性行列）．ポテンシャル関数V(\bm{x})は，{\bm{x}}について2階微分可能で，かつ{\partial^{2}V/\partial x_{i}\partial x_{j}}が有界であると仮定する（普通のマニピュレータならば成立する）．運動方程式は，ハミルトニアン{H=T+V}を用いて
\begin{align} \dot{\bm{x}}&=\left(\frac{\partial H}{\partial\bm{p}}\right)^{\mathrm{T}}=\bm{R}^{-1}(\bm{x})\bm{p} \tag{3} \\ \dot{\bm{p}}&=-\left(\frac{\partial H}{\partial\bm{p}}\right)^{\mathrm{T}}+\bm{B}\bm{u} =-\left(\frac{\partial T}{\partial\bm{p}}\right)^{\mathrm{T}} -\left(\frac{\partial V}{\partial\bm{p}}\right)^{\mathrm{T}}+\bm{B}\bm{u} \tag{4} \end{align}と書ける．ただし，{\bm{u}=(u_{1},\cdots,u_{n})^{\mathrm{T}}}は{\bm{x}}に対応する一般化力，{\bm{B}}は正則な{n\times n}行列で，{\bm{B}=\bm{I}}と仮定しても一般性を失わない．{\bm{x}}を目標点{\bar{\bm{x}}=(\bar{x}_{1},\cdots,\bar{x}_{n})^{\mathrm{T}}}に収束させるには，ポテンシャル関数を{\bm{x}=\bar{\bm{x}}}で最小となるものに置き換え，かつ系を完全減衰系（すなわち正定値散逸関数を持つよう）にすれば良い．
{\bm{w}}を新たな入力として，制御入力
を式(4)に代入すれば，ポテンシャル関数は{V^{0}(\bm{x})}に，ハミルトニアンは{\bar{H}=T+V^{0}(\bm{x})}に置き換わり，運動方程式が
\begin{align} \dot{\bm{x}}&=\left(\frac{\partial\bar{H}}{\partial\bm{p}}\right)^{\mathrm{T}} \tag{8} \\ \dot{\bm{p}}&=-\left(\frac{\partial\bar{H}}{\partial\bm{p}}\right)^{\mathrm{T}}+\bm{w} \tag{9} \end{align}となる．{\bm{w}=-\bm{Q}\dot{\bm{x}}}（{\bm{Q}}は正定値対称行列）として式(9)に代入し，{\bar{H}}を式(8)(9)の解軌道に沿って微分すれば，
{ \begin{split} \dot{\bar{H}} &=\left(\frac{\partial\bar{H}}{\partial\bm{x}}\right)\dot{\bm{x}} +\left(\frac{\partial\bar{H}}{\partial\bm{p}}\right)\dot{\bm{p}} \\ &=\left(\frac{\partial\bar{H}}{\partial\bm{x}}\right) \left(\frac{\partial\bar{H}}{\partial\bm{p}}\right)^{\mathrm{T}} -\left(\frac{\partial\bar{H}}{\partial\bm{p}}\right) \left(\frac{\partial\bar{H}}{\partial\bm{x}}\right)^{\mathrm{T}} -\left(\frac{\partial\bar{H}}{\partial\bm{p}}\right)\bm{Q}\dot{\bm{x}} \\ &=-\dot{\bm{x}}^{\mathrm{T}}\bm{Q}\dot{\bm{x}} \tag{10} \end{split} }を得る．これより，{V^{0}(\bm{x})}が正定値ならば{\bar{H}}はリャプノフ関数になる．平衡点が{\bar{x}}の他になければ，集合{\left\{(\bm{x}^{\mathrm{T}}, \bm{p}^{\mathrm{T}}); \dot{\bar{H}}=0\right\}}は系の運動全体を含まないので，リャプノフ-ラ・サール-レフシェッツの定理より平衡点{(\bar{\bm{x}}^{\mathrm{T}}, \bm{O}^{\mathrm{T}})}は漸近安定となる．
任意の{V^{0}(\bm{x})}に関して{\bm{w}=-\bm{Q}\dot{\bm{x}}}は，運動の滑らかさと関連する性能指標
{ \int_{0}^{\infty}\frac{1}{2}\left( \dot{\bm{x}}^{\mathrm{T}}\bm{Q}\dot{\bm{x}}+\bm{w}^{\mathrm{T}}\bm{Q}^{-1}\bm{w} \right)\mathrm{d}t \tag{11} }を最小化するという意味で最適である．
{V^{0}(\bm{x})}の候補は幾つか考えられるが，ここでは
{ V^{0}(\bm{x})=V(\bm{x})-V(\bar{\bm{x}}) -\left(\frac{\partial V}{\partial\bm{x}}\right)(\bar{\bm{x}})(\bm{x}-\bar{\bm{x}}) +\frac{1}{2}(\bm{x}-\bar{\bm{x}})^{\mathrm{T}}\bm{W}(\bm{x}-\bar{\bm{x}}) \tag{17} }を採用する．ただし，{\bm{W}}は{n\times n}正定値対称行列である．
念のため，{V^{0}(\bm{x})}が正定値であることを証明しよう．式(17)を{\bm{x}}で1階・2階微分すると
\begin{align} \left(\frac{\partial V^{0}}{\partial\bm{x}}\right)^{\mathrm{T}} =\left(\frac{\partial V}{\partial\bm{x}}\right)^{\mathrm{T}}(\bm{x}) -\left(\frac{\partial V}{\partial\bm{x}}\right)^{\mathrm{T}}(\bar{\bm{x}})+\bm{W}(\bm{x}-\bar{\bm{x}}) \tag{19} \\ V^{0}(\bar{\bm{x}})=0,\quad \left(\frac{\partial V^{0}}{\partial\bm{x}}\right)^{\mathrm{T}}(\bar{\bm{x}})=\bm{O} \notag \\ \left(\frac{\partial^{2}V^{0}}{\partial x_{i}\partial x_{j}}\right)(\bm{x}) =\left(\frac{\partial^{2}V}{\partial x_{i}\partial x_{j}}\right)(\bm{x})+\bm{W} \tag{20} \end{align}を得る．{\left(\partial^{2}V/\partial x_{i}\partial x_{j}\right)(\bm{x})}は有界という仮定から，任意の{\bm{x}}に対して
{ \left(\frac{\partial^{2}V}{\partial x_{i}\partial x_{j}}\right)(\bm{x})+\bm{W}>0 \tag{21} }を満たすように{\bm{W}}を選べば，式(19)(21)より{V^{0}(\bm{x})}は凸関数となり，{\bm{x}=\bar{\bm{x}}}において最小値{V^{0}(\bar{\bm{x}})=0}をとる．したがって点{\left(\bar{\bm{x}}^{\mathrm{T}}, \bm{O}^{\mathrm{T}}\right)}は漸近安定である．このとき制御入力は
{ \bm{u}=\left(\frac{\partial V}{\partial\bm{x}}\right)^{\mathrm{T}}(\bar{\bm{x}})-\bm{W}(\bm{x}-\bar{\bm{x}})-\bm{Q}\dot{\bm{x}} \tag{22} }となる．特に{\bm{W}}と{\bm{Q}}が対角行列ならば，（非線形なダイナミクス干渉があるにも関わらず）個々の関節の独立な線形フィードバックによって大局的に漸近安定となると分かる．
過渡応答の解析は簡単ではない．関節空間は{n}次元トーラスなので，収束するまでに何回転もするかも知れない．
ここで，{\dot{\bm{x}}(0)=\bm{0}}であるとして，
とおくと，{V^{0}(\bm{x})}は凸関数なので，{V^{0}(\bm{x})<\mu}ならば{\bm{x}\in A=\left\{\bm{x}: |x_{i}-\bar{x}_{i}|\leq\pi, i=1,\cdots,n\right\}}であり，{\bar{H}}は系の解軌道に沿って減少するので，{V^{0}(\bm{x}(t_{0}))<\mu}ならば{V^{0}(\bm{x}(t))<\mu}すなわち{\bm{x}(t)\in A}（{\forall t>t_{0}}）である．よって，このとき{\bm{x}(t)}は一回転未満で目標位置に収束する．
目標位置を，たとえば絶対座標系における手先位置・姿勢とする方が，マニピュレータ位置精度が向上する．作業座標{\bm{y}=(y_{1}, \cdots, y_{m})^{\mathrm{T}}=\bm{f}(\bm{x})=(f_{1}, \cdots, f_{m})^{\mathrm{T}}}の直接フィードバックによる位置制御を考えよう．
ヤコビ行列{\bm{J}(\bm{x})=\partial\bm{f}/\partial\bm{x}}が正則なら，{\bm{y}}に対応する一般化運動量{\bm{q}}は
{ \bm{q}=\left( \frac{\partial T}{\partial\dot{\bm{y}}}\right)^{\mathrm{T}} =(\bm{J}^{\mathrm{T}})^{-1}\bm{R}\bm{J}^{-1}\dot{\bm{y}} \tag{25} }と書ける．仮想仕事の原理より{\bm{v}\delta\bm{y}=\bm{u}^{\mathrm{T}}\delta\bm{x}=\bm{u}^{\mathrm{T}}\bm{J}^{-1}\delta\bm{y}}であるので，{\bm{y}}に対応する一般化力は{(\bm{J}^{\mathrm{T}})^{-1}\bm{u}}と求まる．よって，{\bm{y}}についての運動方程式は
\begin{align} \dot{\bm{y}}&=\left(\frac{\partial T}{\partial\bm{q}}\right)^{\mathrm{T}} \tag{30} \\ \dot{\bm{q}}&= -\left(\frac{\partial T}{\partial\bm{y}}\right)^{\mathrm{T}} -\left(\frac{\partial V}{\partial\bm{y}}\right)^{\mathrm{T}} +(\bm{J}^{\mathrm{T}})^{-1}\bm{u} \tag{31} \end{align}となる．
式(31)への入力を
{ \bm{u}=\left(\frac{\partial V}{\partial\bm{x}}\right)^{\mathrm{T}} -\bm{J}^{\mathrm{T}}[\bm{W}(\bm{y}-\bar{\bm{y}})+\bm{Q}\dot{\bm{y}}] \tag{32} }とすれば，この系のハミルトニアン{\bar{H}}とその時間微分は
\begin{align} \bar{H}&=T+\frac{1}{2}(\bm{y}-\bar{\bm{y}})^{\mathrm{T}}\bm{W}(\bm{y}-\bar{\bm{y}}) \tag{33} \\ \dot{\bar{H}}&=-\dot{\bm{y}}^{\mathrm{T}}\bm{Q}\dot{\bm{y}} \tag{34} \end{align}となるので，{(\bm{y}^{\mathrm{T}},\bm{q}^{\mathrm{T}})=(\bar{\bm{y}}^{\mathrm{T}},\bm{O}^{\mathrm{T}})}は漸近安定である．また，制御器{\bm{w}=-\bm{J}^{\mathrm{T}}\bm{Q}\dot{\bm{y}}}は，
{ \int_{0}^{\infty}\frac{1}{2}\left( \dot{\bm{y}}^{\mathrm{T}}\bm{Q}\dot{\bm{y}} +\bm{u}^{\mathrm{T}}\bm{J}^{-1}\bm{Q}^{-1}(\bm{J}^{\mathrm{T}})^{-1}\bm{u} \right)\mathrm{d}t }を最小化するという意味で最適である．{\bm{y}}はセンサーで観測するか，あるいは{\bm{x}}から計算すればよい．
ヤコビ行列が特異点や冗長性により非正則である場合は，スラック変数を導入する．
{\bm{J}}の階数が{m(<n)}で，{y_{1}, \cdots, y_{m}}が互いに独立であると仮定しよう．スラック変数ベクトルを{\bm{y}_{\mathrm{s}}=(y_{m+1}, \cdots, y_{n})^{\mathrm{T}}=\bm{f}_{\mathrm{s}}(\bm{x})}とおき，{\bm{f}_{\mathrm{e}}=\left(\bm{f}^{\mathrm{T}}, \bm{f}_{\mathrm{s}}^{\mathrm{T}}\right)}，{\bm{J}_{\mathrm{e}}=\left[ \bm{J}^{\mathrm{T}}, \left(\partial\bm{f}_{\mathrm{s}}/\partial\bm{x}\right)^{\mathrm{T}} \right]^{\mathrm{T}}}とすると，近傍{O}において{\bm{J}_{\mathrm{e}}}は正則なので，{\bm{y}_{\mathrm{e}}^{\mathrm{T}}=(\bm{y}^{\mathrm{T}}, \bm{y}_{\mathrm{s}}^{\mathrm{T}})}についての運動方程式は
と書ける．ただし，{\bm{q}_{\mathrm{s}}}は{\bm{y}_{\mathrm{s}}}に対応する一般化運動量である．
{(\bm{y}^{\mathrm{T}}, \bm{q}^{\mathrm{T}}, \bm{q}_{\mathrm{s}}^{\mathrm{T}})}の安定性を考えよう．{\bm{y}_{\mathrm{s}}}と{\dot{\bm{y}}_{\mathrm{s}}}は未知なので，式(32)の制御器{\bm{u}}において{-\bm{J}_{\mathrm{e}}^{\mathrm{T}}\bm{Q}\dot{\bm{y}}_{\mathrm{e}}}のような減衰力は実現できない．そこで，制御器{\bm{u}}を
{ \bm{u}=\left(\frac{\partial V}{\partial\bm{x}}\right)^{\mathrm{T}} -\bm{J}^{\mathrm{T}}\bm{W}(\bm{y}-\bar{\bm{y}})-\bm{Q}\dot{\bm{x}} \tag{38} }とする．{\bm{W}}は{m\times m}正定値対称行列，{\bm{Q}}は{n\times n}正定値対称行列になる．
式(38)の{\bm{u}}を式(37)に代入し，{(\bm{y}^{\mathrm{T}}, \bm{y}_{\mathrm{s}}^{\mathrm{T}},\bm{q}^{\mathrm{T}}, \bm{q}_{\mathrm{s}}^{\mathrm{T}})=(\bar{\bm{y}}^{\mathrm{T}}, \bar{\bm{y}}_{\mathrm{s}}^{\mathrm{T}},\bm{O}^{\mathrm{T}}, \bm{O}^{\mathrm{T}})}のまわりで式(36)(37)を線形化すると，
\begin{align} \frac{\mathrm{d}}{\mathrm{d}t}\begin{pmatrix} \delta\bm{y} \\ \delta\bm{y}_{\mathrm{s}} \end{pmatrix} =\bm{J}_{\mathrm{e}}\bm{R}^{-1}\bm{J}_{\mathrm{e}}^{\mathrm{T}} \begin{pmatrix} \bm{q} \\ \bm{q}_{\mathrm{s}} \end{pmatrix} =\begin{pmatrix} \tilde{\bm{R}}_{11} & \tilde{\bm{R}}_{12} \\ \tilde{\bm{R}}_{12}^{\mathrm{T}} & \tilde{\bm{R}}_{22} \end{pmatrix} \begin{pmatrix} \bm{q} \\ \bm{q}_{\mathrm{s}} \end{pmatrix} \tag{39} \\ \frac{\mathrm{d}}{\mathrm{d}t}\begin{pmatrix} \bm{q} \\ \bm{q}_{\mathrm{s}} \end{pmatrix} =-\begin{pmatrix} \bm{W} & \bm{O} \\ \bm{O} & \bm{O} \end{pmatrix} \begin{pmatrix} \delta\bm{y} \\ \delta\bm{y}_{\mathrm{s}} \end{pmatrix} -(\bm{J}_{\mathrm{e}}^{\mathrm{T}})^{-1}\bm{Q}\bm{J}_{\mathrm{e}}^{-1} \begin{pmatrix} \delta\dot{\bm{y}} \\ \delta\dot{\bm{y}}_{\mathrm{s}} \end{pmatrix} \tag{40} \end{align}を得る．ただし，{(\delta\bm{y}^{\mathrm{T}}, \delta\bm{y}_{\mathrm{s}}^{\mathrm{T}})=(\bm{y}^{\mathrm{T}}-\bar{\bm{y}}^{\mathrm{T}}, \bm{y}_{\mathrm{s}}^{\mathrm{T}}-\bar{\bm{y}}_{\mathrm{s}}^{\mathrm{T}})}，{\bar{\bm{y}}_{\mathrm{s}}^{\mathrm{T}}}は適当なベクトルである．{\delta\bm{y}_{\mathrm{s}}}を除いた系は
{ \frac{\mathrm{d}}{\mathrm{d}t}\begin{pmatrix} \delta\bm{y} \\ \hline \bm{q} \\ \hline \bm{q}_{\mathrm{s}} \end{pmatrix} =\left(\begin{array}{c|c} \bm{O} & \begin{array}{c|c} \tilde{\bm{R}}_{11} & \tilde{\bm{R}}_{12} \end{array} \\ \hline \begin{matrix} -\bm{W} \\ \hline \bm{O} \end{matrix} & -(\bm{J}_{\mathrm{e}}^{\mathrm{T}})^{-1}\bm{Q}\bm{R}^{-1}\bm{J}_{\mathrm{e}}^{\mathrm{T}} \end{array}\right) \begin{pmatrix} \delta\bm{y} \\ \hline \bm{q} \\ \hline \bm{q}_{\mathrm{s}} \end{pmatrix} \tag{41} }となる．式(33)の関数{\bar{H}}を系(41)（訳注※原文では(46)となっているが，誤り）の解軌道に沿って微分すれば，次式を得る．
{ \dot{\bar{H}}=-(\bm{q}^{\mathrm{T}}, \bm{q}_{\mathrm{s}}^{\mathrm{T}})\bm{J}_{\mathrm{e}}\bm{R}^{-1}\bm{Q}\bm{R}^{-1}\bm{J}_{\mathrm{e}}^{\mathrm{T}} \begin{pmatrix} \bm{q} \\ \bm{q}_{\mathrm{s}} \end{pmatrix} =-\dot{\bm{y}}_{\mathrm{e}}^{\mathrm{T}}(\bm{J}_{\mathrm{e}}^{-1})^{\mathrm{T}}\bm{Q}\bm{J}_{\mathrm{e}}^{-1}\dot{\bm{y}}_{\mathrm{e}} \tag{42} }集合{[(\delta\bm{y}^{\mathrm{T}}, \bm{q}^{\mathrm{T}}, \bm{q}_{\mathrm{s}}^{\mathrm{T}}); \bar{H}=0]}は{(\delta\bm{y}^{\mathrm{T}}, \bm{q}^{\mathrm{T}}, \bm{q}_{\mathrm{s}}^{\mathrm{T}})=\bm{O}^{\mathrm{T}}}以外の軌道を含まないので，系(41)は漸近安定である．
{\bm{y}_{\mathrm{s}}}は便宜のためだけに導入したので，どの点に収束するかは気にしなくて良い．よって，線形近似系(39)(40)に関しては，冗長系でも制御則(38)を適用できる．{\bm{y}_{\mathrm{s}}}の有界性が言えるならば，式(36)(37)のような非線形系についても，文献[10]の結果によって大局的漸近安定性を保証できる．
扉開け，クランク回し，ペグ挿入など，作業の多くは
{ \bm{\Psi}(\bm{x})=(\psi_{1}(\bm{x}), \cdots, \psi_{r}(\bm{x}))^{\mathrm{T}}=\bm{O} \tag{43} }のような幾何学的拘束（ホロノミック拘束）を伴う．これを考慮した制御器設計を考えよう．
{\psi_{1}(\bm{x}), \cdots, \psi_{r}(\bm{x})}は独立であると仮定し，{\bm{x}}を，独立な{n-r}次元ベクトル{\bm{x}_{\mathrm{I}}}と残りの{r}次元ベクトル{\bm{x}_{\mathrm{D}}}に分ける．次を満たす必要がある．
{ \left(\frac{\partial\bm{\Psi}}{\partial\bm{x}}\right)\delta\bm{x}= \left(\frac{\partial\bm{\Psi}}{\partial\bm{x}_{\mathrm{I}}}; \frac{\partial\bm{\Psi}}{\partial\bm{x}_{\mathrm{D}}}\right) \begin{pmatrix} \delta\bm{x}_{\mathrm{I}} \\ \delta\bm{x}_{\mathrm{D}} \end{pmatrix} =\bm{O} \tag{44} }仮定より{\mathrm{det}\partial\bm{\Psi}/\partial\bm{x}_{\mathrm{D}}\neq 0}，かつ{\bm{x}_{\mathrm{D}}}は{\bm{x}_{\mathrm{I}}}の関数と見なせるので，
{ \delta\bm{x}_{\mathrm{D}}=- \left(\frac{\partial\bm{\Psi}}{\partial\bm{x}_{\mathrm{D}}}\right)^{-1} \left(\frac{\partial\bm{\Psi}}{\partial\bm{x}_{\mathrm{I}}}\right) \delta\bm{x}_{\mathrm{I}} =\bm{D}\delta\bm{x}_{\mathrm{I}} \tag{45} }と表せる．{\bm{u}}を，{(\bm{x}_{\mathrm{I}}^{\mathrm{T}}, \bm{x}_{\mathrm{D}}^{\mathrm{T}})}に対応して{\bm{u}^{\mathrm{T}}=(\bm{u}_{\mathrm{I}}^{\mathrm{T}}, \bm{u}_{\mathrm{D}}^{\mathrm{T}})}と分けると，仮想仕事の原理より一般化力
{ \bm{u}_{\mathrm{c}}=\bm{u}_{\mathrm{I}}+\bm{D}^{\mathrm{T}}\bm{u}_{\mathrm{D}} \tag{46} }を得る．運動方程式は，ハミルトニアンを{\bm{x}_{\mathrm{I}}}のみについて書き直すことで
\begin{align} \dot{\bm{x}}_{\mathrm{I}}&=\left(\frac{\partial T_{\mathrm{c}}}{\partial\bm{p}_{\mathrm{I}}}\right)^{\mathrm{T}} \tag{47} \\ \dot{\bm{p}}_{\mathrm{I}}&= -\left(\frac{\partial T_{\mathrm{c}}}{\partial\bm{x}_{\mathrm{I}}}\right)^{\mathrm{T}} -\left(\frac{\partial V_{\mathrm{c}}}{\partial\bm{x}_{\mathrm{I}}}\right)^{\mathrm{T}} +\bm{u}_{\mathrm{I}}+\bm{D}^{\mathrm{T}}(\bm{x}_{\mathrm{I}})\bm{u}_{\mathrm{D}} \tag{48} \end{align}と導ける（訳注※原文の式(47)は誤り）．ただし，{\bm{p}_{\mathrm{I}}}は{\bm{x}_{\mathrm{I}}}に対応する一般化運動量であり，{T_{\mathrm{c}}}および{V_{\mathrm{c}}}は{T(\bm{x},\bm{p})=T_{\mathrm{c}}(\bm{x}_{\mathrm{I}},\bm{p}_{\mathrm{I}})}，{V(\bm{x})=V_{\mathrm{c}}(\bm{x}_{\mathrm{I}})}を満たすように定義される．
{\partial V_{\mathrm{c}}/\partial\bm{x}_{\mathrm{I}}}も
と書き直せる．制御器{\bm{u}}を
{ \bm{u}=\begin{pmatrix} \bm{u}_{\mathrm{I}} \\ \bm{u}_{\mathrm{D}} \end{pmatrix} =\left(\frac{\partial V}{\partial\bm{x}}\right)^{\mathrm{T}}(\bar{\bm{x}}) -\begin{pmatrix} \bm{W}(\bm{x}_{\mathrm{I}}-\bar{\bm{x}}_{\mathrm{I}})+\bm{Q}\bar{\bm{x}}_{\mathrm{I}} \\ \bm{O} \end{pmatrix} \tag{49} }とする．ただし，{\bm{W}}と{\bm{Q}}はいずれも{(n-r)\times(n-r)}正定値対称行列であり，{\bm{W}}は
{ \left(\frac{\partial^{2}V_{\mathrm{c}}}{\partial x_{i}\partial x_{j}}\right)+\bm{W}>0 \tag{50} }を満たすように選ぶ．目標位置{\bar{\bm{x}}}が拘束条件(43)を満たすならば，この場合も制御則(22)が{(\bm{x}_{\mathrm{I}}^{\mathrm{T}}, \bm{p}_{\mathrm{I}}^{\mathrm{T}})=(\bar{\bm{x}}_{\mathrm{I}}^{\mathrm{T}}, \bm{O}^{\mathrm{T}})}を漸近安定化することが分かる．
図1
たとえば，図1のようにマニピュレータにクランクを回させる時は，同図の{r}と{\phi}を作業座標に選ぶ．クランクが水平なX-Y平面上に置かれているとしよう．手の鉛直高さを{Z}とおくと，{r=\mathrm{const.}}および{Z=\mathrm{const.}}が拘束条件として課される．マニピュレータは{\phi}のみフィードバックする．制御則(38)を使えば{\bm{y}-\bar{\bm{y}}=\phi-\bar{\phi}}，{\bm{W}}はスカラー，{\bm{J}=\partial\phi/\partial\bm{x}}はn次元行ベクトルとなる．{(\phi,r)}と{(X,Y)}の関係は
{ \begin{pmatrix} \dot{X} \\ \dot{Y} \end{pmatrix} =\begin{pmatrix} r\cos\phi & \sin\phi \\ r\sin\phi & \cos\phi \end{pmatrix} \begin{pmatrix} \dot{\phi} \\ \dot{r} \end{pmatrix} =\bm{J}_{1} \begin{pmatrix} \dot{\phi} \\ \dot{r} \end{pmatrix} }となるので，
{ \begin{pmatrix} \frac{\partial\phi}{\partial\bm{x}} \\ \frac{\partial r}{\partial\bm{x}} \end{pmatrix} =\bm{J}_{1}^{-1} \begin{pmatrix} \frac{\partial X}{\partial\bm{x}} \\ \frac{\partial Y}{\partial\bm{x}} \end{pmatrix} =\bm{J}_{1}^{-1}\bm{J}_{2} =\frac{1}{r}\begin{pmatrix} \cos\phi & -\sin\phi \\ r\sin\phi & r\cos\phi \end{pmatrix} \bm{J}_{2} }が得られる．ヤコビ行列{\bm{J}=\partial\phi/\partial\bm{x}}は次のようになる．
{ \bm{J}=\left(\frac{\partial\phi}{\partial\bm{x}}\right) =\left(\frac{1}{r}\cos\phi, -\frac{1}{r}\sin\phi\right)\bm{J}_{2} \tag{51} }あとは徐々に{\bar{\phi}}を増やせば良い．
クランクの回転中心位置情報は必要ないことに注意されたい．さらに，{r}の値はフィードバックゲイン{\bm{W}}に含めることができるので，{r}が未知でも良い．
図2
表1
図2の4自由度アームモデルを用いて，提案方法の有効性をシミュレーションにより検証した．各モデルパラメータを表1に示す．
{\bm{x}=(\theta_{1}, \theta_{2}, \theta_{3}, \theta_{4})^{\mathrm{T}}}，{\bm{p}=(p_{1}, p_{2}, p_{3}, p_{4})^{\mathrm{T}}}とおくと，運動方程式は
となる．{\bm{R}}の中身と{g_{i}}（{i,j=1,\cdots,4}）は省略．
作業座標を{\bm{y}=(x,y,z,\theta_{4})}とした．{(x,y,z)}は4番目リンク先端位置である．
関節角度の初期値を{(\theta_{1},\theta_{2},\theta_{3},\theta_{4})=(0,-0.3,1.2,1.57)}[rad]，目標値を{\bar{\bm{y}}=\bm{y}(0)+\varDelta\bm{y}}として式(32)の制御を試した．
図3において，{\varDelta\bm{y}=(0.4\mathrm{m},0.3\mathrm{m},0.2\mathrm{m},0\mathrm{rad})}とした．フィードバックゲインは，何回かの試行を通して{\bm{W}=\mathrm{diag}\left\{30,30,30,5\right\}, \bm{Q}=\mathrm{diag}\left\{15,20,15,5\right\}}と設定した．{\bm{y}}は2秒程度で目標値に収束した．
動作開始時に制御入力{\bm{u}}が大きな値となったが，中間目標値{\bar{\bm{y}}}を滑らかに変化させれば，これは低減できる．
図4においては，点線の目標軌道{\bar{\bm{y}}(t)}を与え，制御則は
とした．オーバーシュートが現れたが，これも目標軌道を滑らかに修正することで抑制できる．
図3
図4
次に，クランク回し作業をシミュレートするため，{\bm{y}=(r,\phi,z,\theta_{4})^{\mathrm{T}}}とした．目標軌道は{\phi=2\pi t/10}，{r=\mathrm{const.}}，{z=\mathrm{const.}}，初期関節角度は{(\theta_{1},\theta_{2},\theta_{3},\theta_{4})=(0,-0.7,1.5,1.57)}[rad]，ゲイン行列は{\bm{W}=\mathrm{diag}\left\{200,5,15,3\right\}, \bm{Q}=\mathrm{diag}\left\{200,5,15,3\right\}}とした（{r}軸方向のゲインを他の軸に比べて大きくした）．図5，6に示すように，アームは目標軌道をほぼ完全になぞった．一方，作業座標{\bm{y}}の一部のみ用いてフィードバック制御を行ったところ，応答は劣化した．
図5
図6
マニピュレータ運動制御の理論的考察に基づいて，一般的な非線形機械システムに有効な制御方法を提案し，作業座標系の制御，拘束された運動制御において有効性を示した．センサー情報の直接フィードバック制御に適している．
動的環境では，しばしば力学的拘束条件を明確に記述することが困難となる．また多くの場合，ヤコビ行列の特異点は回避できない．これらの状況においては，入手可能な部分座標のみフィードバックするのが効果的である．
運動の過渡特性については十分には議論しなかった．これを改善するためには，{\bm{R}(\bm{x})}を考慮してフィードバックゲインを調整すべきである．ただし，ゲイン行列{\bm{W}}と{\bm{Q}}を適切に選択するだけでも目標応答が得られることはシミュレーションで示した通りである．軌道追従のように加速度を無視できない制御は，まだ研究中である．
- G. N. Saridis, H. E. Stephanou, A Hierarchical Approach to the Control of a Prosthetic Arm, IEEE Transactions on System, Man, and Cybernetics, SMC-7-6, pp. 407-420, 1977.
- K. Takase, Generalized Decomposition and Control of a Motion of a Manipulator, Transactions of Society of Instrument and Control Engineers, Vol. 12-3, pp. 300-306, 1976 (in Japanese).
- E. Freunde, and M. Syrbe, Control of Industrial Robots by Means of Microprocessors, Lecture Note in Control and Information Sciences, Springer-Verlag, Vol. 2, pp. 167-175, 1976.
他は略．
現在の産業用ロボットアームが安心して関節角度サーボだけで作業を行えるのは，この研究のお蔭と言っても過言ではない．
本稿の主張を一言で言えば，「ホロノミック拘束下での機械システムの運動は，任意の座標系において線形状態フィードバックのみで漸近安定化できる」ということになる．運動エネルギーは速度フィードバックによって自然に散逸する，という事実が裏にある．冗長な場合も，スラック変数を導入して正則な系に変換しているので結論はほぼ同じである．ただし，スラック変数が安定化されないことについての本稿の議論はやや数学的に粗い（後に有本ら自身がより丁寧な解析を展開している）．
本研究の動機につながった厳密な線形化や逆運動学などの計算のコストは，現在の標準的な計算機の性能では問題にならず，その側面での意義は失われていると言うべきであろう．一方で，安定性の議論の価値が損なわれることは決して無い．
紹介された作業座標系フィードバックを，仮想ばね・ダンパ仮説という人の到達運動制御器モデルとして採用した研究事例もある．
- S. Arimoto, and M. Sekimoto, Human-Like Movements of Robotic Arms with Redundant DOFs: Virtual Spring-Damper Hypothesis to Tackle the Bernstein Problem, in Proceedings of the 2006 IEEE International Conference on Robotics and Automation, pp. 1860-1866, 2006.
身体科学分野における研究によって，人の振る舞いは線形状態フィードバックのみでは表現できないということが明らかにされた（Flash＆Hogan，Kawatoら，Harris＆Wolpertの研究が有名）．人の運動原理を明らかにする上で，実験観察と数学的解析の両輪を大事にしたいものである．
Discussion
