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

その特定の値に対してこれが機能しないのはなぜですか? それが特異点である場合、それはアルゴリズムで 1 つとして決定されず、'test' 値は代わりに 0 に非常に近くなります。

4

1 に答える 1

3

回転スペースはそれ自体にラップします。明らかに、任意の軸を中心に 2PI だけ回転すると、開始した場所に戻ります。同様に、軸を中心に PI だけ回転する場合、同じ軸を中心に -PI だけ回転することと同じです。または、軸を中心に任意の角度で回転する場合、その軸の否定を中心にその角度の否定で回転することと同じです。

これはすべて、クォータニオン変換アルゴリズムが、冗長性を処理するときに何をすべきかを決定する必要があることを意味します。コメントで指定した 2 つの向きは同じ向きです: (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) である必要があります。

于 2012-07-24T15:02:48.783 に答える