Algoritmo de estimación de cuaterniones

Algoritmo para resolver el problema de Wahba

El algoritmo del estimador de cuaterniones ( QUEST ) es un algoritmo diseñado para resolver el problema de Wahba , que consiste en encontrar una matriz de rotación entre dos sistemas de coordenadas a partir de dos conjuntos de observaciones muestreadas en cada sistema respectivamente. La idea clave detrás del algoritmo es encontrar una expresión de la función de pérdida para el problema de Wahba como una forma cuadrática , utilizando el teorema de Cayley-Hamilton y el método de Newton-Raphson para resolver eficientemente el problema de valores propios y construir una representación numéricamente estable de la solución.

El algoritmo fue introducido por Malcolm D. Shuster en 1981, mientras trabajaba en Computer Sciences Corporation . ^[1] Si bien en principio es menos robusto que otros métodos como el método q de Davenport o la descomposición en valores singulares , el algoritmo es significativamente más rápido y confiable en aplicaciones prácticas, ^{[2] [3]} y se utiliza para problemas de determinación de actitud en campos como la robótica y la aviónica . ^{[4] [5] [6]}

Formulación del problema

El problema de Wahba consiste en encontrar una matriz de rotación que minimice la función de pérdida ${\displaystyle \mathbf {A} ^{*}}$

${\displaystyle l\left(\mathbf {A} \right)={\frac {1}{2}}\sum _{i=1}^{n}a_{i}\left\|\mathbf {w} _{i}-\mathbf {A} \mathbf {v} _{i}\right\|^{2}}$

donde son las observaciones vectoriales en el marco de referencia, son las observaciones vectoriales en el marco del cuerpo, es una matriz de rotación entre los dos marcos y son un conjunto de pesos tales que . Es posible reescribir esto como un problema de maximización de una función de ganancia. ${\displaystyle \mathbf {w} _{i}}$ ${\displaystyle \mathbf {v} _{i}}$ ${\displaystyle \mathbf {A} }$ ${\displaystyle a_{i}}$ ${\displaystyle \textstyle \sum _{i}a_{i}=1}$ ${\displaystyle g}$

${\displaystyle g\left(\mathbf {A} \right)=1-l\left(\mathbf {A} \right)=\sum _{i}a_{i}\mathbf {w} _{i}^{\top }\mathbf {A} \mathbf {v} _{i}}$

definida de tal manera que la pérdida alcanza un mínimo cuando se maximiza. La ganancia a su vez puede reescribirse como ${\displaystyle l}$ ${\displaystyle g}$ ${\displaystyle g}$

${\displaystyle g\left(\mathbf {A} \right)=\operatorname {tr} \left(\mathbf {A} \mathbf {B} ^{\top }\right)}$

donde se conoce como la matriz del perfil de actitud. ${\displaystyle \mathbf {B} =\textstyle \sum _{i}a_{i}\mathbf {w} _{i}\mathbf {v} _{i}^{\top }}$

Para reducir el número de variables, el problema puede reformularse parametrizando la rotación como un cuaternión unitario con parte vectorial y parte escalar , que representa la rotación del ángulo alrededor de un eje cuya dirección está descrita por el vector , sujeto a la restricción de unidad . Ahora es posible expresar en términos de la parametrización del cuaternión como ${\displaystyle \mathbf {q} =\left(v_{1},v_{2},v_{3},q\right)}$ ${\displaystyle \mathbf {v} =\left(v_{1},v_{2},v_{3}\right)}$ ${\displaystyle q}$ ${\displaystyle \theta =2\cos ^{-1}q}$ ${\displaystyle \textstyle {\frac {1}{\sin {\frac {\theta }{2}}}}\mathbf {v} }$ ${\displaystyle \mathbf {q} ^{\top }\mathbf {q} =1}$ ${\displaystyle \mathbf {A} }$

${\displaystyle \mathbf {A} =\left(q^{2}-\mathbf {v} \cdot \mathbf {v} \right)\mathbf {I} +2\mathbf {v} \mathbf {v} ^{\top }+2q\mathbf {V} _{\times }}$

¿Dónde está la matriz antisimétrica? ${\displaystyle \mathbf {V} _{\times }}$

${\displaystyle \mathbf {V} _{\times }={\begin{pmatrix}0&v_{3}&-v_{2}\\-v_{3}&0&v_{1}\\v_{2}&-v_{1}&0\\\end{pmatrix}}}$.

Sustituyendo con la representación del cuaternión y simplificando la expresión resultante, la función de ganancia se puede escribir como una forma cuadrática en ${\displaystyle \mathbf {A} }$ ${\displaystyle \mathbf {q} }$

${\displaystyle g(\mathbf {q} )=\mathbf {q} ^{\top }\mathbf {K} \mathbf {q} }$

donde la matriz ${\displaystyle 4\times 4}$

${\displaystyle \mathbf {K} ={\begin{pmatrix}\mathbf {S} -\sigma \mathbf {I} &\mathbf {z} \\\mathbf {z} ^{\top }&\sigma \end{pmatrix}}}$

se define a partir de las cantidades

${\displaystyle {\begin{aligned}\mathbf {S} &=\mathbf {B} +\mathbf {B} ^{\top }\\\mathbf {z} &=\sum _{i}a_{i}\left(\mathbf {w} _{i}\times \mathbf {v} _{i}\right)\\\sigma &=\operatorname {tr} \mathbf {B} .\end{aligned}}}$

Esta forma cuadrática se puede optimizar bajo la restricción de unidad agregando un multiplicador de Lagrange , obteniendo una función de ganancia sin restricciones. ${\displaystyle -\lambda \mathbf {q} ^{\top }\mathbf {q} }$

${\displaystyle {\hat {g}}\left(\mathbf {q} \right)=\mathbf {q} ^{\top }\mathbf {K} \mathbf {q} -\lambda \mathbf {q} ^{\top }\mathbf {q} }$

que alcanza un máximo cuando

${\displaystyle \mathbf {K} \mathbf {q} =\lambda \mathbf {q} }$.

Esto implica que la rotación óptima está parametrizada por el cuaternión que es el vector propio asociado al valor propio más grande de . ^{[1] [2]} ${\displaystyle \mathbf {q} ^{*}}$ ${\displaystyle \lambda _{\text{max}}}$ ${\displaystyle \mathbf {K} }$

Solución de la ecuación característica

El cuaternión óptimo se puede determinar resolviendo la ecuación característica de y construyendo el vector propio para el valor propio más grande. A partir de la definición de , es posible reescribir ${\displaystyle \mathbf {K} }$ ${\displaystyle \mathbf {K} }$

${\displaystyle \mathbf {K} \mathbf {q} =\lambda \mathbf {q} }$

como un sistema de dos ecuaciones

${\displaystyle {\begin{aligned}\mathbf {y} &=\left((\lambda +\sigma )\mathbf {I} -\mathbf {S} \right)^{-1}\mathbf {z} \\\lambda &=\sigma +\mathbf {z} \mathbf {y} \end{aligned}}}$

donde es el vector de Rodrigues . Sustituyendo en la segunda ecuación con la primera, es posible derivar una expresión de la ecuación característica ${\displaystyle \mathbf {y} =\textstyle {\frac {1}{q}}\mathbf {v} }$ ${\displaystyle \mathbf {y} }$

${\displaystyle \lambda =\sigma +\mathbf {z} ^{\top }\left((\lambda +\sigma )\mathbf {I} -\mathbf {S} \right)^{-1}\mathbf {z} }$.

Como , se deduce que y por lo tanto para una solución óptima (cuando la pérdida es pequeña). Esto permite construir el cuaternión óptimo reemplazando en el vector de Rodrigues ${\displaystyle \lambda _{\text{max}}=\max g\left(\mathbf {A} \right)}$ ${\displaystyle \lambda _{\text{max}}=1-\min l\left(\mathbf {A} \right)}$ ${\displaystyle \lambda _{\text{max}}\approx 1}$ ${\displaystyle l}$ ${\displaystyle \mathbf {q} ^{*}}$ ${\displaystyle \lambda _{\text{max}}}$ ${\displaystyle \mathbf {y} }$

${\displaystyle \mathbf {q} ^{*}={\frac {1}{\sqrt {1+\left|\mathbf {y} _{\lambda _{\text{max}}}\right|^{2}}}}(\mathbf {y} ,1)^{\top }}$.

Sin embargo, el vector es singular para . Se puede construir una expresión alternativa de la solución que no involucre al vector de Rodrigues utilizando el teorema de Cayley-Hamilton . La ecuación característica de una matriz es ${\displaystyle \mathbf {y} }$ ${\displaystyle \theta =\pi }$ ${\displaystyle 3\times 3}$ ${\displaystyle \mathbf {S} }$

${\displaystyle \det \left[\mathbf {S} -\xi \mathbf {I} \right]=-\xi ^{3}+2\sigma \xi ^{2}-k\xi +\Delta =0}$

dónde

${\displaystyle {\begin{aligned}\sigma &={\frac {1}{2}}\operatorname {tr} {\mathbf {S} }\\k&=\operatorname {tr} \left(\operatorname {adj} \mathbf {S} \right)\\\Delta &=\det \mathbf {S} \end{aligned}}}$

El teorema de Cayley-Hamilton establece que cualquier matriz cuadrada sobre un anillo conmutativo satisface su propia ecuación característica, por lo tanto

${\displaystyle -\mathbf {S} ^{3}+2\sigma \mathbf {S} ^{2}-k\mathbf {S} +\Delta =0}$

Permitiendo escribir

${\displaystyle \left((\omega +\sigma )\mathbf {I} -\mathbf {S} \right)^{-1}={\frac {\alpha \mathbf {I} +\beta \mathbf {S} +\mathbf {S} ^{2}}{\gamma }}}$

dónde

${\displaystyle {\begin{aligned}\alpha &=\omega ^{2}-\sigma ^{2}+k\\\beta &=\omega -\sigma \\\gamma &=(\omega +\sigma )\alpha -\Delta \end{aligned}}}$

y para ello proporciona una nueva construcción del vector óptimo ${\displaystyle \omega =\lambda _{\text{max}}}$

${\displaystyle {\begin{aligned}\mathbf {y} ^{*}&=\left((\lambda +\sigma )\mathbf {I} -\mathbf {S} \right)^{-1}\mathbf {z} \\&={\frac {\alpha \mathbf {I} +\beta \mathbf {S} +\mathbf {S} ^{2}}{\gamma }}\mathbf {z} \end{aligned}}}$

que da la representación del cuaternión conjugado de la rotación óptima como

${\displaystyle \mathbf {q} ^{*}={\frac {1}{\sqrt {\gamma ^{2}+\left|\mathbf {x} \right|^{2}}}}(\mathbf {x} ,\gamma )^{\top }}$

dónde

${\displaystyle \mathbf {x} =\left(\alpha \mathbf {I} +\beta \mathbf {S} +\mathbf {S} ^{2}\right)\mathbf {z} }$.

El valor de se puede determinar como una solución numérica de la ecuación característica. Reemplazando dentro de la ecuación característica obtenida previamente ${\displaystyle \lambda _{\text{max}}}$ ${\displaystyle \left((\omega +\sigma )\mathbf {I} -\mathbf {S} \right)^{-1}}$

${\displaystyle \lambda =\sigma +\mathbf {z} ^{\top }\left((\lambda +\sigma )\mathbf {I} -\mathbf {S} \right)^{-1}\mathbf {z} }$.

da

${\displaystyle \lambda ^{4}-(a+b)\lambda ^{2}-c\lambda +(ab+c\sigma -d)=0}$

dónde

${\displaystyle {\begin{aligned}a&=\sigma ^{2}-k\\b&=\sigma ^{2}+\mathbf {z} ^{\top }\mathbf {z} \\c&=\Delta +\mathbf {z} ^{\top }\mathbf {S} \mathbf {z} \\d&=\mathbf {z} ^{\top }\mathbf {S} ^{2}\mathbf {z} \end{aligned}}}$

cuya raíz se puede aproximar eficientemente con el método de Newton-Raphson , tomando 1 como estimación inicial de la solución para converger al valor propio más alto (usando el hecho, mostrado arriba, de que cuando el cuaternión está cerca de la solución óptima). ^{[1] [2]} ${\displaystyle \lambda _{\text{max}}\approx 1}$

Véase también

• Método de la tríada
• El problema de Wahba

Referencias

1. Shuster y Oh (1981)
2. Markley y Mortari (2000)
3. Crassidis (2007)
4. Psiaki (2000)
5. Wu y otros (2017)
6. Xiaoping y otros (2008)

Fuentes

• Crassidis, John L; Markley, F Landis; Cheng, Yang (2007). "Estudio de métodos de estimación de actitud no lineal". Revista de orientación, control y dinámica . 30 (1): 12–28. Bibcode :2007JGCD...30...12C. doi :10.2514/1.22452.
• Markley, F Landis; Mortari, Daniele (2000). "Estimación de la actitud de cuaternión utilizando observaciones vectoriales". Revista de Ciencias Astronáuticas . 48 (2). Springer: 359–380. Bibcode :2000JAnSc..48..359M. doi :10.1007/BF03546284.
• Psiaki, Mark L (2000). "Filtrado de determinación de actitud mediante estimación de cuaterniones extendidos". Journal of Guidance, Control, and Dynamics . 23 (2): 206–214. Bibcode :2000JGCD...23..206P. doi :10.2514/2.4540.
• Shuster, MD; Oh, SD (1981). "Determinación de la actitud en tres ejes a partir de observaciones vectoriales". Journal of Guidance and Control . 4 (1): 70–77. Bibcode :1981JGCD....4...70S. doi :10.2514/3.19717.
• Wu, Jin; Zhou, Zebo; Gao, Bin; Li, Rui; Cheng, Yuhua; Fourati, Hassen (2017). "Estimador de actitud de cuaternión lineal rápido usando observaciones vectoriales" (PDF) . IEEE Transactions on Automation Science and Engineering . 15 (1). IEEE: 307–319. doi :10.1109/TASE.2017.2699221. S2CID  3455346.
• Yun, Xiaoping; Bachmann, Eric R; McGhee, Robert B (2008). "Un algoritmo simplificado basado en cuaterniones para la estimación de la orientación a partir de mediciones de la gravedad terrestre y del campo magnético". IEEE Transactions on Instrumentation and Measurement . 57 (3). IEEE: 638–650. doi :10.1109/TIM.2007.911646. hdl : 10945/46081 . S2CID  15571138.

Enlaces externos

• "QUEST — Documentación AHRS".
• Universidad de Colorado en Boulder. "QUEST". Cinemática: descripción de los movimientos de las naves espaciales. Coursera.