はじめに

回転計算について、何度も調べては実装を繰り返しています。今回もまた調べて実装したので、メモっておきます。

姿勢計算は、自動運転やロボティクスでは必ず使われます。3つの軸xyz の正規直行系が、回転操作により３次元空間で変化してきます。
姿勢情報はいくつか表現がありますが、初期の姿勢から回転操作によるある姿勢に達するので、回転操作と姿勢は同じような数式の表現になります。

回転操作あるいは姿勢を表すのには、回転行列、四元数、回転ベクトル(so(3))などあります。
回転操作の時間微分にあたる角速度（ベクトル）については、位置のような単純な積分にはならないので、その計算方法が必要です。
物理の解析力学ではオイラー角はZ-Y-Zの順で回転させると定義します。どちらから見ても同じ順序になって便利と。これは、工学で使用しているオイラー角とは異なるものなので要注意。

Euler角と回転行列

ここでは、良く使うroll, pitch, yaw と回転行列の変換を整理します。
地球上の位置座標は緯度、経度、楕円体高（地球を楕円体でモデル化）が用いられます。
姿勢を表すには、方位角(azimuth)、仰角(elevation)などが用いられます。これらの関係について整理します。

Euler角から回転行列

グローバル座標の軸とローカル座標の軸が一致した状態を最初に考えます。
そこから、

まずローカル座標系のZ軸をつまんで、$\psi$ だけ回転させます。この状態では、ローカル座標系はグローバル座標系と同じＸＹ平面上に乗っています。
つぎにローカル座標系のＹ軸をつまんで$\theta$だけ回転させます。この状態では、ローカル座標系のＸ軸はグローバル座標系のＸＹ平面から$\theta$だけ上向き（回転角が負なら下向き）になっています。
さらに、ローカル座標軸のＸ軸をつまんで$\phi$だけ回転させます。

この3つの操作で、任意のローカル座標系の姿勢に変換することができます。ここで回転させたX軸、y軸、z軸周りの回転はそれぞれroll, pitch, yaw と呼ばれます。これらをEuler角と呼ぶようです。

\textrm{U}_{local} = \textrm{U}_{global} R_z(\psi) R_y(\theta) R_x(\phi)

です。Uは回転行列で、xyz軸を並べたものです。

\textrm{U} = \left[ \textrm{u}_x,\textrm{u}_y,\textrm{u}_z\right]

ローカル座標系のｘｙｚなら姿勢と呼ばれますし、グローバル座標ならxyz軸がそれぞれの軸の単位ベクトルを表すのでＵは単位行列になります。

各軸の周りの回転は以下のように計算できます。

from numpy import ndarray, pi
from numpy import array, sin, cos, dot, arctan2, arcsin, deg2rad, rad2deg

def Rx(r:float) -> ndarray:
    """
    Parameters
    ----------
    r, roll angle in radian
    """
    cr, sr = cos(r), sin(r)
    return array( [ [1.0, 0., 0.0],\
                    [0.0, cr, -sr], \
                    [0.0, sr, cr]])

def Ry(p:float) -> ndarray:
    """
    Parameters
    ----------
    p, pitch angle in radian
    """
    ry = [  [cos(p), 0., sin(p)],\
            [0.0, 1.0, 0.0], \
            [-sin(p), 0.0, cos(p)]]
    return array(ry)

def Rz(y:float) -> ndarray:
    """
    Parameters
    ----------
    y, yaw angle in radian
    """
    rz = [  [cos(y), -sin(y), 0.0],\
            [sin(y), cos(y), 0.0], \
            [0.0, 0.0, 1.0]]
    return array(rz)

これらを計算した結果は、下記の通りになります。当然、直接３つの行列を書けても同じ結果になりますが。

また、式から明らかですが、math U_{local} の各列は、ローカル座標のｘ、ｙ、ｚ軸のグローバル座標での表現になっています。

回転行列からEuler角

回転行列を書き下した形から、逆三角関数を使ってもとめることができます。いろいろな求め方がありますが、次の方法がよく用いられています。

def R2euler(R:ndarray) -> list:
    """
    Convert rotation matrix to Euler angles
    Parameters
    ----------
    R, rotation matrix
    Returns
    -------
    Euelr angles in radian.
    r, roll
    p, pitch
    y, yaw
    """
    # ジンバルロックを無視すると、、、
    return [ arctan2(R[2,1], R[2,2]), arcsin(-R[2,0]), arctan2(R[1,0], R[0,0]) ]

ジンバルロック

注意すべきはpitch が$\theta=\frac{\pi}{2}$ or $\theta=-\frac{\pi}{2}$ のときです。実際に、回転行列を計算してみると分かります。

\begin{eqnarray}
 \textrm{R}_z(\psi)  \textrm{R}_y(\pi/2)  \textrm{R}_x(\phi) 
&=& \left[\begin{array}{ccc}
\cos(\psi) & -\sin(\psi) & 0\\
\sin(\psi) & \cos(\psi) & 0 \\
0 & 0 & 1
\end{array} 
\right]
\left[\begin{array}{ccc}
0 & 0 & 1\\
0 & 1 & 0\\
-1 & 0 & 0
\end{array}
\right]
\left[\begin{array}{ccc}
1 & 0 & 0 \\
0 & \cos\phi & -\sin\phi\\
0 & \sin\phi & \cos\phi  \\
\end{array}
\right] \\
& = &
\left[\begin{array}{ccc}
0 & -\sin(\psi-\phi) & \cos(\psi-\phi) \\
0 & \cos(\psi-\phi) & \sin(\psi-\phi)\\
-1 & 0 & 0  \\
\end{array}
\right]
\end{eqnarray}

となります。ローカル座標のｘ軸が [0, 0, -1] となっている、つまりＹ軸方向に90度回転して下向きになっているのが分かります。そして重要なのが、Ｙ軸、Ｚ軸はグローバル座標のＸＹ平面にいるのですが、回転の自由度は１しかなく、その大きさは$\psi-\phi$です。つまりroll と yaw は差だけが決まるが、個々の値は不定となっています。

では、このような場合、どのようにEuler角に戻したらよいでしょうか。一つの方法では、rollを0としてyaw の値を定める方法があるようです。そのような実装を見かけます。

メモ

scipy

回転計算がquaternion, rotation vector も含めて一通りそろっている様子。最初からこれを使えばよいのか。

scipy.spatial.transform.Rotation のreference

その他

Qiitaの数式チートシート

まとめ

エイやで書いていて、この記事も含め、修正しつつ、良い内容にに仕上げていきたいと思います。下記途中ですが、投稿してしまいます。

本日はまずはEuler角と回転行列の相互変換を実装しました。最終的にはquaternionやso(3)、IMUを用いた慣性航法の式まで説明できたらと思います。

(2021/01/24)

編集メモ

雑なジンバルロックの説明（数式による）を追加 (2021/01/26)

【姿勢計算】Euler角と回転行列