順運動学

軸の角度から手先の位置と姿勢(角度)を求めるのが順運動学です。

このロボットアームには6個のモーターがあり、各モーターの回転軸をアームの根本からJ1, J2, J3, J4, J5, J6とします。

J1の軸とアームの台との交点がp1です。

J2, J3, J4の位置がp2, p3, p4です。

p4からJ5の軸に下ろした垂線の足がp5です。

アームの手先の位置がtcp(tool center point)です。

これを真横から見た側面図です。

点と点の間の距離を図のようにL1, L2, L3, L4, L5とします。

\[ \begin{align}\begin{aligned}\newcommand\v[1]{\boldsymbol{#1}} \newcommand\r[1]{\mathrm{#1}}\\p2 = (0, L1)\end{aligned}\end{align} \]

p1での回転角をj1とします。

図ではj2は原点位置から時計回りに回転しているので値はマイナスです。

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

\[\v{e_2} = \r{rot}((1,0), \frac{\pi}{2} + j2)\]

p3の位置は

\[p3 = p2 + L2 \cdot \v{e_2}\]

ここで \(\r{rot}(\v{v}, \theta)\) はベクトル \(\v{v}\) を \(\theta\) 度回転したベクトルです。

p3での回転角をj3とします。

図ではj3は原点位置から時計回りに回転しているので値はマイナスです。

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

\[\v{e_3} = \r{rot}(\r{unit}(p3 - p2), j3)\]

ここで \(\r{unit}(\v{v})\) は \(\v{v}\) の単位ベクトルです。

p4の位置は

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

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

\[\v{e_4} = \r{rot}(\r{unit}(p4 - p3), j4 + \frac{\pi}{2})\]

p5の位置は

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

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

\[\v{e_5} = \r{rot}(\r{unit}(p4 - p3), j4)\]

tcpの位置は

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