逆運動学の数値解法

準備として，

• ニュートン・ラフソン法
• ガウス・ニュートン法
• 特異値分解

を理解しておくとよい．

逆運動学の数値解法は，自由度に過不足がない場合はニュートン・ラフソン法での解法になる． そうでない場合 ¹⁾ はガウス・ニュートン法による最小二乗解法になる．

いずれにしてもガウス・ニュートン法はニュートン・ラフソン法を 含んでいるので，基本的にはヤコビ行列の擬似逆行列を利用したガウス・ニュートン法での説明と考えてもらって良い．

ヤコビ行列の項目で2自由度アームの逆運動学の数値解法の原理を簡単に説明した．

手始めとして再掲する．

図1 2自由度アームの逆運動学

図1のように目標位置$\boldsymbol{p}_T$が与えられたときの関節角を解く問題である．

これはヤコビ行列の逆行列を利用して以下のように解くことが出来る．

1. まず適当な関節角$\boldsymbol{\theta}_0 = \left( \begin{array}{c} \theta_{0,1} \\ \theta_{0,2} \end{array}\right)$を開始状態とする．
2. $\boldsymbol{\theta}_i$の順運動学解$\boldsymbol{p}_i$を求める．
3. $\boldsymbol{p}_i$が$\boldsymbol{p}_T$に十分一致していれば終了．
   ある程度十分に近ければその差を$\Delta\boldsymbol{p}_i$とし，離れている場合はその方向の微小ステップを$\Delta\boldsymbol{p}_i$とする．
4. $\Delta\boldsymbol{\theta}_i=J^{-1}\Delta\boldsymbol{p}_i$を計算し，$\boldsymbol{\theta}_{i+1} = \boldsymbol{\theta}_i + \Delta\boldsymbol{\theta}_i$とする．
5. 終了するまで2から4を繰り返す．

終了時点での$\boldsymbol{\theta}_i$が$\boldsymbol{p}_T$の逆運動学解の数値解$\boldsymbol{\theta}_T$となっている．

上記，目標に十分近づいたときの手順4がニュートン・ラフソン法になっている ²⁾．

ニュートン・ラフソン法は解から遠いときの求解の安定性が問題になることが多いが， ロボットアームの場合は手順3のように具体的なイメージを持ちながらステップを刻むことで求解が安定する．

これを6自由度以上のロボットアームに適用し逆運動学解を計算する場合，以下の2点を考慮しなくてはならない．

• 位置だけでなく姿勢も一致させなくてはならない．姿勢をどのように扱うか
• 目標位置姿勢から離れている場合に近づけるための刻み幅をどのように決めるか

一般的な$n$自由度アームの逆運動学問題は目標座標系$^0T_\mathrm{g}$が与えられたとき， 手先座標系$^0T_\mathrm{h}$がそれになるように， 関節パラメタ$\boldsymbol{q}=( \begin{array}{c} q_1 & q_2 & ... & q_n \end{array})^T$を求める問題である．

$^0T_\mathrm{g}$と$^0T_\mathrm{h}$が一致するというのをどう表現するかといのは， それらの差をどのように表現するかということになる．

ここで改めてヤコビ行列が何を表していたか考えてみよう．

並進と回転の微小量の関係として表すと， $$ \left( \begin{array}{c} \Delta\boldsymbol{p} \\ \Delta\boldsymbol{\phi} \end{array} \right) = J\Delta\boldsymbol{q} $$

となる．

位置に関しては 2自由度アームでの逆運動学の数値解法の原理で述べたように， $\Delta\boldsymbol{p}$を用いて位置の差のベクトルを減じるように$\Delta\boldsymbol{q}$を 求めてていくことで解を得ることができる． つまり位置の差は原点位置の差のベクトルとして素直に表現できる．

姿勢に関しても同様に微小回転ベクトル$\Delta\boldsymbol{\phi}$を用いて姿勢の差を減じればよい． これを一軸周りの回転の表現で表すと，回転軸$\boldsymbol{n}$は $$ \boldsymbol{n} = \dfrac{\Delta\boldsymbol{\phi}}{\| \Delta\boldsymbol{\phi}\|} $$ 回転角度$\theta$は $$ \theta = \| \Delta\boldsymbol{\phi} \| $$ となる．

これを踏まえると姿勢の差は，一軸周りの回転として表現し，$\Delta\boldsymbol{\phi}$がその回転軸周りに回転角度を減じるように $\Delta\boldsymbol{q}$を定めれば良いことが分かる．

手先座標系の座標変換行列の姿勢と位置を分けて表現すると以下のようになる． $$ ^0T_{\mathrm{h}i} = \left( \begin{array}{c} \begin{array}{c} ^0A_{\mathrm{h}i} \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} ^0\boldsymbol{d}_{\mathrm{h}i} \\ 1 \end{array} \end{array} \right) $$ ここで添字$\mathrm{h}i$の$i$は，計算の初期手先位置姿勢 ³⁾を$0$として繰り返しの回数を表している．左肩にある$0$はアームベース座標の意味なので紛らわしいが注意してほしい．

目標座標系の方は， $$ ^0T_\mathrm{g} = \left( \begin{array}{c} \begin{array}{c} ^0A_\mathrm{g} \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} ^0\boldsymbol{d}_\mathrm{g} \\ 1 \end{array} \end{array} \right) $$ 位置の差はともにベース座標系原点から見たベクトルなのでそのまま差をとってよい．すなわち $$ ^0\boldsymbol{p}_i= {^0\boldsymbol{d}_\mathrm{g}} - {^0\boldsymbol{d}_{\mathrm{h}i}} $$ とすることでベース座標での差分ベクトルが得られる．

ここで注意してほしいのは，一般のニュートン法と逆に中間の手先座標原点から目標座標原点のベクトルを 計算しているということである．逆運動学解の計算方法としてはどちらでも同じではあるがニュートン法の 適用という点では少し紛らわしい． しかし逆に現時点からどちらに進んだら目標に近づくかという分解運動制御との整合性からはこちらのほうが わかりやすい ⁴⁾． 図1もこの形の図になっている．

姿勢に関しても同様に，まず中間手先座標系から見た目標座標系の相対的な回転行列を求める． $$ ^{\mathrm{h}i}A_g = ({^0A_{\mathrm{h}i}})^T　{^0A_{\mathrm{g}}} = {^{\mathrm{h}i}A_0} {^0A_{\mathrm{g}}} $$

直交行列から回転軸，角度表現を求めるにあるように この直交行列 $^{\mathrm{h}i}A_\mathrm{g}$の回転軸ベクトル$^{\mathrm{h}i}\boldsymbol{n}$と回転角度$\phi_i$を求める． この$^{\mathrm{h}i}\boldsymbol{n}$は目標座標系から見たベクトルになっているので， これをベース座標系から見たものに変換する．すなわち， $$ ^0\boldsymbol{n}_i = {^0A_{\mathrm{h}i}} {^{\mathrm{h}i}\boldsymbol{n}} $$ さらにこれを回転ベクトルとして以下のように表す． $$ ^0\boldsymbol{\phi}_i = \phi_i {^0\boldsymbol{n}_i} $$ このようにして得られた，$^0\boldsymbol{p}_i$と${^0\boldsymbol{\phi}_i}$が目標座標と手先座標(仮)の差となる．

ヤコビ行列を用いた計算を行うために位置と姿勢を一つのベクトルとして扱いたい． すなわち(少しノーテーションが紛らわしいが，，) $$ \boldsymbol{p} = \left( \begin{array}{c} p_x \\ p_y \\ p_z \\ \phi_x \\ \phi_y \\ \phi_z \end{array} \right) $$

これをこのように単に結合して並べるだけで良いのだろうか．というのも位置の差は[距離]であり，姿勢の差は[角度]と全く異なる単位になる． そのため単純には大きさを比べることができないものになっている．

解が求まり差がゼロになれば単位が何であれ問題はないように思えるが，安定した収束条件を考えたり差の残差を評価するには単位を揃えるのは大事なことである．

物体に紐付けられた座標系を平行移動した場合，物体の頂点の移動量は座標系の原点の移動量と同じになる． 一方，回転移動した場合，物体の頂点の移動量は回転量と座標原点から頂点までの距離との積になる．

すなわち物体に紐付けられた座標系の移動距離と回転角度を物体頂点などの形状の移動として 考えた場合，角度に対して物体の大きさに比例した重みをつけることで距離と同じ土俵で議論できるようになる．

ロボットのイメージで表現するなら，あるロボットアームで手先の位置が1m移動する間に手先の姿勢が1rad回転したとすると その1/10スケールのロボットなら0.1m移動する間に1rad回転すれば相似的に対応するということである．

実際にどの程度の重みをつけるかは厳密に決めることは難しい． ロボットアームの場合，そのザイズの$1/2$程度というのが良い値ではないだろうか． 大きさで正規化するという意味で考えれば逆に位置の方をアームの大きさで割る方が良い ⁵⁾.

$\boldsymbol{p}_i$に対角行列を掛けることで重み付けをすることができる．すなわち， $$ \boldsymbol{p}_i' = D\boldsymbol{p}_i $$ ここで $$ D=\operatorname{diag}(d_1,d_2,d_3,d_4,d_5,d_6) $$ たとえばロボットアームのサイズによらない正規化を行うことにすると，$d_1$，$d_2$，$d_3$はザイズに対応した値で割り， $d_4$，$d_5$，$d_6$は$ 1.0$などとすることになる．

ガウス・ニュートン法での重み付けによる計算方法は重み付き最小二乗法に書いてあるとおりであり， ヤコビ行列の対応する各行にも重みをつけることになる．

この重み付けはヤコビ行列を見ても分かるように， 位置と姿勢に関して均等化する働きがありヤコビ行列の性質を扱いやすくすることにつながる ⁶⁾．

ガウス・ニュートン法は解から遠い場合は安定した収束が期待できない．

そこで2自由度アームでの逆運動学の数値解法の原理で説明したように 目標に少しずつ近づくようにステップを刻むことにする．

刻むべきステップ幅$s$をどのようにするかは一般には難しい問題である． しかし，ロボットアームの場合は具体的なものが存在しているので，直感的な発想が通用することが多い．

たとえば多くの場合1mのアームが10cm動く適度に刻めば比較的安定して目標に向かうと判断することができる ⁷⁾． このような場合なら$s=0.1$などとすることになる⁸⁾．

このステップの刻み幅を適用する際にも位置と姿勢の重み付けを考慮することでバランスが取れた形で解に収束させることができる．

繰り返しも多くなるが計算手順を整理する．

目標座標系が以下のように与えられたとする． $$ ^0T_\mathrm{g} = \left( \begin{array}{c} \begin{array}{c} ^0A_\mathrm{g} \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} ^0\boldsymbol{d}_\mathrm{g} \\ 1 \end{array} \end{array} \right) \tag{1} $$

これに対して適当な初期姿勢の関節パラメタ$\boldsymbol{q}_0$を定め ⁹⁾， その手先位置姿勢を順運動学計算で求め， $$ ^0T_{\mathrm{h}0} = \left( \begin{array}{c} \begin{array}{c} ^0A_{\mathrm{h}0} \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} ^0\boldsymbol{d}_{\mathrm{h}0} \\ 1 \end{array} \end{array} \right) \tag{2} $$ とする．

ベース座標からみた２つの座標系の原点位置の差を求める． $$ ^0\boldsymbol{p}_i= {^0\boldsymbol{d}_\mathrm{g}} - {^0\boldsymbol{d}_{\mathrm{h}i}} $$

姿勢に関しては，まず中間手先座標系から見た目標座標系の相対的な回転行列を求める． $$ ^{\mathrm{h}i}A_g = ({^0A_{\mathrm{h}i}})^T　{^0A_{\mathrm{g}}} = {^{\mathrm{h}i}A_0} {^0A_{\mathrm{g}}} $$ ここから$^{\mathrm{h}i}A_\mathrm{g}$の回転軸ベクトル$^{\mathrm{h}i}\boldsymbol{n}$と回転角度$\phi_i$を求める．

さらに，これをベース座標系から見た回転ベクトルに変換する． $$ ^0\boldsymbol{\phi}_i = \phi_i {^0\boldsymbol{n}_i} = \phi_i {^0A_{\mathrm{h}i}} {^{\mathrm{h}i}\boldsymbol{n}} $$

このようにして得られた，$^0\boldsymbol{p}_i$と${^0\boldsymbol{\phi}_i}$が目標座標と手先座標(仮)の差となる．

ここで，少し紛らわしいがこれを統合し，さらに重み付け行列$D$を掛けたものを改めて$\boldsymbol{p}_i$とする．

すなわち(相変わらずノーテーションが紛らわしいが，，) $$ \boldsymbol{p}_i = D \left( \begin{array}{c} ^0p_{ix} \\ ^0p_{iy} \\ ^0p_{iz} \\ ^0\phi_{ix} \\ ^0\phi_{iy} \\ ^0\phi_{iz} \end{array} \right) $$ ここで $$ D=\operatorname{diag}(d_1,d_2,d_3,d_4,d_5,d_6) $$

ニュートン・ラフソン法的に考えると$\boldsymbol{p}_i$が十分ゼロに近ければ解が得られたとして良い．

しかし，過剰拘束などの場合でガウス・ニュートン法として最小二乗解を求める場合には，残差$\boldsymbol{p}_i$の変化量 もしくは関節パラメタの変化量で判定を行う必要がある．

$\boldsymbol{p}_i$が大きい場合はいきなり ガウス・ニュートン法を適用しても安定した収束が期待できない．

そこで2自由度アームでの逆運動学の数値解法の原理で説明したように 目標に少しずつ近づくようにステップを刻むことにする．

刻むべき最大ステップ$s$をどのようにするかは一般には難しい問題である． しかし，ロボットアームの場合は具体的なものが存在しているので，直感的な発想が通用することが多い．

たとえば多くの場合1mのアームが10cm動く適度に刻めば比較的安定して目標に向かうと判断することができる ¹⁰⁾． このような場合なら$s=0.1$などとすることになる¹¹⁾．