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

Last updated at 2024-06-03Posted at 2020-08-26

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

やること

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

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

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

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

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)\\

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

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

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

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

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

余弦定理　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\,が決まりました。\\

余弦定理　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\,も決まりました。\\

\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上でシミュレーションできるプログラムを紹介しようと思います。

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