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

为什么这不适合该特定值？如果它是一个奇点，则它不会被确定为算法中的一个，而'test'值将非常接近于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）。

四元数 - YawPitchRoll和EulerAngles之间的转换仅在Pi的音高下产生不正确的结果

1 个答案: