SE(3)-LIO: Smooth IMU Propagation With Jointly Distributed Poses on SE(3) Manifold for Accurate and Robust LiDAR-Inertial Odometry

Resumen Técnico: SE(3)-LIO

1. Planteamiento del Problema

La estimación precisa de la odometría es fundamental para la navegación autónoma. Los sistemas LiDAR-Inercial (LIO) combinan mediciones de alta frecuencia de una Unidad de Medición Inercial (IMU) con datos de sensores exteroceptivos (como LiDAR) para corregir la deriva. Sin embargo, los métodos existentes de propagación de IMU presentan dos limitaciones críticas:

• Predicción de Movimiento Inexacta: Los métodos convencionales representan el pose de 6 grados de libertad (6-DoF) separando la rotación (en la variedad $SO(3)$) y la traslación (en $\mathbb{R}^3$). Al propagarlas en variedades separadas, no se incorporan eficazmente las variaciones rotacionales en la propagación de la traslación. Esto genera errores significativos, especialmente en movimientos agresivos o dinámicos donde la rotación afecta directamente la trayectoria lineal.
• Incertidumbre en la Compensación de Movimiento: Para corregir las distorsiones causadas por el movimiento en las nubes de puntos de LiDAR, se utiliza una transformación relativa entre poses predichas. Dado que las poses predichas por la IMU tienen errores inherentes (ruido del sensor, integración numérica), la transformación relativa también es incierta. Los métodos actuales a menudo ignoran la correlación entre las poses predichas sucesivas, lo que lleva a una sobreestimación de la incertidumbre o a una caracterización inexacta del ruido en la medición compensada.

2. Metodología Propuesta

Los autores proponen SE(3)-LIO, un marco de odometría que integra dos innovaciones principales:

A. Propagación Suave de IMU en la Variedad SE(3)
En lugar de separar rotación y traslación, el método propone propagar el pose completo directamente en la variedad del grupo de Lie $SE(3)$ (Transformación Rígida).

• Modelo Cinemático: Se utiliza un modelo de cuerpo rígido donde el estado incluye la matriz de transformación $T$ y la velocidad en el cuerpo $B_v$.
• Ventaja: Al propagar en $SE(3)$, la traslación propagada tiene en cuenta correctamente las variaciones rotacionales a través del Jacobiano izquierdo de la aplicación exponencial ($J_l$). Esto permite una predicción de movimiento más suave y precisa, incluso bajo aceleraciones globales constantes y movimientos dinámicos agresivos.
• Distribución Conjunta: Se deriva la distribución conjunta de los poses predichos. Dado que la propagación de IMU es recursiva, los errores en los estados actuales dependen de los errores iniciales y del ruido de la IMU a lo largo del tiempo. Esto permite modelar la covarianza cruzada entre diferentes instantes de tiempo.

B. Compensación de Movimiento Consciente de la Incertidumbre (UAMC)
Se introduce un enfoque para tratar la incertidumbre durante la compensación de distorsión de movimiento:

• Caracterización de Incertidumbre: Utilizando la distribución conjunta de los poses, se calcula la covarianza de la transformación relativa ($T_{I_k I_j}$) considerando los términos cruzados (correlación) entre las poses. Esto evita la sobreestimación de la incertidumbre que ocurre al asumir independencia entre poses.
• Nube de Puntos Probabilística: La incertidumbre de la transformación relativa se incorpora directamente en el ruido de la medición de cada punto LiDAR. Esto genera una "nube de puntos sin distorsión probabilística", donde los puntos con mayor incertidumbre (generalmente los adquiridos más atrás en el tiempo relativo a la pose de referencia) tienen un peso de ruido mayor en el filtro.

C. Integración en el Filtro de Kalman (ESKF)
El sistema SE(3)-LIO se implementa dentro de un Filtro de Kalman de Estado de Error (ESKF).

1. Predicción: Se utiliza la propagación en $SE(3)$ para obtener una distribución a priori del estado.
2. Actualización: Se construye una distribución de medición a partir de la nube de puntos sin distorsión (UAMC) y se alinean con un mapa local (utilizando una estructura de mapa de voxels similar a VoxelMap).
3. Optimización: Se minimiza el error cuadrático combinando la incertidumbre de la predicción y la incertidumbre de la medición.

3. Contribuciones Clave

1. Propagación en SE(3): Un método de propagación de IMU que opera en la variedad $SE(3)$, mejorando la precisión de la predicción de movimiento al integrar variaciones rotacionales en la traslación.
2. UAMC (Compensación Consciente de la Incertidumbre): Una técnica que caracteriza con precisión la incertidumbre de la transformación relativa mediante el uso de distribuciones conjuntas y términos cruzados, incorporándola al ruido de medición.
3. Marco SE(3)-LIO: Una implementación completa de odometría LiDAR-Inercial que demuestra superioridad en precisión y robustez.
4. Validación Exhaustiva: Pruebas en escenarios diversos, incluyendo drones con movimientos agresivos y vehículos terrestres en terrenos irregulares.

4. Resultados Experimentales

El sistema fue evaluado en tres conjuntos de datos: NTU-VIRAL (drone con movimientos agresivos), Newer-College (plataforma manual con movimiento dinámico) y un conjunto de datos propio (vehículo terrestre en terreno accidentado a gran escala).

• Precisión (RMSE de ATE): SE(3)-LIO superó a los métodos más avanzados (SOTA) como FAST-LIO2, DLIO y MA-LIO.
  • En el conjunto NTU-VIRAL, logró los mejores resultados en todas las secuencias, reduciendo significativamente el error en la secuencia spms (movimiento muy agresivo) donde otros métodos fallaron o degradaron su rendimiento.
  • En terreno irregular, redujo el error en más de un 50% comparado con el segundo mejor método en secuencias críticas.
• Robustez: El sistema mantuvo la estabilidad en entornos con mediciones dispersas (LiDAR de pocos canales) y vegetación densa, donde otros algoritmos sufrieron deriva o divergencia.
• Eficiencia Computacional: A pesar de la complejidad matemática añadida (cálculo de covarianzas cruzadas y propagación en $SE(3)$), el tiempo de cálculo es comparable a FAST-LIO2. Las nuevas componentes (propagación y UAMC) consumen menos del 20% del tiempo total de procesamiento, demostrando ser viables para aplicaciones en tiempo real.
• Estudio de Ablación: Se confirmó que tanto la propagación en $SE(3)$ como la UAMC contribuyen individualmente a la mejora del rendimiento, siendo la combinación de ambas la que ofrece el mejor resultado.

5. Significado e Impacto

Este trabajo aborda una limitación fundamental en la odometría inercial: la desconexión entre la rotación y la traslación en los modelos de propagación y la gestión subóptima de la incertidumbre correlacionada.

• Avance Teórico: Al tratar el pose como un elemento único en $SE(3)$ y modelar explícitamente la correlación temporal de los errores, se establece un nuevo estándar para la precisión en entornos dinámicos.
• Aplicabilidad Práctica: La capacidad de mantener una odometría precisa en movimientos agresivos y terrenos no estructurados es crucial para el despliegue de robots autónomos en misiones de rescate, exploración y logística en entornos hostiles.
• Escalabilidad: Los autores indican que el método es adaptable a otras configuraciones de sensores (visual-inercial, radar), lo que sugiere un potencial amplio para futuras investigaciones en fusión de sensores.

En conclusión, SE(3)-LIO representa un avance significativo en la fusión LiDAR-Inercial, ofreciendo una solución matemáticamente rigurosa que mejora tanto la precisión como la robustez sin sacrificar la eficiencia computacional.