4

我花了一些时间实现了几个算法来在 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);

我使用了此处讨论的先前方法，并且已经实现了此处描述的另一种方法

    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。

4

1 回答 1

3

旋转空间环绕自身。显然，如果你绕任何轴旋转 2PI，你最终会回到你开始的地方。同样，如果您绕轴旋转 PI，则与绕同一轴旋转 -PI 相同。或者，如果您围绕轴旋转任意角度，则与围绕该轴的负向旋转该角度的负向相同。

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

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

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

于 2012-07-24T15:02:48.783 回答