Resumen Técnico: Asuntos Terminales: Planificación Cinodinámica con un Costo Terminal y Incertidumbre Aprendida en el Espacio de Estado-Creencia

Enunciado del Problema

Los sistemas robóticos que operan en entornos del mundo real deben generar movimientos dinámicamente factibles que satisfagan las restricciones cinodinámicas, considerando al mismo tiempo la incertidumbre de ejecución derivada de errores de modelado, ruido de percepción e interacciones de contacto complejas. Aunque los planificadores cinodinámicos basados en muestreo existentes (por ejemplo, AO-RRT, SST) ofrecen completitud probabilística y optimalidad asintótica, generalmente optimizan solo el costo de ejecución (acumulado a lo largo de la trayectoria, como la longitud del camino o la energía) y tratan la llegada a la meta meramente como una verificación de factibilidad.

Este enfoque presenta dos limitaciones críticas:

1. Calidad del Estado Terminal: Muchas tareas dependen de la calidad del estado final, no solo del camino recorrido. Por ejemplo, en el empuje planar, un camino más corto podría resultar en una alta incertidumbre terminal, reduciendo la probabilidad de alcanzar la meta con éxito. En escenarios de múltiples metas (por ejemplo, estacionamiento de automóviles), los planificadores pueden seleccionar una meta geométricamente más cercana que sea menos deseable o más difícil de abandonar, ignorando las preferencias por regiones terminales específicas.
2. Propagación de la Incertidumbre: La planificación en el espacio de creencia (donde el estado es una distribución de probabilidad) es esencial para la robustez. Sin embargo, los métodos existentes a menudo dependen de umbrales de probabilidad rígidos o sustitutos a nivel de trayectoria en lugar de optimizar explícitamente el estado de creencia terminal. Además, los sistemas complejos (como el empuje rico en contactos) a menudo carecen de modelos analíticos de incertidumbre, lo que requiere enfoques basados en datos que rara vez se integran estrechamente con planificadores en el espacio de creencia.

Metodología

Los autores proponen KiTe (Planificación cinodinámica con un costo terminal), un marco que aumenta el Árbol Aleatorio de Exploración Rápida Asintóticamente Óptimo (AO-RRT) con un objetivo explícito de costo terminal. La metodología opera en tres capas principales:

1. KiTe con Costo Terminal en el Espacio de Estados

El algoritmo central reformula el problema óptimo de planificación cinodinámica en un espacio de estado-costo aumentado $\mathcal{Y} = \mathcal{X} \times \mathbb{R}^+$.

• Dinámica Aumentada: El estado se extiende de $x$ a $y = (x, c)$, donde $c$ acumula el costo de ejecución $\ell(x, u)$. Las dinámicas se extienden para incluir la tasa de costo: $\dot{y} = (f(x, u), \ell(x, u))$.
• Integración del Costo Terminal: A diferencia de formulaciones anteriores donde el costo terminal se ignora durante la búsqueda, KiTe evalúa el costo total $C_T = \int \ell dt + \phi(x(T))$ únicamente al alcanzar la región de la meta.
• Estrategia de Búsqueda: El algoritmo mantiene un límite de costo $c_{best}$. Cuando se encuentra una solución, el límite se actualiza a $c_{nuevo} + \phi(x_{nuevo})$. La estrategia de muestreo se centra entonces en el subnivel definido por este límite, podando las ramas que no pueden mejorar el costo total (ejecución + terminal).
• Garantía Teórica: Los autores proporcionan una prueba rigurosa de que AO-RRT con un costo terminal preserva la optimalidad asintótica, siempre que las dinámicas del sistema, el costo de ejecución y el costo terminal satisfagan condiciones de continuidad Lipschitz.

2. Extensión al Espacio de Creencia

La formulación se extiende al espacio de estado-costo de creencia $\mathcal{Y}_b = \mathcal{B} \times \mathbb{R}^+$, donde $\mathcal{B}$ es el espacio de distribuciones de probabilidad sobre el estado.

• Métrica: Se adopta la distancia 2-Wasserstein como métrica para el espacio de creencia, ya que captura naturalmente los cambios tanto en la media como en la covarianza de las creencias gaussianas.
• Cota Inferior de Llegada a la Meta: Una contribución teórica clave es la prueba de que la probabilidad de alcanzar una región de meta $G$ (definida como una bola de radio $r$) está acotada inferiormente por la distancia de Wasserstein entre la creencia terminal $b(T)$ y la medida de Dirac en la meta $\nu_g$:
  $$\text{Pr}(x(T) \in G) \geq 1 - \frac{W_2^2(b(T), \nu_g)}{r^2}$$
  Esto permite al planificador utilizar la distancia de Wasserstein como un costo terminal $\psi(b(T))$, optimizando directamente una cota inferior del éxito de llegada a la meta sin requerir umbrales de probabilidad rígidos y ajustados manualmente.
• Continuidad Lipschitz: Los autores demuestran que, bajo suposiciones leves sobre las dinámicas estocásticas (específicamente en lo que respecta a la continuidad Lipschitz de la incertidumbre del proceso y la inyección de ruido), las dinámicas de creencia y las funciones de costo permanecen continuas Lipschitz con respecto a la métrica de Wasserstein. Esto asegura que la optimalidad asintótica de KiTe se extienda al espacio de creencia.

3. Aprendizaje de Dinámicas de Creencia

Para abordar sistemas donde los modelos analíticos de incertidumbre no están disponibles (por ejemplo, empuje rico en contactos), KiTe integra un modelo de dinámicas de creencia aprendido.

• Arquitectura del Modelo: Una red neuronal (MLP) se entrena para predecir la transición estocástica local. Dado un estado $x_i$ y un control $u_i$, la red genera una transición media $\Delta x_i$ y un parámetro de covarianza del proceso parametrizado por un vector de varianza $\sigma_i^2$.
• Objetivo de Entrenamiento: El modelo se entrena utilizando la pérdida de Log-Verosimilitud Negativa (NLL) sobre datos de interacción. Esto permite que la red aprenda tanto las dinámicas medias como la incertidumbre aleatoria (ruido del proceso) directamente desde los datos.
• Propagación: La media y la covarianza aprendidas se utilizan para propagar el estado de creencia (media y covarianza) hacia adelante en el planificador, soportando tanto espacios de estados euclidianos como de grupos de Lie (por ejemplo, SE(2)).

Contribuciones Clave

1. Extensión Algorítmica y Prueba: Implementación de AO-RRT con un costo terminal explícito, acompañada de una prueba rigurosa de que la optimalidad asintótica se preserva tanto en espacios de estado como de creencia.
2. Vínculo Teórico con la Probabilidad de Éxito: Prueba de que minimizar la distancia de Wasserstein entre la creencia terminal y la meta induce una cota inferior en la probabilidad de éxito de llegada a la meta, proporcionando un objetivo terminal principista para la fiabilidad.
3. Propagación de Creencia Basada en Datos: Demostración de cómo aprender modelos de dinámicas de creencia (media e incertidumbre) directamente desde datos de interacción utilizando la pérdida NLL e integrarlos en el marco de KiTe para sistemas que carecen de modelos analíticos.
4. Validación Empírica: Evaluación exhaustiva en tres tareas diversas (Flappy Bird, Estacionamiento de Coches, Empuje Planar) tanto en simulación como en el mundo real, comparando KiTe con líneas base de última generación (AO-RRT, SST, Árbol de Creencia Gaussiana).

Resultados Experimentales

El artículo valida KiTe en tres tareas:

• Flappy Bird (Determinista): Demuestra que KiTe puede codificar preferencias de meta (por ejemplo, centrar al pájaro en la brecha) optimizando el costo terminal, mientras que las líneas base dejan de refinar una vez que se alcanza la región de la meta. KiTe logra trayectorias más cercanas al centro de la meta con solo un aumento modesto en el costo de ejecución.
• Estacionamiento de Coches (SE(2) con Incertidumbre): En una tarea con múltiples lugares de estacionamiento factibles, KiTe prioriza con éxito una región de meta "preferida" (lugar frontal) y mejora las tasas de éxito de llegada a la meta en comparación con líneas base que solo optimizan la longitud del camino o utilizan restricciones de probabilidad. La planificación en el espacio de creencia con costo terminal arrojó las tasas de éxito de ejecución más altas.
• Empuje Planar (Rico en Contactos, Mundo Real): Utilizando dinámicas aprendidas para objetos como un Camión de Basura y una Caja de Galletas, KiTe superó a las líneas base (incluida la Planificación Activa con incertidumbre del modelo) en las tasas de éxito de ejecución. El planificador seleccionó caminos geométricos más largos que resultaron en menor incertidumbre terminal y mayores probabilidades de éxito, demostrando el valor de optimizar la calidad de la creencia terminal.

Significado y Afirmaciones

El artículo afirma que KiTe aborda una brecha fundamental en la planificación cinodinámica: la separación de la optimización de trayectorias de la calidad del estado terminal. Al optimizar explícitamente el estado terminal, el método:

• Mejora la Fiabilidad: Aumenta directamente la probabilidad de alcanzar la meta bajo incertidumbre minimizando la distancia de Wasserstein hacia la meta.
• Codifica Preferencias: Permite a los planificadores distinguir entre múltiples metas factibles basándose en la deseabilidad (por ejemplo, facilidad de salida, estabilidad) en lugar de solo en la proximidad geométrica.
• Une el Aprendizaje y la Planificación: Proporciona un marco donde los modelos de incertidumbre aprendidos no se utilizan solo para sesgar el muestreo, sino que se integran estrechamente en el bucle de propagación de creencia y optimización.

Los autores concluyen que, aunque el método introduce parámetros de diseño (pesos terminales) y depende de modelos aprendidos, ofrece un enfoque principista y teóricamente fundamentado para generar movimientos que no solo son dinámicamente factibles, sino también fiables y conscientes de las preferencias en entornos inciertos.