Resumen Técnico: FusionCore

Declaración del Problema

La estimación precisa de la pose es fundamental para la navegación de robots autónomos, sin embargo, el despliegue práctico de sistemas de fusión de sensores sigue siendo un desafío. Las soluciones existentes enfrentan varias limitaciones críticas:

• Deriva de Sesgo: Los sesgos de los giroscopios y acelerómetros de la IMU derivan continuamente, y los sesgos de la tasa de guiñada de los codificadores de ruedas (desplazamientos sistemáticos en tracción diferencial) rara vez se tratan como estados del filtro, lo que conduce a una acumulación de error de orientación no observable durante las interrupciones del GPS.
• Fragilidad Arquitectónica: Los paquetes ROS dominantes como robot_localization dependen de nodos separados para la transformación de coordenadas GPS (UTM), introduciendo fallos en los límites de zona. También utilizan covarianzas de ruido fijas y umbrales de rechazo de valores atípicos escalares que no tienen en cuenta la dimensionalidad de la medición.
• Brechas Operativas: Los sistemas actuales luchan con la operación sin GPS, la recuperación de reinitialización de mapas y la integración de sensores diversos como SLAM Visual (VSLAM) y radar sin una coordinación compleja.

Metodología

FusionCore es un paquete de código abierto para ROS 2 que implementa un Filtro de Kalman Unscented (UKF) de 23 estados que fusiona la pose de la IMU, la odometría de codificadores de ruedas, el GPS y el SLAM Visual en un único flujo de odometría de 100 Hz.

Arquitectura Central

• Vector de Estado: El filtro mantiene un vector de estado de 23 dimensiones que incluye posición ($p$), orientación ($q$), velocidad lineal/angulada ($v, \omega$), aceleración lineal ($a$), sesgo del giroscopio ($b_g$), sesgo del acelerómetro ($b_a$) y un sesgo crítico de la tasa de guiñada del codificador de ruedas ($b_{ewz}$).
• Estimación de Sesgo: A diferencia de enfoques anteriores, los sesgos de guiñada del giroscopio, acelerómetro y codificador son estados de primer orden del filtro con ruido de proceso dedicado. Esto permite que el filtro converja hacia valores de sesgo específicos del sensor en línea sin una fase de calibración separada.
• Fusión GPS: El sistema fusiona los datos del GPS de forma nativa en el marco ECEF (Earth-Centered, Earth-Fixed), convirtiendo a ENU solo para el estado del filtro. Esto elimina la necesidad de un nodo navsat_transform separado y evita fallos en los límites de zona UTM.
• Modelos de Medición:
  • IMU: Maneja datos crudos de 6-DOF y actualizaciones de orientación de 2/3-DOF, teniendo en cuenta la gravedad y el sesgo.
  • Codificadores de Ruedas: Resta el $b_{ewz}$ estimado de la velocidad angular reportada antes de la integración. Hace cumplir las restricciones no holonómicas (velocidad y aceleración verticales cero) con ruido adaptativo.
  • VSLAM: Acepta mediciones de pose de 6-DOF de cualquier sistema de odometría visual/SLAM, incluida la recuperación automática de la reinitialización de mapas mediante el reanclaje de la transformación tras rechazos consecutivos de valores atípicos.
  • Velocidad: Soporta la velocidad Doppler del GPS y la velocidad propia Doppler del radar para la detección de deslizamiento.

Innovaciones en el Diseño del Filtro

1. Puerta Chi-Cuadrado por Sensor: Implementa el rechazo de valores atípicos mediante la distancia de Mahalanobis con umbrales calibrados específicamente para los grados de libertad (DOF) de cada ruta de sensor (por ejemplo, GPS de 3-DOF frente a orientación de 1-DOF), en lugar de usar un único umbral escalar.
2. Covarianza de Ruido Adaptativa: Ajusta automáticamente la covarianza de ruido del sensor ($R$) en tiempo real utilizando una ventana deslizante de secuencias de innovación y una media móvil exponencial, eliminando la necesidad de reajuste manual debido a cambios ambientales.
3. Compensación de Retraso: Utiliza un búfer circular de IMU de 1 segundo para realizar retrodicción, aplicando mediciones de GPS o VSLAM retrasadas al estado histórico correcto antes de repropagar hacia adelante.
4. Actualizaciones de Velocidad Cero (ZUPT): Detecta períodos de estacionamiento para suprimir la deriva del sesgo.
5. Modo de Coste: Durante las interrupciones del GPS, el sesgo de guiñada del codificador estimado se resta de los datos del codificador para reducir la deriva de orientación.

Contribuciones Clave

El artículo presenta diez contribuciones específicas:

1. Una formulación UKF de 23 estados con estimación continua en línea de los sesgos de giroscopio, acelerómetro y tasa de guiñada del codificador de ruedas.
2. Fusión GPS nativa en ECEF, eliminando dependencias de proyección UTM.
3. Puerta chi-cuadrado de Mahalanobis por sensor con umbrales calibrados por dimensionalidad.
4. Adaptación automática de la covarianza de ruido a partir de secuencias de innovación.
5. Actualizaciones automáticas de Velocidad Cero (ZUPT) para períodos de estacionamiento.
6. Retrodicción con búfer circular de IMU para compensación de retraso.
7. Fusión de pose de SLAM Visual con recuperación automática de la reinitialización de mapas.
8. Fusión de velocidad GPS para detección de deslizamiento entre sensores.
9. Fusión de velocidad propia Doppler de radar para plataformas de radar de imagen 4D.
10. Una evaluación cuantitativa frente a robot_localization en doce secuencias completas de NCLT.

Evaluación y Resultados

El sistema fue evaluado en doce secuencias completas (de 55 a 92 minutos cada una) del conjunto de datos público NCLT, que cubren diversas condiciones estacionales. La evaluación comparó FusionCore contra el EKF y UKF de robot_localization utilizando el Error de Trayectoria Absoluta (ATE) RMSE.

• Rendimiento: FusionCore logró un ATE menor que el EKF de robot_localization en 10 de las 12 secuencias. Las mejoras en las secuencias ganadoras oscilaron entre 1.2× y 22.2×.
• Estabilidad: El UKF de robot_localization divergió numéricamente (NaN) en las doce secuencias dentro de los primeros 30 segundos, confirmando problemas conocidos de inestabilidad con entradas de alta tasa.
• Análisis de Fallos:
  • 2012-06-15 (Pérdida de FC): FusionCore perdió frente al EKF de robot_localization (49.2m vs 18.2m) debido a una interrupción de GPS de 7.7 minutos. El artículo atribuye esto a las limitaciones de la navegación inercial basada en codificadores durante tales duraciones sin una referencia de orientación absoluta (por ejemplo, magnetómetro).
  • 2012-08-20 (Pérdida de FC): FusionCore perdió significativamente (98.3m vs 10.6m) debido a un "clúster de GPS adversario" de 105 fijaciones que aparecieron entre 720 y 840m de la verdad terrena en un límite de interrupción. El artículo señala que la puerta mal calibrada de robot_localization rechazó por casualidad estos valores atípicos, mientras que la puerta relajada de FusionCore (necesaria para la recuperación) los aceptó. Los autores identifican una verificación previa de consistencia de velocidad como la solución.
• Incompatibilidad de Covarianza: El artículo destaca que la dependencia de robot_localization de informes de covarianza GPS fijos, a menudo inexactos, conduce al rechazo de datos válidos y a un colapso en la navegación inercial. El modelo de ruido adaptativo de FusionCore mitiga esto inflando el modelo de ruido cuando los errores de innovación superan las covarianzas reportadas.

Significado y Afirmaciones

El artículo afirma que FusionCore aborda las limitaciones prácticas de las pilas de fusión de sensores ROS 2 existentes al proporcionar un único nodo de ciclo de vida que no requiere coordinadores externos. Su importancia radica en:

• Robustez: Lograr una precisión de trayectoria superior en escenarios de larga duración y del mundo real donde las implementaciones UKF existentes fallan numéricamente.
• Adaptabilidad: Manejar la calidad dinámica de los sensores y las incompatibilidades de covarianza GPS mediante ruido adaptativo y puertas conscientes de la dimensionalidad.
• Continuidad Operativa: Habilitar la operación sin GPS y la recuperación fluida de la reinitialización de mapas VSLAM.
• Reproducibilidad: Proporcionar una implementación totalmente de código abierto con scripts de referencia y configuraciones para plataformas estándar (TurtleBot3, Husky) e integración con rtabmap_ros.

Los autores concluyen que, aunque existen modos de fallo específicos (interrupciones largas, clústeres de GPS adversarios), estos son diagnósticos y han identificado correcciones arquitectónicas (integración de magnetómetro, verificaciones previas de velocidad) para trabajos futuros.