実際に使える制御工学：ロボット編（１）「ロボット記号と回転行列。ロボット制御における基本のキ。」

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

• 垂直多関節ロボットは、関節の回転によって動きます
  このため、「関節（軸）をどう回せば、ロボットがどう動くか？」を把握するのは重要です。
  （直動関節を持つロボットもありますが、その動きは直感だけでもかなり分かると思います）
  
  

• 回転行列を使えば、同じ計算でどんな回転にも対応できます
  同じ計算でOKというのは、実装・プログラミングをする上で、とても便利な性質です。

1. はじめに

自己紹介はこちらです。

この記事は、主にロボット制御に関わる学生や若手～中堅エンジニアに向けた技術紹介・解説記事シリーズの第１弾です。
 
今回は、ロボット記号＋$${\alpha}$$と回転行列を紹介・解説します。

2. よく使われるロボット記号

ここでは、業界内で慣例的によく使われるロボット記号を紹介します。
（JIS規格等で決められた正式なものとは、多少違うかもしれません）
 
下図は、回転関節を表す記号です。

回転軸が向かって正面方向にある場合、◎で回転関節を表します。

回転軸が縦や横向きの場合、対角線の入った◇で回転関節を表します。
（対角線の向きで、回転軸の向きを見分けます）

下図は、エンドエフェクタ（ツール）とベース（土台）を表す記号です。

下向きのコの字記号が、エンドエフェクタ（ツール）を表す記号です。
（ロボットアーム先端がどこを向いているかで記号の向きも変わります）

線の下に斜線が入っている記号が、ベースを表す記号です。
（ロボットを壁掛け設置する場合、この記号は縦向きになります）

また、ロボット記号ではありませんが、３次元座標を平面的に捉えたい場合、下２図のような軸の描き方をします。

手前側が軸のプラス方向の場合、◎（内側の丸は黒ぬり）で表します。
一方、奥側が軸のプラス方向の場合、〇の中に×を入れて表します。

下図は、これらを組み合わせて描いた６軸垂直多関節ロボットです。
軸方向は描かなくとも良いですが、描いてあると図を見ながら会話しやすいことが多いです。

また、たまに使うのが、下図の重心記号です。

この記号は、運動方程式導出などのため、重心位置を明示したいときに使います。

3. X,Y,Z軸まわりの回転行列

我々も、ロボットも、三次元空間内に存在します。
三次元空間内には、３つの軸を取れます。
通常、この３軸は$${X,Y,Z}$$軸と名付けられます。
 
ロボット分野では、この３軸まわりの回転をよく使います。
ここでは、これらを紹介しようと思います。

■$${\bm{Z}}$$軸まわりの回転

$${Z}$$軸まわりに角度$${\phi_{[rad]}}$$だけ回転させることを考えます。

回転前の軸を$${X,Y,Z}$$、回転後の軸を$${X’,Y’,Z’}$$とします。
$${Z}$$軸は回転前後で変わりません。
このため、$${Z’=Z}$$です。

回転前後の$${XY}$$軸の関係は、下式で表せます。

$$
\begin{array}{ll}
X'&=X\cdot cos\phi-Y\cdot sin\phi\\
Y'&=X\cdot sin\phi+Y\cdot cos\phi
\end{array}
$$

これらを行列形式でまとめると、以下のようになります。

$$
\begin{array}{c}
\left[
\begin{array}{c}
X'\\
Y'\\
Z'
\end{array}
\right]
=
\left[
\begin{array}{ccc}
C\phi&-S\phi&0\\
S\phi&C\phi&0\\
0&0&1\\
\end{array}
\right]
\left[
\begin{array}{c}
X\\
Y\\
Z
\end{array}
\right]\\
\\
(C\phi=cos\phi,S\phi=sin\phi)
\end{array}
$$

回転前の$${X,Y,Z}$$を回転後の$${X’,Y’,Z’}$$に変換する行列が、$${Z}$$軸まわりの回転行列$${R_{(z:\phi)}}$$です。

$$
R_{(z:\phi)}=
\left[
\begin{array}{ccc}
C\phi&-S\phi&0\\
S\phi&C\phi&0\\
0&0&1
\end{array}
\right]
$$

$${X,Y}$$軸まわりの回転行列も、同じ要領で求められます。

■$${\bm{Y}}$$軸まわりの回転

$${Y}$$軸まわりに角度$${\theta_{[rad]}}$$だけ回転させることを考えます。

回転前の軸を$${X,Y,Z}$$、回転後の軸を$${X’,Y’,Z’}$$とします。
$${Y}$$軸は回転前後で変わりません。
このため、$${Y’=Y}$$です。

回転前後の$${XZ}$$軸の関係は、下式で表せます。

$$
\begin{array}{ll}
X'&=X\cdot cos\theta+Z\cdot sin\theta\\
Z'&=-X\cdot sin\theta+Z\cdot cos\theta
\end{array}
$$

これらを行列形式でまとめると、以下のようになります。

$$
\begin{array}{c}
\left[
\begin{array}{c}
X'\\
Y'\\
Z'
\end{array}
\right]
=
\left[
\begin{array}{ccc}
C\theta&0&S\theta\\
0&1&0\\
-S\theta&0&C\theta\\
\end{array}
\right]
\left[
\begin{array}{c}
X\\
Y\\
Z
\end{array}
\right]\\
\\
(C\theta=cos\theta,S\theta=sin\theta)
\end{array}
$$

回転前の$${X,Y,Z}$$を回転後の$${X’,Y’,Z’}$$に変換する行列が、$${Y}$$軸まわりの回転行列$${R_{(y:\theta)}}$$です。

$$
R_{(y:\theta)}=
\left[
\begin{array}{ccc}
C\theta&0&S\theta\\
0&1&0\\
-S\theta&0&C\theta
\end{array}
\right]
$$

■$${\bm{X}}$$軸まわりの回転

$${X}$$軸まわりに角度$${\psi_{[rad]}}$$だけ回転させることを考えます。

回転前の軸を$${X,Y,Z}$$、回転後の軸を$${X’,Y’,Z’}$$とします。
$${X}$$軸は回転前後で変わりません。
このため、$${X’=X}$$です。

回転前後の$${YZ}$$軸の関係は、下式で表せます。

$$
\begin{array}{ll}
Y'&=Y\cdot cos\psi-Z\cdot sin\psi\\
Z'&=Y\cdot sin\psi+Z\cdot cos\psi
\end{array}
$$

これらを行列形式でまとめると、以下のようになります。

$$
\begin{array}{c}
\left[
\begin{array}{c}
X'\\
Y'\\
Z'
\end{array}
\right]
=
\left[
\begin{array}{ccc}
1&0&0\\
0&C\psi&-S\psi\\
0&S\psi&C\psi\\
\end{array}
\right]
\left[
\begin{array}{c}
X\\
Y\\
Z
\end{array}
\right]\\
\\
(C\psi=cos\psi,S\psi=sin\psi)
\end{array}
$$

回転前の$${X,Y,Z}$$を回転後の$${X’,Y’,Z’}$$に変換する行列が、$${X}$$軸まわりの回転行列$${R_{(x:\psi)}}$$です。

$$
R_{(x:\psi)}=
\left[
\begin{array}{ccc}
1&0&0\\
0&C\psi&-S\psi\\
0&S\psi&C\psi
\end{array}
\right]
$$

4. 回転行列の性質

4.1 回転後の位置が分かること

$${Y}$$軸まわり回転を具体例に説明します。
 
まず、下左図のように、$${X}$$方向に真っすぐ伸びたリンクがあるとします。
そして、このリンクの長さは$${l_{[m]}}$$とします。

このとき、リンク先端位置は$${X,Y,Z}$$の３次元ベクトル形式だと、下式になります。
（リンク先端位置の３次元ベクトルを$${\bm{L}}$$とします）

$$
\bm{L}=
\left[
\begin{array}{c}
X\\
Y\\
Z
\end{array}
\right]
=
\left[
\begin{array}{c}
l\\
0\\
0
\end{array}
\right]
$$

このリンク先端位置ベクトル$${\bm{L}}$$に回転行列を掛けることで、回転後のリンク先端位置ベクトル$${\bm{L’}}$$が求められます。

今回は、$${Y}$$軸まわりに$${30\degree}$$回転させた場合を計算します。
（角度は反時計回りがプラス方向）

計算式は以下のとおりです。

$$
\begin{array}{ll}
\bm{L'}&=R_{(y:30\degree)}\bm{L}\\
&=
\left[
\begin{array}{ccc}
cos30\degree&0&sin30\degree\\
0&1&0\\
-sin30\degree&0&co30\degree
\end{array}
\right]
\left[
\begin{array}{c}
l\\
0\\
0
\end{array}
\right]\\
&=
\left[
\begin{array}{c}
l\cdot cos30\degree\\
0\\
-l\cdot sin30\degree
\end{array}
\right]
\end{array}
$$

下図は、この回転を図示したものです。

この図から、回転行列を使えば、回転後のリンク先端位置が求められるのが分かると思います。

関節角度からアーム先端位置を計算する「キネマティクス」にとって、非常に重要な性質です。

4.2 複数回の回転を扱えること

例えば、$${Y}$$軸まわりに$${30\degree}$$回転させた後、そこから更に$${45\degree}$$回転させたいとします。
（合計で$${75\degree}$$回転）

$${30\degree}$$回転の回転行列と$${45\degree}$$回転の回転行列を掛け合わせると、$${75\degree}$$回転の回転行列が得られます。

これは数式で確認できます。
（加法定理より、$${sin\alpha\cdot cos\beta\pm sin\beta\cdot cos\alpha=sin(\alpha\pm\beta),cos\alpha\cdot cos\beta\mp sin\alpha\cdot sin\beta=cos(\alpha\pm\beta)）}$$

$$
\begin{array}{l}
\left[
\begin{array}{ccc}
cos30\degree&0&sin30\degree\\
0&1&0\\
-sin30\degree&0&cos30\degree\\
\end{array}
\right]
\left[
\begin{array}{ccc}
cos45\degree&0&sin45\degree\\
0&1&0\\
-sin45\degree&0&cos45\degree
\end{array}
\right]\\
=
\left[
\begin{array}{ccc}
cos30\degree\cdot cos45\degree-sin30\degree\cdot sin45\degree&0&cos30\degree\cdot sin45\degree+sin30\degree\cdot cos45\degree\\
0&1&0\\
-sin30\degree\cdot cos45\degree-cos30\degree\cdot sin45\degree&0&-sin30\degree\cdot sin45\degree+cos30\degree\cdot cos45\degree
\end{array}
\right]\\
=
\left[
\begin{array}{ccc}
cos75\degree&0&sin75\degree\\
0&1&0\\
-sin75\degree&0&cos75\degree\\
\end{array}
\right]
\end{array}
$$

なお、「複数回の回転」というのは、１方向の回転に限りません。
あらゆる方向の回転を扱えます。

例えば、$${Y}$$軸まわり$${30\degree}$$回転→$${Z}$$軸まわり$${60\degree}$$回転→$${X}$$軸まわり$${-45\degree}$$回転と３回の回転をさせる回転行列$${R_{xzy}}$$は以下のように求められます。 

$$
R_{xzy}=R_{(y:30\degree)}R_{(z:60\degree)}R_{(x:-45\degree)}
$$

 
ロボットを動かすと、アーム先端側ほど多くの回転が加わります。

行列の掛け算だけで複数の回転の結果が分かるのは非常に便利で、押さえるべきポイントです。

4.3 回転の順序で結果が変わること

同じ軸まわりに同じ角度だけ回転させたつもりでも、回転の順序が違うと結果が変わります。
（高校～大学で教わる「行列の掛け算は、順序によって結果が変わる」という性質によるものです）

例えば、
X軸まわりに回転→Y軸まわりに回転と、
Y軸まわりに回転→X軸まわりに回転と、
いう２回だけの回転ですら、１まとめにした回転行列が変わります。

$$
\begin{array}{ll}
R_{(x:\psi)}R_{(y:\theta)}&=
\left[
\begin{array}{ccc}
1&0&0\\
0&C\psi&-S\psi\\
0&S\psi&C\psi\\
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta&0&S\theta\\
0&1&0\\
-S\theta&0&C\theta\\
\end{array}
\right]\\
&=
\left[
\begin{array}{ccc}
C\theta&0&S\theta\\
S\psi S\theta&C\psi&-S\psi C\theta\\
-C\psi S\theta&S\psi&C\psi C\theta\\
\end{array}
\right]
\end{array}
$$

$$
\begin{array}{ll}
R_{(y:\theta)}R_{(x:\psi)}&=
\left[
\begin{array}{ccc}
C\theta&0&S\theta\\
0&1&0\\
-S\theta&0&C\theta\\
\end{array}
\right]
\left[
\begin{array}{ccc}
1&0&0\\
0&C\psi&-S\psi\\
0&S\psi&C\psi\\
\end{array}
\right]\\
&=
\left[
\begin{array}{ccc}
C\theta&S\theta S\psi&S\theta C\psi\\
0&C\psi&-S\psi\\
-S\theta&C\theta S\psi&C\theta C\psi\\
\end{array}
\right]
\end{array}
$$

「新しい回転の回転行列は、右側に掛けていく」
というのが正しい順序です。

複数の関節を持つロボットを扱うには、これも押さえるべきポイントです。

4.4 回転行列の逆行列＝転置行列であること

回転行列の場合、逆行列と転置行列が一致します。
数式で表すと、以下のようになります。
（添え字$${T}$$は転置行列を表す）

$$
R^{-1}=R^T
$$

例えば、Y軸まわりに回転→X軸まわりに回転させる回転行列の逆行列は、以下のように求められます。

$$
\begin{array}{ll}
R_{(y:\theta)}R_{(x:\psi)}=
\left[
\begin{array}{ccc}
C\theta&S\theta S\psi&S\theta C\psi\\
0&C\psi&-S\psi\\
-S\theta&C\theta S\psi&C\theta C\psi\\
\end{array}
\right]
\end{array}
$$

$$
\begin{array}{ll}
(R_{(y:\theta)}R_{(x:\psi)})^{-1}&=(R_{(y:\theta)}R_{(x:\psi)})^T\\
&=
\left[
\begin{array}{ccc}
C\theta&0&-S\theta\\
S\theta S\psi&C\psi&C\theta S\psi\\
S\theta C\psi&-S\psi&C\theta C\psi\\
\end{array}
\right]
\end{array}
$$

３$${\times}$$３行列だと、逆行列計算に必要な計算量もそれなりに多いです。
この性質を利用すれば、行列の中身の入れ替えだけで逆行列が求まって便利です。

なお、回転行列逆行列をかけることは、逆回転させることを意味します。
回転後のリンク先端位置から、回転前のリンク先端位置を逆算したいとき、逆行列が使えます。
 
また、回転行列の行列式は、必ず１になります。
このため、回転行列には必ず逆行列が存在します。

使う場面は比較的少ないですが、たまに役立つ場面がある性質です。

5. ３次元回転の表現方法

３$${\times}$$３の回転行列は、ロボットアーム先端の３次元姿勢を表現するのにも使えます。
（最根本から最手先までの全関節分の回転行列を掛けたもの＝アーム先端の姿勢を表す回転行列）

ただ、アーム先端姿勢を指定するのに、９つ（＝３$${\times}$$３）ものパラメータを入力するのは、ユーザーにとって負担です。
（しかも、ただ入力するだけでなく、回転行列ならではの制約条件も考慮する必要があります）
 
そこで、よく使われるのが３，４つのパラメータで３次元回転を表現する方法です。
その代表的なものは以下２つです。

• オイラー角（使うパラメータは３つ）

• クォータニオン（使うパラメータは４つ）

5.1 オイラー角

一口にオイラー角と言っても、１２パターンのオイラー角があります。

その中でよく使われるのが、

① $${\bm{Z}}$$軸まわりに$${\bm{\phi}}$$回転$${\\}$$
② $${\bm{Y}}$$軸まわりに$${\bm{\theta}}$$回転$${\\}$$
③ $${\bm{X}}$$軸まわりに$${\bm{\psi}}$$回転

という順序で回転するものです。

これを数式で表すと、以下のようになります。

$$
\begin{array}{ll}
R_{(\phi,\theta,\psi)}&=R_{(z:\phi)}R_{(y:\theta)}R_{(x:\psi)}\\
&=
\left[
\begin{array}{ccc}
C\phi&-S\phi&0\\
S\phi&C\phi&0\\
0&0&1\\
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta&0&S\theta\\
0&1&0\\
-S\theta&0&C\theta\\
\end{array}
\right]
\left[
\begin{array}{ccc}
1&0&0\\
0&C\psi&-S\psi\\
0&S\psi&C\psi\\
\end{array}
\right]\\
&=
\left[
\begin{array}{ccc}
C\phi&-S\phi&0\\
S\phi&C\phi&0\\
0&0&1\\
\end{array}
\right]
\left[
\begin{array}{ccc}
C\theta&S\theta S\psi&S\theta C\psi\\
0&C\psi&-S\psi\\
-S\theta&C\theta S\psi&C\theta C\psi\\
\end{array}
\right]\\
&=
\left[
\begin{array}{ccc}
C\phi C\theta&C\phi S\theta S\psi -S\phi C\psi&C\phi S\theta C\psi+S\phi S\psi\\
S\phi C\theta&S\phi S\theta S\psi +C\phi C\psi&S\phi S\theta C\psi-C\phi S\psi\\
-S\theta&C\theta S\psi&C\theta C\psi\\
\end{array}
\right]
\end{array}
$$

これで、$${\phi,\theta,\psi}$$の３つのパラメータで３次元回転を表現できます。

5.2 クォータニオン

この方法は、
「回転軸方向を表す３次元位置ベクトルを指定し、そのベクトルを中心にどれだけ回転するか？」
という切り口で回転状態を表す方法です。

回転軸方向を表す３次元位置ベクトル$${\bm{n}}$$は、以下のようになります。

$$
\bm{n}=
\left[
\begin{array}{c}
n_x\\
n_y\\
n_z
\end{array}
\right]
$$

ここで、ベクトル$${\bm{n}}$$を単位ベクトルとし、ベクトル$${\bm{n}}$$で表した回転軸まわりに$${\theta}$$回転させる回転行列$${R}$$は以下になります。
（単位ベクトル＝大きさが１のベクトル、ベクトルの向きは任意）

$$
R=
\left[
\begin{array}{c}
C\theta+{n_x}^2(1-C\theta)&n_x n_y(1-C\theta)-n_z S\theta&n_x n_z(1-C\theta)+n_y S\theta\\
n_x n_y(1-C\theta)+n_z S\theta&C\theta+{n_y}^2(1-C\theta)&n_y n_z(1-C\theta-1)-n_x S\theta\\
n_x n_z(1-C\theta)-n_y S\theta&n_y n_z(1-C\theta)+n_x S\theta&C\theta+{n_z}^2(1-C\theta)
\end{array}
\right]
$$

$$
\left(
\begin{array}{l}
\text{単位ベクトルなので、}\\
          \sqrt{{n_x}^2+{n_y}^2+{n_z}^2}=1
\end{array}
\right)
$$

これで、$${n_x,n_y,n_z,\theta}$$の４つのパラメータで３次元回転を表現できます。

オイラー角では、２つの軸が重なって、どっちがどっちか分からなくなる「ジンバルロック」と呼ばれる状態が発生し得ますが、クォータニオンにはそれがないというメリットがあります。

6. おわりに

この記事では、ロボット記号と回転行列について紹介・解説しました。
 
回転というのは、ロボット動作を理解するのに非常に重要です。
押さえるべきポイントはいくつかありますが、例によって丸暗記の必要はありません。
 
何かあったとき、
「そういえば、こんなのがあったかも？」
と頭をよぎる程度に知っておけばOKです。

正確な事は、必要なときに調べれば良いです。
（それを繰り返すうちに自然に覚えます）
 
なお、今回の内容は、本格的な話に入る前の準備的なものです。

次回から、本格的な話をしていこうと思います。

前回：ロボット編ロードマップはこちら
次回：ロボット編（２）「キネマティクス」はこちら