はじめまして、りょーつといいます。高専出身の大学院2年生です。研究の専門は力学や機構学で、Qiitaでは主に制御工学や数学に関する記事を書いています。本稿はスクリュー理論の解説(布教)をする8つ目の記事です。前回までの記事は以下のリンクを参照してください。
スクリュー理論① 外積の歪対称行列表現
スクリュー理論② 行列の指数関数
スクリュー理論③ 行列指数と回転行列の関係
スクリュー理論④ 回転行列の成分表示
スクリュー理論⑤ 回転行列による座標系の表現
スクリュー理論⑥ 剛体の速度・加速度の座標変換公式の導出
スクリュー理論⑦ 三次元空間における右手系の回転行列
スクリュー理論は剛体の運動を記述する方法で、ロボット工学などでよく使われています。日本の高専や大学ではDH法を使った記述を学ぶことが多いですが、国際的にはスクリュー理論を使った記述が一般的となってきているようです。スクリュー理論を扱う日本語の文献は少ないので、この記事が誰かの助けになればいいなと思います。
目次
1.はじめに
2.姿勢変換と回転行列
3.演算子としての回転行列
4.おわりに
1. はじめに
本稿では、回転行列を演算子のように見立てて、座標変換を記述する方法を解説します。基本的な内容は前回の記事に準ずるところが多いですが、本記事では複数の座標系を行き来するという点が異なります。
本記事ではベクトルを"$\boldsymbol{r_{sb}}$"のように記述します。$s$はこのベクトルを記述する際に基準として設定した座標系を意味しており、$\boldsymbol{r_{sb}}$は$\boldsymbol{r_{b}}$を座標系$s$を基準に考えたことを表現しています。またベクトル$\boldsymbol{r_{sb}}$は、座標系$s$からみたベクトル$\boldsymbol{r_{sb}}$の$x$成分である$r_{sbx}$、$y$成分である$r_{sby}$、$z$成分である$r_{sbz}$、の3つの要素で構成される縦ベクトルとします。
\boldsymbol{r_{sb}}
=
\begin{bmatrix}
r_{sbx} \\
r_{sby} \\
r_{sbz}\\
\end{bmatrix}
\tag{1}
さらに、座標系は3つの正規直交基底ベクトル$i$、$j$、$k$で記述されるものとします。(1)式のベクトルを基底を省略せずに記述すると以下のようになります。
\boldsymbol{r_{sb}}
=
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
\begin{bmatrix}
r_{sbx} \\
r_{sby} \\
r_{sbz}\\
\end{bmatrix}
\tag{2}
図1 ベクトルの表記
本記事の内容は線形代数やベクトルの扱いに慣れていると理解しやすいです。
2. 姿勢変換と回転行列
本章では回転行列を用いて異なる座標系の姿勢を記述する方法を復習します。まずは座標系$s$と座標系$b$の2つの座標系を考えます。なおこれ以降は座標系$s$を静止座標系、座標系$b$を剛体座標系と呼ぶことにします。
静止座標系を基準に剛体座標系を表現してみます。剛体座標系を構成する基底ベクトル$\boldsymbol{i_b}$、$\boldsymbol{j_b}$、$\boldsymbol{k_b}$を静止座標系を基準に表記する方法を考えます。たとえば基底ベクトル$\boldsymbol{i_b}$を静止座標系を基準に考えた時の座標が$i_{sbx}$、$i_{sby}$、$i_{sbz}$で表される場合、基底ベクトル$\boldsymbol{i_b}$は静止座標系を基準に以下のように表されます。
\boldsymbol{i_{sb}}
=
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
\begin{bmatrix}
i_{sbx} \\
i_{sby} \\
i_{sbz}\\
\end{bmatrix}
\tag{3}
図2 基底ベクトル$\boldsymbol{i_{b}}$の静止座標系における表記
剛体座標系の他の基底も(3)式と同様に記述すると以下の関係が得られます。
\boldsymbol{j_{sb}}
=
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
\begin{bmatrix}
j_{sbx} \\
j_{sby} \\
j_{sbz}\\
\end{bmatrix}
\tag{4}
\boldsymbol{k_{sb}}
=
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
\begin{bmatrix}
k_{sbx} \\
k_{sby} \\
k_{sbz}\\
\end{bmatrix}
\tag{5}
(3)～(5)式の内容を行列を用いて1つの式にまとめると以下の関係が得られます。
\begin{bmatrix}
\boldsymbol{i_{sb}} & \boldsymbol{j_{sb}} & \boldsymbol{k_{sb}} \\
\end{bmatrix}
=
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
\begin{bmatrix}
i_{sbx} & j_{sbx} & k_{sbx} \\
i_{sbx} & j_{sbx} & k_{sbx} \\
i_{sbx} & j_{sbx} & k_{sbx} \\
\end{bmatrix}
\tag{6}
なお3つのベクトル$\boldsymbol{i_{sb}}$、$\boldsymbol{j_{sb}}$、$\boldsymbol{k_{sb}}$、は正規直交基底であるため、大きさが1かつ互いに直交します。つまり(6)式の右辺の$3\times 3$行列は回転行列となります。この静止座標系と剛体座標系を結ぶ回転行列を$R_{sb}$と表記し、以下のように定義します。
R_{sb}
=
\begin{bmatrix}
i_{sbx} & j_{sbx} & k_{sbx} \\
i_{sbx} & j_{sbx} & k_{sbx} \\
i_{sbx} & j_{sbx} & k_{sbx} \\
\end{bmatrix}
\tag{7}
前置きが長くなりましたが、(6)式と(7)式をまとめると、静止座標系と剛体座標系の正規直交基底の間には
\begin{bmatrix}
\boldsymbol{i_{sb}} & \boldsymbol{j_{sb}} & \boldsymbol{k_{sb}} \\
\end{bmatrix}
=
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
R_{sb}
\tag{8}
の関係があると分かりました。
少し別の見方として、あるベクトルを異なる座標系から見たらどうなるのか考えてみましょう。剛体座標系からみた、とある三次元ベクトル$\boldsymbol{r_{b}}$を考えます。このベクトルは剛体座標系の正規直交基底$\boldsymbol{i_{b}}$、$\boldsymbol{j_{b}}$、$\boldsymbol{k_{b}}$、をもとに以下のように記述できます。
\boldsymbol{r_{b}}
=
\begin{bmatrix}
\boldsymbol{i_{b}} & \boldsymbol{j_{b}} & \boldsymbol{k_{b}} \\
\end{bmatrix}
\begin{bmatrix}
r_{bx} \\
r_{by} \\
r_{bz} \\
\end{bmatrix}
\tag{9}
さらに(9)式の基底が単位行列となるように取れば以下のように単純化できます。
\boldsymbol{r_{b}}
=
\begin{bmatrix}
r_{bx} \\
r_{by} \\
r_{bz} \\
\end{bmatrix}
\tag{10}
図3 剛体座標系からみたベクトル
ではベクトル$\boldsymbol{r_{b}}$を静止座標系から見てみましょう。静止座標系を基準にすることを踏まえると、新しい座標$r_{sx}$、$r_{sy}$、$r_{sz}$を考えることで以下のように記述できそうです。
\boldsymbol{r_{sb}}
=
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
\begin{bmatrix}
r_{sbx} \\
r_{sby} \\
r_{sbz} \\
\end{bmatrix}
\tag{11}
図4 静止座標系からみたベクトル
また、基底$\boldsymbol{i_{b}}$、$\boldsymbol{j_{b}}$、$\boldsymbol{k_{b}}$を静止座標からみたものに変換することでも記述できます。
\boldsymbol{r_{sb}}
=
\begin{bmatrix}
\boldsymbol{i_{sb}} & \boldsymbol{j_{sb}} & \boldsymbol{k_{sb}} \\
\end{bmatrix}
\begin{bmatrix}
r_{bx} \\
r_{by} \\
r_{bz} \\
\end{bmatrix}
\tag{12}
(11)式と(12)式は同じベクトルを表しているので、右辺どうしを等号で結ぶと
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
\begin{bmatrix}
r_{sbx} \\
r_{sby} \\
r_{sbz} \\
\end{bmatrix}
=
\begin{bmatrix}
\boldsymbol{i_{sb}} & \boldsymbol{j_{sb}} & \boldsymbol{k_{sb}} \\
\end{bmatrix}
\begin{bmatrix}
r_{bx} \\
r_{by} \\
r_{bz} \\
\end{bmatrix}
\tag{13}
が得られます。(8)式の関係を(13)式に代入すると、異なる座標系からみた同一のベクトルの座標に関する関係式が以下のように得られます。
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
\begin{bmatrix}
r_{sbx} \\
r_{sby} \\
r_{sbz} \\
\end{bmatrix}
=
\begin{bmatrix}
\boldsymbol{i_{s}} & \boldsymbol{j_{s}} & \boldsymbol{k_{s}} \\
\end{bmatrix}
R_{sb}
\begin{bmatrix}
r_{bx} \\
r_{by} \\
r_{bz} \\
\end{bmatrix}
\therefore
\begin{bmatrix}
r_{sbx} \\
r_{sby} \\
r_{sbz} \\
\end{bmatrix}
=
R_{sb}
\begin{bmatrix}
r_{bx} \\
r_{by} \\
r_{bz} \\
\end{bmatrix}
\tag{14}
便宜上、(14)式の左辺を$\boldsymbol{r_{s}}$、右辺を$\boldsymbol{r_{b}}$と置けば、(14)式は簡略化して
\boldsymbol{r_{s}}
=
R_{sb} \
\boldsymbol{r_{b}}
\tag{15}
のように表すことができます。
本章の内容をざっくりまとめると、異なる座標系からみたベクトルの座標は(15)式のように回転行列を用いることで変換できることが分かりました。
3. 演算子としての回転行列
本章では3つ以上の座標系の間を変換する手法についてまとめます。基本となるのは2章で導出した(15)式の考え方です。(15)式では静止座標系と剛体座標系の2つの座標間の変換を考えましたが、本章ではこれに加えて座標系$a$、座標系$c$、座標系$d \cdots$というふうに複数の座標系について考えます。
まずは静止座標系と座標系$a$の関係を見てみましょう。(15)式の添え字に深い意味はないため、添え字を$b$から$a$に変更するだけで関係が得られそうです。
\boldsymbol{r_{s}}
=
R_{sa} \
\boldsymbol{r_{a}}
\tag{16}
同様に(16)式の添え字を変更することで、剛体座標系と座標系$a$の関係も得られます。
\boldsymbol{r_{a}}
=
R_{ab} \
\boldsymbol{r_{b}}
\tag{17}
ここで(15)～(17)式をまとめて表記すると以下が得られます。
\boldsymbol{r_{s}}
=
R_{sa} \
\boldsymbol{r_{a}}
=
R_{sa} \
R_{ab} \
\boldsymbol{r_{b}}
\therefore
\boldsymbol{r_{s}}
=
R_{sa} \
R_{ab} \
\boldsymbol{r_{b}}
\tag{18}
(18)式は、静止座標系と剛体座標系の関係を、座標系$a$を経由して表したものと捉えることができます。これは(15)式と(18)式をもとに回転行列$R_{ab}$が、座標系$a$に関する回転行列$R_{sa}$と$R_{ab}$をもとに生成できることを意味しています。
R_{sb}
=
R_{sa} \
R_{ab} \
\tag{19}
同様の手順で以下の関係も得られます。
R_{sa}
=
R_{sb} \
R_{ba} \
\tag{20}
なんとなく法則が見えてきたかと思いますが、(19)式や(20)式のように回転行列を生成するときは隣り合う添え字がキャンセルされるのです。(19)式では添え字$a$がキャンセルされて$s$と$b$の、(20)式では添え字$b$がキャンセルされて$s$と$a$の関係を記述する回転行列が左辺に生成されています。
添え字に関するもう1つのルールを導出します。(20)式の両辺に逆行列${R_{ba}}^{-1}$をかけると
R_{sb}
=
R_{sa} \
{R_{ba}}^{-1} \
\tag{21}
が得られます。ここで(19)式と(20)式の右辺を見比べると以下の関係が見えてきます。
R_{ab}
=
{R_{ba}}^{-1}
\tag{22}
(22)式は、(7)式のルールに沿って回転行列を定義したときに、逆行列（逆変換）を添え字の反転により表記できることを意味しています。
これらのルールをもとに座標系$a$と座標系$d$の関係を考えてみましょう。座標系$a$と座標系$d$の関係が直接明らかになっていなくても、座標系$a$と座標系$b$、座標系$b$と座標系$c$、座標系$c$と座標系$d$、の関係が明らかであれば、以下のように表記可能であることが分かります。
R_{ad}
=
R_{ab}\ R_{bc}\ R_{cd}\
\tag{23}
また(23)式の逆行列をとることで、(22)式の添え字反転ルールは行列の演算ルールにも対応していることが確認できます。
{R_{ad}}^{-1}
=
{R_{cd}}^{-1}\ {R_{dc}}^{-1}\ {R_{ab}}^{-1}\
\therefore
R_{da}
=
R_{dc}\ R_{cb}\ R_{ba}\
\tag{24}
(23)式や(24)式からわかるように、回転行列は(19)式と(22)式のルールのもとで座標変換を行う演算子のような使い方ができます。ロボット工学では特に座標変換を多用するため、回転行列の演算子のような使い方についても慣れておくと便利です。
4. おわりに
本稿では回転行列を用いたベクトルの座標変換と複数座標間を行き来する演算子のような解釈について解説しました。次回はベクトルの回転変換を回転行列を用いて記述する手法についてまとめようと思います。
最後まで読んでいただき、ありがとうございました！