articles:ik_numerical [RoboLab.Suehiro]

この文書は読取専用です。文書のソースを閲覧することは可能ですが、変更はできません。もし変更したい場合は管理者に連絡してください。
====== 逆運動学の数値解法 ======
===== 逆運動学の数値解法の準備 =====

準備として，
  * [[articles:newton_raphson|ニュートン・ラフソン法]]
  * [[articles:gauss_newton|ガウス・ニュートン法]]
  * [[articles:svd|特異値分解]]
を理解しておくとよい．

逆運動学の数値解法は，自由度に過不足がない場合は[[articles:newton_raphson|ニュートン・ラフソン法]]での解法になる．
そうでない場合
((アームの特異点，冗長自由度アーム，センサ情報を加味した過剰拘束問題など))
は[[articles:gauss_newton|ガウス・ニュートン法]]による最小二乗解法になる．

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

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

手始めとして再掲する．

[{{ articles:jacobian_matrix_06.png?200|図1 2自由度アームの逆運動学}}]
図1のように目標位置$\boldsymbol{p}_T$が与えられたときの関節角を解く問題である．

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

  - まず適当な関節角$\boldsymbol{\theta}_0 = \left( \begin{array}{c} \theta_{0,1} \\ \theta_{0,2} \end{array}\right)$を開始状態とする．
  - $\boldsymbol{\theta}_i$の順運動学解$\boldsymbol{p}_i$を求める．
  - $\boldsymbol{p}_i$が$\boldsymbol{p}_T$に十分一致していれば終了．\\ ある程度十分に近ければその差を$\Delta\boldsymbol{p}_i$とし，離れている場合はその方向の微小ステップを$\Delta\boldsymbol{p}_i$とする．
  - $\Delta\boldsymbol{\theta}_i=J^{-1}\Delta\boldsymbol{p}_i$を計算し，$\boldsymbol{\theta}_{i+1} = \boldsymbol{\theta}_i + \Delta\boldsymbol{\theta}_i$とする．
  - 終了するまで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自由度アームでの[[articles:jacobian_matrix#逆運動学の数値解法]]の原理で述べたように，
$\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} \|
$$
となる．
<!--
つまり，
$$
R(\boldsymbol{n},\theta) = R(\dfrac{\Delta\boldsymbol{\phi}}{\| \Delta\boldsymbol{\phi}\|}, \; \| \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}}}
$$

[[articles:rotation#直交行列から回転軸，角度表現を求める]]にあるように
この直交行列 $^{\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$などとすることになる．

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

この重み付けは[[articles:jacobian_matrix#ｎ自由度回転関節のヤコビ行列|ヤコビ行列]]を見ても分かるように，
位置と姿勢に関して均等化する働きがありヤコビ行列の性質を扱いやすくすることにつながる
((
関節角度の回転に対して並進成分は回転軸から手先までの腕の長さが掛けられている．回転成分に関して
それに相当する長さを掛けるのは妥当と言える
))．

==== ステップの刻み幅 ====
[[articles:gauss_newton|ガウス・ニュートン法]]は解から遠い場合は安定した収束が期待できない．

そこで2自由度アームでの[[articles:jacobian_matrix#逆運動学の数値解法]]の原理で説明したように
目標に少しずつ近づくようにステップを刻むことにする．
ステップ幅を大きく取れば収束は早くなるが安定性が下がるというトレードオフの関係になっている．

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

たとえば多くの場合1mのアームが10cm動く適度に刻めば比較的安定して目標に向かうと判断することができる
((
特異点近傍では必ずしも正しくないが．
))．
このような場合なら$s=0.1$などとすることになる((重み付けがサイズで正規化されていれば，多くの場合$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$を定める
((その時点の関節パラメタとすることが多い．そうすることにより複数ある解の中でその時点の関節パラメタに近い解が得られる可能性が高くなる))．

==== １． 手先の位置姿勢，ヤコビ行列の計算 ====

$\boldsymbol{q}_i$に対してその手先位置姿勢$^0T_{\mathrm{h}i}$およびヤコビ行列$J_i$を計算する．

手先の位置姿勢を以下のように表す．
$$
^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)    \tag{2}
$$

==== ２．目標との差の計算 ====

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

姿勢に関しては，まず中間手先座標系から見た目標座標系の相対的な回転行列を求める．
$$
^{\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}}  \tag{4}
$$

このようにして得られた，$^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) \tag{5}
$$
ここで
$$
D=\operatorname{diag}(d_1,d_2,d_3,d_4,d_5,d_6)
$$
==== ３．収束判定とステップ刻み ====

=== 収束判定 ===

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

しかし，過剰拘束などの場合でガウス・ニュートン法として最小二乗解を求める場合には，残差$\boldsymbol{p}_i$がゼロにはならない．
したがって，その変化量，すなわち$\boldsymbol{p}_{i-1}$との差がほぼなくなるか，
もしくは，解いて得られた関節パラメタの変化量がなくなることで判定を行う必要がある．

=== ステップ刻み ===

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

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

最大ステップ$s$を用いて以下のように$ \Delta \boldsymbol{p}_i$を決める．

$| \boldsymbol{p}_i| < s$の場合，
$$ \Delta \boldsymbol{p}_i = \boldsymbol{p}_i $$

$| \boldsymbol{p}_i| \ge s$の場合，
$$ \Delta \boldsymbol{p}_i = \frac {s}{| \boldsymbol{p}_i|} \boldsymbol{p}_i $$

==== ４．関節パラメタの変化量を求める ====
解に近づけるためのステップ$\boldsymbol{p}_i$に対応する関節パラメタの変化量を重みをつけたヤコビ行列の擬似逆行列を用いて解く．
$$
\Delta \boldsymbol{q}= (DJ_i)^+ \Delta \boldsymbol{p}_i
$$
得られた変化量を現在の関節パラメタに加え(($\Delta\boldsymbol{p}_i$の向きが解の方向に向いているので「加え」でよい))関節パラメタを更新する．
$$
\boldsymbol{q}_{i+1} = \boldsymbol{q}_i + \Delta \boldsymbol{q}
$$

更新された$\boldsymbol{q}_{i+1}$を用いて，収束するまで1から4を繰り返す．