Safe Learning Predictive Control for Ego-World Robotic Systems

Resumen Técnico: Modelo de Control Predictivo por Horizonte con Aprendizaje de Políticas para un Mundo Online Seguro (SOWL-MPC)

Planteamiento del Problema
El artículo aborda el desafío de la navegación autónoma segura para un robot "ego" que opera en un entorno compartido con un robot "mundo" cuya política de control es desconocida y potencialmente no estacionaria. Este escenario, denominado marco EGO–WORLD, asume que el robot ego tiene acceso únicamente a mediciones de estado ruidosas del robot mundo ($\tilde{z}_t = z_t + w_t$) y conocimiento previo de la dinámica nominal del robot mundo (vector de deriva $g(z)$ y matriz de control $H(z)$), pero carece de conocimiento de la política de control real del robot mundo $\pi(z)$. Los métodos reactivos tradicionales (por ejemplo, el Método de la Ventana Dinámica, Funciones de Barrera de Control) suelen ser miopes, mientras que los enfoques predictivos existentes dependen típicamente de modelos entrenados fuera de línea o suposiciones de movimiento fijas que no logran adaptarse a comportamientos no vistos. El problema central es diseñar un marco de control en línea que pueda aprender iterativamente la política latente del mundo a partir de datos en flujo y utilizar este aprendizaje para realizar maniobras seguras y libres de colisiones mediante el Control Predictivo basado en Modelo (MPC).

Metodología
La solución propuesta, SOWL-MPC, integra el aprendizaje en línea con un esquema de control de horizonte de deslizamiento a través de tres etapas principales:

1. Modelado de la Política y Preentrenamiento Fuera de Línea:
   La política desconocida del mundo $\pi(z)$ se modela como un conjunto de Procesos Gaussianos Variacionales Dispersos (SVGP) independientes, uno para cada componente del vector de política. Inicialmente, los hiperparámetros se seleccionan mediante preentrenamiento fuera de línea en un conjunto de datos de pares estado-entrada ruidosos. Los datos de entrada (entradas de control del mundo) se reconstruyen en línea a partir de observaciones consecutivas de estados ruidosos utilizando un enfoque de mínimos cuadrados basado en la dinámica nominal conocida.

2. Aprendizaje en Línea mediante Condicionamiento Variacional en Línea (OVC):
   Para adaptarse a nuevos datos en tiempo real sin el costo computacional prohibitivo del reentrenamiento, los autores emplean el Condicionamiento Variacional en Línea (OVC). En lugar de almacenar todo el historial de datos, el OVC mantiene estadísticas suficientes (ubicaciones de inducción, medias variacionales y covarianzas) para actualizar los parámetros del SVGP de forma recursiva. El algoritmo proyecta la posterior actual en "pseudo-datos", los combina con la nueva observación y actualiza los puntos de inducción y los parámetros variacionales. Esto permite al sistema aprender la política latente directamente de las mediciones ruidosas en flujo.

3. Predicción de Trayectoria y Control Conscientes de la Incertidumbre:
   La política aprendida se propaga a través de la dinámica no lineal del mundo utilizando un esquema de propagación de momentos gaussianos de primer orden (análogo a un paso de predicción de un Filtro de Kalman Extendido). Esto genera un pronóstico probabilístico multietapa de la trayectoria futura del robot mundo, incluyendo una medida de la incertidumbre predictiva.
   Estas predicciones se introducen en un controlador MPC no lineal. La formulación del MPC incluye:

• Función de Costo: Un costo de seguimiento estándar con penalizaciones por esfuerzo de control y suavidad de la entrada.
• Restricciones de Seguridad: La evitación de colisiones se impone inflando la distancia de seguridad entre los robots. El margen de seguridad se ajusta dinámicamente basándose en la incertidumbre predictiva (traza de la matriz de covarianza) escalada por un parámetro de seguridad $n_\sigma$ definido por el usuario. Se aplican restricciones estrictas al siguiente paso inmediato, mientras que las variables de holgura permiten la relajación temporal en el horizonte de predicción posterior para asegurar la factibilidad.

Contribuciones Clave
El artículo describe tres contribuciones primarias:

1. Marco SOWL-MPC: Una novedosa estrategia de control para sistemas EGO–WORLD que aprende la política de retroalimentación de estado latente de un robot mundo no cooperativo en línea, en lugar de aprender la dinámica del sistema o depender de predictores fuera de línea. Esta formulación aprovecha la menor dimensionalidad del espacio de entrada en comparación con el espacio de estados para la eficiencia de los datos.
2. Aplicación de OVC a la Robótica: La primera aplicación del Condicionamiento Variacional en Línea para la identificación de políticas en línea y la predicción de trayectorias en sistemas robóticos. Esto permite que los modelos SVGP se actualicen continuamente con datos en flujo con una complejidad computacional fija ($O(M^3)$ por actualización, donde $M$ es el número de puntos de inducción).
3. Validación en Tiempo Real: Una implementación de pila completa en ROS 2 (integrada con la caja de herramientas CHOIRBOT) que demuestra la factibilidad en tiempo real. El enfoque fue validado mediante extensos experimentos virtuales de Monte Carlo y pruebas de hardware en el mundo real que involucraron maniobras de adelantamiento y cruce.

Resultados Experimentales
La estrategia fue evaluada tanto en entornos virtuales (Webots/ROS 2) como en el mundo real (arena interior con coches NVIDIA JetRacer):

• Precisión de Predicción: En experimentos virtuales en las pistas ETHZ MOBIL y LEMNISCATE, SOWL-MPC superó significativamente a una línea base de GP entrenada fuera de línea y a una línea base de Velocidad Constante (CV). El Error de Desplazamiento Promedio (ADE) para SOWL-MPC se estabilizó aproximadamente en 0.05 m tras la fase inicial de aprendizaje, comparado con errores que superan los 0.4 m para la línea base CV y picos periódicos por encima de 0.7 m para la línea base fuera de línea.
• Garantías de Seguridad: Las simulaciones de Monte Carlo demostraron que el ajuste del parámetro de seguridad $n_\sigma$ permite un compromiso entre agresividad y seguridad. Con $n_\sigma = 3$, el método logró una tasa de éxito del 100% en la evitación de colisiones en 50 ensayos, mientras que un enfoque determinista ($n_\sigma = 0$) falló en el 36% de los ensayos.
• Desempeño en Tiempo Real: El tiempo de computación promedio por paso de control se mantuvo muy por debajo del tiempo de muestreo de 50 ms incluso para horizontes de predicción de hasta $T=40$, confirmando la factibilidad en tiempo real.
• Validación en el Mundo Real: En el hardware físico, el sistema ejecutó con éxito maniobras de evasión anticipatoria, con el ADE estabilizándose alrededor de 0.05 m, confirmando la transferibilidad de los resultados virtuales a la dinámica del mundo real.

Significancia y Reivindicaciones
El artículo afirma que SOWL-MPC representa un avance significativo al cerrar la brecha entre la seguridad reactiva y la planificación predictiva en entornos con agentes adaptativos desconocidos. Al modelar y aprender explícitamente la política del robot mundo en lugar de solo su dinámica, y al utilizar OVC para actualizaciones eficientes en línea, el marco logra garantías de seguridad que se adaptan al comportamiento observado del robot mundo. Los autores enfatizan que esta es la primera aplicación de OVC a este problema robótico específico, permitiendo el aprendizaje eficiente de datos de políticas latentes que pueden integrarse directamente en bucles de control conscientes de la incertidumbre. El trabajo demuestra que la navegación anticipatoria segura es alcanzable incluso cuando la lógica de control del robot mundo es completamente desconocida y solo se dispone de mediciones de estado ruidosas.