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

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

目次

1.はじめに
 2.異なる座標系からみた位置ベクトル
 3.同次変換行列による表現
 4.おわりに

1. はじめに

　本稿では、原点の異なる座標系どうしの変換を記述することができる同次変換行列についてまとめてみようと思います。まずは最も簡単な例として、異なる座標系からみた位置ベクトルの変換と同次変換行列の関係をまとめます。

　本記事ではこれまでと同様に基準座標系$\Sigma_s$とボディ座標系$\Sigma_b$を用います。回転行列の議論を行う際には、これらの原点が一致しているという制約を課しましたが、本記事では原点が異なる位置にあっても良いとします。複数の座標系を扱うため、同じ点を表す位置ベクトル$\boldsymbol{p}$であっても、その成分は基準とする座標系に応じて変化します。そのため、基準座標系$\Sigma_s$における位置ベクトル$\boldsymbol{p}$の座標を$\boldsymbol{p}_s$、ボディ座標系$\Sigma_b$における位置ベクトル$\boldsymbol{p}$の座標を$\boldsymbol{p}_b$のように表記することとします。図1にこれらについて軽くまとめました。

図1　異なる座標系からみた位置ベクトル$\boldsymbol{p}$

　また、基準座標系$\Sigma_s$の基底は正規直交基底$\boldsymbol{i}$、$\boldsymbol{j}$、$\boldsymbol{k}$であり、単位行列$I$でまとめられるとします。ボディ座標系の基底も同様に正規直交基底$\boldsymbol{u_1}$、$\boldsymbol{u_2}$、$\boldsymbol{u_3}$であり、回転行列$R_{sb}$でまとめられるとします。詳細はこちらの記事を参照ください。

2. 異なる座標系からみた位置ベクトル

　基準座標系$\Sigma_s$とボディ座標系$\Sigma_b$の2つから見た位置ベクトル$\boldsymbol{p}$について考えます。基準座標系$\Sigma_s$を基準としたボディ座標系$\Sigma_b$の原点の座標を$\boldsymbol{q}_{sb}$とします。
基準座標系$\Sigma_s$から見た位置ベクトル$\boldsymbol{p}$は以下のようにまとめられます。

\boldsymbol{p}
=
p_{xs} \boldsymbol{i} + p_{ys} \boldsymbol{j} + p_{zs} \boldsymbol{k}
=
\begin{bmatrix}
\boldsymbol{i} & \boldsymbol{j} & \boldsymbol{k}\\
\end{bmatrix}
\begin{bmatrix}
p_{xs} \\
p_{ys} \\
p_{zs} \\
\end{bmatrix}
=
I \ \boldsymbol{p_s}
=
\boldsymbol{p_s}
\tag{1}

同様に基準座標系$\Sigma_b$から見た位置ベクトル$\boldsymbol{p}$は以下のようにまとめられます。

\boldsymbol{p}
=
p_{xb} \boldsymbol{u_1} + p_{yb} \boldsymbol{u_2} + p_{zb} \boldsymbol{u_3} + \boldsymbol{q}_{sb}
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3}\\
\end{bmatrix}
\begin{bmatrix}
p_{xb} \\
p_{yb} \\
p_{zb} \\
\end{bmatrix}
+
\boldsymbol{q}_{sb}
=
R_{sb}\  \boldsymbol{p_b} + \boldsymbol{q}_{sb}
\tag{2}

　(2)式について、前半の$R_{sb}\ \boldsymbol{p_b}$の部分はまさに回転行列で出てきた基底変換を意味しています。一方で後半の$+ \boldsymbol{q}_{sb}$の部分は基準座標系$\Sigma_s$とボディ座標系$\Sigma_b$の原点のズレを補正している項となります。

　(1)式と(2)式は同じ位置ベクトル$\boldsymbol{p}$を表しているため、等号でつなぐことが可能であり、

\boldsymbol{p_s}
=
R_{sb}\  \boldsymbol{p_b} + \boldsymbol{q}_{sb}
\tag{3}

の関係が得られます。(3)式はボディ座標系$\Sigma_b$からみたベクトルの座標を基準座標系$\Sigma_s$からみた座標へ変換する式であると言えます。

　反対に(3)式を$\boldsymbol{p_b}$について解くことで、基準座標系$\Sigma_s$からみたベクトルの座標を基準座標系$\Sigma_b$からみた座標へ変換する式が以下のように得られます。

R_{sb}\  \boldsymbol{p_b}
=
\boldsymbol{p_s} - \boldsymbol{q}_{sb}

\boldsymbol{p_b}
=
{R_{sb}}^{-1}\ ( \boldsymbol{p_s} - \boldsymbol{q}_{sb})

\therefore
\boldsymbol{p_b}
=
{R_{sb}}^{T}\  \boldsymbol{p_s} - {R_{sb}}^{T}\ \boldsymbol{q}_{sb}
\tag{4}

3. 同次変換行列による表現

　同次変換行列は異なる座標系から見たベクトルの座標変換を表現する行列であり、イメージは(3)式を行列にまとめたものとなります。たとえば(3)式はボディ座標系$\Sigma_b$から基準座標系$\Sigma_s$への座標変換であり、これを同次変換行列$T_{sb}$にまとめると以下のようになります。

T_{sb}
=
\begin{bmatrix}
R_{sb} & \boldsymbol{q}_{sb} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{5}

　このように、同次変換行列$T_{sb}$は回転行列$R_{sb}$と原点のズレ$\boldsymbol{q}_{sb}$の2つで構成されます。
　では同次変換行列を使って(3)式を表現してみます。同次変換行列の右からベクトル${[ \boldsymbol{p}_b \ \ 1 \ ]}^T$をかけてみましょう。

T_{sb}
\begin{bmatrix}
\boldsymbol{p}_b\\
 1 \\
\end{bmatrix}
=
\begin{bmatrix}
R_{sb} & \boldsymbol{q}_{sb} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\begin{bmatrix}
\boldsymbol{p}_b\\
 1 \\
\end{bmatrix}
=
\begin{bmatrix}
R_{sb}\ \boldsymbol{p}_b + \boldsymbol{q}_{sb}\\
 1 \\
\end{bmatrix}

\therefore
\begin{bmatrix}
\boldsymbol{p}_s \\
1 \\
\end{bmatrix}
=
T_{sb}
\begin{bmatrix}
\boldsymbol{p}_b\\
 1 \\
\end{bmatrix}
\tag{6}

するとベクトル${[ \boldsymbol{p}_s \ \ 1 \ ]}^T$が得られました。このように、同じ変換行列は異なる座標系からみたベクトルの座標を変換する行列であることが確認できました。

　さらに(6)式の逆変換を考えてみましょう。つまり同次変換行列$T_{bs}$を考えます。(6)式をもとに$T_{bs}$は以下の関係を満たすはずです。

\begin{bmatrix}
\boldsymbol{p}_b \\
1 \\
\end{bmatrix}
=
T_{bs}
\begin{bmatrix}
\boldsymbol{p}_s\\
 1 \\
\end{bmatrix}
\tag{7}

　(6)式が(3)式に対応するのと同様に、(7)式は(4)式と対応します。つまり

T_{bs}
=
\begin{bmatrix}
R_{bs} & \boldsymbol{q}_{bs} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
=
\begin{bmatrix}
{R_{sb}}^{T} & - {R_{sb}}^{T} \ \boldsymbol{q}_{sb} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{8}

です。(6)式と(7)式の間には

T_{bs}
=
{T_{sb}}^{-1}
\tag{9}

の関係があるため、(8)式を使えば同次変換行列の逆行列は比較的シンプルに求まります。実際に$T_{sb}$と$T_{bs}$を掛け合わせると単位行列が得られます。

T_{sb}\ T_{bs}
=
\begin{bmatrix}
R_{sb} & \boldsymbol{q}_{sb} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\begin{bmatrix}
{R_{sb}}^{T} & - {R_{sb}}^{T} \ \boldsymbol{q}_{sb} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
=
\begin{bmatrix}
R_{sb} {R_{sb}}^{T} &\boldsymbol{q}_{sb} - R_{sb} {R_{sb}}^{T} \ \boldsymbol{q}_{sb} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
=
\begin{bmatrix}
I &\boldsymbol{0} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}

\therefore
T_{sb}\ T_{bs}
=
I
\tag{10}

4. おわりに

　本記事では同次変換行列の導入として、ことなる座標系から見たベクトルの座標についてまとめました。同次変換行列は回転行列とも密接に関係しており、来週以降は同次変換行列による座標系の表現やそれらの時間微分についてまとめていきたいと思います。さらに同次変換行列も回転行列と同様に行列指数で表記することが可能であり、これがスクリュー理論の本題となります。
　最後までよんでいただきありがとうございました(^▽^)