差分

このページの2つのバージョン間の差分を表示します。

--- articles:ik_numerical [2021/10/01 22:19] – [３．収束判定とステップ刻み] Takashi Suehiro
+++ articles:ik_numerical [2021/10/23 16:36] (現在) – [１．手先の位置姿勢，ヤコビ行列の計算] Takashi Suehiro
@@ 行 211: / 行 211: @@
 そこで2自由度アームでの[[articles:jacobian_matrix#逆運動学の数値解法]]の原理で説明したように
 目標に少しずつ近づくようにステップを刻むことにする．
+ステップ幅を大きく取れば収束は早くなるが安定性が下がるというトレードオフの関係になっている．
 刻むべきステップ幅$s$をどのようにするかは一般には難しい問題である．
@@ 行 222: / 行 223: @@
 このステップの刻み幅を適用する際にも位置と姿勢の重み付けを考慮することでバランスが取れた形で解に収束させることができる．
 ===== 逆運動学解の計算手順 =====
 繰り返しも多くなるが計算手順を整理する．
@@ 行 236: / 行 239: @@
               \begin{array}{c} ^0\boldsymbol{d}_\mathrm{g} \\ 1 \end{array}
             \end{array}
-            \right)
+            \right)     \tag{1}
 $$
-==== １． 初期解の設定 ====
+まずこれに対して適当な初期姿勢の関節パラメタ$\boldsymbol{q}_0$を定める
-これに対して適当な初期姿勢の関節パラメタ$\boldsymbol{q}_0$を定め
+((その時点の関節パラメタとすることが多い．そうすることにより複数ある解の中でその時点の関節パラメタに近い解が得られる可能性が高くなる))．
-((その時点の関節パラメタとすることが多い．そうすることにより複数ある解の中でその時点の関節パラメタに近い解が得られる可能性が高くなる))，
-その手先位置姿勢を順運動学計算で求め，
+==== １． 手先の位置姿勢，ヤコビ行列の計算 ====
+$\boldsymbol{q}_i$に対してその手先位置姿勢$^0T_{\mathrm{h}i}$およびヤコビ行列$J_i$を計算する．
+手先の位置姿勢を以下のように表す．
 $$
-^0T_{\mathrm{h}0} = \left( \begin{array}{c}
+^0T_{\mathrm{h}i} = \left( \begin{array}{c}
-              \begin{array}{c} ^0A_{\mathrm{h}0} \\
+              \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}0} \\ 1 \end{array}
+              \begin{array}{c} ^0\boldsymbol{d}_{\mathrm{h}i} \\ 1 \end{array}
             \end{array}
-            \right)
+            \right)    \tag{2}
 $$
-とする．
 ==== ２．目標との差の計算 ====
@@ 行 258: / 行 264: @@
 ベース座標からみた２つの座標系の原点位置の差を求める．
 $$
-^0\boldsymbol{p}_i= {^0\boldsymbol{d}_\mathrm{g}} - {^0\boldsymbol{d}_{\mathrm{h}i}}
+^0\boldsymbol{p}_i= {^0\boldsymbol{d}_\mathrm{g}} - {^0\boldsymbol{d}_{\mathrm{h}i}}  \tag{3}
 $$
@@ 行 269: / 行 275: @@
 さらに，これをベース座標系から見た回転ベクトルに変換する．
 $$
-^0\boldsymbol{\phi}_i = \phi_i {^0\boldsymbol{n}_i} = \phi_i {^0A_{\mathrm{h}i}} {^{\mathrm{h}i}\boldsymbol{n}}
+^0\boldsymbol{\phi}_i = \phi_i {^0\boldsymbol{n}_i} = \phi_i {^0A_{\mathrm{h}i}} {^{\mathrm{h}i}\boldsymbol{n}}  \tag{4}
 $$
@@ 行 278: / 行 284: @@
 すなわち(相変わらずノーテーションが紛らわしいが，，)
 $$
-\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)
+\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}
 $$
 ここで
@@ 行 288: / 行 294: @@
 === 収束判定 ===
-ニュートン・ラフソン法的に考えると$\boldsymbol{p}_i$が十分ゼロに近ければ解が得られたとして良い．
+ニュートン・ラフソン法的に考えると式(5)の$\boldsymbol{p}_i$が十分ゼロに近ければ解が得られたとして良い．
-しかし，過剰拘束などの場合でガウス・ニュートン法として最小二乗解を求める場合には，残差$\boldsymbol{p}_i$の変化量
+しかし，過剰拘束などの場合でガウス・ニュートン法として最小二乗解を求める場合には，残差$\boldsymbol{p}_i$がゼロにはならない．
-もしくは関節パラメタの変化量で判定を行う必要がある．
+したがって，その変化量，すなわち$\boldsymbol{p}_{i-1}$との差がほぼなくなるか，
+もしくは，解いて得られた関節パラメタの変化量がなくなることで判定を行う必要がある．
+=== ステップ刻み ===
 $\boldsymbol{p}_i$が大きい場合はいきなり
@@ 行 297: / 行 306: @@
 そこで2自由度アームでの[[articles:jacobian_matrix#逆運動学の数値解法]]の原理で説明したように
-目標に少しずつ近づくようにステップを刻むことにする．
+目標に少しずつ近づくようにステップを刻む．
-刻むべき最大ステップ$s$をどのようにするかは一般には難しい問題である．
+最大ステップ$s$を用いて以下のように$ \Delta \boldsymbol{p}_i$を決める．
-しかし，ロボットアームの場合は具体的なものが存在しているので，直感的な発想が通用することが多い．
-たとえば多くの場合1mのアームが10cm動く適度に刻めば比較的安定して目標に向かうと判断することができる
+$| \boldsymbol{p}_i| < s$の場合，
-((
+$$ \Delta \boldsymbol{p}_i = \boldsymbol{p}_i $$
-特異点近傍では必ずしも正しくない．
-))．
+$| \boldsymbol{p}_i| \ge s$の場合，
-このような場合なら$s=0.1$などとすることになる((重み付けがサイズで正規化されていれば，多くの場合$s=0.1$で良い．))．
+$$ \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を繰り返す．