四元数与欧拉角（RPY角）的相互转换_rpy角转四元数

<<Quaternions`     （* This loads the package *）
Quaternion[2, 1, 1, 3] ** Quaternion[2, 1, 1, 0] ** Quaternion[1, 1, 1, 1]    (* Be sure to use ** rather than * when multiplying quaternions *)

　　计算结果为：Quaternion[-12, 4, 14, 2]

　　θ = ATan(y / x)求出的θ取值范围是[-PI/2, PI/2]；

　　θ = ATan2(y, x)求出的θ取值范围是[-PI,   PI]。

• 当 (x, y) 在第一象限, 0 < θ < PI/2

• 当 (x, y) 在第二象限 PI/2 < θ≤PI

• 当 (x, y) 在第三象限, -PI < θ < -PI/2

• 当 (x, y) 在第四象限, -PI/2 < θ < 0

　　 上面的代码存在一个问题，即奇异性没有考虑。下面看一种特殊的情况（参考Maths - Conversion Quaternion to Euler）：假设一架飞机绕Y轴旋转了90°（俯仰角pitch=90），机头垂直向上，此时如何计算航向角和横滚角？

　　这时会发生自由度丢失的情况，即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题：

　　的定义域为，因此，当时（在程序中浮点数不能直接进行等于判断，要使用合理的阈值），俯仰角为90°，将其带入正向公式计算出四元数，然后可以发现逆向公式中atan2函数中的参数全部为0，即出现了的情况！无法计算。

　　时，，将其带入公式中有

　　则，于是有

 　　通常令，这时。可以进行验证：当四元数为(w,x,y,z)=(0.653,-0.271,0.653,0.271)时，根据这些规则计算出来的ZYX欧拉角为α=0°，β=90°，γ=45°

　　当俯仰角为-90°，即机头竖直向下时的情况也与之类似，可以推导出奇异姿态时的计算公式。比较完整的四元数转欧拉角（Z-Y-X order）的代码如下：

CameraSpacePoint QuaternionToEuler(Vector4 q) // Z-Y-X Euler angles
{
    CameraSpacePoint euler = { 0 };
    const double Epsilon = 0.0009765625f;
    const double Threshold = 0.5f - Epsilon;

    double TEST = q.w*q.y - q.x*q.z;

    if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
    {
        int sign = Sign(TEST);

        euler.Z = -2 * sign * (double)atan2(q.x, q.w); // yaw

        euler.Y = sign * (PI / 2.0); // pitch

        euler.X = 0; // roll

    }
    else
    {
        euler.X = atan2(2 * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
        euler.Y = asin(-2 * (q.x*q.z - q.w*q.y));
        euler.Z = atan2(2 * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
    }
        
    return euler;
}

　　在DirectXMath Library中有许多与刚体姿态变换相关的函数可以直接调用：

• 四元数乘法：XMQuaternionMultiply method --Computes the product of two quaternions.
• 旋转矩阵转四元数：XMQuaternionRotationMatrix method --Computes a rotation quaternion from a rotation matrix.
• 四元数转旋转矩阵：XMMatrixRotationQuaternion method -- Builds a rotation matrix from a quaternion.
• 欧拉角转四元数：XMQuaternionRotationRollPitchYaw method --Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
• 四元数转Axis-Angle：XMQuaternionToAxisAngle method --Computes an axis and angle of rotation about that axis for a given quaternion.
• 欧拉角转旋转矩阵：XMMatrixRotationRollPitchYaw method --Builds a rotation matrix based on a given pitch, yaw, and roll (Euler angles).
• Axis-Angle转旋转矩阵：XMMatrixRotationAxis method --Builds a matrix that rotates around an arbitrary axis.
• 构造绕X/Y/Z轴的旋转矩阵：XMMatrixRotationX method --Builds a matrix that rotates around the x-axis.(Angles are measured clockwise when looking along the rotation axis toward the origin)

 　　下面的代码中坐标系绕X轴旋转90°（注意这里不是按照右手定则的方向，而是沿着坐标轴向原点看过去以顺时针方式旋转，因此与传统的右手定则刚好方向相反），来进行变换：

  View Code

　　结果如下图所示: