実際に使える制御工学：ロボット編（４）「逆キネマティクス。アームの位置・姿勢に対応する関節角度の割り出し方法。」

＜この記事で伝えたい事＞ 

• 逆キネマティクスは、ロボットアーム先端の位置・姿勢から関節角度を計算する方法です
  逆キネマティクスの別名は、逆運動学です。
  アーム先端位置・姿勢だけでなく動作軌跡まで指定したいとき等にも使えます。
  （例えば、直線軌跡動作や円弧軌跡動作）
  
  

• 逆キネマティクスには、（順）キネマティクスほどには定番と言える方法がありません
  ロボット形状がどんなに複雑でも、同次変換行列を使えば、キネマティクスは必ず解けます。
  しかし、逆キネマティクスの場合は、そうもいきません。
  
  そこで、多くのメーカは、垂直多関節ロボットの形状を逆キネマティクスが解きやすい形に設計します。
  （このため、どこのメーカのロボットも似通った形状になります）

1. はじめに

自己紹介はこちらです。

この記事は、主にロボット制御に関わる学生や若手～中堅エンジニアに向けた技術紹介・解説記事シリーズの第４弾です。
 
今回は、ロボットの逆キネマティクスについて紹介・解説します。

2. ２リンクマニピュレータの逆キネマティクス

まずは、簡単な話から始めましょう。

下図の２リンクマニピュレータを考えます。

逆キネマティクスは、アーム先端の位置（や姿勢）が分かっていて、それに対応する関節角度を導出する問題です。

通常、逆キネマティクスは、関節角度とアーム先端位置との関係性を把握するところから始めます。

上図の２リンクマニピュレータの場合、その関係式は以下になります。

$$
\left\{
\begin{array}{ll}
X&=L_1\cdot C\theta_1+L_2\cdot C\theta_{12}\\
Y&=L_1\cdot S\theta_1+L_2\cdot S\theta_{12}
\end{array}
\right.
$$

$$
\left(
\begin{array}{lll}
S\theta_{1}=sin\theta_1&,&S\theta_{12}=sin(\theta_1+\theta_2)\\
C\theta_{1}=cos\theta_1&,&C\theta_{12}=cos(\theta_1+\theta_2)
\end{array}
\right)
$$

まず、アーム先端位置$${X,Y}$$の２乗和を取ります。

$$
\begin{array}{ll}
X^2+Y^2&={L_1}^2\cdot {C\theta_1}^2+2\cdot L_1\cdot L_2\cdot C\theta_1\cdot C\theta_{12}+{L_2}^2\cdot {C\theta_{12}}^2\\
&     +{L_1}^2\cdot {S\theta_1}^2+2\cdot L_1\cdot L_2\cdot S\theta_1\cdot S\theta_{12}+{L_2}^2\cdot {S\theta_{12}}^2\\
\\
&={L_1}^2\cdot({C\theta_1}^2+{S\theta_1}^2)+2\cdot L_1\cdot L_2\cdot(C\theta_{12}\cdot C\theta_1+S\theta_{12}\cdot S\theta_1)\\
&     +{L_2}^2\cdot({C\theta_{12}}^2+{S\theta_{12}}^2)\\
\\
&={L_1}^2+2\cdot L_1\cdot L_2\cdot (C\theta_{12}\cdot C\theta_1+S\theta_{12}\cdot C\theta_1)+{L_2}^2\\
\\
&                    (C\theta_{2}=cos\theta_2)
\end{array}
$$

加法定理$${(cos(\theta_a\pm\theta_b)=C\theta_a\cdot C\theta_b\mp S\theta_a\cdot S\theta_b)}$$より、数式を以下のように変形できます。

$$
\begin{array}{rl}
X^2+Y^2&={L_1}^2+2\cdot L_1\cdot L_2\cdot C\theta_2+{L_2}^2\\
2\cdot L_1\cdot L_2\cdot C\theta_2&=X^2+Y^2-({L_1}^2+{L_2}^2)\\
\end{array}
$$

$$
\begin{array}{c}
\therefore C\theta_2=\cfrac{X^2+Y^2-({L_1}^2+{L_2}^2)}{2\cdot L_1\cdot L_2}\\
(C\theta_2=cos\theta_2)
\end{array}
$$

よって、第２関節角度$${\theta_2}$$は以下になります。

$$
\theta_2=cos^{-1}
\left(
\cfrac{X^2+Y^2-({L_1}^2+{L_2}^2)}{2\cdot L_1\cdot L_2}
\right)
$$

さて、ここで思い出してみましょう。
$${cos\theta=cos(-\theta)}$$のため、$${cos^{-1}}$$は２パターンの解を持ちます。

この２つの解がどういう意味を持つかは、下図を見ると分かりやすいです。

アーム先端位置が同じでも、一点鎖線（ー・－・－のオレンジ色線）の右側にアームがあるパターンと、左側にアームがあるパターン（点線部）があります。

「解が２つある」ことは、アーム姿勢が２パターンあることを意味します。

工学分野での回転方向は、反時計回りがプラス、時計回りがマイナスです。

$${cos^{-1}}$$計算で得られた$${\theta_2}$$がプラスの数値なら$${\theta_2=\theta_{2a}}$$、マイナスの数値なら$${\theta_2=\theta_{2b}}$$です。
（$${\theta_{2a}}$$に対応する第１関節角度は$${\theta_{1a}}$$、$${\theta_{2b}}$$に対応する第１関節角度は$${\theta_{1b}}$$）

これを把握しないと、ロボットを思いどおりに動かすのは困難です。

ここまでで、$${\theta_2}$$が明らかになりました。
次は、第１関節角度$${\theta_1}$$を求めます。

そのために、関節角度とアーム先端位置の関係式を変形します。

$$
\begin{array}{ll}
X&=L_1\cdot C\theta_1+L_2\cdot C\theta_{12}\\
&=L_1\cdot C\theta_1+L_2\cdot (C\theta_1\cdot C\theta_2-S\theta_1\cdot S\theta_2)\\
&=(L_1+L_2\cdot C\theta_2)\cdot C\theta_1-L_2\cdot S\theta_2\cdot S\theta_1
\end{array}
$$

$$
\begin{array}{ll}
Y&=L_1\cdot S\theta_1+L_2\cdot S\theta_{12}\\
&=L_1\cdot S\theta_1+L_2\cdot (S\theta_1\cdot C\theta_2+C\theta_1\cdot S\theta_2)\\
&=L_2\cdot S\theta_2\cdot C\theta_1+(L_1+L_2\cdot C\theta_2)\cdot S\theta_1
\end{array}
$$

これらを行列形式でまとめます。

$$
\left[
\begin{array}{c}
X\\Y
\end{array}
\right]
=
\left[
\begin{array}{cc}
L_1+L_2\cdot C\theta_2&-L_2\cdot S\theta_2\\
L_2\cdot S\theta_2&L_1+L_2\cdot C\theta_2
\end{array}
\right]
\left[
\begin{array}{c}
C\theta_1\\S\theta_1
\end{array}
\right]
$$

逆行列計算をすれば$${cos\theta_1,sin\theta_1}$$が求められます。
（$${\theta_2}$$は導出済なので、$${cos\theta_2,sin\theta_2}$$の具体的な数値は分かっています）

$$
\left[
\begin{array}{c}
C\theta_1\\S\theta_1
\end{array}
\right]
=
\left[
\begin{array}{cc}
L_1+L_2\cdot C\theta_2&-L_2\cdot S\theta_2\\
L_2\cdot S\theta_2&L_1+L_2\cdot C\theta_2
\end{array}
\right]^{-1}
\left[
\begin{array}{c}
X\\Y
\end{array}
\right]
$$

$${cos\theta_1,sin\theta_1}$$が分かれば、$${tan^{-1}}$$計算によって$${\theta_1}$$を求められます。

$$
\theta_1=\tan^{-1}
\left(
\cfrac{S\theta_1}{C\theta_1}
\right)
$$

$${cos\theta_1,sin\theta_1}$$を計算するときの$${\theta_2}$$に$${\theta_{2a}}$$を使えば、$${\theta_{1a}}$$が求まります。
一方、$${\theta_{2b}}$$を使えば、$${\theta_{1b}}$$が求まります。

このようにして、２リンクマニピュレータのアーム先端位置から関節角度を求められます。

なお、実装の際は$${tan^{-1}}$$計算には、atan2関数を使うのが望ましいです。
atan2関数は象限も考慮した結果を返すので、実用性が高いです。

3. ６軸垂直多関節ロボットの逆キネマティクス

下図の６軸垂直多関節ロボットを考えます。
（アームのリーチを稼ぐため、$${L_{x1}\neq0}$$とするロボットはよくあります）

この６軸垂直多関節ロボットのDHパラメータは下表のとおりです。

この６軸垂直多関節ロボットの第５関節位置は、$${\theta_1,\theta_2,\theta_3}$$のみによって決まります。

本当にそうかは、キネマティクス計算をしてみれば分かります。

以下、各関節間の同時変換行列です。

$$
^nH_{n+1}=
\left[
\begin{array}{cccc}
C\theta_n&-S\theta_n\cdot C\alpha_n&S\theta_n\cdot S\alpha_n&a_n\cdot C\theta_n\\
S\theta_n&C\theta_n\cdot C\alpha_n&-C\theta_n\cdot S\alpha_n&a_n\cdot S\theta_n\\
0&S\alpha_n&C\alpha_n&d_n\\
0&0&0&1
\end{array}
\right]
$$

$$
^0H_{1}=
\left[
\begin{array}{cccc}
1&0&0&0\\
0&1&0&0\\
0&0&1&L_{z0}\\
0&0&0&1
\end{array}
\right]
$$

$$
^1H_{2}=
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&L_{x1}\cdot C\theta_1\\
S\theta_1&0&C\theta_1&L_{x1}\cdot S\theta_1\\
0&-1&0&L_{z1}\\
0&0&0&1
\end{array}
\right]
$$

$$
^2H_{3}=
\left[
\begin{array}{cccc}
C\theta_2&-S\theta_2&0&L_{x2}\cdot C\theta_2\\
S\theta_2&C\theta_2&0&L_{x2}\cdot S\theta_2\\
0&0&1&0\\
0&0&0&1
\end{array}
\right]
$$

$$
^3H_{4}=
\left[
\begin{array}{cccc}
C\theta_3&0&-S\theta_3&0\\
S\theta_3&0&C\theta_3&0\\
0&-1&0&0\\
0&0&0&1
\end{array}
\right]
$$

$$
^4H_{5}=
\left[
\begin{array}{cccc}
C\theta_4&0&S\theta_4&0\\
S\theta_4&0&-C\theta_4&0\\
0&1&0&L_{z4}\\
0&0&0&1
\end{array}
\right]
$$

$$
^5H_{6}=
\left[
\begin{array}{cccc}
C\theta_5&0&-S\theta_5&0\\
S\theta_5&0&C\theta_5&0\\
0&-1&0&0\\
0&0&0&1
\end{array}
\right]
$$

$$
^6H_{f}=
\left[
\begin{array}{cccc}
C\theta_6&-S\theta_6&0&0\\
S\theta_6&C\theta_6&0&0\\
0&0&1&L_{z6}\\
0&0&0&1
\end{array}
\right]
$$

第５関節位置は、$${^0H_1}$$～$${^4H_5}$$までの同次変換行列から求められます。

$$
\begin{array}{ll}
^0H_5&={^0H_1}{^1H_2}{^2H_3}{^3H_4}{^4H_5}\\
&=
\left[
\begin{array}{cccc}
1&0&0&0\\
0&1&0&0\\
0&0&1&L_{z0}\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&L_{x1}\cdot C\theta_1\\
S\theta_1&0&C\theta_1&L_{x1}\cdot S\theta_1\\
0&-1&0&L_{z1}\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_2&-S\theta_2&0&L_{x2}\cdot C\theta_2\\
S\theta_2&C\theta_2&0&L_{x2}\cdot S\theta_2\\
0&0&1&0\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_3&0&-S\theta_3&0\\
S\theta_3&0&C\theta_3&0\\
0&-1&0&0\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_4&0&S\theta_4&0\\
S\theta_4&0&-C\theta_4&0\\
0&1&0&L_{z4}\\
0&0&0&1
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&L_{x1}\cdot C\theta_1\\
S\theta_1&0&C\theta_1&L_{x1}\cdot S\theta_1\\
0&-1&0&L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_2\cdot C\theta_3-S\theta_2\cdot S\theta_3&0&-C\theta_2\cdot S\theta_3-S\theta_2\cdot C\theta_3&L_{x2}\cdot C\theta_2\\
S\theta_2\cdot C\theta_3+C\theta_2\cdot S\theta_3&0&-S\theta_2\cdot S\theta_3+C\theta_2\cdot C\theta_3&L_{x2}\cdot S\theta_2\\
0&-1&0&0\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_4&0&S\theta_4&0\\
S\theta_4&0&-C\theta_4&0\\
0&1&0&L_{z4}\\
0&0&0&1
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&L_{x1}\cdot C\theta_1\\
S\theta_1&0&C\theta_1&L_{x1}\cdot S\theta_1\\
0&-1&0&L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_{23}&0&-S\theta_{23}&L_{x2}\cdot C\theta_2\\
S\theta_{23}&0&C\theta_{23}&L_{x2}\cdot S\theta_2\\
0&-1&0&0\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_4&0&S\theta_4&0\\
S\theta_4&0&-C\theta_4&0\\
0&1&0&L_{z4}\\
0&0&0&1
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&L_{x1}\cdot C\theta_1\\
S\theta_1&0&C\theta_1&L_{x1}\cdot S\theta_1\\
0&-1&0&L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_{23}\cdot C\theta_4&-S\theta_{23}&C\theta_{23}\cdot S\theta_4&-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2\\
S\theta_{23}\cdot C\theta_4&C\theta_{23}&S\theta_{23}\cdot S\theta_4&L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2\\
-S\theta_4&0&C\theta_4&0\\
0&0&0&1
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{cccc}
C\theta_1\cdot C\theta_{23}\cdot C\theta_4+S\theta_1\cdot S\theta_4&-C\theta_1\cdot S\theta_{23}&C\theta_1\cdot C\theta_{23}\cdot S\theta_4-S\theta_1\cdot C\theta_4&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2)\cdot C\theta_1+L_{x1}\cdot C\theta_1\\
C\theta_1\cdot S\theta_{23}\cdot C\theta_4-C\theta_1\cdot S\theta_4&-S\theta_1\cdot S\theta_{23}&S\theta_1\cdot C\theta_{23}\cdot S\theta_4+C\theta_1\cdot C\theta_4&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2)\cdot S\theta_1+L_{x1}\cdot S\theta_1\\
-S\theta_{23}\cdot C\theta_4&-C\theta_{23}&-S\theta_{23}\cdot S\theta_4&-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)+L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{cccc}
C\theta_1\cdot C\theta_{23}\cdot C\theta_4+S\theta_1\cdot S\theta_4&-C\theta_1\cdot S\theta_{23}&C\theta_1\cdot C\theta_{23}\cdot S\theta_4-S\theta_1\cdot C\theta_4&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot C\theta_1\\
C\theta_1\cdot S\theta_{23}\cdot C\theta_4-C\theta_1\cdot S\theta_4&-S\theta_1\cdot S\theta_{23}&S\theta_1\cdot C\theta_{23}\cdot S\theta_4+C\theta_1\cdot C\theta_4&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot S\theta_1\\
-S\theta_{23}\cdot C\theta_4&-C\theta_{23}&-S\theta_{23}\cdot S\theta_4&-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)+L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\end{array}
$$

同次変換行列$${^0H_5}$$の最右列（の上から１～３番目）が、第５関節の位置ベクトルです。

よって、第５関節位置$${X_5,Y_5,Z_5}$$は以下のようになります。

$$
\begin{array}{ll}
X_5&=(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot C\theta_1\\
Y_5&=(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot S\theta_1\\
Z_5&=-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)+L_{z1}+L_{z0}
\end{array}
$$

これらから分かるように、第５関節位置に影響を与える関節角度は$${\theta_1,\theta_2,\theta_3}$$のみです。
$${\theta_4,\theta_5,\theta_6}$$は第５関節位置に影響しません。

この調子でアーム先端位置を計算してみましょう。
ひとまず、ベースから第６関節まで同次変換行列を計算します。

$$
\begin{array}{ll}
{^0H_6}&={^0H_5}{^5H_6}\\
&=
\left[
\begin{array}{cccc}
C\theta_1\cdot C\theta_{23}\cdot C\theta_4+S\theta_1\cdot S\theta_4&-C\theta_1\cdot S\theta_{23}&C\theta_1\cdot C\theta_{23}\cdot S\theta_4-S\theta_1\cdot C\theta_4&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot C\theta_1\\
C\theta_1\cdot S\theta_{23}\cdot C\theta_4-C\theta_1\cdot S\theta_4&-S\theta_1\cdot S\theta_{23}&S\theta_1\cdot C\theta_{23}\cdot S\theta_4+C\theta_1\cdot C\theta_4&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot S\theta_1\\
-S\theta_{23}\cdot C\theta_4&-C\theta_{23}&-S\theta_{23}\cdot S\theta_4&-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)+L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_5&0&-S\theta_5&0\\
S\theta_5&0&C\theta_5&0\\
0&-1&0&0\\
0&0&0&1
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{cccc}
N_{x6}&O_{x6}&A_{x6}&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot C\theta_1\\
N_{y6}&O_{y6}&A_{y6}&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot S\theta_1\\
N_{z6}&O_{z6}&A_{z6}&-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)+L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\end{array}
$$

ここで、$${N_{x6}}$$～$${A_{z6}}$$がいくつかは、さして重要ではありません。
また、第５関節と第６関節の位置が同じなのも、重要ではありません。

次は、$${^0H_6}$$に$${^6H_f}$$をかけて、ベースからアーム先端への同次変換行列を求めます。

$$
\begin{array}{ll}
{^0H_f}&={^0H_6}{^6H_f}\\
&=
\left[
\begin{array}{cccc}
N_{x6}&O_{x6}&A_{x6}&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot C\theta_1\\
N_{y6}&O_{y6}&A_{y6}&(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot S\theta_1\\
N_{z6}&O_{z6}&A_{z6}&-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)+L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_6&-S\theta_6&0&0\\
S\theta_6&C\theta_6&0&0\\
0&0&1&L_{z6}\\
0&0&0&1
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{cccc}
N_{x6}\cdot C\theta_6+O_{x6}\cdot S\theta_6&-N_{x6}\cdot S\theta_6+O_{x6}\cdot C\theta_6&A_{x6}&L_{z6}\cdot A_{x6}+(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot C\theta_1\\
N_{y6}\cdot C\theta_6+O_{y6}\cdot S\theta_6&-N_{y6}\cdot S\theta_6+O_{y6}\cdot C\theta_6&A_{y6}&L_{z6}\cdot A_{y6}+(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot S\theta_1\\
N_{z6}\cdot C\theta_6+O_{z6}\cdot S\theta_6&-N_{z6}\cdot S\theta_6+O_{z6}\cdot C\theta_6&A_{z6}&L_{z6}\cdot A_{z6}-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)+L_{z1}+L_{z0}\\
0&0&0&1
\end{array}
\right]
\end{array}
$$

ここで重要なのが、$${A_{x6}=A_{xf},A_{y6}=A_{yf},A_{z6}=A_{zf}}$$であることです。
これを踏まえると、アーム先端位置$${X_f,Y_f,Z_f}$$は以下になると分かります。

$$
\begin{array}{ll}
X_f&=L_{z6}\cdot A_{xf}+(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot C\theta_1\\
&=L_{z6}\cdot A_{xf}+X_5
\end{array}
$$

$$
\begin{array}{ll}
Y_f&=L_{z6}\cdot A_{yf}+(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot S\theta_1\\
&=L_{z6}\cdot A_{yf}+Y_5
\end{array}
$$

$$
\begin{array}{ll}
Z_f&=L_{z6}\cdot A_{zf}-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)+L_{z1}+L_{z0}\\
&=L_{z6}\cdot A_{zf}+Y_5
\end{array}
$$

これらの数式から、アーム先端位置・姿勢が与えられれば、以下のように第５関節位置の具体的数値が得られると分かります。

$$
\begin{array}{ll}
X_5&=X_f-L_{z6}\cdot A_{xf}\\
Y_5&=Y_f-L_{z6}\cdot A_{yf}\\
Z_5&=Z_f-L_{z6}\cdot A_{zf}
\end{array}
$$

前述のとおり、第５関節位置に影響を与える関節角度は$${\theta_1,\theta_2,\theta_3}$$のみです。

第５関節位置が分かれば、そこから$${\theta_1,\theta_2,\theta_3}$$だけを逆算できます。
$${\theta_1,\theta_2,\theta_3}$$だけ計算するのは、$${\theta_1}$$～$${\theta_6}$$を一気に計算するより格段に楽です。

こうして逆キネマティクスを解くのは、よく使われるテクニックです。

なんとなくでも、知っておいて損はありません。
「そういうものがある」とだけ知っておき、必要なとき詳しく調べればそれでOKです。

ここから、$${\theta_1,\theta_2,\theta_3}$$を求めていきます。
まず、$${X_5,Y_5}$$を再確認します。

$$
\begin{array}{ll}
X_5&=(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot C\theta_1\\
Y_5&=(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})\cdot S\theta_1
\end{array}
$$

これらを見ると、$${(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})}$$部分は共通で、それに掛かるのが$${C\theta_1}$$か$${S\theta_1}$$かの違いと分かります。

$${(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})}$$部分は$${\theta_2,\theta_3}$$によって変わりますが、$${X_5}$$と$${Y_5}$$の比率に$${\theta_2,\theta_3}$$は影響しません。

$${X_5}$$と$${Y_5}$$の比率は、$${\theta_1}$$のみで決まります。

この場合、$${tan^{-1}}$$計算で$${\theta_1}$$を求められます。

$$
\theta_1=tan^{-1}
\left(
\cfrac{Y_5}{X_5}
\right)
$$

$${\theta_2,\theta_3}$$の求め方は、２リンクマニピュレータの逆キネマティクス計算と似ています。
まず、$${X_5,Y_5}$$の２乗和をとります。

$$
\begin{array}{ll}
{X_5}^2+{Y_5}^2&=(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})^2\cdot {C\theta_1}^2\\
&          +(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})^2\cdot {S\theta_1}^2\\
&=(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})^2\cdot({C\theta_1}^2＋{S\theta_1}^2)\\
&=(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1})^2
\end{array}
$$

そして、両辺の平方根をとった後に、数式を整理します。

$$
\begin{array}{l}
\sqrt{{X_5}^2+{Y_5}^2}=-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2+L_{x1}
\\
\\
-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2=\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}\cdots(1)
\end{array}
$$

次に、第５関節の$${Z}$$方向位置の数式を変形します。

$$
-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2)=Z_5-(L_{z1}+L_{z0}) \cdots(2)
$$

(1)式と(2)式の両辺の２乗和は下式になります。

$$
\begin{array}{l}
(-L_{z4}\cdot S\theta_{23}+L_{x2}\cdot C\theta_2)^2+(-(L_{z4}\cdot C\theta_{23}+L_{x2}\cdot S\theta_2))^2
\\                                                  =\left(
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}
\right)^2+
\left(
Z_5-(L_{z1}+L_{z0})
\right)^2
\\
\\
{L_{z4}}^2\cdot {S\theta_{23}}^2-2\cdot L_{x2}\cdot L_{z4}\cdot S\theta_{23}\cdot C\theta_2 +{L_{x2}}^2\cdot {C\theta_{2}}^2\\
                         +{L_{z4}}^2\cdot {C\theta_{23}}^2+2\cdot L_{x2}\cdot L_{z4}\cdot C\theta_{23}\cdot S\theta_2 +{L_{x2}}^2\cdot {S\theta_{2}}^2\\
                                                  =\left(
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}
\right)^2+
\left(
Z_5-(L_{z1}+L_{z0})
\right)^2
\\
\\
{L_{z4}}^2\cdot ({S\theta_{23}}^2+{C\theta_{23}}^2)+{L_{x2}}^2\cdot ({S\theta_{2}}^2+{C\theta_{2}}^2)\\
                         -2\cdot L_{x2}\cdot L_{z4}\cdot(S\theta_{23}\cdot C\theta_2-C\theta_{23}\cdot S\theta_2)\\
                                                  =\left(
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}
\right)^2+
\left(
Z_5-(L_{z1}+L_{z0})
\right)^2
\\
\\
{L_{z4}}^2 +{L_{x2}}^2-2\cdot L_{x2}\cdot L_{z4}\cdot S\theta_3\\
                                                  =\left(
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}
\right)^2+
\left(
Z_5-(L_{z1}+L_{z0})
\right)^2
\\
\\
-2\cdot L_{x2}\cdot L_{z4}\cdot S\theta_3=
\left(
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}
\right)^2+
\left(
Z_5-(L_{z1}+L_{z0})
\right)^2-({L_{z4}}^2 +{L_{x2}}^2)
\end{array}
$$

$$
\begin{array}{l}
S\theta_3=\cfrac{
\left(
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}
\right)^2+
\left(
Z_5-(L_{z1}+L_{z0})
\right)^2-({L_{z4}}^2 +{L_{x2}}^2)
}
{-2\cdot L_{x2}\cdot L_{z4}}
\\
\\
\therefore \theta_3=sin^{-1}
\left(
\cfrac{
\left(
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}
\right)^2+
\left(
Z_5-(L_{z1}+L_{z0})
\right)^2-({L_{z4}}^2 +{L_{x2}}^2)
}
{-2\cdot L_{x2}\cdot L_{z4}}
\right)
\end{array}
$$

$${cos^{-1}}$$の解が２つあるように、$${sin^{-1}}$$の解も２つあります。
このあたりが、２リンクマニピュレータ逆キネマティクスと、６軸垂直多関節ロボットのそれが似ている点です。

ここまでで$${\theta_3}$$が求められたので、$${S\theta_3,C\theta_3}$$の具体的数値が得られます。

すると、(1)式から下式が得られます。

$$
\begin{array}{l}
-L_{z4}\cdot (S\theta_{2}\cdot C\theta_3+C\theta_2\cdot S\theta_3)+L_{x2}\cdot C\theta_2=\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}
\\
\\
-L_{z4}\cdot C\theta_{3}\cdot S\theta_2+(-L_{z4}\cdot S\theta_3+L_{x2})\cdot C\theta_2=\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}\cdots(3)
\end{array}
$$

また、(2)式から下式が得られます。

$$
\begin{array}{l}
-L_{z4}\cdot (C\theta_{2}\cdot C\theta_3-S\theta_2\cdot S\theta_3)-L_{x2}\cdot S\theta_2=Z_5-(L_{z1}+L_{z0})
\\
\\
(L_{z4}\cdot S\theta_3-L_{x2})\cdot S\theta_2-L_{z4}\cdot C\theta_3\cdot C\theta_2=Z_5-(L_{z1}+L_{z0})\cdots(4)
\end{array}
$$

(3),(4)式を行列形式でまとめると、下式になります。

$$
\left[
\begin{array}{cc}
-L_{z4}\cdot C\theta_{3}&-L_{z4}\cdot S\theta_3+L_{x2}\\
L_{z4}\cdot S\theta_3-L_{x2}&-L_{z4}\cdot C\theta_3
\end{array}
\right]
\left[
\begin{array}{c}
S\theta_2\\C\theta_2
\end{array}
\right]=
\left[
\begin{array}{c}
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}\\
Z_5-(L_{z1}+L_{z0})
\end{array}
\right]
$$

よって、逆行列計算を使って、以下のように$${S\theta_2,C\theta_2}$$を求められます。

$$
\begin{array}{ll}
\left[
\begin{array}{c}
S\theta_2\\C\theta_2
\end{array}
\right]
&=
\left[
\begin{array}{cc}
-L_{z4}\cdot C\theta_{3}&-L_{z4}\cdot S\theta_3+L_{x2}\\
L_{z4}\cdot S\theta_3-L_{x2}&-L_{z4}\cdot C\theta_3
\end{array}
\right]^{-1}
\left[
\begin{array}{c}
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}\\
Z_5-(L_{z1}+L_{z0})
\end{array}
\right]
\\
\\
&=\cfrac
{
\left[
\begin{array}{cc}
-L_{z4}\cdot C\theta_{3}&L_{z4}\cdot S\theta_3-L_{x2}\\
-L_{z4}\cdot S\theta_3+L_{x2}&-L_{z4}\cdot C\theta_3
\end{array}
\right]
\left[
\begin{array}{c}
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}\\
Z_5-(L_{z1}+L_{z0})
\end{array}
\right]
}
{{L_{z4}}^2\cdot {C\theta_3}^2+(L_{z4}\cdot S\theta_3-L_{x2})^2}
\\
\\
&=\cfrac
{
\left[
\begin{array}{cc}
-L_{z4}\cdot C\theta_{3}&L_{z4}\cdot S\theta_3-L_{x2}\\
-L_{z4}\cdot S\theta_3+L_{x2}&-L_{z4}\cdot C\theta_3
\end{array}
\right]
\left[
\begin{array}{c}
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}\\
Z_5-(L_{z1}+L_{z0})
\end{array}
\right]
}
{{L_{z4}}^2\cdot {C\theta_3}^2+{L_{z4}}^2\cdot {S\theta_3}^2-2\cdot L_{x2}\cdot L_{z4}\cdot S\theta_3 +{L_{x2}}^2}
\\
\\
&=\cfrac
{
\left[
\begin{array}{cc}
-L_{z4}\cdot C\theta_{3}&L_{z4}\cdot S\theta_3-L_{x2}\\
-L_{z4}\cdot S\theta_3+L_{x2}&-L_{z4}\cdot C\theta_3
\end{array}
\right]
\left[
\begin{array}{c}
\sqrt{{X_5}^2+{Y_5}^2}-L_{x1}\\
Z_5-(L_{z1}+L_{z0})
\end{array}
\right]
}
{{L_{z4}}^2-2\cdot L_{x2}\cdot L_{z4}\cdot S\theta_3 +{L_{x2}}^2}
\end{array}
$$

$${S\theta_2,C\theta_2}$$が求められれば、$${tan^{-1}}$$計算で$${\theta_2}$$を求められます。

$$
\theta_2=tan^{-1}
\left(
\cfrac{S\theta_2}{C\theta_2}
\right)
$$

$${tan^{-1}}$$計算によって$${\theta_2}$$を求める一連の流れも、２リンクマニピュレータと６軸垂直多関節ロボットの逆キネマティクスが似ています。

ここまでで、$${\theta_1,\theta_2,\theta_3}$$が明らかになりました。
このため、同時変換行列$${^0H_4}$$の数値が分かります。

ここからは、同次変換行列$${H}$$内の回転行列$${R}$$（最左上から３$${\times}$$３の部分）に着目して話を進めます。
アーム先端姿勢に対応する回転行列$${^0R_f}$$は、以下のように表せます。

$$
{^0R_f}={^0R_4}{^4R_f}
$$

よって、第４関節からアーム先端までの回転行列$${^4R_f}$$は以下になります。

$$
{^4R_f}={^0R_4}^{-1}{^0R_f}
$$

ここで、$${^0R_4}$$を確認します。

$$
\begin{array}{ll}
{^0R_4}&={^0R_1}{^1R_2}{^2R_3}{^3R_4}
\\
&=
\left[
\begin{array}{ccc}
1&0&0\\
0&1&0\\
0&0&1
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta_1&0&-S\theta_1\\
S\theta_1&0&C\theta_1\\
0&-1&0\\
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta_2&-S\theta_2&0\\
S\theta_2&C\theta_2&0\\
0&0&1
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta_3&0&-S\theta_3\\
S\theta_3&0&C\theta_3\\
0&-1&0
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{ccc}
C\theta_1&0&-S\theta_1\\
S\theta_1&0&C\theta_1\\
0&-1&0\\
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta_2\cdot C\theta_3-S\theta_2\cdot S\theta_3&0&-C\theta_2\cdot S\theta_3-S\theta_2\cdot C\theta_3\\
S\theta_2\cdot C\theta_3+C\theta_2\cdot S\theta_3&0&-S\theta_2\cdot S\theta_3+C\theta_2\cdot C\theta_3\\
0&-1&0
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{ccc}
C\theta_1&0&-S\theta_1\\
S\theta_1&0&C\theta_1\\
0&-1&0\\
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta_{23}&0&-S\theta_{23}\\
S\theta_{23}&0&C\theta_{23}\\
0&-1&0
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{ccc}
C\theta_1\cdot C\theta_{23}&S\theta_1&-C\theta_1\cdot S\theta_{23}\\
S\theta_1\cdot C\theta_{23}&-C\theta_1&-S\theta_1\cdot S\theta_{23}\\
-S\theta_{23}&0&-C\theta_{23}\\
\end{array}
\right]
\end{array}
$$

数式からも、$${\theta_1,\theta_2,\theta_3}$$が明らかであれば、$${^0R_4}$$の数値が分かると言えます。

また、逆キネマティクスでは、アーム先端位置・姿勢が与えられるので、$${^0R_f}$$は元より数値が分かっています。

よって、今の段階で$${^4R_f}$$は数値的に求められます。
行列$${^4R_f}$$は以下のようになるとしましょう。

$$
\begin{array}{ll}
{^4R_f}&={^0R_4}^{-1}{^0R_f}
\\
&=
\left[
\begin{array}{ccc}
{^4N_{xf}}&{^4O_{xf}}&{^4A_{xf}}\\
{^4N_{yf}}&{^4O_{yf}}&{^4A_{yf}}\\
{^4N_{zf}}&{^4O_{zf}}&{^4A_{zf}}
\end{array}
\right]
\end{array}
$$

後は、そこから$${\theta_4,\theta_5,\theta_6}$$をどう得るのか？というだけの話になります。
$${\theta_4,\theta_5,\theta_6}$$を得るため、$${^4R_f}$$がどうなるのか確認しましょう。

$$
\begin{array}{ll}
{^4R_f}&={^4R_5}{^5R_6}{^6R_f}\\
&=
\left[
\begin{array}{ccc}
C\theta_4&0&S\theta_4\\
S\theta_4&0&-C\theta_4\\
0&1&0
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta_5&0&-S\theta_5\\
S\theta_5&0&C\theta_5\\
0&-1&0
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta_6&-S\theta_6&0\\
S\theta_6&C\theta_6&0\\
0&0&1
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{ccc}
C\theta_4&0&S\theta_4\\
S\theta_4&0&-C\theta_4\\
0&1&0
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta_5\cdot C\theta_6&-C\theta_5\cdot S\theta_6&-S\theta_5\\
S\theta_5\cdot C\theta_6&-S\theta_5\cdot S\theta_6&C\theta_5\\
-S\theta_6&-C\theta_6&0
\end{array}
\right]
\\
\\
&=
\left[
\begin{array}{ccc}
C\theta_4\cdot C\theta_5\cdot C\theta_6-S\theta_4\cdot S\theta_6&-C\theta_4\cdot C\theta_5\cdot S\theta_6-S\theta_4\cdot C\theta_6&-C\theta_4\cdot S\theta_5\\
S\theta_4\cdot C\theta_5\cdot C\theta_6+C\theta_4\cdot S\theta_6&-S\theta_4\cdot C\theta_5\cdot S\theta_6+C\theta_4\cdot C\theta_6&-S\theta_4\cdot S\theta_5\\
S\theta_5\cdot C\theta_6&-S\theta_5\cdot S\theta_6&C\theta_5
\end{array}
\right]
\end{array}
$$

この結果から、$${{^4A_{zf}}=C\theta_5}$$と分かります。
よって、第５関節角度$${\theta_5}$$は下式で求められます。
（$${cos^{-1}}$$計算なので、２パターンの解があります）

$$
\theta_5=cos^{-1}(^4A_{zf})
$$

$${\theta_5}$$が得られたので、$${S\theta_5,C\theta_5}$$が分かります。
すると、$${\theta_4,\theta_6}$$は$${tan^{-1}}$$計算によって、以下のように求められます。

$$
\begin{array}{ll}
\theta_4&=tan^{-1}
\left(
\cfrac{-^4A_{yf}/S\theta_5}{-^4A_{xf}/S\theta_5}
\right)
\\
\\
&=tan^{-1}
\left(
\cfrac{S\theta_4}{C\theta_4}
\right)
\end{array}
$$

$$
\begin{array}{ll}
\theta_6&=tan^{-1}
\left(
\cfrac{-^4O_{zf}/S\theta_5}{^4N_{zf}/S\theta_5}
\right)
\\
\\
&=tan^{-1}
\left(
\cfrac{S\theta_6}{C\theta_6}
\right)
\end{array}
$$

ただし、これで$${\theta_4,\theta_6}$$が求められるのは、$${S\theta_5\neq0}$$の場合のみです。
$${S\theta_5=0}$$だとゼロ割が発生して、コンピュータが計算を実行できません。
 
$${S\theta_5=0}$$のときは、例外処理が必要です。
この例外処理は、$${C\theta_5=1}$$か$${C\theta_5=-1}$$かで変わります。

$${C\theta_5=1}$$のとき、$${{^4N_{xf}},{^4N_{yf}}}$$は以下になります。

$$
\begin{array}{ll}
{^4N_{xf}}&=C\theta_4\cdot C\theta_6-S\theta_4\cdot S\theta_6\\
&=C\theta_{46}
\end{array}
$$

$$
\begin{array}{ll}
{^4N_{yf}}&=S\theta_4\cdot C\theta_6+S\theta_4\cdot S\theta_6\\
&=S\theta_{46}
\end{array}
$$

$$
\left(
\begin{array}{l}
&S\theta_{46}=sin(\theta_4+\theta_6),\\
&C\theta_{46}=cos(\theta_4+\theta_6)
\end{array}
\right)
$$

したがって、$${tan^{-1}}$$計算によって、$${\theta_4+\theta_6}$$が求められます。

$$
\theta_4+\theta_6=tan^{-1}
\left(
\cfrac{^4N_{yf}}{^4N_{xf}}
\right)
$$

一方、$${C\theta_5=-1}$$のとき、$${{^4N_{xf}},{^4N_{yf}}}$$は以下になります。

$$
\begin{array}{ll}
{^4N_{xf}}&=-C\theta_4\cdot C\theta_6-S\theta_4\cdot S\theta_6\\
&=C\theta_{4-6}
\end{array}
$$

$$
\begin{array}{ll}
{^4N_{yf}}&=-S\theta_4\cdot C\theta_6+S\theta_4\cdot S\theta_6\\
&=-S\theta_{4-6}
\end{array}
$$

$$
\left(
\begin{array}{l}
&S\theta_{4-6}=sin(\theta_4-\theta_6),\\
&C\theta_{4-6}=cos(\theta_4-\theta_6)
\end{array}
\right)
$$

したがって、$${tan^{-1}}$$計算によって、$${\theta_4-\theta_6}$$が求められます。

$$
\theta_4-\theta_6=tan^{-1}
\left(
\cfrac{-^4N_{yf}}{-^4N_{xf}}
\right)
$$

このように、例外処理では$${\theta_4}$$と$${\theta_6}$$を分けて求めることはできません。
これが手首特異点$${\theta_5=0}$$で生じる現象です。

こういう場合、特異点に入る直前の$${\theta_4}$$を保持しておき、特異点に入ったら保持した$${\theta_4}$$をそのまま使うような工夫が必要です。

そのとき、$${\theta_6}$$は下式で求めます。

$$
\begin{array}{ll}
\theta_6&=tan^{-1}
\left(
\cfrac{^4N_{yf}}{^4N_{xf}}
\right)-\theta_4          (C\theta_5=1\text{のとき})
\end{array}
$$

$$
\begin{array}{ll}
\theta_6&=\theta_4-tan^{-1}
\left(
\cfrac{^4N_{yf}}{^4N_{xf}}
\right)          (C\theta_5=-1\text{のとき})
\end{array}
$$

このようにして、６軸垂直多関節ロボットのアーム先端位置・姿勢から関節角度$${\theta_1}$$～$${\theta_6}$$を求めます。

4. ヤコビアンを用いた逆キネマティクス

ロボット編（３）の4章前半部で、各関節の微小角度$${\delta\bm{\theta}}$$と、それによるアーム先端の微小変位$${\delta\bm{x}}$$の関係は下式になると言いました。
（$${J_{(\theta)}}$$はヤコビアン）

$$
\delta\bm{x}=J_{(\theta)}\delta\bm{\theta}
$$

ロボットは高速動作するモノと言っても、コントローラの制御周期１回（長くとも$${1_{[ms]}}$$程度）のうちに動く量は微小です。

現在のアーム先端位置・姿勢を$${\bm{x}_{cur}}$$、制御周期１回後のアーム先端位置・姿勢を$${\bm{x}_{next}}$$とすると、$${\delta\bm{x}=\bm{x}_{next}-\bm{x}_{cur}}$$とみなせます。

すると、アーム先端位置・姿勢の微小変化に応じた関節角度の微小変化量は以下になります。
（行列サイズの都合で$${{J_{(\theta)}}^{-1}}$$が計算できない場合、ロボット編（３）の4章後半部で紹介した疑似逆行列で代替可能）

$$
\delta\bm{\theta}={J_{(\theta)}}^{-1}(\bm{x}_{next}-\bm{x}_{cur})
$$

この$${\delta\bm{\theta}}$$を現在関節角度$${\bm{\theta}_{cur}}$$に足し込めば、１制御周期後に取るべき関節角度$${\bm{\theta}_{next}}$$が求められます。

$$
\bm{\theta}_{next}=\bm{\theta}_{cur}+\delta\bm{\theta}
$$

ただし、これは近似計算です。
真の正解とは、少し数値がズレると思っておいた方が良いです。

そこで、精度向上のため、よく使われる方法が繰り返し計算です。
繰り返し計算は、以下３ステップの計算を繰り返すものです。

この方法のメリット・デメリットは以下です。

メリット：どんなロボットにも使える
デメリット：計算量が多く、マシンパワーが必要（高コストになりがち）

5. おわりに

この記事では、逆キネマティクスについて紹介・解説しました。
 
これで、
・ （順）キネマティクス
・ ヤコビアン（と仮想仕事の原理）
・ 逆キネマティクス
と、ロボットの「関節座標」と「直交座標」を行き来する方法が出そろったと思います。

ロボットのモーション制御アルゴリズムを設計するときは、「関節座標」と「直交座標」のどちらかだけ見ていると、足をすくわれることがあります。
（例．直交座標では高軌跡精度動作ができても、関節には大きな負荷がかかってしまっていた）

そういう事がないよう、ある程度キリの良いところまできたら、関節座標と直交座標のどちらでも問題ない制御方法になっているかチェックするようにしましょう。

次回は、逆動力学計算である「ニュートン・オイラー法」を紹介・解説しようと思います。

前回：ロボット編（３）「ヤコビアン」はこちら
次回：ロボット編（５）「ニュートン・オイラー法」はこちら