３次元における方向の表現

3次元における方向の表現

はじめに

飛行機、ロケット、ロボット等の物体あるいはそれらの搭載物といった任意の物体（剛体）の３次元空間での方向をどの様に表現するのかについて考えてみます。

はじめに
方向と向き
一つのベクトルだけでは物体の方向は表せない
二つのベクトルを使用した場合
３つの直交したベクトルで物体の方向を表す
行列による方向の表記
方向を表す行列の性質
座標系の前にベクトルありき
基底ベクトルの選び方でベクトルの成分は変わる
座標変換とは
方向を表す行列と座標変換
方向余弦行列
- 方向余弦とは
具体的に方向を表す行列を決める
方法① ロドリゲスの回転公式を用いた基底ベクトルの回転
方法② 座標系の回転
オイラー角
方向余弦行列とクォータニオン
- クォータニオンによるベクトルの回転行列
- クォータニオンによる座標の回転行列
クォータニオンによる回転行列の成分を決める
おわりに

方向と向き

方向と向きというのは日本語においては同じ様なものとして捉えられていると思います。英語では方向はOrientation、向きはDirectionです。

我々が物理や工学で多用するベクトルは方向とその向きと大きさを表すものです。

ここで、あえて方向と向きと表現しているのは方向に対して正負の向きがあるからです。

次の図のベクトルはお互い方向は同じで向きが反対と考えます。青いベクトルが正ならば赤いベクトルは負です。正負はどちらを正に取るかで反対になります。

ここでは、方向と向きと言う言葉をこの様に区別してお話をします。

また、我々が一般的に物体の方向を考えるときは、方向と向きを合わせて考えなければそれを表現できないこともわかります。

ただ、物体の「方向と向き」といつも記述するのは煩雑なのでここでは方向とだけ記述することにします。

さらに、日本語には方向と同じ様な言葉で「姿勢」という言葉もあります。ロボット工学等では位置と方向を合わせて姿勢(Pose)という場合があり区別しています。

したがって本記事では姿勢という言葉は使わず、方向で統一します。

一つのベクトルだけでは物体の方向は表せない

ベクトルの長手方向を軸にして、いくら回転させてもベクトルに変化が生じず、方向と向きが変わることはありません。

しかし、大きさと形を持つ物体・・・例えば飛行機は後ろから先頭を貫く軸があるとして、その軸をベクトルと考え、そのベクトルを回転軸として回転させることで機体の方向が変化します。

つまり飛行機を後ろから前へ貫くベクトルだけでは飛行機の方向を表現することはできず、そのベクトル周りの回転の量が判って初めて飛行機の方向が定まると言うことになります。

これは一つのベクトルだけでは３次元空間での形と大きさを持つ物体の方向を表現することができないことを表しています。

二つのベクトルを使用した場合

軸にするベクトル（以後軸ベクトルと呼びます）とその軸周りの回転量がわかれば物体の方向が定まると言うお話をしましたが、回転量を使う代わりに、ベクトルを使うお話をします。

今、軸ベクトルに垂直な１個のベクトルを想像します。軸ベクトルに垂直なベクトルは無限に存在します。無限に存在しますが、その中の一つで物体の傾きを表すことになります。

つまり、ここまでの話で物体の方向を決める最後の要素である、軸ベクトル周りの回転量を、回転した結果である時計の針の様なベクトルが指し示す方向を使って表すと言うことです。

物体に直交する２本のベクトルを貼り付けておくと、それらのベクトルの方向によって物体の方向を定められると言うことです。

直交する二つのベクトルを定めると、それら両方に直交する方向が一つ定まります。方向は一つ定まりますが、向きは正負二つとりうるので、直交する二つのベクトル両方に直交するベクトルは２通りあります。

直交する３つのベクトルを使うと、それらによって座標空間を作ることができるので３本目があることは重要ですが、３本目をどちらの向きにすれば良いのでしょうか。

これは、外積の計算にしたがって求められるベクトルを使った場合は「右手系」とよび、外積の計算により求められたベクトルの反対向きのベクトルを使う場合を「左手系」といって区別します。

これは、計算する人がどちらにするか決めて、一貫してそのルールを守ることになります。

親指をx軸、人差し指をy軸、中指をz軸として、右手で直交座標を作った場合に、親指を上、人差し指を左に向けると中指は自分の方を向きます。これが右手系となります。わからなくなったら右手で座標空間を作ってみると良いと思います。

また、左手で同じ様にした場合は中指は自分とは反対方向を向きます。

物理工学系では右手系を多く使い、コンピュータグラフィックの世界や３Dゲームの世界では左手系が多いと聞きます。

３つの直交したベクトルで物体の方向を表す

ここで、この直交する３つのベクトルを物体に貼り付けておいて、この３つのベクトルが物体の方向を表すとします。実際は２つのベクトルがあれば物体の方向は示せることがわかりましたが、それら２本のベクトルに直交する３本目のベクトルは２通り考えられ、採用している座標系が右手系なのか左手系なのかを定めるために３本目のベクトルを用いて定めます。

行列による方向の表記

物体の方向を物体に張り付いた直交する３本のベクトルで表現すると決めたので、あとは、どの様に表現するかを決めてしまえば、物体の方向を定め表現したことになります。

ベクトルは方向・向きと大きさをもちますが、方向を表現するときは大きさは重要ではありません。そこで、ベクトルの大きさを１とします。

こうすると、これらの３つのベクトルは物体に張り付いている座標空間の基底ベクトルと考えることができます。

そして物体の方向を表記する最も単純で直感的な方法は、これら３つの基底ベクトルを３つの列に持つ３x３の行列で表記することです。

方向を表す行列の性質

方向を表す行列（これは回転行列とも呼ばれています。）の各列は大きさ１の単位ベクトルです。そして、３つのベクトルは互いに直交しています。この様な成分を持つ行列を直交行列と呼びます。

直交行列には行列計算的に嬉しい性質があって、逆行列が転置と等しくなります。つまり、直交行列を ${\bf{A}}$ とした時

$\begin{eqnarray} {\bf{A}}^{-1}={\bf{A}}^\mathsf{T}\\ \\ \end{eqnarray}$

となります。

実はこの様な性質を持つ行列が直交行列と呼ぶと言うのが正式な直交行列の定義となっています。

逆行列を求めるのは大変ですが転置はすぐにできるので大変便利な性質です。

ちなみに行列の積の転置に関しては以下の関係があるので、方向の計算には便利です。

$\begin{eqnarray} \left(\bf{AB}\right)^\mathsf{T}=\bf{B}^\mathsf{T}\bf{A}^\mathsf{T}\\ \\ \end{eqnarray}$

座標系の前にベクトルありき

話が前後してしまいましたが、ここまででは物体の方向を行列で表すということに落ち着いています。しかし、行列で表すということはその成分を具体的な数値で示すことができると言うことです。

実は、ベクトルの成分というものは、なんらかの座標系があり、その基底ベクトルが定まって初めて決められる量です。

座標系の取り方はいろいろあります。それによってベクトルの成分はいろいろ変わりますが、ベクトルが表しているのは変わりません。図形的には先にベクトルがあり、座標系を定めることで成分を表示できると言えば良いのかもしれません。

基底ベクトルが ${\boldsymbol {e}}_1$ 、 ${\boldsymbol {e}}_2$ 、 ${\boldsymbol {e}}_3$ だとして、ベクトル ${\boldsymbol{v}}$ が以下の様に表されるとすると

$\begin{eqnarray} {\boldsymbol{v}}={\boldsymbol {e}}_1 + 2{\boldsymbol {e}}_2+3{\boldsymbol {e}}_3\\ \\ \end{eqnarray}$

ベクトル $\boldsymbol {v}$ の成分表示は

$\begin{eqnarray} {\boldsymbol {v}}= \begin{bmatrix} 1&2&3 \end{bmatrix}^\mathsf{T} \end{eqnarray}$

となります。

さて、ここでは基底ベクトルの中身（成分）については言及していません。

もし、座標系がひとつしかなく、それ以外に登場しないのならば、基底ベクトルの成分がなんであるかは問題になることはありません。単に３つの基底ベクトルが直行していて大きさが１ということが定まっていれば良いことになります。それで、あらゆるベクトルの成分を定めることができます。

ところが、今回のこの記事は物体の方向を表すのに物体に張り付いた座標系の基底ベクトルを行列の各列に並べて表現しようと話をしています。従って、通常物体は一つとは限りませんから、座標系が複数あることになります。

例えば地球に張り付いた座標系があり、飛行機に張り付いた座標系があるということになります。

そうすると、地球の基底ベクトルと飛行機の基底ベクトルは違うものとなります。

前述した様に、ベクトルの成分は座標系とその基底ベクトルが定められないと決められません。地球の基底ベクトルも、飛行機の基底ベクトルも、それぞれベクトルと言う事には変わりありませんので、成分表示するためには基底ベクトルが必要となります。

一つの方法としては、全宇宙の基底ベクトルを決めて、それに対して地球の基底ベクトルの成分と飛行機の基底ベクトルの成分を定めると言う方法です。

これは考え方がシンプルで良い方法だと思いますが、もし、地球とそれに対して飛行機の様に、地球に対しての何かという場合は、地球の基底ベクトルを基準にして、飛行機の基底ベクトルを定めると言う方法もあります。これも良い方法で、物体の数や相対的な関係によって、適宜定めると良いです。

基底ベクトルの選び方でベクトルの成分は変わる

ここまでで「ベクトルの成分を決めるためには基底ベクトルを定める必要がある。」と言うことを説明しました。

また、基底ベクトルは全宇宙にとったり、地球にとったりと色々と取ることができると言うことも判ってきました。

ここで、同じベクトルであっても基底ベクトルを変えるとその成分は同じでいられるでしょうか。当然ながら、同じベクトルも基底ベクトルを変えるとそれに合わせた成分に変えなければ逆に同じベクトルを表す事になりません。

座標変換とは

我々が、よく悩ませられる座標変換というのは、この基底ベクトルを変えた時に、成分がどう変わるかを考える事です。

基準を変えると、表現（成分）を変えなければなりません。

全宇宙から見た時の地球の方向、全宇宙から見た時の飛行機の方向これらは、全宇宙を基準にして、その基底ベクトルにもとづいてそれぞれの成分を表現する事でわかります。

このとき、地球から見た、飛行機の方向を考えようとすることが座標変換になります。

方向を表す行列と座標変換

全宇宙の座標系があり、その中に地球と飛行機があるとします。

地球の全宇宙に対しての方向を表す行列を ${\bf{E}}$ 、飛行機の全宇宙に対しての方向を表す行列を ${\bf{A}}$ とします。（地球はEarth、飛行機はAirplaneなのでEとA）

いま全宇宙座標系で成分が定められているベクトル ${\boldsymbol {x}}$ があるとします。

このベクトルを地球座標系に座標変換して、地球座標系での成分を定めた成分ベクトルを ${\bf{x}}_E$ とすると

$\begin{eqnarray} {\boldsymbol {x}}={\bf{E}}{\boldsymbol {x}}_E\\ \\ \end{eqnarray}$

と、書くことができます。

この様に、地球の方向を表す行列は、地球座標系の成分ベクトルを全宇宙座標系のベクトルに座標変換する座標変換行列となります。

同様に、飛行機座標系に座標変換した、飛行機座標系での成分を定めた成分ベクトルを ${\bf{x}}_A$ とすると

$\begin{eqnarray} {\boldsymbol {x}}={\bf{A}}{\boldsymbol {x}}_A\\ \\ \end{eqnarray}$

以上から地球から飛行機への座標変換、あるいはその逆を考えてみると

$\begin{eqnarray} {\bf{E}}{\boldsymbol {x}}_E={\bf{A}}{\boldsymbol {x}}_A\\ \\ \end{eqnarray}$

となるので

飛行機から地球への座標変換は

$\begin{eqnarray} {\boldsymbol {x}}_E={\bf{E}}^\mathsf{T} {\bf{A}}{\boldsymbol {x}}_A\\ \\ \end{eqnarray}$

となり、逆に

地球から飛行機への座標変換は

$\begin{eqnarray} {\boldsymbol {x}}_A={\bf{A}}^\mathsf{T} {\bf{E}}{\boldsymbol{x}}_E\\ \\ \end{eqnarray}$

となります。

いま、全宇宙の座標系に地球座標系と飛行機座標系があると言う風に話してきましたが、もし、地球を基準にして飛行機座標系があると考えると、地球の方向を表していた行列 ${\bf{E}}$ を単位行列と考えて良い事になります。

よって、

飛行機から地球への座標変換は

$\begin{eqnarray} {\boldsymbol {x}}_E={\bf{A}}{\boldsymbol {x}}_A\\ \\ \end{eqnarray}$

となり、逆に

地球から飛行機への座標変換は

$\begin{eqnarray} {\boldsymbol {x}}_A={\bf{A}}^\mathsf{T} {\boldsymbol{x}}_E\\ \\ \end{eqnarray}$

となります。

繰り返しになりますが、この様に、方向を表す行列（回転行列）は、基準系への座標変換行列になります。

方向余弦行列

方向を表す行列は、回転行列とも呼ばれると前述しました。そして方向を表す行列は、その行列が示す座標空間から、基準となる座標系への座標変換行列であることも説明しました。

そして、この方向を表す行列（回転行列）の逆行列あるいは転置行列は、方向余弦行列（Direction cosine matrix）と呼ばれます。方向余弦行列は英語の頭文字をとってDCMと記述されます。

方向余弦行列は回転行列の逆行列ですので、回転行列が基準となる座標系への座標変換行列を表すのならばその逆で、基準となる座標系からの座標変換を表すことになります。

方向余弦とは

方向余弦というのは、二つのベクトルのなす角を $\theta$ とした時に $\cos \theta$ の事です。

方向を表す行列、すなわち方向余弦行列の各成分を以下の様にするとします。

$\begin{eqnarray} \begin{bmatrix} \lambda_{11}&\lambda_{12}&\lambda_{13}\\ \lambda_{21}&\lambda_{22}&\lambda_{23}\\ \lambda_{31}&\lambda_{32}&\lambda_{33}\\ \end{bmatrix} \end{eqnarray}$

上記の行列の各成分が方向余弦となります。

では、この各成分は、どのベクトルと、どのベクトルとの方向余弦かということですが

上に図の様に、濃い色の座標系から、少し回転したのがパステル調の座標系とします。濃い色の座標系が基準となる座標系で、パステル調の座標系の基底ベクトルは濃い色の座標系で表現されているとします。

そうした場合にパステル調の座標系の方向余弦行列の成分はたとえば

$\lambda_{11}$ はベクトル ${\boldsymbol{e}}_{1}$ とベクトル ${\boldsymbol{a}}_{1}$ の方向余弦となります。

$\lambda_{12}$ はベクトル ${\boldsymbol{e}}_{1}$ とベクトル ${\boldsymbol{a}}_{2}$ の方向余弦となります。

$\lambda_{13}$ はベクトル ${\boldsymbol{e}}_{1}$ とベクトル ${\boldsymbol{a}}_{3}$ の方向余弦となります。

以下同様に、元となる座標系の基底ベクトルと回転後の座標系の基底ベクトルと順に組み合わせていき、その方向余弦が行列の各成分となり現れます。

具体的に方向を表す行列を決める

ここまでで、方向を表す方法と、方向を表す行列の正体が判ってきました。

実際に方向を表す行列を使って計算をするときは、オイラー角やクォータニオンというものを使って、方向を表す行列を作って計算を進めます、本記事は長くなったのでオイラー角やクォータニオンについては簡単に説明するに留めます。

オイラー角ははすでに違う記事で説明していますが、クォータニオンについても、また後日取り上げてみたいと思います。

rikei-tawamure.com

飛行機に座標系は右手系でx軸を前後方向にとります。そして、y軸は後ろから見ると右に出る様にとります。 Z軸は下向きにします。

地球の座標系についてもZ軸が下向きになる様な右手系とします。

ここでの説明はz軸周りに $\psi$ 回転して飛行機の方位を決めます。次にそれをy軸周りに $\theta$ 回して飛行機の仰角を決めます。最後にx軸周りに $\phi$ 回して方向を定めます。

方法① ロドリゲスの回転公式を用いた基底ベクトルの回転

ベクトルを同じ座標系の中で回転させることと、座標系を回転させることは違うものと区別しなければなりません。基本的にはベクトルの回転と座標の回転は回転方向が逆のものとして数式の中に現れます。

本記事では、物体の方向は３つの基底ベクトルを列成分に持つ行列で表すとのことでした。従って、物体の方向を変えるには、この３つのベクトルを３つとも同じく回転させる事で達成可能です。

しかし、この計算をするのは中々難しいのです。

ベクトルを３次元空間で回転させるには、回転軸をベクトルとして指定し、その軸周りの回転を行います。

いま回転されるベクトルを $\boldsymbol{r}$ 、回転軸方向の単位ベクトルを $\boldsymbol{n}$ として、 $\theta$ 回転の結果 $\boldsymbol{r}^{\prime}$ が得られるとします。

この様なベクトルの回転をベクトルの計算式で表すと、次の式が得られます。

$\begin{eqnarray} {\boldsymbol{r}}^{\prime}= {\boldsymbol{r}} \cos \theta + ({\boldsymbol{r}} \cdot {\boldsymbol{n}}){\boldsymbol{n}} -({\boldsymbol{r}} \cdot {\boldsymbol{n}}){\boldsymbol{n}} \cos \theta + ({\boldsymbol{n}} \times {\boldsymbol{r}}) \sin \theta \end{eqnarray}$

この式をロドリゲスの回転公式と言います。

回転軸のベクトルの成分を以下の様に定めると、回転を回転行列 ${\bf{R}}_n(\theta)$ で表すことができます。

$\begin{eqnarray} {\bf{n}}= \begin{bmatrix} n_1 & n_2 & n_3 \\ \end{bmatrix}^\mathsf{T} \end{eqnarray}$

$\begin{eqnarray} {\boldsymbol {r}}^{\prime}= {\bf{R}}_n(\theta) {\boldsymbol {r}}\\ \end{eqnarray}$

ここで、 ${\bf{R}}_n(\theta)$ は以下です。

$\begin{eqnarray} {\bf{R}}_n(\theta)= \begin{bmatrix} n_1^2 (1- \cos \theta) + \cos \theta & n_1 n_2 (1-\cos \theta ) -n_3 \sin \theta & n_1 n_3 (1 -\cos \theta ) + n_2 \sin \theta \\ n_1 n_2 (1 -\cos \theta ) + n_3 \sin \theta & n_2^2 (1- \cos \theta) + \cos \theta & n_2 n_3 (1 -\cos \theta ) - n_1 \sin \theta \\ n_1 n_3 (1 -\cos \theta ) - n_2 \sin \theta & n_2 n_3 (1 -\cos \theta ) + n_1 \sin \theta & n_3^2 (1- \cos \theta) + \cos \theta\\ \end{bmatrix} \end{eqnarray}$

回転軸を決めて、ロドリゲスの公式を適用すると言う流れが煩雑です。

最初のz軸周りはz軸を回転軸にしてロドリゲスの公式を用いて３つの基底ベクトルの回転した結果を算出します、次のy軸周りの回転に関しては、 z軸周りの回転によって得られた新たなy軸を回転軸にしたベクトルの回転を考えます。

そして、y軸周りの回転が終わったら、新しいx軸を回転軸にした回転を行なって、方向を定める３つのベクトルが求められます。

後ほど、次に示す方法と合わせて数値計算をして有効性を確認しようと思います。

方法② 座標系の回転

先に説明したロドリゲスの公式により基底ベクトルを回転して求める方法にたいして、座標系の回転の考え方を適用する際は、回転軸を毎回考える必要がありません。

次に示す、座標系の回転公式を順に適用するだけで、３回の回転の結果が得られます。

z軸周りの回転

$\begin{eqnarray} {\bf{R}}_z(\psi)= \begin{bmatrix} \cos \psi & \sin \psi & 0 \\ -\sin \psi &\cos \psi & 0 \\ 0 & 0 & 1\\ \end{bmatrix} \end{eqnarray}$

y軸周りの回転

$\begin{eqnarray} {\bf{R}}_y(\theta)= \begin{bmatrix} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta\\ \end{bmatrix} \end{eqnarray}$

x軸周りの回転

$\begin{eqnarray} {\bf{R}}_x(\phi)= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \phi & \sin \phi \\ 0 & -\sin \phi & \cos \phi\\ \end{bmatrix} \end{eqnarray}$

統合された座標の回転行列

以上の３軸周りの座標の回転行列を、z軸周りに $\psi$ 、y軸周りに $\theta$ 、x軸周りに $\phi$ の順に適用しまとめた回転行列は次の様になります。

$\begin{eqnarray} {\bf{R}}(\phi, \theta, \psi)= \begin{bmatrix} \cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta\\ \sin \phi \sin \theta \cos \psi -\cos \phi \sin \psi & \sin \phi \sin \theta \sin \psi + \cos \phi \cos \psi & \sin \phi \cos \theta \\ \cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi & \cos \phi \sin \theta \sin \psi-\sin \phi \cos \psi & \cos \phi \cos \theta\\ \end{bmatrix} \end{eqnarray}$

この行列は、地球座標系の成分ベクトル ${\boldsymbol{x}}_E$ を、飛行機座標系の成分ベクトル ${\boldsymbol{x}}_A$ に座標変換する行列になります。従って次の式が成り立ちます。

$\begin{eqnarray} {\boldsymbol {x}}_A={\bf{R}} {\boldsymbol{x}}_E\\ \\ \end{eqnarray}$

ここから、逆に飛行機座標系の成分ベクトル ${\boldsymbol{x}}_A$ を地球座標系の成分ベクトル ${\boldsymbol{x}}_E$ に座標変換するには ${\bf{R}}$ の転置行列（直交行列では逆行列を表す）を用いて、以下の様に書けます。

$\begin{eqnarray} {\boldsymbol {x}}_E={\bf{R}}^\mathsf{T} {\boldsymbol{x}}_A\\ \\ \end{eqnarray}$

今までのお話を思い出して頂きますと、この ${\bf{R}}^\mathsf{T}$ は飛行機の方向を表す行列ということを意味しています。

では、次のプログラムを用いて、方法①で飛行機の方向を表す行列を求めた場合と方法②で求めた場合を比較します。

import numpy as np

#ロドリゲスの公式
def Rod(n,th):
    n1=n[0]
    n2=n[1]
    n3=n[2]
    R=np.matrix(\
        [[n1**2*(1-np.cos(th))+np.cos(th), n1*n2*(1-np.cos(th))-n3*np.sin(th), n1*n3*(1-np.cos(th))+n2*np.sin(th)],\
         [n1*n2*(1-np.cos(th))+n3*np.sin(th), n2**2*(1-np.cos(th))+np.cos(th), n2*n3*(1-np.cos(th))-n1*np.sin(th)],\
         [n1*n3*(1-np.cos(th))-n2*np.sin(th), n2*n3*(1-np.cos(th))+n1*np.sin(th), n3**2*(1-np.cos(th))+np.cos(th)]])
    
    return R

#座標の回転行列の転置（逆行列）
def Rot_inv(phi,th,psi):
    R=np.matrix(\
        [[np.cos(th)*np.cos(psi), np.sin(phi)*np.sin(th)*np.cos(psi)-np.cos(phi)*np.sin(psi), np.cos(phi)*np.sin(th)*np.cos(psi)+np.sin(phi)*np.sin(psi)],\
         [np.cos(th)*np.sin(psi), np.sin(phi)*np.sin(th)*np.sin(psi)+np.cos(phi)*np.cos(psi), np.cos(phi)*np.sin(th)*np.sin(psi)-np.sin(phi)*np.cos(psi)],\
         [-np.sin(th), np.sin(phi)*np.cos(th), np.cos(phi)*np.cos(th)]])
    return R


angle=30*np.pi/180

z0=[0.0, 0.0, 1.0]

#z軸周りの回転
X1=Rod(z0, angle)
x1=[X1[0,0],X1[1,0],X1[2,0]]
y1=[X1[0,1],X1[1,1],X1[2,1]]
z1=[X1[0,2],X1[1,2],X1[2,2]]

#y軸周りの回転
X2=Rod(y1,angle)*X1
x2=[X2[0,0],X2[1,0],X2[2,0]]
y2=[X2[0,1],X2[1,1],X2[2,1]]
z2=[X2[0,2],X2[1,2],X2[2,2]]

#x軸周りの回転
X3=Rod(x2,angle)*X2
x3=[X3[0,0],X3[1,0],X3[2,0]]
y3=[X3[0,1],X3[1,1],X3[2,1]]
z3=[X3[0,2],X3[1,2],X3[2,2]]

#X3が方向を表す行列となる
print(X3)

#座標変換の場合と比較
print('------------------------------------------')
print(Rot_inv(angle,angle,angle))

このプログラムの実行結果は

[[ 0.75       -0.21650635  0.625     ]
 [ 0.4330127   0.875      -0.21650635]
 [-0.5         0.4330127   0.75      ]]
------------------------------------------
[[ 0.75       -0.21650635  0.625     ]
 [ 0.4330127   0.875      -0.21650635]
 [-0.5         0.4330127   0.75      ]]

こうなって、方法①と方法②で計算した飛行機の方向（物体の方向）を表す行列が一致することが示せました。

オイラー角

以上の様に、地球座標系の様な基準の座標系を決まった順番で決まった軸周りに回転させて、飛行機等の物体の座標系に一致させる際の回転角をオイラー角と言います。

以上から、オイラー角を用いて表した、座標変換行列の転置が物体の方向を表す行列（回転行列）です。

オイラー角は回転する座標軸とその順番に依存するので、同じオイラー角が必ずしも同じものを表していないかもしれませんので、回転順を確認する必要があります。

ここで、大事なことはオイラー角は３つの角度情報で物体の方向を表せるということです。

結果得られた行列は３x３で９個の情報を有している様に見えますが、物体の方向は３つの次元で記述可能ということがわかってきます。

オイラー角の欠点で有名なものが特異点があると言うことです。

オイラー角で求められた回転行列をよく見ると $\cos \theta$ が含まれている部分があります。 $\theta$ が90°に相当する角になると、 $\cos \theta$ が０となり、他の角がどんなに変化しても方向余弦行列の複数の成分が変化しなくなります。つまり、他の角で回転しようとしても動かなくなるので、ここが特異点になります。この特異点をジンバルロックと呼びます。

方向余弦行列とクォータニオン

最後にお話をするのはクォータタニオン（四元数（しげんすう））についてです。実際に物体の方向を計算するのにクォータタニオンが多用されています。

これはオイラー角を用いた角度計算では特異点が生じて計算が続行できない場面が生じます。これを回避するためには特異点が生じないクォータタニオンを用いるしかありません。

クォータニオンを詳しく説明するのは、またの機会にするとし、ここでは方向を計算するのに必要な部分に絞って説明しようと思います。

クォータニオンは複素数を拡張した様なもので、次の式の様に表します。

$\begin{eqnarray} {\boldsymbol{q}}=q_0 + q_1 {\boldsymbol{i}}+ q_2 {\boldsymbol {j}}+ q_3 {\boldsymbol {k}}\\ \\ \end{eqnarray}$

${\boldsymbol{i}}$ 、 ${\boldsymbol{j}}$ 、 ${\boldsymbol{k}}$ は虚数単位と同じ２乗すると−１となります。また

$\begin{eqnarray} {\boldsymbol{i}}{\boldsymbol {j}}{\boldsymbol {k}}=-1\\ \\ \end{eqnarray}$

と言う性質もあります。

４次のベクトル風に成分表示すると以下の様に表すこともあります。

$\begin{eqnarray} {\boldsymbol{q}}= \begin{bmatrix} q_0&q_1&q_2&q_3\\ \end{bmatrix} \\ \end{eqnarray}$

また、クォータニオンのノルムは１とします。

$\begin{eqnarray} \|{\boldsymbol{q}}\|=\sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2}=1 \\ \end{eqnarray}$

ここで、クォータニオンの各成分を次の様に置くと

$\begin{eqnarray} {\boldsymbol{q}}= \begin{bmatrix} q_0&q_1&q_2&q_3\\ \end{bmatrix} = \begin{bmatrix} \cos \frac{\theta}{2}&n_1 \sin \frac{\theta}{2}&n_2 \sin \frac{\theta}{2}&n_3 \sin \frac{\theta}{2}\\ \end{bmatrix} \\ \end{eqnarray}$

単位ベクトル

$\begin{eqnarray} {\boldsymbol{n}}= \begin{bmatrix} n_1&n_2&n_3\\ \end{bmatrix} \\ \end{eqnarray}$

が回転の軸を表し、 $\theta$ が回転角とする、回転をクォータニオンが表す事になります。

クォータニオンの回転もベクトルの回転と座標の回転を表すことができます。内容的にはロドリゲスの回転公式と全く同じものが出てくることになり、任意軸周りの回転とクォータニオンの回転は全く同じことを違う書き方をしていることがわかります。ただし、それらの導出にはクォータニオンの積などの特有の演算ルールの理解が必要なので以下では結果だけ示します。

クォータニオンによるベクトルの回転行列

次の行列はクォータニオンによるベクトルの回転行列となります。

$\begin{eqnarray} \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2 (q_1 q_2 -q_0 q_3 ) & 2 (q_1 q_3 -q_0 q_2 )\\ 2 (q_1 q_2 + q_0 q_3 ) & q_0^2-q_1^2+q_2^2-q_3^2 & 2 (q_2 q_3 -q_0 q_1 )\\ 2 (q_1 q_3 -q_0 q_2 ) & 2 (q_2 q_3 -q_0 q_1 ) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} \\ \end{eqnarray}$

こちらは、飛行機などの物体の座標系から地球などの基準座標系への座標変換行列になっており、各列の成分は物体座標系の基底ベクトルとなるため物体の方向を示す行列にもなっています。

クォータニオンによる座標の回転行列

座標の回転はベクトルの回転の逆回転の形になるので、転置行列になります

$\begin{eqnarray} \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2 (q_1 q_2 + q_0 q_3 ) & 2 (q_1 q_3 -q_0 q_2 )\\ 2 (q_1 q_2 -q_0 q_3 ) & q_0^2-q_1^2+q_2^2-q_3^2 & 2 (q_2 q_3 -q_0 q_1 )\\ 2 (q_1 q_3 -q_0 q_2 ) & 2 (q_2 q_3 -q_0 q_1 ) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} \\ \end{eqnarray}$