Motor Angular Speed Preintegration for Multirotor UAV State Estimation

Resumen Técnico: Preintegración de la Velocidad Angular del Motor para la Estimación del Estado de un UAV Multirrotor

Planteamiento del Problema
La estimación precisa del estado es fundamental para un control de retroalimentación estrecho, lo que permite vuelos ágiles y cercanos a obstáculos para los Vehículos Aéreos No Tripulados (UAV). Mientras que los métodos de vanguardia típicamente fusionan mediciones de pose lentas (por ejemplo, de LiDAR o cámaras) con mediciones inerciales de alta frecuencia de una Unidad de Medición Inercial (IMU), estos enfoques basados en IMU enfrentan limitaciones significativas cuando se despliegan en UAVs. Específicamente, las mediciones de la IMU se ven degradadas por vibraciones de alta amplitud provenientes de las hélices en rotación, las cuales oscurecen la trayectoria de aceleración real e impactan severamente la precisión de la estimación. Además, los métodos inerciales son altamente sensibles a la calibración de los parámetros extrínsecos e intrínsecos, y las IMU de sistemas microelectromecánicos (MEMS) de bajo costo comúnmente utilizadas en UAVs exhiben un alto ruido de medición y deriva de sesgo (bias drift). Adicionalmente, obtener aceleraciones angulares directamente para controladores avanzados (como la Inversión Dinámica No Lineal Incremental) es difícil, requiriendo a menudo la diferenciación numérica de datos de IMU ruidosos.

Metodología
Los autores proponen un nuevo enfoque de estimación de estado que reemplaza la preintegración de la IMU con la preintegración de las mediciones de la Velocidad Angular del Motor (MAS, por sus siglas en inglés). El núcleo del proceso implica:

1. Adquisición de Datos: El método utiliza las mediciones de velocidad angular ($w_i$) reportadas por los Controladores Electrónicos de Velocidad (ESC) que impulsan los motores del UAV. Estas mediciones son inherentemente libres de vibraciones y precisas en comparación con los datos de la IMU.
2. Modelado de Propulsión: Las mediciones de MAS pasan a través de un modelo de propulsión de UAV. Este modelo convierte las velocidades individuales de los motores en fuerzas lineales y angulares, así como en torques que actúan sobre el Centro de Gravedad (COG) del UAV, contabilizando las posiciones de los rotores, los coeficientes de empuje y los coeficientes de arrastre.
3. Preintegración: Para evitar la ineficiencia computacional de integrar aceleraciones para cada nueva medición y para asegurar la independencia de la posición inicial, los autores derivan un esquema de preintegración en el espacio tangente del manifold del estado del UAV. Este agrega múltiples mediciones de MAS entre dos pasos de tiempo en una única medición "delta" ($\Delta x$), que representa el cambio relativo en el estado (posición, orientación, velocidad, velocidad angular).
4. Optimización de Grafos de Factores: Las mediciones preintegradas forman un "Factor MAS", una restricción binaria entre dos estados consecutivos en un grafo de factores. Este factor se combina con un factor de Odometría LiDAR (LO) (derivado de un algoritmo de emparejamiento de escaneo LiDAR) y factores de sesgo para estimar el estado completo del UAV. Los autores implementan esto dentro del marco de trabajo GTSAM, creando un nuevo algoritmo denominado Odometría LiDAR de Velocidad Angular del Motor (MAS-LO).
5. Manejo de Sesgos: Para compensar las imprecisiones del modelo (por ejemplo, efectos aerodinámicos no modelados, efecto suelo, desplazamientos en la distribución de masa) y los sesgos de los sensores, el sistema incluye un factor de sesgo de paseo aleatorio (random walk) para las aceleraciones lineales y angulares.

Contribuciones Clave

• Esquema de Preintegración MAS: Una derivación novedosa de la preintegración para velocidades angulares de motor que opera en el manifold del estado del UAV, permitiendo una propagación de estado eficiente sin necesidad de recomputación al linealizar.
• Factor MAS: El diseño de un factor binario basado en la preintegración de MAS que puede reemplazar directamente el factor de la IMU en marcos de localización existentes basados en grafos de factores.
• Algoritmo MAS-LO: La integración del factor MAS con la odometría LiDAR para crear un algoritmo completo de estimación de estado que no depende de una IMU para la propagación del estado.
• Lanzamiento de Código Abierto: Los autores liberan el código de preintegración de MAS, el factor MAS, el algoritmo completo MAS-LO y seis nuevos conjuntos de datos que contienen datos de MAS, LiDAR, IMU y verdad de terreno (ground truth) mediante RTK para diversas maniobras de vuelo.

Resultados Experimentales
El método propuesto fue evaluado contra el algoritmo de vanguardia LIO-SAM (LiDAR-Inertial Odometry Smoothing and Mapping) utilizando seis conjuntos de datos de vuelo en exteriores (Hover, Forward, Lateral, Vertical, Rectangle, Loop) con verdad de terreno RTK.

• Precisión: MAS-LO demostró una reducción del 28% en el Error de Traslación Absoluta (ATE) y una reducción del 65% en el Error de Velocidad Absoluta (AVE) en comparación con LIO-SAM a través de los conjuntos de datos.
• Compromiso (Trade-off): Esta mejora en la estimación lineal se produjo a costa de un incremento del 20% en el Error de Rotación Absoluta (ARE), atribuido a la doble integración de las aceleraciones angulares en el modelo MAS.
• Ruido y Retraso: El análisis mostró que las aceleraciones derivadas de MAS tienen una amplitud de ruido significativamente menor que los datos de la IMU (incluso la IMU filtrada). Además, la preintegración MAS exhibió un retraso de medición un 14% menor (0.063s frente a 0.073s para la IMU), un factor crítico para el control de vuelo ágil.
• Robustez: Un estudio de sensibilidad reveló que el algoritmo MAS-LO es robusto ante valores de parámetros incorrectos (por ejemplo, coeficientes de empuje, dimensiones del cuerpo), manteniendo un desempeño razonable incluso cuando los parámetros se desviaban significativamente de sus valores reales.

Significancia
El artículo sostiene que, al aprovechar las mediciones de velocidad de motor precisas y libres de vibraciones, es posible eliminar la necesidad de las IMU en el bucle de propagación de estado, eliminando así la principal fuente de ruido y sensibilidad de calibración en la localización de UAVs. El método proporciona una estimación más precisa de la posición lineal y la velocidad, las cuales son cruciales para el vuelo ágil y la evitación de obstáculos. Los autores posicionan este trabajo como un paso fundacional hacia la combinación de las fortalezas complementarias de los factores MAS e IMU en futuros sistemas de estimación de estado híbridos, y como una alternativa viable para sistemas visual-inerciales donde la calibración y el ruido de la IMU son factores limitantes. La naturaleza de código abierto del trabajo tiene como objetivo fomentar el desarrollo posterior de métodos basados en MAS dentro de la comunidad de la robótica.