移動体のジャイロセンサーから取得したオイラー角、角速度から回転行列を算出する方法のメモです。

■X軸、Y軸、Z軸のオイラー角から回転行列下記のように変換できます。

X軸Y軸Z軸の順であるベクトルを回転させるとすると、[Rz][Ry][Rx]の掛け算になります。回転行列は下図になります。

C++のプログラムは下記ようになります。

struct ROT_MATRIX {
    double R11,R12,R13,R21,R22,R23,R31,R32,R33;
};



ROT_MATRIX EulerToRotationMatrix(double x, double y, double z)
{
    struct ROT_MATRIX R;
    R.R11 = cos(y) * cos(z);
    R.R12 = sin(x) * sin(y) * cos(z) - cos(x) * sin(z);
    R.R13 = cos(x) * sin(y) * cos(z) + sin(x) * sin(z);

    R.R21 = cos(y) * sin(z);
    R.R22 = sin(x) * sin(y) * sin(z) + cos(x) * cos(z);
    R.R23 = cos(x) * sin(y) * sin(z) - sin(x) * cos(z);

    R.R31 = -sin(y);
    R.R32 = sin(x) * cos(y);
    R.R33 = cos(x) * cos(y);
    return R;
}

■回転行列からオイラー角を下記ように求められます。

C++のプログラムは下記ようになります。

void RotationMatrixToEuler(const ROT_MATRIX& R, double& roll, double& pitch, double& yaw)
{
    roll = atan2(R.R32, R.R33);
    pitch = asin(-R.R31);
    yaw = atan2(R.R21, R.R11);
}

注意！オイラー角で作った回転行列からまたオイラー角に戻せますが、正しく戻す範囲は下記のように限られます。

■回転行列を時間で微分すれば、オイラー角加速度が出てきます。オイラー角速度による回転行列変化を算出することができます。式は下図に示します。

S(w)xR(t)は、角速度により回転行列の変化です。それを利用すればオイラー角微小変化や短時間でのオイラー角速度により回転行列の変化を算出することができます。C++のプログラムは下記ようになります。パラメーターのRは回転行列で、ωx,ωy,ωzは各軸のオイラー角速度で、tは短い時間です。

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

ROT_MATRIX RotationMatrixUpdate(ROT_MATRIX& R, double wx, double wy, double wz, double t)
{
    struct ROT_MATRIX R1;
    double x = wx * t;
    double y = wy * t;
    double z = wz * t;
    R1.R11 = R.R11 - z * R.R21 + y * R.R31;
    R1.R12 = R.R12 - z * R.R22 + y * R.R32;
    R1.R13 = R.R13 - z * R.R23 + y * R.R33;
  
    R1.R21 = R.R21 + z * R.R11 - x * R.R31;
    R1.R22 = R.R22 + z * R.R12 - x * R.R32;
    R1.R23 = R.R23 + z * R.R13 - x * R.R33;
  
    R1.R31 = R.R31 - y * R.R11 + x * R.R21;
    R1.R32 = R.R32 - y * R.R12 + x * R.R22;
    R1.R33 = R.R33 - y * R.R13 + x * R.R23;
    return R1;
}

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

ROT_MATRIX RotationMatrixUpdate(ROT_MATRIX& R, double wx, double wy, double wz, double t)
{
    struct ROT_MATRIX R1;
    double x = wx * t;
    double y = wy * t;
    double z = wz * t;

    R1.R11 = R.R11 + R.R12 * z - R.R13 * y;
    R1.R21 = R.R21 + R.R22 * z - R.R23 * y;
    R1.R31 = R.R31 + R.R32 * z - R.R33 * y;

    R1.R12 = R.R12 - R.R11 * z + R.R13 * x;
    R1.R22 = R.R22 - R.R21 * z + R.R23 * x;
    R1.R32 = R.R32 - R.R31 * z + R.R33 * x;

    R1.R13 = R.R13 + R.R11 * y - R.R12 * x;
    R1.R23 = R.R23 + R.R21 * y - R.R22 * x;
    R1.R33 = R.R33 + R.R31 * y - R.R32 * x;
    return R1;
}