順運動学

ロボットの3軸それぞれの番号を$n$とする．

n = 0,1,2

このとき，根本のヨー角に取り付けられたモータを0番目，根元のピッチ角に取り付けられたモータを1番目，残りのピッチ角に取り付けられたモータを2番目とする．

各軸の現在角$\theta_{n}$，リンクの長さ$L_1$，$L_2$が既知であり，ロボット原点からアーム先端までの距離$\mathbf{d}=[d_{x},d_{y},d_{z}]$を求めた場合は以下の順運動学式で求められる．

\begin{array}{}

L_{xy} &= L_1 \cos{\theta_1} + L_2 \cos{(\theta_1 + \theta_2)}\\
d_{x} &= L_{xy} \cos{\theta_0} \\
d_{y} &= L_{xy} \sin{\theta_0} \\
d_{z} &= L_1 \sin{\theta_1} + L_2 \sin{(\theta_1 + \theta_2)}

\end{array}

逆運動学

逆に，ロボット原点からアーム先端までの距離$\mathbf{d}=[d_{x},d_{y},d_{z}]$から各軸の角度$\theta_{n}$を求めたい場合は以下の逆運動学式を用いる．

\begin{array}{}
\theta_{0} & = \; \; \; 
\tan^{-1}(\frac{d_{y}}{d_{x}}) \\

\theta_{1} & = 
-{\cos^{-1}(\frac{d_{x}^2 + d_{y}^2  + d_{z}^2 - L_1^2 - L_2^2 }{2L_1L_2})}\\

\theta_{2} & =
-\sin^{-1}(\frac{L_1 + L_2\cos(\theta_{1})}{\sqrt{d_{x}^2 + d_{y}^2  + d_{z}^2}}) + \tan^{-1}\frac{d_{z}}{\sqrt{d_{x}^2 + d_{y}^2}} + \frac{1}{2}\pi
\end{array}