スクリュー理論⑨ 回転行列によるベクトルの回転

　はじめまして、[りょーつ](https://twitter.com/Gomachan_2021)といいます。高専出身の大学院2年生です。研究の専門は力学や機構学で、Qiitaでは主に制御工学や数学に関する記事を書いています。本稿はスクリュー理論の解説(布教)をする8つ目の記事です。前回までの記事は以下のリンクを参照してください。
 
[スクリュー理論① 外積の歪対称行列表現](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)

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

# 目次
[1.はじめに](#1-はじめに)
[2.ボディ座標系を基準とした回転変換](#2-ボディ座標系を基準とした回転変換)
[3.基準座標系を基準とした回転変換](#3-基準座標系を基準とした回転変換)
[4.回転行列による座標系の回転](#4-回転行列による座標系の回転)
[5.おわりに](#5-おわりに)

# 1. はじめに
　本稿では、回転行列を用いてベクトルを回転させる方法についてまとめます。基本的にはベクトルと回転行列の積を考えることで回転変換を表現することができます。しかし、複数座標系がからむ場合は回転行列を掛ける順番に注意する必要があります。本記事ではボディ座標系$\Sigma b$を基準にベクトルを回転させた場合と、基準座標系$\Sigma s$を基準にベクトルを回転させた場合の2通りについて違いを明確にしつつまとめていきたいと考えています。そして、ベクトルを組み合わせて構成される「座標系」の回転についても触れていきます。
　大学で学ぶ線形変換の知識が身についていると理解しやすいです。また、[前回の記事](https://qiita.com/Gomachan_2021/items/d02b525f47c4fef8c079)の内容も良く使います。

　まずは本記事で使用するベクトルと座標系の定義を行います。基準座標系$\Sigma s$の基底を以下のように設定します。

```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$と同一の原点を有するボディ座標系$\Sigma b$の正規直交基底を以下のように定義します。
```math
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} \\
\end{bmatrix}
=
\begin{bmatrix}
u_{11} & u_{12} & u_{13} \\
u_{21} & u_{22} & u_{23} \\
u_{31} & u_{32} & u_{33} \\
\end{bmatrix}
\tag{2}
```
(2)式において、たとえば$u_{23}$は$\boldsymbol{u_3}$の第2成分を表しています。(少し直観に反するような気がしますが、行列の成分表示に表記を揃えました。)

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3031572/c5679d31-266c-40f4-bcb2-64b24cb74a25.png" width=40%>

図1　ボディ座標系$\Sigma b$の基底

 ここでこれら2つの座標系の原点から生えているベクトル$\boldsymbol{r}$を考えます。基準座標系$\Sigma s$におけるベクトル$\boldsymbol{r}$の座標を$\boldsymbol{r_s}$、ボディ座標系$\Sigma b$におけるベクトル$\boldsymbol{r}$の座標を$\boldsymbol{r_b}$とすれば、座標と基底の関係から以下がなりたちます。

```math
\boldsymbol{r}
=
\begin{bmatrix}
\boldsymbol{i} & \boldsymbol{j} & \boldsymbol{k} \\
\end{bmatrix}
\ \boldsymbol{r_s}
=
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
r_{s1} \\
r_{s2} \\
r_{s3} \\
\end{bmatrix}
=
\boldsymbol{r_s}
\tag{3}
```
```math
\boldsymbol{r}
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} \\
\end{bmatrix}
\ \boldsymbol{r_b}
=
\begin{bmatrix}
u_{11} & u_{12} & u_{13} \\
u_{21} & u_{22} & u_{23} \\
u_{31} & u_{32} & u_{33} \\
\end{bmatrix}
\begin{bmatrix}
r_{b1} \\
r_{b2} \\
r_{b3} \\
\end{bmatrix}
\tag{4}
```
(3)式と(4)式の関係については図2にまとめました。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3031572/63c68809-52e0-4714-b076-55d93a78c48b.png" width=85%>

図2　ベクトルの表記方法

　また、基準座標系$\Sigma s$を基準とした回転軸$\boldsymbol{\omega_s}$まわりの角度$\theta$の回転を表現する回転行列を$Rot(\boldsymbol{\omega_s}, \theta)$と表記します。同様にボディ座標系$\Sigma b$を基準とした回転軸$\boldsymbol{\omega_b}$まわりの角度$\theta$の回転を表現する回転行列を$Rot(\boldsymbol{\omega_b}, \theta)$と表記します。

# 2. ボディ座標系を基準とした回転変換
　まずはボディ座標系$\Sigma b$を基準にベクトルを回転させる場合の表記についてまとめます。ここでは回転行列を使った「座標」の変換を使用します。少し復習をしましょう。あるベクトル$\boldsymbol{p}$の座標が、座標系$\Sigma a$において$\boldsymbol{p_a}$であったとします。基底については言及しません。このとき、回転行列$Rot(\boldsymbol{\omega_a}, \theta)$によって座標が$\boldsymbol{{p_a}'}$へ変換されると考えると、これらの間には

```math
\boldsymbol{{p_a}'}
=
Rot(\boldsymbol{\omega_a}, \theta)
\ \boldsymbol{p_a}
\tag{5}
```
の関係があります。詳しくは[前回の記事](https://qiita.com/Gomachan_2021/items/d02b525f47c4fef8c079)を参照してください。

　本章では(5)式の関係をボディ座標系$\Sigma b$に対して適用します。ベクトル$\boldsymbol{r}$を回転行列$Rot(\boldsymbol{\omega_b}, \theta)$によって回転することで得られるベクトルを$\boldsymbol{r'}$とします。このとき、$\boldsymbol{r'}$のボディ座標系$\Sigma b$から見た座標$\boldsymbol{{r_b}'}$は、(5)式と同様の形式で以下のように表されます。

```math
\boldsymbol{{r_b}'}
=
Rot(\boldsymbol{\omega_b}, \theta)
\ \boldsymbol{r_b}
\tag{6}
```
座標$\boldsymbol{r_b}$と$\boldsymbol{{r_b}'}$は共通の基底$\boldsymbol{u_1}$、$\boldsymbol{u_2}$、$\boldsymbol{u_3}$を持つので、ベクトル$\boldsymbol{r'}$は以下のように表記できます。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3031572/e1386316-f8ba-4acc-9664-cde96461aca1.png" width=35%>

図3　ボディ座標系$\Sigma b$を基準とした回転変換

```math
\boldsymbol{r'}
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} \\
\end{bmatrix}
\ \boldsymbol{{r_b}'}
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} \\
\end{bmatrix}
Rot(\boldsymbol{\omega_b}, \theta)
\ \boldsymbol{r_b}
\tag{7}
```

このように、ボディ座標系$\Sigma b$を基準とした回転変換はボディ座標系$\Sigma b$の基底のあと、座標の前に回転行列を掛けることで表現できます。

# 3. 基準座標系を基準とした回転変換
　次に基準座標系$\Sigma s$を基準にベクトルを回転させる場合の表記についてまとめます。ここでも回転行列を使った「座標」の変換である(5)式の関係を使用します。基準座標系を基準とした回転変換$Rot(\boldsymbol{\omega_s}, \theta)$を考えるため、基準座標系$\Sigma s$にもとづく座標$\boldsymbol{r_s}$を使用します。回転変換後のベクトルを$\boldsymbol{r''}$とするとその座標$\boldsymbol{{r_s}''}$は(5)式と同様に以下のように表記されます。

```math
\boldsymbol{{r_s}''}
=
Rot(\boldsymbol{\omega_s}, \theta)
\ \boldsymbol{r_s}
\tag{8}
```
ここで、(3)式と(4)式から$\boldsymbol{r}$を消去して得られる以下の関係を考えます。
```math
\boldsymbol{r_s}
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} \\
\end{bmatrix}
\ \boldsymbol{r_b}

\tag{9}
```
ちなみに(9)式は、基準座標系$\Sigma s$とボディ座標系$\Sigma b$の間の基底変換を意味しています。(9)式を(8)式に代入して整理すると以下の関係が得られます。

```math
\boldsymbol{{r_s}''}
=
Rot(\boldsymbol{\omega_s}, \theta)
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} \\
\end{bmatrix}
\ \boldsymbol{r_b}
\tag{10}
```

このように基準座標系$\Sigma s$を基準とした回転変換はボディ座標系$\Sigma b$の基底の前に回転行列を掛けることで表現できます。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3031572/5b6d598d-9139-459e-a865-f10267844bc6.png" width=35%>

図4　基準座標系$\Sigma s$を基準とした回転変換

　2章と3章の内容をまとめます。ボディ座標系$\Sigma b$を基準とした回転変換は、ボディ座標系の基底のうしろに回転行列$Rot(\boldsymbol{\omega_b}, \theta)$をかけることで表現できます。反対に、基準座標系$\Sigma s$を基準とした回転変換は、ボディ座標系の基底のまえに回転行列$Rot(\boldsymbol{\omega_s}, \theta)$をかけることで表現できます。

　これらの計算結果は基準座標系$\Sigma s$とボディ座標系$\Sigma b$で回転軸と回転角が一致しており、以下が成り立つ場合においてのみ結果が一致します。

```math
\begin{bmatrix}
\boldsymbol{i} & \boldsymbol{j} & \boldsymbol{k} \\
\end{bmatrix}
\boldsymbol{\omega_s}
\ \theta
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} \\
\end{bmatrix}
\ \boldsymbol{\omega_b}
\ \theta

\tag{11}
```

# 4. 回転行列による座標系の回転
　本章では2章と3章でまとめたベクトルの回転を応用して、座標系を回転させる方法についてまとめます。本章では簡単のためにボディ座標系$\Sigma b$の基底を基準座標系からみた回転行列$R_{sb}$と表記します。
 
```math
R_{sb}
=
\begin{bmatrix}
\boldsymbol{u_1} & \boldsymbol{u_2} & \boldsymbol{u_3} \\
\end{bmatrix}

\tag{12}
```
回転行列を用いた座標系の表記については[こちらの記事](https://qiita.com/Gomachan_2021/items/f99a0d18420e0d69c37d)を参照ください。

　次にボディ座標系$\Sigma b$を基準に、ボディ座標系$\Sigma b$を回転して得られる座標系$\Sigma b'$を考えます。座標系$\Sigma b'$を回転行列$R_{sb'}$で表現することに。$\Sigma b'$の各基底をベクトルとしてみたときに、回転角が$0$ [rad]の場合は以下の関係がなりたちます。

```math
R_{sb'}(0)
=
R_{sb}
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
=
R_{sb} \ I
\tag{13}
```
ボディ座標系$\Sigma b$を基準とした回転を考えるので、(7)式と同様に基底のうしろへ回転行列$Rot(\boldsymbol{\omega_b}, \theta)$を掛けることで、座標系$\Sigma b'$は以下のように表現できます。

```math
R_{sb'}(\theta)
=
R_{sb} \ Rot(\boldsymbol{\omega_b}, \theta) \ I
=
R_{sb} \ Rot(\boldsymbol{\omega_b})
\tag{14}
```

　同様の流れで基準座標系$\Sigma s$を基準に、ボディ座標系$\Sigma b$を回転して得られる座標系$\Sigma b''$を考えます。座標系$\Sigma b''$を回転行列$R_{sb''}$で表現することにします。$\Sigma b''$の各基底をベクトルとしてみたときにも、回転角が$0$ [rad]の場合は(13)式の結果が得られます。
　基準座標系$\Sigma s$を基準とした回転を考えるので、(10)式と同様に基底の前へ回転行列$Rot(\boldsymbol{\omega_s}, \theta)$を掛けることで、座標系$\Sigma b''$は以下のように表現できます。


```math
R_{sb''}(\theta)
=
Rot(\boldsymbol{\omega_s}, \theta) \ R_{sb}  \ I
=
Rot(\boldsymbol{\omega_s}, \theta) \ R_{sb}
\tag{15}
```

このように、どの座標系を基準に回転を行うのかを明確にしておかないと、行列を掛ける位置を間違えてしまう可能性があります。ロボット工学の授業で登場する変換は(14)式のように、ボディ座標系$\Sigma b$を基準とした回転を扱うものが多いように感じます。

　複数の座標系が混在する場合についても考えてみましょう。座標系$\Sigma b$を基準として定義される座標系$\Sigma c$を回転させます。座標系$\Sigma c$は回転行列$R_{bc}$を用いて表記できます。また基準座標系$\Sigma s$と座標系$\Sigma c$の間には

```math
R_{sc} = R_{sb}\ R_{bc}
\tag{16}
```
の関係があります。このとき、ボディ座標系$\Sigma b$を基準に座標系$\Sigma c$を回転させて得られる座標系$\Sigma c'$を$R_{bc'}$、基準座標系$\Sigma s$を基準に座標系$\Sigma c$を回転させて得られる座標系$\Sigma c''$を$R_{bc''}$と表記する場合、(14)、(15)式と同様に以下の関係が得られます。

```math
R_{sc'}
=
R_{sb} \ Rot(\boldsymbol{\omega_b}, \theta) \ R_{bc}
\tag{17}
```
```math
R_{sc''}
=
Rot(\boldsymbol{\omega_b}, \theta) \ R_{sb} \ R_{bc}
=
Rot(\boldsymbol{\omega_b}, \theta) \ R_{sc}
\tag{18}
```

これらの例からなんとなくノリをつかんでもらえたら嬉しいです。

# 5. おわりに
　本稿では、異なる座標系を基準とした回転変換において、回転行列を掛ける順番について解説しました。この考え方は今後登場する同次変換行列においても登場するため、しっかりと理解していただけると嬉しいです。
 
　また、本記事で取り上げたように、剛体の運動は基準座標系$\Sigma s$を使う方法とボディ座標系$\Sigma b$を使う方法の2種類で記述できます。スクリュー理論も同様に2種類の記述方法があるのですが、基本的にはボディ座標系$\Sigma b$を使う手法について取り上げていこうと思います。ボディ座標系$\Sigma b$を使うと計算式が多少複雑になりますが、計算式からロボットアームの動きのイメージがしやすいです。
 
　来週からは原点が異なる座標系どうしの変換を表現する方法についてまとめていこうと思います。最後まで読んでいただき、ありがとうございました！