IK 逆運動学 入門：２リンクのIKを解く（余弦定理）

合成公式よりこっちの方がシンプルだった。

やること

２本のアームと２つの回転軸からなる平面上のアームロボットについて、
与えられた座標にアームの先端が来るような軸の角度を逆運動学の計算で求めます。
前回は合成公式をつかいましたが、余弦定理を使う方法を教えてもらいました。よりスマートです。
・前回記事：IK 逆運動学 入門：２リンクのIKを解く（合成公式）
・次回記事：IK 逆運動学 入門：Processing3で2リンクアームを逆運動学で動かす

難易度

高校の数Iぐらいのレベルです。
（三角関数、逆三角関数のごく初歩的な解説は省いています。）

参考

・Watako-Lab.：AIさんの足の逆運動学(IK)を幾何的に解く
・ロボット開発の技術情報：逆運動学 (inverse kinematics)

当記事は、ほぼ上記の記事を書き直したような内容になります。

ステップ1:　順運動学を理解する

まず順運動学を考えます。
順運動学はアームの長さと角度を与えた場合に、先端がどの座標を示すかを計算するものです。
図のように緑のアームと青のアームが回転軸で接続されている時、先端の座標は、

　\\
x = L_1\cos(\theta_1) + L_2\cos(\theta_1+\theta_2)\\
y = L_1\sin(\theta_1) + L_2\sin(\theta_1+\theta_2)

となります。この説明は省きます。

ステップ2:　補助線を引いて三角を作る

図形の性質を使って逆運動学を解いていきます。

L1,L2を2辺とする三角形ができるように補助円（赤い点線）を加えます。
これで余弦定理を当てはめる準備が整いました。

ステップ3:　余弦定理を思い出す

ここで余弦定理を復習します。
詳しい内容は高校の教科書にお任せするとして、せっかくの機会ですので二名川流の余弦定理の覚え方（思い出し方）を示しておきます。

高校時代は数学の問題を解かずにこんな語呂合わせばかり作っていました。なかでもこの一作は言葉にほとんど無駄がなく、言葉からの数式再現性も高い理想的な仕上がりになっています。この手の語呂が山ほどあるのでどこかでまとめて紹介します。

ステップ4:　余弦定理でθ1を求める

　\\
余弦定理　a^2 = b^2 + c^2 -2bc\cos A に上図のβを当てはめると\\
　\\
L_2\!^2 = L_1\!^2 + (\sqrt{x^2+y^2})^2-2L_1\sqrt{x^2+y^2}\cos\beta \\
　\\
変形すると\\
　\\
\cos\beta= \frac{L_1\!^2 -L_2\!^2 + (x^2+y^2)}{2L_1\sqrt{x^2+y^2}}\\
　\\
\beta= \arccos(\frac{L_1\!^2 -L_2\!^2 + (x^2+y^2)}{2L_1\sqrt{x^2+y^2}})\\
　\\
また、\tan\gamma=\frac{y}{x}\,より\\
\gamma=\arctan(\frac{y}{x})\\\
　\\
図より\,\theta_1 = \gamma-\beta\,なので\\
　\\
\theta_1 = \arctan(\frac{y}{x}) - \arccos(\frac{L_1\!^2 -L_2\!^2 + (x^2+y^2)}{2L_1\sqrt{x^2+y^2}})\\
　\\
これで\,\theta_1\,が決まりました。\\

ステップ5:　余弦定理でθ2を求める

　\\
余弦定理　a^2 = b^2 + c^2 -2bc\cos A に上図のαを当てはめると\\
　\\
(\sqrt{x^2+y^2})^2 = L_1\!^2 + L_2\!^2 -2L_1L_2\cos\alpha \\
　\\
変形すると\\
　\\
\cos\alpha= \frac{L_1\!^2 + L_2\!^2 - (x^2+y^2)}{2L_1L_2}\\
　\\
\alpha= \arccos(\frac{L_1\!^2 + L_2\!^2 - (x^2+y^2)}{2L_1L_2})\\
　\\
図より\,\theta_2 = \pi-\alpha\,なので\\
　\\
\theta_2 = \pi- \arccos(\frac{L_1\!^2 + L_2\!^2 - (x^2+y^2)}{2L_1L_2})\\
　\\
これで\,\theta_2\,も決まりました。\\

ステップ6:　結論を並べる

\theta_1 = \arctan(\frac{y}{x}) - \arccos(\frac{L_1\!^2 -L_2\!^2 + (x^2+y^2)}{2L_1\sqrt{x^2+y^2}})\\
　\\
\theta_2 = \pi- \arccos(\frac{L_1\!^2 + L_2\!^2 - (x^2+y^2)}{2L_1L_2})\\
　\\
これがθ_1、θ_2を(x,y)から求める場合の計算式になります。　\\

合成公式と比べて

計算式が圧倒的にシンプルになりました。
θ1は合成公式で導いた場合と同じ式になりましたが、θ2はarccosのみを使うため、角度により条件分けが必要なarctanを使う場合よりもプログラムが少しラクになります。

次回

他にも始点と終点それぞれにアームの長さを半径とする円を描いてその交点と始点、終点を結ぶ方法などもありそうです。
次回はこれをProcessing3上でシミュレーションできるプログラムを紹介しようと思います。

・次回記事：IK 逆運動学 入門：Processing3で2リンクアームを逆運動学で動かす

へんなところがあったらご指摘ください。