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

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

•  キネマティクスは、関節角度からロボットアーム先端の位置・姿勢を計算する方法です
  キネマティクスの別名は、運動学と言います。
  アーム先端だけでなく、各関節の位置・姿勢も割り出せます。
  （逆キネマティクスと明確に区別するため、「順」キネマティクスや「順」運動学と呼ぶこともあります）
  
  

• DH法という、コンピュータにキネマティクス計算をやらせるのに便利な方法があります
  DH法は、リンク機構の記述方法の１つです。
  これを使うと、計算式をコンピュータが計算するのに都合が良い形に落とし込めます。

1. はじめに

自己紹介はこちらです。

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

2. 同次変換行列とキネマティクス

同次変換行列とは、
前回紹介した３$${\times}$$３回転行列$${R}$$と、
３次元位置ベクトル$${\bm{p}}$$と、
を組み合わせて、１つの行列にまとめたものです。
 
ベースの回転行列$${^0R_1}$$、ベースから第一関節までの３次元位置ベクトル$${\bm{^0p_1}}$$を下式とします。
（左上の$${0}$$はベースを意味し、右下の$${1}$$は第１関節を意味します）

$$
^0R_1=
\left[
\begin{array}{ccc}
N_{x0}&O_{x0}&A_{x0}\\
N_{y0}&O_{y0}&A_{y0}\\
N_{z0}&O_{z0}&A_{z0}
\end{array}
\right]\\
\bm{^0p_1}=
\left[
\begin{array}{c}
p_{x0}\\
p_{y0}\\
p_{z0}
\end{array}
\right]
$$

このとき、ベースから第１関節までの同次変換行列$${^0H_1}$$は下式になります。
（最下行の$${0}$$と$${1}$$は、同次変換行列のサイズを４$${\times}$$４にするための数合わせです）

$$
\begin{array}{ll}
^0H_1&=
\left[
\begin{array}{cc}
^0R_1&\bm{^0p_1}\\
\bm{0}&1
\end{array}
\right]\\
&=
\left[
\begin{array}{cccc}
N_{x0}&O_{x0}&A_{x0}&p_{x0}\\
N_{y0}&O_{y0}&A_{y0}&p_{y0}\\
N_{z0}&O_{z0}&A_{z0}&p_{z0}\\
0&0&0&1
\end{array}
\right]
\end{array}
\\
\left(
\begin{array}{l}
\bm{0}=
\left[
\begin{array}{ccc}
0&0&0
\end{array}
\right]
\\
\text{太字の}\bm{0}\text{は全要素が}0\text{のゼロベクトル}
\end{array}
\right)
$$

この同次変換行列は、ロボットの関節・リンク数だけ存在します。
（エンドエフェクタをリンクとみなし、数を＋１するケースも多いです）
 
同次変換行列をどんどん掛け合わせていけば、ベース原点に対し、
その関節がどこにあるか？
３次元的な回転姿勢がどうなっているか？
が分かります。

例として、２軸分の同次変換行列を掛け合わせてみます。

$$
\begin{array}{ll}
^0H_2&={^0H_1}{^1H_2}\\
&=
\left[
\begin{array}{cc}
^0R_1&\bm{^0p_1}\\
\bm{0}&1
\end{array}
\right]
\left[
\begin{array}{cc}
^1R_2&\bm{^1p_2}\\
\bm{0}&1
\end{array}
\right]\\
&=
\left[
\begin{array}{cc}
{^0R_1}{^1R_2}&^0R_1\bm{^1p_2}+\bm{^0p_1}\\
\bm{0}&1
\end{array}
\right]\\
&=
\left[
\begin{array}{cc}
^0R_2&\bm{^0p_2}\\
\bm{0}&1
\end{array}
\right]
\end{array}
$$

回転行列を掛け合わせれば、複数の回転を１つの回転行列にまとめられるのは、ロボット編（１）で説明しました。

このため、$${{^0R_1}{^1R_2}}$$の結果が、ベースからみた２軸分の回転行列$${^0R_2}$$になるのは分かってもらえると思います。
（通常、ベースは回転しませんが、回転可能なロボット用ポジショナをベースに使うケースは稀にあります）

ベースからみた第１関節の位置は、回転の影響を受けません。
しかし、ベースからみた第２関節の位置は、第１関節の回転による影響を受けます。

位置ベクトルに対して回転行列をかけると、回転後の位置が分かることはロボット編（１）で説明しました。

これらを総合的に考えると、$${\bm{^0p_2}}$$がベースからみた第２関節の位置になることも分かってもらえると思います。
（同次変換行列の最右下が$${0}$$でなく$${1}$$なのは、回転前の位置ベクトルをそのまま足し合わせていくためです）

これを最後まで続けていけば、最終的にアーム先端（またはエンドエフェクタ先端）の位置および３次元的な回転姿勢が求められます。

６軸垂直多関節ロボット（エンドエフェクタなし）の場合、ベースからみた同次変換行列は以下のように求められます。
（$${f}$$はフランジを意味しており、フランジとはアーム先端にあるエンドエフェクタ取付け部分です）

$$
\begin{array}{ll}
^0H_f&={^0H_1}{^1H_2}{^2H_3}{^3H_4}{^4H_5}{^5H_6}{^6H_f}\\
&=
\left[
\begin{array}{cc}
^0R_1&\bm{^0p_1}\\
\bm{0}&1
\end{array}
\right]
\left[
\begin{array}{cc}
^1R_2&\bm{^1p_2}\\
\bm{0}&1
\end{array}
\right]
\left[
\begin{array}{cc}
^2R_3&\bm{^2p_3}\\
\bm{0}&1
\end{array}
\right]
\left[
\begin{array}{cc}
^3R_4&\bm{^3p_4}\\
\bm{0}&1
\end{array}
\right]
\left[
\begin{array}{cc}
^4R_5&\bm{^4p_5}\\
\bm{0}&1
\end{array}
\right]
\left[
\begin{array}{cc}
^5R_6&\bm{^5p_6}\\
\bm{0}&1
\end{array}
\right]
\left[
\begin{array}{cc}
^6R_f&\bm{^6p_f}\\
\bm{0}&1
\end{array}
\right]\\
&=
\left[
\begin{array}{cc}
^0R_f&
+^0R_6\bm{^6p_f}+^0R_5\bm{^5p_6}+^0R_4\bm{^4p_5}+^0R_3\bm{^3p_4}+^0R_2\bm{^2p_3}+^0R_1\bm{^1p_2}+\bm{^0p_1}\\
\bm{0}&1
\end{array}
\right]\\
&=\left[
\begin{array}{cccc}
N_{xf}&O_{xf}&A_{xf}&p_{xf}\\
N_{yf}&O_{yf}&A_{yf}&p_{yf}\\
N_{zf}&O_{zf}&A_{zf}&p_{zf}\\
0&0&0&1
\end{array}
\right]
\end{array}
$$

$${p_{xf},p_{yf},p_{zf}}$$はベースからみた$${X,Y,Z}$$方向アーム先端位置、$${N_{xf} \sim A_{zf}}$$はアーム先端の３次元回転姿勢です。

つまり、全軸分の同次変換行列の掛け算が、そっくりそのままキネマティクスです。
（キネマティクス＝関節角度からアーム先端座標を求める計算）

ちなみに、ベースからでなく第$${n}$$関節から同次変換行列の掛け算をしていくと、第$${n}$$関節原点からみたアーム先端位置や各関節位置が求められます。

また、直動関節の場合、
回転しないので回転行列は単位行列、
位置ベクトルが一定値でなく変数
になります。

このように、同次変換行列は、様々な応用を利かせた使い方もできる便利なものです。

3. DH法（Denavit-Hartenbergの記法）

DH法の正式名称は、「Denavit-Hartenberg（デナビット-ハーテンベルグ）の記法」と言います。
また、この方法に使うパラメータは「DHパラメータ」と呼ばれます。

業界内では、「DHパラメータ」と言えば、それで「あぁ、DH法を使うんだな」と伝わるほど浸透している言葉です。

この方法では、関節間の位置関係を、以下の手順で表現します。
（４ステップ、$${\theta,d,a,\alpha}$$がDHパラメータ）

この方法を適用したとき、１軸あたりの同次変換行列は以下になります。

$$
\begin{array}{ll}
^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]\\
&=
\left[
\begin{array}{cccc}
C\theta_n&-S\theta_n&0&0\\
S\theta_n&C\theta_n&0&0\\
0&0&1&0\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
1&0&0&0\\
0&1&0&0\\
0&0&1&d_n\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
1&0&0&a_n\\
0&1&0&0\\
0&0&1&0\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
1&0&0&0\\
0&C\alpha_n&-S\alpha_n&0\\
0&S\alpha_n&C\alpha_n&0\\
0&0&0&1
\end{array}
\right]\\
\\
&\left(
\begin{array}{lll}
S\theta_n=sin\theta_n&,&C\theta_n=cos\theta_n\\
S\alpha_n=sin\alpha_n&,&C\alpha_n=cos\alpha_n
\end{array}
\right)
\end{array}
$$

ベースやエンドエフェクタを含めた全関節間の同次変換行列を、この形で表せます。
これは、形としては全く同じ計算の繰り返しで、キネマティクス計算ができることを意味します。
 

単純な繰り返し計算はコンピュータの得意分野なので、DH法はプログラミング・実装との相性が良い方法と言えます。

4. キネマティクス計算の具体例

ここでは、下図の３軸ロボットを例にキネマティクス計算をしてみます。

下表は、このロボットのDHパラメータをまとめたものです。
（モータで駆動するので、$${\theta_1,\theta_2,\theta_3}$$は任意に指定可能）

各同次変換行列は下式になります。

$$
\begin{array}{ll}
^0H_1&=
\left[
\begin{array}{cccc}
C\theta_0&-S\theta_0&0&0\\
S\theta_0&C\theta_0&0&0\\
0&0&1&l_0\\
0&0&0&1
\end{array}
\right]\\
&=
\left[
\begin{array}{cccc}
1&0&0&0\\
0&1&0&0\\
0&0&1&l_0\\
0&0&0&1
\end{array}
\right]
\end{array}
$$

$$
^1H_2=
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&0\\
S\theta_1&0&C\theta_1&0\\
0&-1&0&l_1\\
0&0&0&1
\end{array}
\right]
$$

$$
^2H_3=
\left[
\begin{array}{cccc}
C\theta_2&-S\theta_2&0&l_2\cdot C\theta_2\\
S\theta_2&C\theta_2&0&l_2\cdot S\theta_2\\
0&0&1&0\\
0&0&0&1
\end{array}
\right]
$$

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

これら全てを掛け合わせて、ベースからフランジまでの同次変換行列を求めます。

$$
\begin{array}{ll}
^0H_f&={^0H_1}{^1H_2}{^2H_3}{^3H_f}\\
&=
\left[
\begin{array}{cccc}
1&0&0&0\\
0&1&0&0\\
0&0&1&l_0\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&0\\
S\theta_1&0&C\theta_1&0\\
0&-1&0&l_1\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_2&-S\theta_2&0&l_2\cdot C\theta_2\\
S\theta_2&C\theta_2&0&l_2\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&l_3\cdot C\theta_3\\
S\theta_3&0&-C\theta_3&l_3\cdot S\theta_3\\
0&1&0&0\\
0&0&0&1
\end{array}
\right]\\
&=
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&0\\
S\theta_1&0&C\theta_1&0\\
0&-1&0&l_1+l_0\\
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_3\cdot(C\theta_2\cdot C\theta_3-S\theta_2\cdot S\theta_3)+l_2\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_3\cdot(S\theta_2\cdot C\theta_3+C\theta_2\cdot S\theta_3)+l_2\cdot S\theta_2\\
0&1&1&0\\
0&0&0&1
\end{array}
\right]\\
&=
\left[
\begin{array}{cccc}
C\theta_1&0&-S\theta_1&0\\
S\theta_1&0&C\theta_1&0\\
0&-1&0&l_1+l_0\\
0&0&0&1
\end{array}
\right]
\left[
\begin{array}{cccc}
C\theta_{23}&0&S\theta_{23}&l_3\cdot C\theta_{23}+l_2\cdot C\theta_2\\
S\theta_{23}&0&-C\theta_{23}&l_3\cdot S\theta_{23}+l_2\cdot S\theta_2\\
0&1&1&0\\
0&0&0&1
\end{array}
\right]\\
&=
\left[
\begin{array}{cccc}
C\theta_1\cdot C\theta_{23}&-S\theta_1&C\theta_1\cdot S\theta_{23}&(l_3\cdot C\theta_{23}+l_2\cdot C\theta_2)\cdot C\theta_1\\
S\theta_1\cdot C\theta_{23}&C\theta_1&S\theta_1\cdot S\theta_{23}&(l_3\cdot C\theta_{23}+l_2\cdot C\theta_2)\cdot S\theta_1\\
-S\theta_{23}&0&C\theta_{23}&-(l_3\cdot S\theta_{23}+l_2\cdot S\theta_2)+l_1+l_0\\
0&0&0&1
\end{array}
\right]\\
&\left(
\begin{array}{lll}
S\theta_{23}=sin(\theta_2+\theta_3)&,&C\theta_{23}=cos(\theta_2+\theta_3)
\end{array}
\right)
\end{array}
$$

計算結果から、アーム先端位置の３次元位置$${X_f,Y_f,Z_f}$$は以下になると分かります。

$$
\begin{array}{ll}
X_f&=(l_3\cdot C\theta_{23}+l_2\cdot C\theta_2)\cdot C\theta_1\\
Y_f&=(l_3\cdot C\theta_{23}+l_2\cdot C\theta_2)\cdot S\theta_1\\
Z_f&=-(l_3\cdot S\theta_{23}+l_2\cdot S\theta_2)+l_1+l_0
\end{array}
$$

また、ベースからアーム先端までの３次元回転行列$${^0R_f}$$は以下になると分かります。

$$
^0R_f=
\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]
$$

なお、今回は人間（私）が計算したので、$${{^0H_1}{^1H_2}}$$と$${{^2H_3}{^3H_f}}$$を計算をした後、それらを掛け合わせて$${^0H_f}$$を求めています。
（そうした方が計算しやすかったため）

しかし、コンピュータに計算させるなら、そうする必要はありません。
シンプルに、$${^0H_1}$$に対して、$${{^1H_2},{^2H_3},{^3H_f}}$$を順番に掛けていけばOKです。

コンピュータはこのような計算を得意とするため、掛け算の順番を変える等の工夫は不要です。

5. おわりに

この記事では、キネマティクスについて紹介・解説しました。

改めて言いますが、キネマティクスとは関節角度からアーム先端位置を計算する方法です。

ただ、実態としては、同次変換行列を掛け合わせて、４列目の要素を上から３つ取り出すだけの計算です。
（回転姿勢まで知りたい場合は、同次変換行列中の最右上から３$${\times}$$３行列を取り出します）

キネマティクスは難しい計算ではないと、分かってもらえたのではないかと思います。
確立された方法に乗っかれば、サラっとできます。

そうして節約できた頭のリソースは、もっと難しい、あるいは、複雑なことを考えるのに回すのが良いと思います。
 
次回は、ロボットの関節角速度からアーム先端速度を割り出す「ヤコビアン」を紹介・解説しようと思います。

前回：ロボット編（１）「ロボット記号と回転行列」はこちら
次回：ロボット編（３）「ヤコビアン」はこちら