理系的な戯れ

理工学系とくにロボットやドローンに関する計算・プログラミング等の話題を扱って、そのようなことに興味がある人たちのお役に立てればと思っております。

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

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

はじめに

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

方向と向き

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

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

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

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

方向が同じでも向きが違うベクトル

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

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

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

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

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

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

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

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

同じ軸ベクトル周りの回転

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

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

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

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

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

直交するベクトルは無限に存在する

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

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

二つのベクトルで物体の方向が定められる

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

二つのベクトルに直交するベクトル

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

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

右手系と左手系

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

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

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

右手系の例

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

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

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

物体に固定された直交座標

行列による方向の表記

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

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

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

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

方向を表す行列

方向を表す行列の性質

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

直交行列には行列計算的に嬉しい性質があって、逆行列が転置と等しくなります。つまり、直交行列を{\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}

となります。

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

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

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

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

地球の座標系と飛行機の座標系

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

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

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

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

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

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

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

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

座標変換とは

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

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

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

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

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

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

地球の全宇宙に対しての方向を表す行列を{\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回して 方向を定めます。

座標が一致した状態z軸周りに回した状態y軸周りに回した状態x軸周りに回した状態
座標をz→y→x軸周りの順で回転する様子

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

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

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

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

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

いま回転されるベクトルを\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軸を回転軸にしてロドリゲスの公式を用いて3つの基底ベクトルの回転した結果を算出します、次のy軸周りの回転に関しては、 z軸周りの回転によって得られた新たなy軸を回転軸にしたベクトルの回転を考えます。

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

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

方法② 座標系の回転

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

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

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}

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

以上の3軸周りの座標の回転行列を、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      ]]
​

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

オイラー角

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

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

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

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

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

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

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

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

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

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

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

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


\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}}は虚数単位と同じ2乗すると−1となります。また


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

と言う性質もあります。

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


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

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


\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}

こちらは、基準座標系から物体座標系への座標変換行列となります。

クォータニオンによる回転行列の成分を決める

クォータニオンを用いて、回転行列の初期値等を定めるのにクォータニオンの成分を直感的に定めるのは困難です。

方法としては、他の方法で方向余弦行列をつくり、それらと上記クォータニオンによって表現された回転行列を比較して 成分を求めます。

その際、3つある対角成分すべての比較からクォータニオンの一つの成分を決めることができます。残念ながら対角成分は3つしかなく4つ全てを 決定することはできません。この時点で好きな成分を決めて構わないので、残りの3つの成分は、他の行列の成分の比較から順に求めることができます。

おわりに

物体の方向を表すものとして、物体の座標系の基底ベクトルを列成分にもつ行列を用います。

これは、物体の座標系から基準の座標系への座標変換行列にもなります。

この様な行列は直交行列で回転行列であり、方向余弦行列とも呼ばれています。

方向余弦行列を定めるには基底ベクトルを回転して定める方法や、オイラー角を用いた回転で 求められることを示しました。

また、オイラー角表現での欠点である特異値を回避するためにクォータニオンを用いた表現位ついても触れました。

簡単に分かりやすくを目標に描き始めましたが、長く読みにくい記事になりました。 こうやって整理してみると、簡潔に表現することの難しさを改めて痛感しました。

それでは、また次の話題で!