Improvement of Robot's Simultaneous Localization and Mapping Using an Effective Transformation to Achieve Linear Model

Resumen Técnico: Mejora de la Localización y Mapeo Simultáneo de un Robot Mediante una Transformación Efectiva para Lograr un Modelo Lineal

Planteamiento del Problema
La Localización y Mapeo Simultáneo (SLAM) es una tarea fundamental para los robots móviles, sin embargo, las implementaciones estándar basadas en el Filtro de Kalman Extendido (EKF) sufren de divergencia e inconsistencia. Las causas primarias de estos fallos son la no linealidad inherente en los modelos de movimiento y observación del robot, lo que requiere la linealización. Esta linealización introduce errores que se acumulan con el tiempo, provocando actualizaciones incorrectas del vector de estado, saltos en las posiciones estimadas y una reducción en la matriz de covarianza estimada que no refleja la incertidumbre real. Aunque se han propuesto varios métodos como Fast SLAM, Unscented Fast SLAM (UnFS) y variantes del Filtro de Kalman Cubatura (CKF) para mitigar estos problemas, a menudo dependen de aproximaciones complejas o siguen teniendo dificultades con la consistencia en entornos grandes o bajo alta incertidumbre de los sensores.

Metodología
El artículo propone un nuevo enfoque denominado LMKF SLAM (SLAM de Filtro de Kalman de Modelo Lineal). La metodología central consiste en transformar el modelo de espacio de estados no lineal estándar en un modelo estrictamente lineal, permitiendo así la aplicación directa del Filtro de Kalman (KF) estándar sin la necesidad de aproximaciones de linealización o cálculos de Jacobian.

1. Transformación del Espacio de Estados:

   • Modelo de Movimiento: Los autores asumen que el robot está equipado con una brújula electrónica simple para medir el ángulo absoluto ($\theta_k$) del robot en cada paso. Al incorporar esta medición, el vector de estado se reduce para excluir el ángulo de orientación. Las ecuaciones de movimiento, originalmente no lineales debido a las funciones trigonométricas de $\theta_k$, son transformadas. Los términos de ruido se linealizan alrededor de cero, lo que resulta en un modelo de movimiento lineal donde la actualización del estado depende linealmente del estado anterior y de las entradas de control.
   • Modelo de Observación: Del mismo modo, el modelo de observación, que relaciona el rango ($r_k$) y el acimut ($\phi_k$) con las posiciones de los hitos (landmarks), es transformado. Al utilizar el ángulo del robot medido $\theta_k$, las coordenadas polares de los hitos se convierten en coordenadas cartesianas relativas al marco global. Esta transformación ($\zeta_k$) linealiza la relación entre los datos observados y el vector de estado (posiciones del robot y de los hitos).

2. Algoritmo de Filtrado:

   • Con los modelos de movimiento y observación linealizados, se aplica directamente el Filtro de Kalman estándar.
   • El algoritmo emplea un procesamiento secuencial para las actualizaciones de los hitos con el fin de aumentar la velocidad de cómputo, tratando las observaciones de diferentes hitos como independientes.
   • El vector de estado consiste en las coordenadas $(x, y)$ del robot y las coordenadas de todos los hitos observados.

Contribuciones Clave

• Linealización mediante Transformación: El artículo introduce una transformación simple que convierte los modelos de SLAM no lineales en modelos lineales utilizando únicamente una brújula electrónica estándar, eliminando la necesidad de técnicas de linealización complejas como los Jacobianos de EKF o las Transformaciones Unscented.
• Convergencia Teórica: A diferencia de los métodos basados en EKF donde las pruebas de convergencia son difíciles debido a las no linealidades, el modelo lineal propuesto permite una prueba rigurosa de convergencia, garantizando que el error de posicionamiento absoluto de los hitos tienda hacia un límite inferior.
• Robustez: El método está diseñado para ser altamente estable con respecto a las incertidumbres de los sensores (velocidad, ángulo de dirección y ángulo del robot) y a los cambios en los parámetros del sistema.

Resultados Experimentales
Los autores evaluaron LMKF SLAM frente a los métodos estándar EKF, UnFS e ICKF (Improved CKF) utilizando tanto simulaciones artificiales como el conjunto de datos real de Victoria Park.

• Simulación (Entorno Artificial):

  • Las pruebas se realizaron tanto en trayectorias de bucle cerrado como de bucle abierto.
  • Precisión: LMKF demostró un Error Cuadrático Medio (RMSE) significativamente menor tanto para el posicionamiento del robot como para el de los hitos en comparación con EKF, UnFS e ICKF.
  • Robustez: El rendimiento de LMKF se mantuvo estable incluso cuando se incrementaron la velocidad del robot ($v$) y el ruido de los sensores ($\sigma_G, \sigma_\theta$). En contraste, el RMSE de EKF e ICKF creció rápidamente con el aumento del ruido y la velocidad.
  • Dependencia de la Trayectoria: Mientras que otros métodos mostraron una reducción significativa del error en trayectorias de bucle cerrado (debido a la corrección de cierre de bucle), LMKF mantuvo una alta precisión en trayectorias de bucle abierto, demostrando que su rendimiento es menos dependiente de la existencia de bucles de cierre.
  • Estabilidad: La desviación estándar del RMSE ($\sigma_{RMSE}$) fue la más baja para LMKF, lo que indica una mayor fiabilidad a través de diferentes secuencias de ruido.

• Datos del Mundo Real (Conjunto de Datos de Victoria Park):

  • Utilizando un conjunto de datos con un escáner láser 2D y GPS, los autores estimaron los datos faltantes del ángulo del robot utilizando el GPS y la información de la trayectoria.
  • A pesar de la estimación imprecisa del ángulo del robot (una desventaja para el método propuesto), LMKF logró el RMSE más bajo (1.2 m) y el Error Absoluto Máximo más bajo (3.46 m) en comparación con EKF (2.58 m), UnFS (2.1 m) e ICKF (2.33 m).
  • LMKF también requirió el menor tiempo de ejecución (45.43 segundos) en comparación con los otros métodos.

Significado y Reivindicaciones
El artículo afirma que el método LMKF SLAM propuesto representa una mejora significativa sobre los métodos de filtrado de vanguardia. Al lograr un modelo lineal mediante una transformación simple, el método garantiza la convergencia y ofrece una precisión y estabilidad superiores. Los autores enfatizan que LMKF es particularmente robusto contra las incertidumbres de los sensores y los cambios en los parámetros del sistema, superando a EKF, UnFS e ICKF incluso en condiciones donde los métodos competidores eran teóricamente favorecidos (por ejemplo, cuando los parámetros de simulación se ajustaron para beneficiar a los modelos no lineales). El estudio concluye que este enfoque aborda eficazmente los problemas de divergencia comunes en el SLAM basado en EKF sin requerir una carga computacional compleja.