スクリュー理論⑫ 同次変換行列による座標系の表現と基底変換

　はじめまして、[りょーつ](https://twitter.com/Gomachan_2021)といいます。高専出身の大学院2年生です。研究の専門は力学や機構学で、Qiitaでは主に制御工学や数学に関する記事を書いています。本稿はスクリュー理論の解説(布教)をする12個目の記事です。前回までの記事は以下のリンクを参照してください。
 
[スクリュー理論① 外積の歪対称行列表現](https://qiita.com/Gomachan_2021/items/4a1bc967785b525bc857)
[スクリュー理論② 行列の指数関数](https://qiita.com/Gomachan_2021/items/f97bff915946933a65c5)
[スクリュー理論③ 行列指数と回転行列の関係](https://qiita.com/Gomachan_2021/items/942da3e90bef49d98196)
[スクリュー理論④ 回転行列の成分表示](https://qiita.com/Gomachan_2021/items/01ccd63ab1f5e2266fb9)
[スクリュー理論⑤ 回転行列による座標系の表現](https://qiita.com/Gomachan_2021/items/f99a0d18420e0d69c37d)
[スクリュー理論⑥ 剛体の速度・加速度の座標変換公式の導出](https://qiita.com/Gomachan_2021/items/4be2aa4cd2fa1649db2e)
[スクリュー理論⑦ 三次元空間における右手系の回転行列](https://qiita.com/Gomachan_2021/items/92bfb473d9165b7e54f8)
[スクリュー理論⑧ 回転行列による座標変換](https://qiita.com/Gomachan_2021/items/d02b525f47c4fef8c079)
[スクリュー理論⑨ 回転行列によるベクトルの回転](https://qiita.com/Gomachan_2021/items/b358b887808d6eccf210)
[スクリュー理論⑩ 回転行列と多様体の関係](https://qiita.com/Gomachan_2021/items/03434d62160ffb4b330e)
[スクリュー理論⑪ 同次変換行列の導入](https://qiita.com/Gomachan_2021/items/6884086892276e054544)

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

# 目次
[1.はじめに](#1-はじめに)
[2.同次変換行列による座標系の表現](#2-同次変換行列による座標系の表現)
[3.同次変換行列による基底変換](#3-同次変換行列による基底変換)
[4.おわりに](#4-おわりに)

# 1. はじめに
　本稿では、同次変換行列による座標系の表現と、基底変換についてまとめてみようと思います。少し前に回転行列で座標系を表現する方法について[こちらの記事](https://qiita.com/Gomachan_2021/items/f99a0d18420e0d69c37d)にまとめましたが、今回はそれの同次変換行列バージョンを考えていきます。先に結論を述べておくと、同次変換行列は回転行列とは異なり、原点が一致していない座標系を表現することができます。

　本記事でのベクトルや座標系の表記ルールは[前回の記事の「はじめに」](https://qiita.com/Gomachan_2021/items/6884086892276e054544#1-%E3%81%AF%E3%81%98%E3%82%81%E3%81%AB)に揃えます。ベクトルの基底や座標に関する知識があると読みやすくなるかなあと思います。

# 2. 同次変換行列による座標系の表現
　一言でいうと、同次変換行列は、それそのものが正規直交座標系を意味しています。落ち着いて例を見てみましょう。基準座標系$\Sigma_s$の基底を$\boldsymbol{i}$、$\boldsymbol{j}$、$\boldsymbol{k}$とします。具体的な成分は以下のとおりで、よくあるx-y-z座標系と同じだと思ってください。
```math
\begin{bmatrix}
\boldsymbol{i} & \boldsymbol{j} & \boldsymbol{k}\\
\end{bmatrix}
=
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
\tag{1}
```
次に、基準座標系$\Sigma_s$を基準として、ある同次変換行列$T$を考えてみましょう。同次変換行列$T$は回転行列$R$とベクトル$\boldsymbol{q}$で以下のように構成されます。
```math
T
=
\begin{bmatrix}
R & \boldsymbol{q} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{2}
```
(2)式のうち、回転行列$R$は座標系の姿勢変化を記述しています。一方で、(2)式はベクトル$\boldsymbol{q}$が追加されることにより、姿勢変化だけでなく、座標系の原点のズレまで表現することができるようになっています。これらを図示してみましょう。回転行列は3つの直交した単位ベクトルで表現できるため、
```math
R
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3}\\
\end{bmatrix}
\tag{3}
```
となるため
```math
T
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} & \boldsymbol{q} \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{4}
```
となります。(4)式が意味するものを図1にまとめてみました。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3031572/d44e5b61-a354-42f7-a477-e2b3ee625229.png" width=60%>

図1　同次変換行列による座標系の表現

　ここで1つ注意点があります。座標系を回転行列で表現するとき、基準となる座標系を明確にする必要がありました。これは同次変換行列でも同じことが言えます。仮に(2)式を座標系$\Sigma_a$とすると、これは基準座標系$\Sigma_s$を基準にしたものであるため、以下のように表記するほうが丁寧です。
```math
T_{sa}
=
\begin{bmatrix}
R_{sa} & \boldsymbol{q}_{sa} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{5}
```
　同様のノリで、たとえば座標系$\Sigma_a$を基準に座標系$\Sigma_b$を記述したいのであれば、以下のように表現できます。
```math
T_{ab}
=
\begin{bmatrix}
R_{ab} & \boldsymbol{q}_{ab} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{6}
```
分かりやすいように(5)式と(6)式を図2にまとめてみました。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3031572/1318326b-3f65-4e4a-8859-01d9f5f88485.png" width=60%>

図2　基準座標系と同次変換行列

# 3. 同次変換行列による基底変換
　[少し前の記事](https://qiita.com/Gomachan_2021/items/d02b525f47c4fef8c079#3-%E6%BC%94%E7%AE%97%E5%AD%90%E3%81%A8%E3%81%97%E3%81%A6%E3%81%AE%E5%9B%9E%E8%BB%A2%E8%A1%8C%E5%88%97)で回転行列を掛け算すると、座標系の基底を変換できると述べました。たとえば回転行列$R_{sa}$、$R_{ab}$があれば、これらの間には以下の関係がなりたちます。
```math
R_{sb}
=
R_{sa} \ R_{ab}
\tag{7}
```
回転行列どうしを掛け算すると、挟まれた添え字がキャンセルされるんでしたね。(7)式の例でいうと添え字$a$がキャンセルされています。結論から述べると、同次変換行列にも(7)式のように以下の関係が知られています。
```math
T_{sb}
=
T_{sa} \ T_{ab}
\tag{8}
```
いきなり言われても信じられないと思うので、丁寧に確認していきましょう。(8)式に(5)式と(6)式を代入して右辺を展開してみます。
```math
T_{sa} \ T_{ab}
=
\begin{bmatrix}
R_{sa} & \boldsymbol{q}_{sa} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}

\begin{bmatrix}
R_{ab} & \boldsymbol{q}_{ab} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
=
\begin{bmatrix}
R_{sa} \ R_{ab} & R_{sa}\boldsymbol{q}_{ab} + \boldsymbol{q}_{sa} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
=
\begin{bmatrix}
R_{sb} & R_{sa}\boldsymbol{q}_{ab} + \boldsymbol{q}_{sa} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{9}
```
(9)式の右辺の右上ブロックについて注目してみます。$R_{sa}\boldsymbol{q_{ab}}$のイメージを図3にまとめました。文章で表すのは難しいですが、この項は、「$\boldsymbol{q}_{sa}$を、$\Sigma_a$と同じ原点をもって$\Sigma_s$と同じ姿勢の座標系$\Sigma_s'$からみた座標」を意味しています。つまり
```math
R_{sa}\boldsymbol{q}_{ab} + \boldsymbol{q}_{sa} = \boldsymbol{q}_{sb} 
\tag{10}
```
となります。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3031572/a8b7d94c-c31e-4db9-ba33-54387bad0974.png" width=99%>

図3　$R_{sa}\boldsymbol{q_{ab}}$のイメージ

　ややこしいですが、回転行列の添え字がキャンセルできる一方で、$R_{sa}\boldsymbol{q_{ab}}$の添え字はキャンセルできません。ご注意ください。

　最終的に(10)式を(9)式に代入することで、たしかに(8)式の関係が得られます。
```math
T_{sa} \ T_{ab}
=
\begin{bmatrix}
R_{sb} & \boldsymbol{q}_{sb} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
=
T_{sb}
\tag{11}
```
　今回は座標系が2つの場合について例を挙げましたが、座標系が3つ以上になっても添え字キャンセルルールはなりたちます。例えば以下のようなイメージ。
```math
T_{az}
=
T_{ab} \ T_{bc} \ \cdots T_{yz}
\tag{12}
```
　特にロボット工学では、関節上に複数の座標系を定義し、それらの間を基底変換で行き来しながらロボットの運動を記述することが多いです。冒頭で述べたDH法はロボットアームの形状をもとに、どのようにして同次変換行列をさだめるのかを明確にルール化した手法となります。

# 4. おわりに
　本稿では、同次変換行列を用いた座標系の表現と基底変換についてフワッとまとめました。来週はスクリュー理論導入のための最後の準備として、同次変換行列の微分についてまとめておきたいと思います。さらに理論の抽象度が上がるので、うまく説明できるように頑張ります。
　今週も最後までよんでいただき、ありがとうございました!