[kaze's test] プログラミングメモ
オイラー角とクォータニオン間の変換とベクトルの回転
Euler Angles, Quaternion, Rotation

→目次

四元数(クォータニオン:q0+q1i+q2j+q3k)の形式で移動体の姿勢を求める手法を記述するメモです。 座標系をX軸(roll、ϕ)Y軸(pitch、θ)Z軸(yaw、ψ)としてメモを記述してまします。 「参照座標系」は地面から見る座標系で、「ボディー座標系」は移動体上のセンサーから取得したデータの座標系です。 最初は、参照座標系がボディー座標系と重なるとし、移動体の姿勢が参照座標系で表現されます。

1.オイラー角からクォータニオン(四元数)へ変換

X軸をΦ角度で回転させると、下記のようなクォータニオン(四元数)を得ます。

error

XYZ軸のオイラー角をϕ、θ、ψとし、X軸Y軸Z軸の順で回転すると、三次元のクォータニオン(四元数)は下記のように求めることができます。

C++プログラムは下記ようです。

void EulerAnglesToQuaternion(double roll, double pitch, double yaw,
                            double& q0, double& q1, double& q2, double& q3)
{
    double cosRoll = cos(roll / 2.0);
    double sinRoll = sin(roll / 2.0);
    double cosPitch = cos(pitch / 2.0);
    double sinPitch = sin(pitch / 2.0);
    double cosYaw = cos(yaw / 2.0);
    double sinYaw = sin(yaw / 2.0);

    q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
    q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
    q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
    q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
}

2.回転クォータニオンの乗算から回転行列の求め方

回転クォータニオンQでベクトルPを回転させるとして、回転行列Cを下記のように求めます。

Q x P x Q*=
=(q0+q1i+q2j+q3k)(p1i+p2j+p3k)(q0-q1i-q2j-q3k)
=(q0p1i+q0p2j+q0p3k + q1p1ii+q1p2ij+q1p3ik + q2p1ji+q2p2jj+q2p3jk + q3p1ki+q3p2kj+q3p3kk)(q0-q1i-q2j-q3k)
=[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k](q0-q1i-q2j-q3k)

=[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k]q0
-[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k]q1i
-[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k]q2j
-[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k]q3k

=-(q0q1p1+q0q2p2+q0q3p3)  + (q0q0p1+q0q2p3-q0q3p2)i + (q0q0p2-q0q1p3+q0q3p1)j + (q0q0p3+q0q1p2-q0q2p1)k
 +(q1q1p1+q1q2p2+q1q3p3)i + (q0q1p1+q1q2p3-q1q3p2)  + (q0q1p2-q1q1p3+q1q3p1)k - (q0q1p3+q1q1p2-q1q2p1)j
 +(q1q2p1+q2q2p2+q2q3p3)j - (q0q2p1+q2q2p3-q2q3p2)k + (q0q2p2-q1q2p3+q2q3p1)  + (q0q2p3+q1q2p2-q2q2p1)i
 +(q1q3p1+q2q3p2+q3q3p3)k + (q0q3p1+q2q3p3-q3q3p2)j - (q0q3p2-q1q3p3+q3q3p1)i + (q0q3p3+q1q3p2-q2q3p1)

=(+q0q0p1+q0q2p3-q0q3p2 +q1q1p1+q1q2p2+q1q3p3 +q0q2p3+q1q2p2-q2q2p1 -q0q3p2+q1q3p3-q3q3p1)i
+(+q0q0p2-q0q1p3+q0q3p1 -q0q1p3-q1q1p2+q1q2p1 +q1q2p1+q2q2p2+q2q3p3 +q0q3p1+q2q3p3-q3q3p2)j
+(+q0q0p3+q0q1p2-q0q2p1 +q0q1p2-q1q1p3+q1q3p1 -q0q2p1-q2q2p3+q2q3p2 +q1q3p1+q2q3p2+q3q3p3)k

=(+q0q0p1+q1q1p1-q2q2p1-q3q3p1 +q1q2p2+q1q2p2-q0q3p2-q0q3p2 +q0q2p3+q0q2p3+q1q3p3+q1q3p3)i
+(+q0q3p1+q0q3p1+q1q2p1+q1q2p1 +q0q0p2-q1q1p2+q2q2p2-q3q3p2 -q0q1p3-q0q1p3+q2q3p3+q2q3p3)j
+(+q1q3p1+q1q3p1-q0q2p1-q0q2p1 +q0q1p2+q0q1p2+q2q3p2+q2q3p2 +q0q0p3-q1q1p3-q2q2p3+q3q3p3)k

=[(q0q0+q1q1-q2q2-q3q3)p1 + (q1q2+q1q2-q0q3-q0q3)p2 + (q0q2+q0q2+q1q3+q1q3)p3]i
+[(q0q3+q0q3+q1q2+q1q2)p1 + (q0q0-q1q1+q2q2-q3q3)p2 + (q2q3+q2q3-q0q1-q0q1)p3]j
+[(q1q3+q1q3-q0q2-q0q2)p1 + (q0q1+q0q1+q2q3+q2q3)p2 + (q0q0-q1q1-q2q2+q3q3)p3]k

=[(q02+q12-q22-q32)p1 + 2(q1q2-q0q3)p2 + 2(q0q2+q1q3)p3]i
+[2(q0q3+q1q2)p1 + (q02-q12+q22-q32)p2 + 2(q2q3-q0q1)p3]j
+[2(q1q3-q0q2)p1 + 2(q0q1+q2q3)p2 + (q02-q12-q22+q32)p3]k

上記の結果から下図に示す回転行列Cを作れました。

3.クォータニオン(四元数)からオイラー角へ変換

「オイラー角と回転行列間の変換」で出来た回転行列を回転クォータニオンからできた回転行列と比べると、クォータニオン(四元数)からオイラー角への変換が下記のように求められます。

C++プログラムは下記ようです。

void QuaternionToEulerAngles(double q0, double q1, double q2, double q3,
                             double& roll, double& pitch, double& yaw)
{
    double q0q0 = q0 * q0;
    double q0q1 = q0 * q1;
    double q0q2 = q0 * q2;
    double q0q3 = q0 * q3;
    double q1q1 = q1 * q1;
    double q1q2 = q1 * q2;
    double q1q3 = q1 * q3;
    double q2q2 = q2 * q2;
    double q2q3 = q2 * q3;
    double q3q3 = q3 * q3;
    roll = atan2(2.0 * (q2q3 + q0q1), q0q0 - q1q1 - q2q2 + q3q3);
    pitch = asin(2.0 * (q0q2 - q1q3));
    yaw = atan2(2.0 * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3);
}


4.ボディー座標系と参照座標系間の回転行列作成

ボディー座標系⇒参照座標系の回転行列は下図のようになります。

{xn,yn,zn}は参照座標系のベクトルで、{xb,yb,zb}はボディー座標系のベクトルです。C++プログラムは下記ようです。センサーから取得した加速度を参照座標系へ変換の例です。

(ソースの中で、q0*q0 + q1*q1 + q2*q2 + q3*q3 = 1を利用しています)

double q0, double q1, double q2, double q3;
double ax, double ay, double az;   // ボディ座標
double ax_ref, double ay_ref, double az_ref;  //参照系座標
void BodyAccelToRefAcce()
{
    double q0q0 = q0 * q0;
    double q0q1 = q0 * q1;
    double q0q2 = q0 * q2;
    double q0q3 = q0 * q3;
    double q1q1 = q1 * q1;
    double q1q2 = q1 * q2;
    double q1q3 = q1 * q3;
    double q2q2 = q2 * q2;
    double q2q3 = q2 * q3;
    double q3q3 = q3 * q3;

    ax_ref = 2 * ax*(0.5 - q2q2 - q3q3) + 2 * ay*(q1q2 - q0q3) + 2 * az*(q1q3 + q0q2);
    ay_ref = 2 * ax*(q1q2 + q0q3) + 2 * ay*(0.5 - q1q1 - q3q3) + 2 * az*(q2q3 - q0q1);
    az_ref = 2 * ax*(q1q3 - q0q2) + 2 * ay*(q2q3 + q0q1) + 2 * az*(0.5 - q1q1 - q2q2);
}

この回転行列の転置行列を使用して、参照座標系のベクトルをボディー座標系へ変換できます。回転行列の転置行列は下図になります。

プログラムは省略です。

5.オイラー角変化(微分)によりクォータニオンの変化

下図は、クォータニオンの微分方程式、オイラー角の変化(微分)によりクォータニオンの変化の式を示します。

前周期のクォータニオンと当周期のクォータニオンの関係はqi+1=qi + (1/2)Tωqiになります。
C++プログラムは下記ようです。ジャイロの角速度で周期的に姿勢のクォータニオンを変更させる。

struct QUATERNION {
    double q0; double q1; double q2; double q3;
};

角速度が参照座標系(地面座標系)の場合:

QUATERNION QuaternionUpdate(QUATERNION& quat, double wx, double wy, double wz, double t)
{
    double gx = wx * t / 2.0;
    double gy = wy * t / 2.0;
    double gz = wz * t / 2.0;
    QUATERNION quat1 = quat;
    quat1.q0 += (        0      - quat1.q1 * gx - quat1.q2 * gy - quat1.q3 * gz);
    quat1.q1 += ( quat1.q0 * gx        +0       - quat1.q2 * gz - quat1.q3 * gy);
    quat1.q2 += ( quat1.q0 * gy + quat1.q1 * gz      +0         - quat1.q3 * gx);
    quat1.q3 += ( quat1.q0 * gz - quat1.q1 * gy + quat1.q2 * gx     +0         );
    return quat1;
}

角速度が移動体座標系(ボディー座標系)の場合:

QUATERNION QuaternionUpdate(QUATERNION& quat, double wx, double wy, double wz, double t)
{
    double gx = wx * t / 2.0;
    double gy = wy * t / 2.0;
    double gz = wz * t / 2.0;
    QUATERNION quat1 = quat;
    quat1.q0 += (        0      - quat1.q1 * gx - quat1.q2 * gy - quat1.q3 * gz);
    quat1.q1 += ( quat1.q0 * gx        +0       + quat1.q2 * gz - quat1.q3 * gy);
    quat1.q2 += ( quat1.q0 * gy - quat1.q1 * gz      +0         + quat1.q3 * gx);
    quat1.q3 += ( quat1.q0 * gz + quat1.q1 * gy - quat1.q2 * gx     +0         );
    return quat1;
}