Conversión de la matriz de rotación en cuaternión

Question

Utilizamos Quaternion de longitud unitaria para representar las rotaciones. A continuación se obtiene una matriz de rotación general ${\begin{bmatrix}m_{00} & m_{01}&m_{02} \\ m_{10} & m_{11}&m_{12}\\ m_{20} & m_{21}&m_{22}\end{bmatrix}}_{3\times 3}\tag 1 $ .

¿Cómo puedo calcular con precisión el cuaternión $q = q_1i+q_2j+q_3k+q_4$ para esta matriz Es decir, ¿cómo podemos escribir $q_i$ s en términos de $m_{ij}$ ¿con exactitud?

Answer 1

El eje y el ángulo se codifican directamente en esta matriz.

Calcular el vector propio unitario para el valor propio $1$ para esta matriz (¡debe existir!) y llamarla $u=(u_1,u_2,u_3)$ . Lo escribirá como $u=u_1i+u_2j+u_2k$ a partir de ahora. Este es precisamente el eje de rotación que, geométricamente, tienen todas las rotaciones no identitarias.

Se puede recuperar el ángulo a partir de la traza de la matriz: $tr(M)=2\cos(\theta)+1$ . Esto es una consecuencia del hecho de que se puede cambiar de base a una base ortonormal que incluya el eje encontrado anteriormente, y la matriz de rotación será la identidad en esa dimensión, y será una rotación plana en las otras dos dimensiones. Es decir, tendrá que ser de la forma

$$\begin{bmatrix}\cos(\theta)&-\sin(\theta)&0\\\sin(\theta)&\cos(\theta)&0\\0&0&1\end{bmatrix}$$

Como la traza es invariante entre cambios de base, puedes ver cómo obtuve mi ecuación.

Una vez que haya resuelto para $\theta$ lo usarás para construir tu cuaternión de rotación $q=\cos(\theta/2)+u\sin(\theta/2)$ .

Answer 2

Hay una solución en https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf

En resumen, su código final es el siguiente:

if (m22 < 0) {
    if (m00 >m11) {
        t = 1 + m00 -m11 -m22;
        q = Quaternion( t, m01+m10, m20+m02, m12-m21 );
    }
    else {
        t = 1 -m00 + m11 -m22;
        q = Quaternion( m01+m10, t, m12+m21, m20-m02 );
    }
}
else {
    if (m00 < -m11) {
        t = 1 -m00 -m11 + m22;
        q = Quaternion( m20+m02, m12+m21, t, m01-m10 );
    }
    else {
        t = 1 + m00 + m11 + m22;
        q = Quaternion( m12-m21, m20-m02, m01-m10, t );
    }
}
q *= 0.5 / Sqrt(t);

Answer 3

Referencia: Shuster, M. 1993, "Un estudio sobre las representaciones de las actitudes" Journal of the Astronautical Sciences, 41(4):349-517

Véanse las ecuaciones y la discusión en el documento anterior, p463-464.

Se garantiza que uno de los elementos del cuaternión tiene una magnitud de superior a 0,5 y, por tanto, un valor al cuadrado de 0,25. Podemos utilizar esto para determinar el "mejor" conjunto de parámetros a utilizar para calcular el cuaternión a partir de una matriz de rotación

double b1_squared = 0.25 * (1.0 + R11 + R22 + R33);
if (b1_squared >= 0.25)
{
    // Equation (164)
    double b1 = sqrt(b1_squared);

    double over_b1_4 = 0.25 / b1;
    double b2 = (R32 - R23) * over_b1_4;
    double b3 = (R13 - R31) * over_b1_4;
    double b4 = (R21 - R12) * over_b1_4;

    // Return the quaternion
    Eigen::Vector4d q(b1, b2, b3, b4);
    return q;
}

Dejaré como ejercicio al OP completar los otros tres.

Véase también:

• Farrell, 2008, "Aided Navigation: GPS with High Rate Sensors", McGraw Hill, Apéndice D.

Answer 4

Dejemos que $\mathcal{A} \subset \mathcal{M}_{3\times 3}(\mathbb{R})$ sea el espacio de $3 \times 3$ matrices simétricas reales. Es un tramo de tres matrices

$$L_x = \begin{bmatrix}0 & 0 & 0\\0 & 0 & -1\\0 & 1 & 0\end{bmatrix},\quad L_y = \begin{bmatrix}0 & 0 & 1\\0 & 0 & 0\\-1& 0 & 0\end{bmatrix},\quad L_z = \begin{bmatrix}0 & -1 & 0\\1 & 0 & 0\\0 & 0 & 0\end{bmatrix} $$ Para cada $A \in \mathcal{A}$ podemos ampliar $A$ como $x L_x + y L_y + z L_z$ y asociado un vector $\vec{A} \in \mathbb{R}^3$ y un cuaternión $\tilde{A} \in \mathbb{H}$ a ella.

$$A = x L_x + y L_y + z L_z \quad\leftrightarrow\quad \vec{A} = (x,y,z) \quad\leftrightarrow\quad \tilde{A} = x{\bf i} + y{\bf j} + z{\bf k}$$

Dejemos que $\mathcal{A}_u = \{\; A \in \mathcal{A} : |\vec{A}| = 1\; \}$ . Dada cualquier matriz de rotación $M \in SO(3)$ podemos encontrar un $\theta \in [0,\pi]$ y $L \in \mathcal{A}_u$ tal que

$$M = e^{\theta L} = I_3 + \sin\theta L + (1-\cos\theta) L^2$$

El $\theta$ es el ángulo de rotación asociado a $M$ y $\vec{L}$ será un vector unitario en la dirección del eje de rotación.

Cuando $\theta \ne 0$ ni $\pi$ podemos extraer $L$ tomando la parte antisimétrica de $M$ y luego "normalizar" el vector correspondiente porque

$$ \sin\theta L = \frac12 \left(M - M^T\right)$$

Para fijar el valor de $\theta$ podemos utilizar la relación $\text{Tr}(M) = 1 + 2\cos\theta$ .

Una vez $\theta$ y $L$ es conocida, el cuaternión correspondiente a la matriz de rotación $M$ viene dada por

$$e^{\frac{\theta}{2} \tilde{L}} = \cos\frac{\theta}{2} + \sin\frac{\theta}{2} \tilde{L} = \frac{\sqrt{1+\text{Tr}(M)}}{2}\left[ 1 + \frac{\widetilde{M - M^{T}}}{1 + \text{Tr}(M)}\right] $$