はじめまして、りょーつといいます。高専出身の大学院1年生です。研究の専門は力学や機構学で、主に制御工学や数学に関する記事を書いています。
本稿はスクリュー理論の解説(布教)をする4つ目の記事です。前回までの記事は以下のリンクを参照してください。
スクリュー理論① 外積の歪対称行列表現
スクリュー理論② 行列の指数関数
スクリュー理論③ 行列指数と回転行列の関係
スクリュー理論は剛体の運動を記述する手法で、ロボット工学などでよく使われています。日本の高専や大学ではDH法を使った記述を学ぶことが多いですが、国際的にはスクリュー理論を使った記述が一般的となってきているようです。スクリュー理論を扱う日本語の文献は少ないので、この記事が誰かの助けになればいいなと思います。
目次
1.はじめに
2.歪対称行列の積
3.回転軸の行列指数
4.回転行列の成分表示
5.おわりに
1. はじめに
本稿では行列指数で表される回転行列の具体的な成分表示について紹介します。前回までの記事では、回転行列が$e^{[\hat{\boldsymbol{\omega}}]\theta}$という記号で表されるよく分からない$3\times 3$行列という風に扱ってきました。本記事ではその中身について注目するので、少しカロリーの高い計算が続きますが、回転行列の実体がイメージしやすくなると思います。
2. 歪対称行列の積
大きさが$1$の回転軸$\hat{\boldsymbol{\omega}}$を考えます。
\hat{\boldsymbol{\omega}}
=
\begin{bmatrix}
\omega_1 \\
\omega_2 \\
\omega_3 \\
\end{bmatrix}
\tag{1}
||\hat{\boldsymbol{\omega}}||
=
\sqrt{{\omega_1}^2 + {\omega_2}^2 + {\omega_3}^2}
=
1
\tag{2}
この回転軸の歪対称行列表現は第1回の記事を参考に以下のように表現されます。
[\hat{\boldsymbol{\omega}}]
=
\begin{bmatrix}
0 & -\omega_3 & \omega_2 \\
\omega_3 & 0 & -\omega_1 \\
-\omega_2 & \omega_1 & 0 \\
\end{bmatrix}
=
-{[\hat{\boldsymbol{\omega}}]}^T
\tag{3}
本章では(3)式の歪対称行列の積について考えてみます。手始めに2乗してみましょう。
{[\hat{\boldsymbol{\omega}}]}^2
=
[\hat{\boldsymbol{\omega}}][\hat{\boldsymbol{\omega}}]
=
\begin{bmatrix}
0 & -\omega_3 & \omega_2 \\
\omega_3 & 0 & -\omega_1 \\
-\omega_2 & \omega_1 & 0 \\
\end{bmatrix}
\begin{bmatrix}
0 & -\omega_3 & \omega_2 \\
\omega_3 & 0 & -\omega_1 \\
-\omega_2 & \omega_1 & 0 \\
\end{bmatrix}
=
\begin{bmatrix}
-{\omega_2}^2 - {\omega_3}^2 & \omega_1 \omega_2 & \omega_1 \omega_3 \\
\omega_1 \omega_2 & -{\omega_1}^2 - {\omega_3}^2 & \omega_2 \omega_3 \\
\omega_1 \omega_3 & \omega_2 \omega_3 & -{\omega_1}^2 - {\omega_2}^2 \\
\end{bmatrix}
\tag{4}
ちょっとややこしい見た目をしていますが、歪対称行列$[\hat{\boldsymbol{\omega}}]$の2乗${[\hat{\boldsymbol{\omega}}]}^2$は対称行列になります。
{[\hat{\boldsymbol{\omega}}]}^2
=
{\big({[\hat{\boldsymbol{\omega}}]}^2\big)}^T
\tag{5}
次に$[\hat{\boldsymbol{\omega}}]$を3乗してみましょう。
{[\hat{\boldsymbol{\omega}}]}^3
=
[\hat{\boldsymbol{\omega}}]{[\hat{\boldsymbol{\omega}}]}^2
=
\begin{bmatrix}
0 & -\omega_3 & \omega_2 \\
\omega_3 & 0 & -\omega_1 \\
-\omega_2 & \omega_1 & 0 \\
\end{bmatrix}
\begin{bmatrix}
-{\omega_2}^2 - {\omega_3}^2 & \omega_1 \omega_2 & \omega_1 \omega_3 \\
\omega_1 \omega_2 & -{\omega_1}^2 - {\omega_3}^2 & \omega_2 \omega_3 \\
\omega_1 \omega_3 & \omega_2 \omega_3 & -{\omega_1}^2 - {\omega_2}^2 \\
\end{bmatrix}
=
\begin{bmatrix}
0 & \omega_3({\omega_1}^2 + {\omega_2}^2 + {\omega_3}^2) & -\omega_2({\omega_1}^2 + {\omega_2}^2 + {\omega_3}^2) \\
-\omega_3({\omega_1}^2 + {\omega_2}^2 + {\omega_3}^2) & 0 & \omega_1({\omega_1}^2 + {\omega_2}^2 + {\omega_3}^2) \\
\omega_2({\omega_1}^2 + {\omega_2}^2 + {\omega_3}^2) & -\omega_1({\omega_1}^2 + {\omega_2}^2 + {\omega_3}^2) & 0 \\
\end{bmatrix}
\tag{5}
けっこう複雑な式ですが、(5)式に(2)式を代入することで以下の関係が得られます。
{[\hat{\boldsymbol{\omega}}]}^3
=
\begin{bmatrix}
0 & \omega_3 & -\omega_2 \\
-\omega_3 & 0 & \omega_1 \\
\omega_2 & -\omega_1 & 0 \\
\end{bmatrix}
=
-[\hat{\boldsymbol{\omega}}]
\tag{6}
とてもきれいになりましたね。${[\hat{\boldsymbol{\omega}}]}^3$も$[\hat{\boldsymbol{\omega}}]$の定数倍なので歪対称行列になります。これまでのルールをもとに4乗以降も計算していくと..?
{[\hat{\boldsymbol{\omega}}]}^4
=
{[\hat{\boldsymbol{\omega}}]}^3[\hat{\boldsymbol{\omega}}]
=
\big( -[\hat{\boldsymbol{\omega}}] \big)[\hat{\boldsymbol{\omega}}]
=
-{[\hat{\boldsymbol{\omega}}]}^2
\tag{7}
{[\hat{\boldsymbol{\omega}}]}^5
=
{[\hat{\boldsymbol{\omega}}]}^4[\hat{\boldsymbol{\omega}}]
=
\big( -{[\hat{\boldsymbol{\omega}}]}^2 \big)[\hat{\boldsymbol{\omega}}]
=
-{[\hat{\boldsymbol{\omega}}]}^3
=
[\hat{\boldsymbol{\omega}}]
\tag{8}
{[\hat{\boldsymbol{\omega}}]}^6
=
{[\hat{\boldsymbol{\omega}}]}^5[\hat{\boldsymbol{\omega}}]
=
\big( [\hat{\boldsymbol{\omega}}] \big)[\hat{\boldsymbol{\omega}}]
=
{[\hat{\boldsymbol{\omega}}]}^2
\tag{9}
{[\hat{\boldsymbol{\omega}}]}^7
=
{[\hat{\boldsymbol{\omega}}]}^6[\hat{\boldsymbol{\omega}}]
=
\big( {[\hat{\boldsymbol{\omega}}]}^2 \big)[\hat{\boldsymbol{\omega}}]
=
{[\hat{\boldsymbol{\omega}}]}^3
=
-[\hat{\boldsymbol{\omega}}]
\tag{10}
{[\hat{\boldsymbol{\omega}}]}^8
=
{[\hat{\boldsymbol{\omega}}]}^7[\hat{\boldsymbol{\omega}}]
=
\big( -[\hat{\boldsymbol{\omega}}] \big)[\hat{\boldsymbol{\omega}}]
=
-{[\hat{\boldsymbol{\omega}}]}^2
\tag{11}
\vdots
なんだか規則性が見えてきましたね。奇数乗したら$[\hat{\boldsymbol{\omega}}]$、偶数乗したら${[\hat{\boldsymbol{\omega}}]}^2$が登場します。この規則をもとに(6)~(11)式を一般化すると以下のようにまとめられます。
{[\hat{\boldsymbol{\omega}}]}^{2k-1}
=
{(-1)}^{k-1}[\hat{\boldsymbol{\omega}}]
\tag{12}
{[\hat{\boldsymbol{\omega}}]}^{2k}
=
{(-1)}^{k-1}{[\hat{\boldsymbol{\omega}}]}^2
\tag{13}
3. 回転軸の行列指数
本章では回転軸$[\hat{\boldsymbol{\omega}}]$の行列指数を考えてみましょう。第2回の記事で紹介した定義に基づいて、行列指数を級数の形で表すと以下のようになります。
e^{[\hat{\boldsymbol{\omega}}]\theta}
=
\sum_{n=1}^\infty \dfrac{1}{n!} {[\hat{\boldsymbol{\omega}}]}^n \theta^n
\tag{14}
ここではさらに(14)式を(12)(13)式を用いて分解していきます。
\sum_{n=1}^\infty \dfrac{1}{n!} {[\hat{\boldsymbol{\omega}}]}^n \theta^n
=
I
+
\sum_{k=1}^\infty \dfrac{1}{(2k-1)!} {[\hat{\boldsymbol{\omega}}]}^{2k-1} \theta^{2k-1}
+
\sum_{k=1}^\infty \dfrac{1}{(2k)!} {[\hat{\boldsymbol{\omega}}]}^{2k} \theta^{2k}
=
I
+
\sum_{k=1}^\infty \dfrac{{(-1)}^{k-1}}{(2k-1)!} [\hat{\boldsymbol{\omega}}] \theta^{2k-1}
+
\sum_{k=1}^\infty \dfrac{{(-1)}^{k-1}}{(2k)!} {[\hat{\boldsymbol{\omega}}]}^2 \theta^{2k}
=
I
+
[\hat{\boldsymbol{\omega}}]
\sum_{k=0}^\infty \dfrac{{(-1)}^{k}}{(2k+1)!} \theta^{2k+1}
-
{[\hat{\boldsymbol{\omega}}]}^2
\sum_{k=1}^\infty \dfrac{{(-1)}^{k}}{(2k)!} \theta^{2k}
=
I
+
[\hat{\boldsymbol{\omega}}]
\sum_{k=0}^\infty \dfrac{{(-1)}^{k}}{(2k+1)!} \theta^{2k+1}
-
{[\hat{\boldsymbol{\omega}}]}^2
\Bigg(
-1 + \sum_{k=0}^\infty \dfrac{{(-1)}^{k}}{(2k)!} \theta^{2k}
\Bigg)
\tag{15}
ここで、三角関数の級数表示が
\sin{\theta}
=
\sum_{k=0}^\infty \dfrac{{(-1)}^{k}}{(2k+1)!} \theta^{2k+1}
\tag{16}
\cos{\theta}
=
\sum_{k=0}^\infty \dfrac{{(-1)}^{k}}{(2k)!} \theta^{2k}
\tag{17}
であることを(15)式に代入すれば
e^{[\hat{\boldsymbol{\omega}}]\theta}
=
I
+
[\hat{\boldsymbol{\omega}}]
\sin{\theta}
+
{[\hat{\boldsymbol{\omega}}]}^2
\big(
1 - \cos{\theta}
\big)
\tag{18}
のようにシンプルな形で記述できます。なお(18)式からわかるように、回転行列は対称行列$I, {[\hat{\boldsymbol{\omega}}]}^2$と歪対称行列$[\hat{\boldsymbol{\omega}}]$の重ね合わせで記述できます。
4. 回転行列の成分表示
第3章で、回転行列$e^{[\hat{\boldsymbol{\omega}}]\theta}$が(18)式のようにいくつかの行列の和で表されることが分かりました。本章では(18)式の右辺を展開し、具体的な成分を求めてみようと思います。(2)式を変形することで
-{\omega_2}^2 - {\omega_3}^2
=
-1 + {\omega_1}^2
\tag{19}
-{\omega_1}^2 - {\omega_3}^2
=
-1 + {\omega_2}^2
\tag{20}
-{\omega_1}^2 - {\omega_2}^2
=
-1 + {\omega_3}^2
\tag{21}
となることに注意しながらゴリゴリと(18)式を展開していきます。
e^{[\hat{\boldsymbol{\omega}}]\theta}
=
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
+
\sin{\theta}
\begin{bmatrix}
0 & -\omega_3 & \omega_2 \\
\omega_3 & 0 & -\omega_1 \\
-\omega_2 & \omega_1 & 0 \\
\end{bmatrix}
+
\big(
1 - \cos{\theta}
\big)
\begin{bmatrix}
-{\omega_2}^2 - {\omega_3}^2 & \omega_1 \omega_2 & \omega_1 \omega_3 \\
\omega_1 \omega_2 & -{\omega_1}^2 - {\omega_3}^2 & \omega_2 \omega_3 \\
\omega_1 \omega_3 & \omega_2 \omega_3 & -{\omega_1}^2 - {\omega_2}^2 \\
\end{bmatrix}
=
\begin{bmatrix}
1 & -\omega_3\sin{\theta} & \omega_2\sin{\theta} \\
\omega_3\sin{\theta} & 1 & -\omega_1\sin{\theta} \\
-\omega_2\sin{\theta} & \omega_1\sin{\theta} & 1 \\
\end{bmatrix}
+
\big(
1 - \cos{\theta}
\big)
\begin{bmatrix}
-1+{\omega_1}^2 & \omega_1 \omega_2 & \omega_1 \omega_3 \\
\omega_1 \omega_2 & -1+{\omega_2}^2 & \omega_2 \omega_3 \\
\omega_1 \omega_3 & \omega_2 \omega_3 & -1+{\omega_3}^2 \\
\end{bmatrix}
\therefore
e^{[\hat{\boldsymbol{\omega}}]\theta}
=
\begin{bmatrix}
\cos{\theta} + {\omega_1}^2\big(1 - \cos{\theta}\big) & \omega_1\omega_2\big(1 - \cos{\theta}\big)-\omega_3\sin{\theta} & \omega_1\omega_3\big(1 - \cos{\theta}\big) + \omega_2\sin{\theta} \\
\omega_1\omega_2\big(1 - \cos{\theta}\big) + \omega_3\sin{\theta} & \cos{\theta} + {\omega_2}^2\big(1 - \cos{\theta}\big) & \omega_2\omega_3\big(1 - \cos{\theta}\big) - \omega_1\sin{\theta} \\
\omega_1\omega_3\big(1 - \cos{\theta}\big)-\omega_2\sin{\theta} & \omega_2\omega_3\big(1 - \cos{\theta}\big)+\omega_1\sin{\theta} & \cos{\theta} + {\omega_3}^2\big(1 - \cos{\theta}\big) \\
\end{bmatrix}
\tag{22}
ずいぶんエグイ式になりましたが、(22)式が回転行列の中身となります。さらに特殊な場合について考えてみましょう。
$x$軸まわりの回転を考えてみます。このときの回転軸$\hat{\boldsymbol{\omega}}_x$は
\hat{\boldsymbol{\omega}}_x
=
\begin{bmatrix}
1 \\
0 \\
0 \\
\end{bmatrix}
\tag{23}
であり、回転行列$e^{[\hat{\boldsymbol{\omega}}_x]\theta}$は
e^{[\hat{\boldsymbol{\omega}}_x]\theta}
=
\begin{bmatrix}
\ \ 1 \ & 0 & 0 \\
\ \ 0 & \cos{\theta} & -\sin{\theta} \\
\ \ 0 & \sin{\theta} & \cos{\theta} \\
\end{bmatrix}
\tag{24}
になります。同様に$y$軸、$z$軸を
\hat{\boldsymbol{\omega}}_y
=
\begin{bmatrix}
0 \\
1 \\
0 \\
\end{bmatrix}
\tag{25}
\hat{\boldsymbol{\omega}}_z
=
\begin{bmatrix}
0 \\
0 \\
1 \\
\end{bmatrix}
\tag{26}
のように設定すると、各回転軸に対応する回転行列は以下のような見た目になります。
e^{[\hat{\boldsymbol{\omega}}_y]\theta}
=
\begin{bmatrix}
\cos{\theta} & \ 0 \ & \sin{\theta} \\
0 & \ 1 \ & 0 \\
-\sin{\theta} & \ 0 \ & \cos{\theta} \\
\end{bmatrix}
\tag{27}
e^{[\hat{\boldsymbol{\omega}}_z]\theta}
=
\begin{bmatrix}
\cos{\theta} & -\sin{\theta} & 0 \ \ \\
\sin{\theta} & \cos{\theta} & 0 \ \ \\
0 & 0 & 1 \ \ \\
\end{bmatrix}
\tag{28}
スクリュー理論の中では指数を使った表記$e^{[\hat{\boldsymbol{\omega}}]\theta}$がよく用いられますが、ロボット工学の教科書の多くは(24)(27)(28)式の回転行列を以下のように表記することが多いです。
\rm{R}_x
=
\begin{bmatrix}
\ \ 1 \ & 0 & 0 \\
\ \ 0 & \cos{\theta} & -\sin{\theta} \\
\ \ 0 & \sin{\theta} & \cos{\theta} \\
\end{bmatrix}
\tag{24}
\rm{R}_y
=
\begin{bmatrix}
\cos{\theta} & \ 0 \ & \sin{\theta} \\
0 & \ 1 \ & 0 \\
-\sin{\theta} & \ 0 \ & \cos{\theta} \\
\end{bmatrix}
\rm{R}_z
=
\begin{bmatrix}
\cos{\theta} & -\sin{\theta} & 0 \ \ \\
\sin{\theta} & \cos{\theta} & 0 \ \ \\
0 & 0 & 1 \ \ \\
\end{bmatrix}
スクリュー理論を展開するにあたって、(22)式のような成分表示が重要になることは少ないです。しかし、実際にロボットの運動学を考えたりスクリュー理論をプログラムへ落とし込む際に重要となります。
5. おわりに
本稿では回転行列$e^{[\hat{\boldsymbol{\omega}}]\theta}$の具体的な成分表示について紹介しました。また、教科書によく登場する$x, y, z$の各軸まわりの回転行列$\rm{R}_x, \rm{R}_y, \rm{R}_z$の成分表示についても紹介しました。この記事を通じて回転行列の実体が少しでもイメージしやすくなればうれしいです。
これまでの記事では回転行列というものを雑に扱ってきたので、来週の記事では回転行列の定義をフワッと紹介し、その性質や使用用途についてまとめたいなあと考えています。
今週も最後まで読んでいただきありがとうございました！