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.
