差分

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

--- articles:ik_numerical [2021/10/01 22:58] – [関節パラメタの変化量を求める] Takashi Suehiro
+++ articles:ik_numerical [2021/10/23 16:36] (現在) – [１．手先の位置姿勢，ヤコビ行列の計算] Takashi Suehiro
@@ 行 211: / 行 211: @@
 そこで2自由度アームでの[[articles:jacobian_matrix#逆運動学の数値解法]]の原理で説明したように
 目標に少しずつ近づくようにステップを刻むことにする．
+ステップ幅を大きく取れば収束は早くなるが安定性が下がるというトレードオフの関係になっている．
 刻むべきステップ幅$s$をどのようにするかは一般には難しい問題である．
@@ 行 222: / 行 223: @@
 このステップの刻み幅を適用する際にも位置と姿勢の重み付けを考慮することでバランスが取れた形で解に収束させることができる．
 ===== 逆運動学解の計算手順 =====
 繰り返しも多くなるが計算手順を整理する．
@@ 行 238: / 行 241: @@
             \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)    \tag{2}
 $$
-とする．
 ==== ２．目標との差の計算 ====
@@ 行 302: / 行 308: @@
 目標に少しずつ近づくようにステップを刻む．
-刻むべき最大ステップ$s$をどのようにするかは一般には難しい問題である．
+最大ステップ$s$を用いて以下のように$ \Delta \boldsymbol{p}_i$を決める．
-しかし，ロボットアームの場合は具体的なものが存在しているので，直感的な発想が通用することが多い．
-たとえば多くの場合1mのアームが10cm動く適度に刻めば比較的安定して目標に向かうと判断することができる
-((
-特異点近傍では必ずしも正しくない．場合によっては関節パラメタの変化量に制限加える必要がある．
-))．
-このような場合なら$s=0.1$などとすることになる((重み付けがサイズで正規化されていれば，多くの場合$s=0.1$で良い．))．
-$s$を用いて以下のようにステップを刻む．
 $| \boldsymbol{p}_i| < s$の場合，
@@ 行 319: / 行 316: @@
 $$ \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を繰り返す．