Extracción de los ángulos de Euler del cuaternión cerca de la singularidad

Question

He escrito un código para convertir un cuaternión en ángulos de Euler, suponiendo una secuencia de Euler ZYX. Siguiendo las convenciones detalladas en este documento https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770019231.pdf

m11=qin(:,1).^2+qin(:,2).^2-qin(:,3).^2-qin(:,4).^2;
m12=2.*(qin(:,2).*qin(:,3)-qin(:,1).*qin(:,4)); 
m13=2.*(qin(:,2).*qin(:,4)+qin(:,1).*qin(:,3));   

m21=2.*(qin(:,2).*qin(:,3)+qin(:,1).*qin(:,4));
m22=qin(:,1).^2-qin(:,2).^2+qin(:,3).^2-qin(:,4).^2;   

m31=2.*(qin(:,2).*qin(:,4)-qin(:,1).*qin(:,3));
m32=2.*(qin(:,3).*qin(:,4)+qin(:,1).*qin(:,2));
m33=qin(:,1).^2-qin(:,2).^2-qin(:,3).^2+qin(:,4).^2;

rz=atan2(m21,m11);    
ry=zeros(length(m31),1);
idx=m31.^2<1;
ry(idx)=atan(-m31(idx)./sqrt(1-m31(idx).^2));
ry(~idx)=-atan2(m31(~idx),0);

rx=atan(m32./m33);

Sin embargo, cuando implemento el código anterior en un microprocesador el ángulo de balanceo (rx) se vuelve erróneo a ~ 85 grados. Creo que este es un problema común conocido como 'gimbal lock', que resulta de las singularidades debido a una rotación en el tono que está cerca de 90 grados (creo que esto ocurre cerca de 85 grados debido a los valores de precisión única utilizados por el micro).

He estado investigando cómo se suele tratar el problema y he encontrado el siguiente documento https://pdfs.semanticscholar.org/6681/37fa4b875d890f446e689eea1e334bcf6bf6.pdf . Este documento defiende las ventajas de esta metodología cuando se utilizan operaciones de precisión única, frente a otras técnicas de tratamiento del problema, como la metodología detallada por Ken Shoemake. El autor maneja la conversión cerca de las singularidades calculando los dos primeros ángulos y luego calculando el tercer ángulo usando los otros ángulos.

He intentado aplicar la misma metodología para extraer una secuencia ZYX Euler.

Matriz de rotación Z:

$$\begin{bmatrix} c_{3} & s_{3} & 0\\ -s_{3} & c_{3} & 0\\ 0 & 0 & 1 \end{bmatrix}$$

Matriz de rotación Y:

$$\begin{bmatrix} c_{2} & 0 & -s_{2}\\ 0 & 1 & 0\\ s_{2} & 0 & c_{2} \end{bmatrix}$$

Multiplicando los dos juntos obtengo

$$\begin{bmatrix} c_{3}c_{2} & s_{3} & -s_{2}c_{3}\\ -s_{3}c_{2} & c_{3} & s_{2}s_{2}\\ s_{2} & 0 & c_{2} \end{bmatrix}$$

Tomando la transposición de lo anterior obtengo:

$$\begin{bmatrix} c_{3}c_{2} & -s_{3}c_{2} & s_{2}\\ s_{3} & c_{3} & 0\\ -s_{2}c_{3} & s_{2}s_{2} & c_{2} \end{bmatrix}$$

Multiplicando esto con una matriz de rotación arbitraria obtengo:

$$\begin{bmatrix} c_{3}c_{2}m_{11}-s_{3}c_{2}m_{21}+s_{2}m_{31} & c_{3}c_{2}m_{01}-s_{3}c_{2}m_{22}+s_{2}m_{32} & c_{3}c_{2}m_{13}-s_{3}c_{2}m_{23}+s_{2}m_{31}\\ s_{3}m_{11}+c_{3}m_{21} & s_{3}m_{12}+c_{3}m_{22}& s_{3}m_{13}+c_{3}m_{22}\\ -s_{2}c_{3}m_{11}+s_{2}s_{2}m_{21}+c_{2}m_{31} & -s_{2}c_{3}m_{12}+s_{2}s_{2}m_{22}+c_{2}m_{32} & -s_{2}c_{3}m_{13}+s_{2}s_{2}m_{23}+c_{2}m_{33} \end{bmatrix}$$

La rotación final es en este caso sobre el eje X:

Matriz de rotación X:

$$\begin{bmatrix} 1 & 0 & 0\\ 0 & c_{1} & s_{1}\\ 0 & -s_{1} & c_{1} \end{bmatrix}$$

De esto obtengo el siguiente código:

rz=atan2(m21,m11);
c2=sqrt((m11.^2)+(m21.^2));
ry=atan2(-m31,c2);
s3=sin(rz);
c3=cos(rz);        
rx=atan2(s3.*m13+c3.*m12, c3.*m22+s3.*m23);

Por desgracia, esto no parece dar el mismo resultado que el primer conjunto de ecuaciones. ¿Podría alguien indicarme mi error, o un método alternativo mejor para hacer esto? ¿Hay algún problema con este método? Idealmente, se agradecerían algunos ejemplos de código.

Answer 1

Revisé un documento y escribí este código que funciona en las singularidades de los polos norte y sur.

    const double radToDeg = (180.0 / Math.PI);
    const double degToRad = (Math.PI / 180.0);

    public static Vector3D ToEulerRad(Quaternion rotation)
    {
        double sqw = rotation.Real * rotation.Real;
        double sqx = rotation.ImagX * rotation.ImagX;
        double sqy = rotation.ImagY * rotation.ImagY;
        double sqz = rotation.ImagZ * rotation.ImagZ;
        double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
        double test = rotation.ImagX * rotation.Real - rotation.ImagY * rotation.ImagZ;

        if (test > 0.4995f * unit)
        { // singularity at north pole
            double Y = 2f * Math.Atan2(rotation.ImagY, rotation.ImagX);
            double X = Math.PI / 2;
            double Z = 0;
            Vector3D v = new Vector3D(X, Y, Z);
            return NormalizeAngles(radToDeg * v);
        }
        if (test < -0.4995f * unit)
        { // singularity at south pole
            double Y = -2f * Math.Atan2(rotation.ImagY, rotation.ImagX);
            double X = -Math.PI / 2;
            double Z = 0;
            Vector3D v = new Vector3D(X, Y, Z);
            return NormalizeAngles(radToDeg * v);
        }
        Quaternion q = new Quaternion(rotation.ImagY,rotation.Real, rotation.ImagZ, rotation.ImagX);
        double y = System.Math.Atan2(2f * q.ImagX * q.Real + 2f * q.ImagY * q.ImagZ, 1 - 2f * (q.ImagZ * q.ImagZ + q.Real * q.Real));     // Yaw
        double x = System.Math.Asin(2f * (q.ImagX * q.ImagZ - q.Real * q.ImagY));                             // Pitch
        double z = System.Math.Atan2(2f * q.ImagX * q.ImagY + 2f * q.ImagZ * q.Real, 1 - 2f * (q.ImagY * q.ImagY + q.ImagZ * q.ImagZ));      // Roll
        return NormalizeAngles( radToDeg * new Vector3D(x,y,z));
    }
    private static Vector3D NormalizeAngles(Vector3D angles)
    {
        double X = NormalizeAngle(angles.X);
        double Y = NormalizeAngle(angles.Y);
        double Z = NormalizeAngle(angles.Z);
        return new Vector3D(X, Y, Z);
    }
    private static double NormalizeAngle(double angle)
    {
        while (angle > 360)
        {
            angle -= 360;
        }
        while (angle < 0)
        {
            angle += 360;
        }
        return angle;
    }

    public static Vector3D ToEulerAngles(Quaternion rotation)
    {
        return radToDeg * ToEulerRad(rotation);
    }