３次元における回転座標変換行列

f:id:kouhei_ito:20200505165906p:plain

はじめに

飛行機、ミサイル、ドローン、潜水艦、それに２次元平面上を動く場合もピッチやロールの自由度がある場合などおおよそ３次元に動き回れるものの運動をモデル化するには回転運動をどう扱うかと言う話から避けては通れないと思います。今回は、その基礎となる回転行列と座標変換行列について話をしようと思います。探せばこの手の話題はたくさん見つかるのですが、航空工学の授業で６自由度の運動方程式の説明の章でオイラー角の説明で、毎年この話を学生にするので自分の頭の整理のための記事ですが、何かのお役に立てば幸いです。間違いが有っても自己責任という事でお願いいたします。

三角関数の加法定理

回転行列について理解するのに役立つ数学は三角関数の加法定理です。「咲いたコスモス、コスモス咲いた」みたいな呪文で高校時代に覚えようとしたあれです。

${\displaystyle \begin{equation} \sin(\alpha + \beta)=\sin \alpha \cos \beta + \cos \alpha \sin \beta \end{equation} }$

${\displaystyle \begin{equation} \cos(\alpha + \beta)=\cos \alpha \cos \beta - \sin \alpha \sin \beta \end{equation} }$

２次元の回転行列を導く

３次元の回転行列を理解する前に２次元の回転行列を作ってみます。点Pが座標 ${(x_p, y_p)}$ にあるとします。

点Pを極座標的に表すとして、原点Oから点Pまでの長さをr、線分OPと横軸となす角を ${\alpha}$ とすると

${\displaystyle x_p=r \cos \alpha\\ y_p=r \sin \alpha\\ }$

線分OPを更に、反時計回りに角 ${\beta}$ 回した点をQとして ${(x_q, y_q)}$ とすると

${\displaystyle x_q=r \cos ( \alpha + \beta)\\ y_q=r \sin ( \alpha + \beta)\\ }$

さてここで点Qの式に先ほどの加法定理を適用すると

${\displaystyle x_q=r ( \cos \alpha \cos \beta - \sin \alpha \sin \beta )\\ y_q=r ( \sin \alpha \cos \beta + \cos \alpha \sin \beta)\\ }$

展開すると、点Pの式を代入出来るのがわかります

${\displaystyle x_q=r \cos \alpha \cos \beta - r \sin \alpha \sin \beta\\ y_q=r \sin \alpha \cos \beta + r \cos \alpha \sin \beta\\ }$

${x_p}$ や ${y_p}$ で置き換えると

${\displaystyle x_q=x_p \cos \beta - y_p \sin \beta\\ y_q=y_p \cos \beta + x_p \sin \beta\\ }$

あとで行列形式で表すので、それ用に並びを少し変えると

${\displaystyle x_q=x_p \cos \beta - y_p \sin \beta\\ y_q=x_p \sin \beta + y_p \cos \beta\\ }$

これで点Pを ${\beta}$ 回転させたら、点Qがどこに来るか計算できるようになりました。あとは、これを行列形式で表すと、回転行列がはっきりわかります。

${\displaystyle \begin{bmatrix} x_q \\\ y_q \end{bmatrix}= \begin{bmatrix} \cos \beta & -\sin \beta \\\ \sin \beta & \cos \beta \end{bmatrix} \begin{bmatrix} x_p \\\ y_p \end{bmatrix} }$

２次元の回転座標変換

同じ座標空間で点を回転するにはどうするかはわかりました。では点を固定して、座標を回して、回り終わった座標の上で点が新たにどのような座標にあるかを求めることを座標変換と言います。 f:id:kouhei_ito:20200505170810p:plain 上の図は迷路から見たロボットとロボットから見た迷路を表しています。迷路座標系の迷路をロボット座標系に変換した場合どのように見えるかと言うことです。

f:id:kouhei_ito:20200505180341p:plain

上の図を見ながら考えます、黒い座標軸を反時計回りに ${\beta}$ 回すと回りきった赤い座標上での点の位置は元の座標空間における点の位置関係と比べるとちょうど、黒い座表上で点を ${-\beta}$ 回転した位置と同じだと言うことがわかります。

最初黒い座標A上にある点P ${(x_A, y_A)}$ はその座標を原点Oを回転中心として ${\beta}$ 回した新たな赤い座標Bの上で ${(x_B, y_B)}$ の座標にあるとすると。先に書いた事を数式で表すと

${\displaystyle \begin{bmatrix} x_B \\\ y_B \end{bmatrix}= \begin{bmatrix} \cos (-\beta) & -\sin (-\beta) \\\ \sin (-\beta) & \cos (-\beta) \end{bmatrix} \begin{bmatrix} x_A \\\ y_A \end{bmatrix}\\ }$

$\cos$ は偶関数、 $\sin$ は奇関数なので符号について書き換えると次の式になります。

${\displaystyle \begin{bmatrix} x_B \\\ y_B \end{bmatrix}= \begin{bmatrix} cos \beta & sin \beta \\\ -\sin \beta & cos \beta \end{bmatrix} \begin{bmatrix} x_A \\\ y_A \end{bmatrix}\\ }$

回転の座標変換の行列は、回転行列の $\sin$ の符号が逆転したものになります。

３次元の回転座標変換

２次元の時は説明せずに座標と言いましたが、２次元の座標は右方向が水平方向の正方向、上むきが垂直方向の正方向でした。３次元については実はいろいろ有って、最初に断らないと分からなくなるのでどのような座標を取り扱うのかここで決めておきます。これから扱う３次元の直交座標空間は右手系です。

f:id:kouhei_ito:20200505171059p:plain

右手系というのは、右手を出して、親指x軸、人差し指y軸、中指z軸に見立てて、直交座標軸を作ってみてください。そこで出来た座標空間を右手系の座標空間とします。親指をみぎに向けて、人差し指を上に向けると、中指が自分自身を指しますね。

３次元の座標変換も２次元の座標変換と意味合いは同じで、同じ点をそれぞれの座標のなかで表したら、何処になるのかを求めることです。３次元になると頭の中で考えるのが難しくなるのは我々が３次元にいるからで普通は仕方がないことです。

数式も３×３の行列が出てきて一見すると難しくなります。おそらく覚える必要はないと思いますが、一度は求めておくと納得しやすいのではと思います。ここでは当ブログ風の導出方法を展開しようと思います。

３次元の回転については回転軸がx軸、y軸、z軸と３本あります。我々が明らかにすることはそれぞれの軸に関する回転を求めることです。

ものの向き（姿勢）を表すのに、各軸ひとつだけの回転では表すことが難しいのは容易に想像できます。これは、あらゆる座標系の回転状態を表現するときも１軸だけの回転では無理だと言うことになります。ただし、各軸を決まった順で回していくと、あらゆる回転状態を表現できることがわかっていますので、これから話す、各軸を回転の中心軸とした座標の回転を知っておけば良いこととなります。

z軸周りの回転座標変換

まず最初にz軸周りの回転座標変換行列を求めます。

何故最初がz軸なのかと言いますと、一番２次元の座標変換の説明と結びつけやすいからです。

f:id:kouhei_ito:20200505181104p:plain

もう一度、ほぼ同じ図を掲載しました。違いは回転軸であるz軸が明記されています。ただしz軸を軸方向から眺めているので、z軸は点にしか見えません。このときz軸の矢印は自分を指し示していると頭の中で描いてください。

この見え方は２次元のxy座標を回転するときの見え方と全く同じです。

さて、ここで考えてください。z軸まわりの回転において、３次元の座標で値が変わらない成分がないでしょうか？

そうです！z成分はz軸回転では変化しません。

図には表されていませんが点のz成分は図の手前か奥行きにあたり、z軸の回転ではその成分は不変です。

回転軸の成分は不変

座標変換において回転軸の成分は不変だと言うことは、座標変換行列を導く際に重要な点です。

では、２次元の回転座標変換の式を再掲載します。

${\displaystyle \begin{bmatrix} x_B \\\ y_B \end{bmatrix}= \begin{bmatrix} \cos \beta & \sin \beta \\\ -\sin \beta & \cos \beta \end{bmatrix} \begin{bmatrix} x_A \\\ y_A \end{bmatrix}\\ }$

３次元の座標変換であっても、x成分とy成分を求めるだけであれば、上の式で済んでしまいます。しかし、これではz軸だけ別扱いで、統一して扱うときに面倒になりますので、上式を３次元に拡張します。 z成分が変わらないわけですから以下のように表現できるはずです。

${\displaystyle \begin{bmatrix} x_B \\\ y_B \\\ z_B \end{bmatrix}= \begin{bmatrix} \cos \beta & \sin \beta & 0\\\ -\sin \beta & \cos \beta & 0 \\\ 0&0&1 \end{bmatrix} \begin{bmatrix} x_A \\\ y_A \\\ z_A\end{bmatrix}\\ }$

これで、z軸周りの回転座標変換行列がわかりました。２次元のものがわかれば、殆ど同じで簡単です。

y軸周りの回転座標変換

zから始めたので次はy軸周りを考えてみます。（最後にxにします。）

z軸回転と同じようにy軸方向から座標を眺めるところを想像します。y軸の矢印は自分を向いています。図にすると下図のように見えると思います。

f:id:kouhei_ito:20200505183744p:plain

この図はz軸回転の説明の時とそっくりです。違うのは軸に付けられた記号だけです。

と、言うことは式で表したときも記号だけ入れ替えれば良いので以下のようになります。

${\displaystyle \begin{bmatrix} z_B \\\ x_B \\\ y_B \end{bmatrix}= \begin{bmatrix} cos \beta & sin \beta & 0\\\ -\sin \beta & cos \beta & 0 \\\ 0&0&1 \end{bmatrix} \begin{bmatrix} z_A \\\ x_A \\\ y_A\end{bmatrix}\\ }$

この式で各成分の値は正しく求まりますが、これでは各成分の順が普通とは違うので、この式を使うときだけ一般的な座標の並び方をわざわざ変えて式に与えないと正しく計算できず面倒です。

慎重に順番を入れ替えます。この際、行を入れ替えるだけではダメですので注意してください。できる場合は、以下は読み飛ばして構いません。

まずは、行列の計算を展開します

${\displaystyle z_B = z_A \cos \beta + x_A \sin \beta + y_A \cdot 0\\ x_B = -z_A \sin \beta + x_A \cos \beta + y_A \cdot 0\\ y_B = z_A \cdot 0 + x_A \cdot 0 + y_A \cdot 1\\ }$

つづいて、式をx、y、zの順に並べ替えます

${\displaystyle x_B = -z_A \sin \beta + x_A \cos \beta + y_A \cdot 0\\ y_B = z_A \cdot 0 + x_A \cdot 0 + y_A \cdot 1\\ z_B = z_A \cos \beta + x_A \sin \beta + y_A \cdot 0\\ }$

次に各式の中の項をx、y、zの順に並べ替えます

${\displaystyle x_B = x_A \cos \beta + y_A \cdot 0 -z_A \sin \beta\\ y_B =x_A \cdot 0 + y_A \cdot 1 + z_A \cdot 0\\ z_B = x_A \sin \beta + y_A \cdot 0+z_A \cos \beta\\ }$

最後に行列計算形式に書き直します

${\displaystyle \begin{bmatrix} x_B \\\ y_B \\\ z_B \end{bmatrix}= \begin{bmatrix} \cos \beta & 0& -\sin \beta \\\ 0 & 1 & 0 \\\ \sin \beta & 0 &\cos \beta \end{bmatrix} \begin{bmatrix} x_A \\\ y_A \\\ z_A\end{bmatrix}\\ }$

以上からy軸周りの回転座標変換行列が明らかになりました。

x軸周りの回転座標変換

x軸についても同じようにします。同じことの繰り返しになりますから説明は省略しますが、練習と思って以上の作業をやってみて、答え合わせをしてみてください。

結論の数式を以下に示します。

${\displaystyle \begin{bmatrix} x_B \\\ y_B \\\ z_B \end{bmatrix}= \begin{bmatrix} 1 & 0 & 0 \\\ 0 & \cos \beta & \sin \beta \\\ 0 & -\sin \beta & \cos \beta \end{bmatrix} \begin{bmatrix} x_A \\\ y_A \\\ z_A\end{bmatrix}\\ }$

各軸ごとの回転座標変換行列まとめ

説明では回転角は統一して $\beta$ を用いて来ましたが、航空工学の流儀に合わせるとx軸、y軸、z軸の回転量はそれぞれ記号 $\phi$ 、 $\theta$ 、 $\psi$ を使います。これらを使って各軸周りの回転座標変換行列だけ以下にまとめます。

z軸周り

${\displaystyle R_z(\psi)= \begin{bmatrix} \cos \psi & \sin \psi & 0\\\ -\sin \psi & \cos \psi & 0 \\\ 0&0&1 \end{bmatrix} \\ }$

y軸周り

${\displaystyle R_y(\theta)= \begin{bmatrix} \cos \theta & 0& -\sin \theta \\\ 0 & 1 & 0 \\\ \sin \theta & 0 &\cos \theta \end{bmatrix} \\ }$

x軸周り

${\displaystyle R_x(\phi)= \begin{bmatrix} 1 & 0 & 0 \\\ 0 & \cos \phi & \sin \phi \\\ 0 & -\sin \phi & \cos \phi \end{bmatrix}\\ }$

回転座標変換行列とオイラー角

飛行機を例にとって回転座標変換の説明をします。

地面に固定された地面固定座標系から飛行機に固定された機体固定座標系への座標変換は座標をz軸周りに回転し、その結果の座標をy軸周りに回転し、最後に、その結果をx軸周りに回転します。そのときの回転角 $\phi$ 、 $\theta$ 、 $\psi$ をオイラー角と呼び飛行機の姿勢を表します。

回転座標変換行列は、それぞれの行列の積となりますので回転の順を考慮すると以下のようになります

${\displaystyle E(\phi, \theta, \psi)= \begin{bmatrix} 1 & 0 & 0 \\\ 0 & \cos \phi & \sin \phi \\\ 0 & -\sin \phi & \cos \phi \end{bmatrix} \begin{bmatrix} \cos \theta & 0& -\sin \theta \\\ 0 & 1 & 0 \\\ \sin \theta & 0 &\cos \theta \end{bmatrix} \begin{bmatrix} \cos \psi & \sin \psi & 0\\\ -\sin \psi & \cos \psi & 0 \\\ 0&0&1 \end{bmatrix} \\ }$