逆運動学

逆運動学は、手先の位置と姿勢(角度)から軸の角度を求めます。

\[\newcommand\v[1]{\boldsymbol{#1}} \newcommand\r[1]{\mathrm{#1}}\]

tcpからp5に向かう単位ベクトル \(\v{e_5}\) は

\[\begin{split}\begin{flalign} & \theta_2 = \pi - \theta \\ & \v{e_5} = (\cos \theta_2, \sin \theta_2) \end{flalign}\end{split}\]

p5の位置は

\[p5 = tcp + L5 \cdot \v{e_5}\]

p5からp4に向かう単位ベクトル \(\v{e_4}\) は

\[\v{e_4} = \r{rot}(\v{e_5}, \frac{\pi}{2})\]

p4の位置は

\[p4 = p5 + L4 \cdot \v{e_4}\]

p4とp2の距離を \(L_a\) とします。

\[L_a = \| p4 - p2 \|\]

三角形p2, p3, p4で、p4の角度を \(\alpha\) とすると、余弦定理から

\[\cos \alpha = \frac{L3^2 + L_a^2 - L2^2}{2 \cdot L3 \cdot L_a}\]

\(cos\) の逆関数 \(\arccos\) を使い

\[\alpha = \arccos (\cos \alpha)\]

p4からp2に向かう単位ベクトルを \(\v{e_a}\) とすると

\[\v{e_a} = \r{unit}(p2 - p4)\]

p4からp3に向かう単位ベクトル \(\v{e_3}\) は

\[\v{e_3} = \r{rot}(\v{e_a}, - \alpha)\]

p3の位置は

\[p3 = p4 + L3 \cdot \v{e_3}\]

p2, p3, p4, p5, tcpのすべての位置が分かったので、それぞれの軸の角度は容易に計算できます。

\[j1 = \arctan \frac{y}{x}\]

\[j2 = \arctan(p3 - p2) - \frac{\pi}{2}\]

\[j3 = \arctan(p4 - p3) - \arctan(p3 - p2)\]

\[j4 = \arctan(tcp - p5) - \arctan(p4 - p3)\]

\[j5 = \phi\]