CC-VPSTO: Chance-Constrained Via-Point-Based Stochastic Trajectory Optimisation for Online Robot Motion Planning under Uncertainty

Resumen Técnico: CC-VPSTO

1. Planteamiento del Problema

La autonomía robotica fiable en entornos reales requiere sistemas de toma de decisiones que gestionen la incertidumbre (ruido en sensores, actuadores imprecisos, dinámicas ambientales desconocidas) sin imponer restricciones excesivamente conservadoras que degraden el rendimiento de la tarea.

El problema central abordado es la planificación de trayectorias en línea bajo incertidumbre, donde las restricciones (como la evitación de colisiones) no pueden garantizarse con certeza absoluta, sino que deben satisfacerse con una alta probabilidad (enfoque de Chance Constraints o Restricciones de Probabilidad).

• Desafío: Las formulaciones de restricciones de probabilidad son generalmente intratables computacionalmente.
• Limitaciones de métodos existentes:
  • Los métodos de control robusto (peor caso) son demasiado conservadores.
  • Las aproximaciones de promedio de muestras (Sample Average Approximation - SAA) ingenuas tienden a ser demasiado optimistas con pocos datos, llevando a soluciones que parecen factibles en la muestra pero violan la restricción real.
  • Muchos enfoques actuales asumen distribuciones de incertidumbre específicas (ej. Gaussianas) o dinámicas lineales, lo que limita su aplicabilidad general.

2. Metodología Propuesta: CC-VPSTO

Los autores proponen CC-VPSTO (Optimización de Trayectoria Estocástica Basada en Puntos de Paso con Restricciones de Probabilidad), un marco de trabajo en tiempo real que integra la optimización estocástica dentro de un esquema de Control Predictivo Modelado (MPC) de horizonte deslizante.

Componentes Clave:

• Formulación de Restricción de Probabilidad:
  El objetivo es minimizar una función de costo $J(x)$ sujeta a que la probabilidad de violar una restricción $g(x, \delta) \leq 0$ sea menor que un umbral $\eta$.
  $$\min_{x} J(x) \quad \text{s.t.} \quad P[g(x, \delta) > 0] \leq \eta$$

• Aproximación de Promedio de Muestras (SAA) con Ajuste de Confianza:
  Dado que calcular la probabilidad exacta es intratable, se utiliza un conjunto finito de $N$ muestras de incertidumbre. En lugar de usar un umbral ingenuo ($k = \eta N$), los autores derivan un umbral de confianza acotado ($k_{thresh}$) basado en la distribución binomial.

  • Se modela el conteo de violaciones como un proceso de Bernoulli.
  • Se calcula $k_{thresh}$ tal que, si la solución satisface la restricción en las muestras, existe una confianza del $1-\beta$ de que satisface la restricción de probabilidad real.
  • Esto introduce un "relleno" (padding) estadístico que compensa el error de aproximación debido al tamaño finito de la muestra, evitando el optimismo excesivo.

• Optimización Libre de Gradientes (Zero-Order):
  Debido a que la función de restricción basada en muestras es no suave (discontinua), el método utiliza CMA-ES (Estrategia Evolutiva de Adaptación de Matriz de Covarianza) para optimizar los puntos de paso (via-points) que definen la trayectoria.

  • Se utiliza un término de penalización en la función objetivo para manejar las violaciones de la restricción de muestra.
  • Esto permite manejar restricciones no lineales y distribuciones de incertidumbre arbitrarias sin asumir convexidad o diferenciabilidad.

• Integración en MPC:
  El marco se ejecuta en un bucle de horizonte deslizante. En cada paso de control, se generan nuevas muestras de la dinámica futura de los obstáculos, se optimiza una nueva trayectoria y se ejecuta solo el primer segmento, permitiendo reactividad ante nuevos datos.

3. Contribuciones Principales

1. Nueva Formulación Surrogada: Un método para aproximar restricciones de probabilidad que mantiene una baja conservaduría incluso con tamaños de muestra pequeños, corrigiendo el sesgo de las aproximaciones ingenuas mediante umbrales estadísticos basados en la confianza.
2. Análisis Teórico de Validez: Demostración de que, bajo ciertas condiciones de independencia (o en el contexto de MPC donde la solución cambia suavemente entre pasos), la solución del problema surrogado satisface la restricción original con alta confianza.
3. Generalidad: El enfoque no asume ninguna distribución específica de incertidumbre (funciona con Gaussianas, mezclas Gaussianas, etc.), ni requiere linealidad en la dinámica o convexidad en las restricciones.
4. Validación Empírica: Evaluación exhaustiva en simulación y en un robot real (Franka Emika), demostrando la viabilidad en tiempo real.

4. Resultados Experimentales

• Simulación (Planificación Offline y Online):

  • Precisión: El umbral binomial propuesto ($\eta_{binom}$) proporciona una aproximación mucho más precisa y menos conservadora que los límites basados en complejidad de Rademacher o métodos de promedio de muestras ingenuos.
  • Distribuciones Multimodales: El método logra encontrar trayectorias óptimas incluso con distribuciones de incertidumbre complejas (mezclas Gaussianas), donde otros métodos fallarían o requerirían suposiciones incorrectas.
  • Comparativa en MPC: En entornos con obstáculos dinámicos, CC-VPSTO logra tasas de éxito altas y tasas de colisión bajas.
    • El método de "Máxima Verosimilitud" (usar la media de los obstáculos) resultó ser extremadamente inseguro (hasta 50% de colisiones en escenarios difíciles).
    • CC-VPSTO mantiene la tasa de colisión por debajo del umbral $\eta$ definido por el usuario, incluso con un número bajo de muestras (ej. $N=100$).
  • Trade-off: Se observa un equilibrio claro: aumentar $\eta$ (permitir más riesgo) reduce la duración de la trayectoria (mejor rendimiento), mientras que reducir $\eta$ aumenta la seguridad pero alarga la tarea.

• Experimento en Robot Real (Franka Emika):

  • Escenario: El robot debe cruzar una cinta transportadora evitando una caja que se mueve estocásticamente.
  • Rendimiento: El sistema operó a 3 Hz en un CPU estándar, demostrando capacidad de tiempo real.
  • Comportamiento: El robot pudo decidir dinámicamente si pasar por delante o detrás de la caja, o esperar, basándose en la probabilidad de colisión.
  • Conclusión: Se confirmó que el enfoque genera trayectorias eficientes que respetan los límites de probabilidad de colisión en el mundo real.

5. Significado e Impacto

El trabajo de CC-VPSTO es significativo porque cierra la brecha entre la teoría de optimización estocástica y la aplicación práctica en robótica en tiempo real.

• Seguridad vs. Eficiencia: Proporciona un marco principista para equilibrar la seguridad (satisfacción de restricciones) y la eficiencia (tiempo de tarea), evitando tanto el conservadurismo paralizante como el riesgo inaceptable.
• Generalidad: Al no depender de suposiciones sobre la distribución de ruido o la linealidad del sistema, es aplicable a una gama mucho más amplia de problemas robóticos (manipulación, navegación, interacción física) que los métodos anteriores.
• Viabilidad en Línea: Demuestra que la optimización estocástica rigurosa puede ejecutarse en bucles de control de alta frecuencia (MPC), lo cual es un requisito fundamental para la autonomía robótica en entornos dinámicos e impredecibles.

En resumen, CC-VPSTO ofrece una solución robusta y flexible para la planificación de movimientos bajo incertidumbre, permitiendo a los robots operar de manera más segura y eficiente en el mundo real.