　はじめまして、りょーつといいます。高専出身の大学院2年生です。研究の専門は力学や機構学で、Qiitaでは主に制御工学や数学に関する記事を書いています。本稿はスクリュー理論の解説(布教)をする19個目の記事です。前回までの記事は以下のリンクを参照してください。

スクリュー理論は剛体の運動を記述する方法で、ロボット工学などでよく使われています。日本の高専や大学ではDH法を使った記述を学ぶことが多いですが、国際的にはスクリュー理論を使った記述が一般的となってきているようです。スクリュー理論を扱う日本語の文献は少ないので、この記事が誰かの助けになればいいなと思います。

目次

1.はじめに
 2.ロドリゲスの公式の積分
 3.スクリューの指数写像
 4.おわりに

1. はじめに

　本記事では、同次変換行列を行列指数としてみたときの各成分について考察しようと思います。少し前の記事で、同次変換行列は回転行列と同様に行列指数で表現可能であることを示しました。しかし、その成分については詳しく触れていなかったので、本記事ではそこらへんについて詳しく解説していきたいと考えています。
　また、前回の記事で、回転行列の積分についてまとめましたが、この内容をロドリゲスの公式を用いて一般化したものを用います。

2. ロドリゲスの公式の積分

　ある単位回転軸$\boldsymbol{\hat{\omega}}$まわりの角度$\theta$の回転を表す回転行列を$Rot(\boldsymbol{\hat{\omega}}, \theta)$とします。このとき、ロドリゲスの公式より、回転行列$Rot(\boldsymbol{\hat{\omega}}, \theta)$は、$\boldsymbol{\hat{\omega}}$の歪対称行列表現$[\boldsymbol{\hat{\omega}}]$と単位行列$I$を用いて以下のように展開できます。

Rot(\boldsymbol{\hat{\omega}}, \theta)
=
I
+
[\boldsymbol{\hat{\omega}}] \sin{\theta}
+
{[\boldsymbol{\hat{\omega}}]}^2 (1 - \cos{\theta})
\tag{1}

ここで、回転行列$Rot(\boldsymbol{\hat{\omega}}, \theta)$を定積分して得られる行列を$G(\theta)$としたとき、$G(\theta)$は以下のように一般化されます。

G(\theta)
=
\int_{0}^{\theta} Rot(\boldsymbol{\hat{\omega}}, \theta) \  d\theta
=
I \int_{0}^{\theta} d\theta
+
[\boldsymbol{\hat{\omega}}] \int_{0}^{\theta} \sin{\theta} \ d\theta
+
{[\boldsymbol{\hat{\omega}}]}^2 \int_{0}^{\theta} (1 - \cos{\theta}) d\theta

\therefore
G(\theta)
=
\theta I
+
(1 - \cos{\theta} )[\boldsymbol{\hat{\omega}}]
+
(\theta - \sin{\theta}){[\boldsymbol{\hat{\omega}}]}^2
\tag{2}

　以上のことから、回転行列$Rot(\boldsymbol{\hat{\omega}}, \theta)$に定数ベクトル$\boldsymbol{v}$を掛けたものを積分した結果は$G(\theta) \boldsymbol{v}$になると考えられます。式で表すと以下のとおりです。

\int_{0}^{\theta} Rot(\boldsymbol{\hat{\omega}}, \theta) \ \boldsymbol{v} \  d\theta
=
\bigg( \int_{0}^{\theta} Rot(\boldsymbol{\hat{\omega}}, \theta) \  d\theta \bigg) \boldsymbol{v}
=
G(\theta) \boldsymbol{v}
\tag{3}

また、前回の記事の内容より回転行列に定数ベクトルを掛けたものを積分した結果がつるまき線の軌跡になることを示したので、$G(\theta) \boldsymbol{v}$も同様につるまき線の軌跡を表すと考えられます。

3. スクリューの指数写像

　基準座標系におけるスクリュー$\boldsymbol{\zeta}$の行列表記$[\boldsymbol{\zeta}]$を角速度$\boldsymbol{\hat{\omega_s}}$と速度$\boldsymbol{v_s}$を用いて以下のように定義します。

[\boldsymbol{\zeta}]
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}] & \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\tag{4}

このとき、スクリュー$\boldsymbol{\zeta}$に対応する同次変換行列$T$は、初期値を$M$として以下の行列指数で表現できます。

T
=
e^{[\boldsymbol{\zeta}] \theta}M
\tag{5}

　本章では、(5)式中の行列指数$e^{[\boldsymbol{\zeta}] \theta}$について、成分を明らかにしたいと思います。行列指数の定義より、指数関数は以下の無限級数に展開できます。

e^{[\boldsymbol{\zeta}] \theta}
=
\sum_{n=0}^\infty
\frac{1}{n!}[\boldsymbol{\zeta}]^n \theta^n
\tag{6}

まずは行列$[\boldsymbol{\zeta}]$の累乗について規則性を考えてみましょう。

[\boldsymbol{\zeta}]^2
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}] & \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}] & \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}]^2 & [\boldsymbol{\hat{\omega_s}}] \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\tag{7}

[\boldsymbol{\zeta}]^3
=
[\boldsymbol{\zeta}]^2 [\boldsymbol{\zeta}]
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}]^2 & [\boldsymbol{\hat{\omega_s}}] \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}] & \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}]^3 & [\boldsymbol{\hat{\omega_s}}]^2 \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\tag{8}

ここで(7)、(8)式の規則性より、

[\boldsymbol{\zeta}]^n
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}]^n & [\boldsymbol{\hat{\omega_s}}]^{n-1} \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\tag{9}

になることが予想されます。実際に

[\boldsymbol{\zeta}]^{n+1}
=
[\boldsymbol{\zeta}]^n [\boldsymbol{\zeta}]
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}]^n & [\boldsymbol{\hat{\omega_s}}]^{n-1} \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}] & \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}]^{n+1} & [\boldsymbol{\hat{\omega_s}}]^n \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\tag{10}

となることから、帰納的に(9)式の予想は正しそうです。
　さらに、歪対称行列$[\boldsymbol{\hat{\omega_s}}]$の累乗に関して

[\boldsymbol{\hat{\omega_s}}]^{2k+1}
=
(-1)^{k} [\boldsymbol{\hat{\omega_s}}]
\ \ \ \ \ (k\geqq 0)
\tag{11}

[\boldsymbol{\hat{\omega_s}}]^{2k}
=
(-1)^{k-1} [\boldsymbol{\hat{\omega_s}}]^2
\ \ \ \ \ (k\geqq 1)
\tag{12}

の関係がなりたつことから、以下の関係がなりたちます。

[\boldsymbol{\zeta}]^{2k}
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}]^{2k} & [\boldsymbol{\hat{\omega_s}}]^{2k-1} \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
=
\begin{bmatrix}
(-1)^{k-1} [\boldsymbol{\hat{\omega_s}}]^2 & (-1)^{k-1} [\boldsymbol{\hat{\omega_s}}] \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\ \ \ \ \ (k\geqq 1)
\tag{13}

[\boldsymbol{\zeta}]^{2k+1}
=
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}]^{2k+1} & [\boldsymbol{\hat{\omega_s}}]^{2k} \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
=
\begin{bmatrix}
(-1)^{k} [\boldsymbol{\hat{\omega_s}}] & (-1)^{k-1} [\boldsymbol{\hat{\omega_s}}]^2 \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\ \ \ \ \ (k\geqq 1)
\tag{14}

以上の関係をもとに(6)式を展開してみましょう。無限級数を奇数項と偶数項に分割して和を取ると以下のようになります。

e^{[\boldsymbol{\zeta}] \theta}
=
\sum_{k=0}^\infty
\frac{1}{(2k)!}[\boldsymbol{\zeta}]^{2k} \theta^{2k}
+
\sum_{k=0}^\infty
\frac{1}{(2k+1)!}[\boldsymbol{\zeta}]^{2k+1} \theta^{2k+1}

=
I
+
\begin{bmatrix}
[\boldsymbol{\hat{\omega_s}}] & \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}
\theta
+
\sum_{k=1}^\infty
\frac{1}{(2k)!}\begin{bmatrix}
(-1)^{k} [\boldsymbol{\hat{\omega_s}}] & (-1)^{k-1} [\boldsymbol{\hat{\omega_s}}]^2 \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix} \theta^{2k}
+
\sum_{k=1}^\infty
\frac{1}{(2k+1)!}\begin{bmatrix}
(-1)^{k} [\boldsymbol{\hat{\omega_s}}] & (-1)^{k-1} [\boldsymbol{\hat{\omega_s}}]^2 \boldsymbol{v_s} \\
 \boldsymbol{0}^T & 0\\
\end{bmatrix}\theta^{2k+1}

=
\begin{bmatrix}
\sum_{n=0}^\infty
\frac{1}{n!}[\boldsymbol{\hat{\omega_s}}]^n \theta^n
& 
\Big(
I \theta
+
\sum_{k=1}^\infty
\frac{1}{(2k)!}
(-1)^{k-1} [\boldsymbol{\hat{\omega_s}}] \theta^{2k}
+
\sum_{k=1}^\infty
\frac{1}{(2k+1)!}
(-1)^{k-1} [\boldsymbol{\hat{\omega_s}}]^2 \theta^{2k+1}
\Big)
\boldsymbol{v_s} \\
 \boldsymbol{0}^T & 1\\
\end{bmatrix}

=
\begin{bmatrix}
\sum_{n=0}^\infty
\frac{1}{n!}[\boldsymbol{\hat{\omega_s}}]^n \theta^n
& 
\Big(
I \theta
-
[\boldsymbol{\hat{\omega_s}}]
\sum_{k=1}^\infty
\frac{1}{(2k)!}
(-1)^{k}  \theta^{2k}
-
[\boldsymbol{\hat{\omega_s}}]^2
\sum_{k=1}^\infty
\frac{1}{(2k+1)!}
(-1)^{k}  \theta^{2k+1}
\Big)
\boldsymbol{v_s} \\
 \boldsymbol{0}^T & 1\\
\end{bmatrix}
\tag{15}

ここで、無限級数について

\cos{\theta}
=
\sum_{k=0}^\infty
\frac{1}{(2k)!}
(-1)^{k}  \theta^{2k}
=
1
+
\sum_{k=1}^\infty
\frac{1}{(2k)!}
(-1)^{k}  \theta^{2k}

\therefore
-
\sum_{k=1}^\infty
\frac{1}{(2k)!}
(-1)^{k}  \theta^{2k}
=
1 - \cos{\theta}
\tag{16}

\sin{\theta}
=
\sum_{k=0}^\infty
\frac{1}{(2k+1)!}
(-1)^{k}  \theta^{2k+1}
=
\theta
+
\sum_{k=1}^\infty
\frac{1}{(2k+1)!}
(-1)^{k}  \theta^{2k+1}

\therefore
-
\sum_{k=1}^\infty
\frac{1}{(2k+1)!}
(-1)^{k}  \theta^{2k+1}
=
\theta - \sin{\theta}
\tag{17}

e^{[\boldsymbol{\hat{\omega_s}}] \theta}
=
\sum_{n=0}^\infty
\frac{1}{n!} [\boldsymbol{\hat{\omega_s}}]^n \theta^{n}
\tag{18}

の関係がなりたつので、(16)～(18)式を(15)式に代入すれば行列指数$e^{[\boldsymbol{\zeta}] \theta}$は以下のように簡単化されます。

e^{[\boldsymbol{\zeta}] \theta}
=
\begin{bmatrix}
e^{[\boldsymbol{\hat{\omega_s}}] \theta}
& 
\Big(
I \theta
+
(1 - \cos{\theta})[\boldsymbol{\hat{\omega_s}}]
+
(\theta - \sin{\theta})[\boldsymbol{\hat{\omega_s}}]^2
\Big)
\boldsymbol{v_s} \\
 \boldsymbol{0}^T & 1\\
\end{bmatrix}
\tag{19}

ここで(19)式の右上成分に注目すると、括弧でくくられた部分が(2)式の$G(\theta)$と一致することが分かります。

e^{[\boldsymbol{\zeta}] \theta}
=
\begin{bmatrix}
e^{[\boldsymbol{\hat{\omega_s}}] \theta}
& 
G(\theta)
\boldsymbol{v_s} \\
 \boldsymbol{0}^T & 1\\
\end{bmatrix}
\tag{20}

　(20)式の同次変換行列の右上の成分は、変換前(後)の座標系の原点を意味しています。この部分が$G(\theta)\boldsymbol{v_s}$で計算できることは、原点がスクリュー$\boldsymbol{\zeta}$によって記述されるつるまき線の軌跡上にあることへ対応しており、座標変換(座標系の移動)が「ねじ対偶」によって記述されることを表しています。とてもスクリュー理論の本質という感じがしますよね。
　なお、行列$G(\theta)$は、同次変換行列の行列対数を求める際にも重要な役割を果たします。その話についてもまた後ほど紹介したいと思います。

4. おわりに

　もう一度ゆる～く記事の内容をまとめておきます。本記事では、同次変換行列$T$は行列指数によって以下のように表記できることについて解説しました。

T
=
e^{[\boldsymbol{\zeta}] \theta} M
=
\begin{bmatrix}
e^{[\boldsymbol{\hat{\omega_s}}] \theta}
& 
G(\theta)
\boldsymbol{v_s} \\
 \boldsymbol{0}^T & 1\\
\end{bmatrix}
M
\tag{21}

なお、式中の$G(\theta)$はロドリゲスの回転公式を積分して得られる行列であり、以下の式から計算できます。

G(\theta)
=
\int_{0}^{\theta} Rot(\boldsymbol{\hat{\omega}}, \theta) \  d\theta
=
I \theta
+
(1 - \cos{\theta} )[\boldsymbol{\hat{\omega}}]
+
(\theta - \sin{\theta}){[\boldsymbol{\hat{\omega}}]}^2
\tag{22}

　$G(\theta)$とベクトルの積はつるまき線の軌跡を表すので、同次変換行列による座標変換がねじ対偶によって記述できることが明らかになりました！
　行列$G(\theta)$は同次変換行列の行列対数を求める際にも重要な役割をはたします。来週以降にそこらへんについてフワッとまとめられたらいいなと思います。
　今週も最後まで読んでいただきありがとうございました！