math - 四元数 - YawPitchRoll 和 EulerAngles 之间的转换仅在 Pi 的间距时产生不正确的结果

Question

我花了一些时间实现了几种算法来在 EulerAngles 和 Quaternions 之间进行转换。

我正在测试四元数值是否与此代码相同

        Quaternion orientation0 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(0, 0, 0);
        Vector3 rotation = orientation0.ToEulerAngles();
        Quaternion orientation1 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z);

        Console.WriteLine(orientation0);
        Console.WriteLine(orientation1);

我使用了以前讨论过的方法 here并自此实现了另一种方法 here

    public static Quaternion QuaternionFromYawPitchRoll(float yaw, float pitch, float roll)
    {
        float rollOver2 = roll * 0.5f;
        float sinRollOver2 = (float)Math.Sin((double)rollOver2);
        float cosRollOver2 = (float)Math.Cos((double)rollOver2);
        float pitchOver2 = pitch * 0.5f;
        float sinPitchOver2 = (float)Math.Sin((double)pitchOver2);
        float cosPitchOver2 = (float)Math.Cos((double)pitchOver2);
        float yawOver2 = yaw * 0.5f;
        float sinYawOver2 = (float)Math.Sin((double)yawOver2);
        float cosYawOver2 = (float)Math.Cos((double)yawOver2);

        // X = PI is giving incorrect result (pitch)

        // Heading = Yaw
        // Attitude = Pitch
        // Bank = Roll

        Quaternion result;
        //result.X = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
        //result.Y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
        //result.Z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
        //result.W = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;

        result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 - sinYawOver2 * sinPitchOver2 * sinRollOver2;
        result.X = sinYawOver2 * sinPitchOver2 * cosRollOver2 + cosYawOver2 * cosPitchOver2 * sinRollOver2;
        result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 + cosYawOver2 * sinPitchOver2 * sinRollOver2;
        result.Z = cosYawOver2 * sinPitchOver2 * cosRollOver2 - sinYawOver2 * cosPitchOver2 * sinRollOver2;

        return result;
    }

    public static Vector3 ToEulerAngles(this Quaternion q)
    {
        // Store the Euler angles in radians
        Vector3 pitchYawRoll = new Vector3();

        double sqx = q.X * q.X;
        double sqy = q.Y * q.Y;
        double sqz = q.Z * q.Z;
        double sqw = q.W * q.W;

        // If quaternion is normalised the unit is one, otherwise it is the correction factor
        double unit = sqx + sqy + sqz + sqw;

        double test = q.X * q.Y + q.Z * q.W;
        //double test = q.X * q.Z - q.W * q.Y;

        if (test > 0.4999f * unit)                              // 0.4999f OR 0.5f - EPSILON
        {
            // Singularity at north pole
            pitchYawRoll.Y = 2f * (float)Math.Atan2(q.X, q.W);  // Yaw
            pitchYawRoll.X = PIOVER2;                           // Pitch
            pitchYawRoll.Z = 0f;                                // Roll
            return pitchYawRoll;
        }
        else if (test < -0.4999f * unit)                        // -0.4999f OR -0.5f + EPSILON
        {
            // Singularity at south pole
            pitchYawRoll.Y = -2f * (float)Math.Atan2(q.X, q.W); // Yaw
            pitchYawRoll.X = -PIOVER2;                          // Pitch
            pitchYawRoll.Z = 0f;                                // Roll
            return pitchYawRoll;
        }
        else
        {
            pitchYawRoll.Y = (float)Math.Atan2(2f * q.Y * q.W - 2f * q.X * q.Z, sqx - sqy - sqz + sqw);       // Yaw
            pitchYawRoll.X = (float)Math.Asin(2f * test / unit);                                              // Pitch
            pitchYawRoll.Z = (float)Math.Atan2(2f * q.X * q.W - 2f * q.Y * q.Z, -sqx + sqy - sqz + sqw);      // Roll

            //pitchYawRoll.Y = (float)Math.Atan2(2f * q.X * q.W + 2f * q.Y * q.Z, 1 - 2f * (sqz + sqw));      // Yaw 
            //pitchYawRoll.X = (float)Math.Asin(2f * (q.X * q.Z - q.W * q.Y));                                // Pitch 
            //pitchYawRoll.Z = (float)Math.Atan2(2f * q.X * q.Y + 2f * q.Z * q.W, 1 - 2f * (sqy + sqz));      // Roll 
        }

        return pitchYawRoll;
    }

除了音高值为 ±PI 时，我的所有实现都有效。

    Quaternion orientation0 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(0, PI, 0);
    Vector3 rotation = orientation0.ToEulerAngles();
    Quaternion orientation1 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z);

    Console.WriteLine(orientation0);
    Console.WriteLine(orientation1);     // Not the same quaternion values

为什么这不适用于该特定值？如果它是一个奇点，那么它不会在算法中被确定为一个，而“测试”值将非常接近于 0。

Answer 1

旋转空间包裹在自身上。显然，如果您围绕任何轴旋转 2PI，您最终会回到起点。同样，如果您围绕轴旋转 PI，则与围绕同一轴旋转 -PI 相同。或者，如果您围绕轴旋转任何角度，则与通过该角度的否定围绕该轴的否定旋转相同。

所有这些都意味着您的四元数转换算法必须决定在处理冗余时要做什么。您在评论中提供的两个方向是相同的方向:(0,0,0,1) 和 (0,0,0,-1) [我更喜欢按字母顺序使用 'w']。

你应该确保你总是标准化你的四元数，否则你最终会得到一些奇怪的漂移。除此之外，似乎正在发生的事情是，当您围绕“z”轴旋转 PI 时，浮点舍入或“小于”与“小于或等于”的差异是将围绕圆的表示推到您的算法决定将角度表示为围绕 z 轴的 -PI 旋转的点。那是同样的事情。

以类似的方式，如果您围绕任何轴旋转 2PI，您的四元数可能是 (-1,0,0,0)。但是如果你旋转零，它将是 (1,0,0,0)。然而，从这些四元数中的任何一个返回的欧拉角表示应该是 (0,0,0)。