差分

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

--- articles:inverse_kinematics [2021/07/17 23:23] – [３自由度アームの逆運動学] Takashi Suehiro
+++ articles:inverse_kinematics [2022/03/26 22:21] (現在) – [３自由度アームの逆運動学のまとめ] Takashi Suehiro
@@ 行 75: / 行 75: @@
 $$
 $$
-\mathrm{cos}\theta_3 = {{X^2+Z^2-l_2^2-l_3^2} \over {2l_2l_3}}
+\mathrm{cos}\theta_3 = {{X^2+Z^2-l_2^2-l_3^2} \over {2l_2l_3}}　\tag{7}
 $$
 よって，
 $$
-\theta_3 = \pm\mathrm{cos}^{-1}\left({{X^2+Z^2-l_2^2-l_3^2} \over {2l_2l_3}}\right) \tag{7}
+\theta_3 = \pm\mathrm{cos}^{-1}\left({{X^2+Z^2-l_2^2-l_3^2} \over {2l_2l_3}}\right) \tag{8}
 $$
 正の値が実線，負の値は点線の三角形になる．
@@ 行 85: / 行 85: @@
 ここで$\phi$を求めると，
 $$
-\phi = \mathrm{atan2}\left({X ,\; Z}\right) \tag{8}
+\phi = \mathrm{atan2}\left({X ,\; Z}\right) \tag{9}
 $$
 であり,
@@ 行 98: / 行 98: @@
 $$
 $$
-\theta_2 = \mathrm{atan2}\left({X ,\; Z}\right) \mp\mathrm{cos}^{-1} \left( {X^2+Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(X^2+Z^2)}}\right) \tag{9}
+\theta_2 = \mathrm{atan2}\left({X ,\; Z}\right) \mp\mathrm{cos}^{-1} \left( {X^2+Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(X^2+Z^2)}}\right) \tag{10}
 $$
 $\theta_2$は$\mp$の正の側が点線の三角形になる．
 \\
 [{{ articles:inverse_kinematics_03.png?300| 図4 $\theta_1$を反対側（$\pi$回した側）にした解}}]
-図4の$\theta_1$を反対側（$\pi$回した側）にした解を考える．これは，
+つぎに図4の$\theta_1$を反対側（$\pi$回した側）にした解を考える．これは，
 $$
-\theta_1 = \mathrm{atan2}(-{^0y_{wrist}},\,-{^0x_{wrist}})　\tag{10}
+\theta_1 = \mathrm{atan2}(-{^0y_{wrist}},\,-{^0x_{wrist}})　\tag{11}
 $$
 とすることになる．
@@ 行 113: / 行 114: @@
 とする．
-$\theta_3$に着目すると，式(7)は変更なしで大丈夫だが，形は同じでもY軸が紙面向こう側になるので
+$\theta_3$に着目すると，式(8)は変更なしで大丈夫だが，形は同じでもY軸が紙面向こう側になるので
 正の値の実線が図3とは逆になる．
@@ 行 123: / 行 124: @@
 $\theta_2<0$であるので，式(11)の$\phi$から余弦定理で得られた$\mathrm{cos}^{-1}$を引けば実線に対する
-$\theta_2$が正しく求められるので，ここは式(9)の$\phi$の部分を式(11)で置き換えて，
+$\theta_2$が正しく求められるので，ここは式(10)の$\phi$の部分を式(11)で置き換えて，
 $$
 \theta_2 = \mathrm{atan2}\left({-X ,\; Z}\right) \mp\mathrm{cos}^{-1} \left( {X^2+Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(X^2+Z^2)}}\right)
@@ 行 131: / 行 132: @@
 [{{ articles:inverse_kinematics_04.png?200 | 図5 $X=0$$Z>0$の場合}}]
 [{{ articles:inverse_kinematics_05.png?200 | 図6 $X=0$,$Z<0$の場合}}]
-図5，図6の${^0x_{wrist}} ={^0y_{wrist}} = 0$すなわち式(5)の$X=0$の場合を考える．
+さらに図5，図6の${^0x_{wrist}} ={^0y_{wrist}} = 0$すなわち式(5)の$X=0$の場合を考える．
 図5は$Z>0$で，図6は$Z<0$である．
@@ 行 139: / 行 141: @@
 )).
-$\theta_3$は，式(7)で$X=0$としたものと同じになる．
+$\theta_3$は，式(8)で$X=0$としたものと同じになる．
 ただし，値が正のときに対応する姿勢は図5，図6に示す実線になる．
 $\theta_2$については，
 図5の$X>$の場合
-$\phi=0$でありこれについては式(8),$\theta_2$については式(9)で$X=0$としたものと同じになる．
+$\phi=0$でありこれについては式(8),$\theta_2$については式(10)で$X=0$としたものと同じになる．
 \\
 図6の$Z<0$の場合$\phi=\pi$であり式(8)で$X=0$としたものと同じになる．
 $\theta_2$についても，実線の姿勢はそこから余弦定理で求めた内角を減じたものになっているので，
-これも式(9)で整合が取れている．
+これも式(10)で整合が取れている．
 \\
 \\
@@ 行 159: / 行 161: @@
 また$\phi$も不定となるため$\theta_2$も不定になる．
-これらは式(7),(8),(9)で$X=0$，$Z=0$として得られる結果と同じである．
+これらは式(8),(9),(10)で$X=0$，$Z=0$として得られる結果と同じである．
+==== 解の存在範囲 ====
+解の存在範囲は式(7)から以下のように制限される．
+$$
+\ge \mathrm{cos}\, \theta_3 \ge -1
+$$
+すなわち
+$$
+(l_2+l_3)^2 \ge X^2+Z^2 \ge (l_2-l_3)^2
+$$
+これは，$\Sigma_1$の原点を中心とした半径$l_2+l_3$の球の内側で半径$|l_2-l_3|$の球の外側の範囲である．
+等号の場合，すなわちそれぞれの球面上であり，それぞれ腕が伸び切った状態（$\theta_3=0$），縮みきった状態（$\theta_3=\pi$）となっている．
+この場合は，式(8)の２つのが縮退して1つになる．
+ただし，縮みきった状態に関しては，関節の可動域を考えた場合，$\theta_3=-\pi$も解とな得る．
+実際には関節角度の制限や自身および環境との干渉があるために解の存在範囲や解の数はさらに制限される．
 ==== ３自由度アームの逆運動学のまとめ ====
 手先位置，
@@ 行 174: / 行 194: @@
 $$
 を計算する．
+このとき以下を満たさなければ解は存在しない．
+$$
+(l_2+l_3)^2 \ge X^2+Z^2 \ge (l_2-l_3)^2
+$$
+等号のときは，$\theta_2$の解が一つに縮退する．
 $X \ne 0$の一般姿勢の場合，
@@ 行 184: / 行 210: @@
 $$
 $$
-\theta_2 = \mathrm{atan2}\left({X \over Z}\right) \mp\mathrm{cos}^{-1} \left( {X^2+Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(X^2+Z^2)}}\right)
+\theta_2 = \mathrm{atan2}\left({X ,\; Z}\right) \mp\mathrm{cos}^{-1} \left( {X^2+Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(X^2+Z^2)}}\right)
 $$
 の2つの解が得られる．また，
@@ 行 195: / 行 221: @@
 $$
 $$
-\theta_2 = \mathrm{atan2}\left({-X \over Z}\right) \mp\mathrm{cos}^{-1} \left( {X^2+Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(X^2+Z^2)}}\right)
+\theta_2 = \mathrm{atan2}\left({-X ,\; Z}\right) \mp\mathrm{cos}^{-1} \left( {X^2+Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(X^2+Z^2)}}\right)
 $$
 の2つの解が得られ，合計4つの解が存在する．
 \\
-ただし，$\theta_3$が$0$もしくは$\pi$になる場合はそれぞれアームが伸びきった状態，縮みきった状態となり2つの解が1つに縮退し，
+ただし，前述のように$\theta_3$が$0$もしくは$\pi$になる場合はそれぞれアームが伸びきった状態，縮みきった状態となり2つの解が1つに縮退し，
-解の数が合計2つとなる特異姿勢になっている．
+解の数が合計2つとなる特異姿勢になっている
+（縮みきった状態に関しては，関節の可動域を考えた場合，$\theta_3=-\pi$も解とな得る）．
 \\
 \\
@@ 行 209: / 行 236: @@
 $$
 $$
-\theta_2 = \mathrm{atan2}\left({0 \over Z}\right) \mp\mathrm{cos}^{-1} \left( {Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(Z^2)}}\right)
+\theta_2 = \mathrm{atan2}\left({0 ,\; Z}\right) \mp\mathrm{cos}^{-1} \left( {Z^2+l_2^2-l_3^2} \over {2l_2\sqrt{(Z^2)}}\right)
 $$
 上記の$\theta_2$に関する式はさらに簡略化できるが場合分けが増えるのでここで止めておく．
@@ 行 218: / 行 245: @@
 \\
 \\
-いずれの場合も，$\mathrm{cos}^{-1}$の引数が$[-1,1]$の範囲でなければ解が存在しない．
-これは三角不等式（縮退した等号部分も含む）の条件になっている．
-具体的には，解が存在するのはリンク2の原点を中心として，
-半径$(l_2-l_3)^2$の球の外側で半径$(l_2+l_3)^2$の球の内側の領域となる
-((
-$r^2=X^2+Z^2$として計算すればすぐ分かる．
-))
-((
-もちろん関節角度のリミットやリンクの干渉を考慮するとさらに狭まる．
-))．
 ==== おまけのまとめ ====
@@ 行 243: / 行 259: @@
 このように解が複数存在するため，解の選択を誤ると手先の位置はほとんど変わらないのにアームの姿勢が大きく変化するということが起きるので解の選択には注意が必要である．
+この辺に関しては[[note_on_ik|逆運動学を用いた動作の注意点]]でもう少し詳しく説明する．
 ===== ６自由度アームの逆運動学 =====