A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

Resumen Técnico: Un MPC con Restricción $\epsilon$ Adaptativa al Terreno para la Planificación Cinodinámica en Terrenos Irregulares

Enunciado del Problema
La navegación autónoma para vehículos tipo automóvil en entornos no estructurados y accidentados (por ejemplo, sitios de construcción, zonas de desastres, exploración planetaria) presenta un doble desafío: generar trayectorias libres de colisiones respetando al mismo tiempo complejas restricciones cinodinámicas. Los planificadores basados en mallas tradicionales, como los que utilizan el Método de Marcha Rápida (FMM), sobresalen en la producción de trayectorias geométricas conscientes del terreno, pero a menudo fallan al incorporar la viabilidad dinámica, como los límites de velocidad y la estabilidad del vehículo. Por el contrario, los planificadores cinodinámicos frecuentemente luchan por equilibrar objetivos en competencia, específicamente la eficiencia de la trayectoria versus la estabilidad de la pose (evitar el vuelco), sin caer en óptimos locales o depender de suposiciones 2D excesivamente simplificadas. Los enfoques basados en datos existentes (por ejemplo, Aprendizaje por Refuerzo Profundo) a menudo carecen de generalización a través de diversas configuraciones de inicio-objetivo y fallan en integrar estructuras de guía global continuas. Además, los métodos estándar de optimización multiobjetivo a menudo dependen de la escalarización ponderada, lo que puede permitir que la longitud de la trayectoria domine la solución, descuidando las restricciones críticas de estabilidad en terrenos ásperos.

Metodología
Los autores proponen un marco de control de horizonte rodante que integra un modelo semi-paramétrico vehículo-terreno con un Control Predictivo por Modelo (MPC) de restricción $\epsilon$ adaptativo al terreno.

1. Modelado Vehículo-Terreno:

   • Enfoque Semi-Paramétrico: El sistema combina un modelo cinemático analítico para vehículos tipo automóvil con una corrección residual basada en datos. El modelo nominal predice la evolución del estado basada en velocidades lineales y angulares.
   • Proceso Gaussiano Escaso (SGP): Para tener en cuenta efectos del terreno no modelados (por ejemplo, deslizamiento, dinámicas de contacto complejas), un SGP se entrena sobre descriptores locales del terreno para predecir el error residual entre el modelo nominal y el comportamiento real del sistema.
   • Descriptores del Terreno: El modelo utiliza un conjunto rico de descriptores que incluyen vectores normales ponderados por área, rugosidad de la superficie ($\sigma_z$), curvaturas gaussianas y medias, y área total del vecindario.
   • Función de Altura: Un método basado en proyección determina la altura del terreno $h(x,y)$ mapeando puntos de consulta a la cara más cercana en una malla triangular, asegurando cálculos precisos de orientación.

2. Marco de Optimización:

   • Formulación Multiobjetivo: El planificador optimiza dos objetivos en competencia:
     • Eficiencia de la Trayectoria ($f_1$): Minimiza la desalineación direccional con un campo vectorial global FMM, ponderado por un factor de escala de proximidad al objetivo.
     • Estabilidad de la Pose ($f_2$): Minimiza las desviaciones acumuladas de orientación (alabeo y cabeceo) con respecto a la normal local del terreno, penalizando desviaciones más allá de un umbral seguro ($\alpha_{safe}$) y haciendo cumplir un límite estricto ($\alpha_{max}$) para prevenir el vuelco.
   • Restricción $\epsilon$ Adaptativa al Terreno: En lugar de la escalarización ponderada, el método convierte el problema en una optimización de un solo objetivo con restricciones. La restricción de estabilidad ($f_2 \leq \epsilon_{stab}$) se ajusta dinámicamente en función de la rugosidad y la pendiente local del terreno. En terrenos desafiantes, el presupuesto de estabilidad se estrecha; en terrenos benignos, se relaja, permitiendo una exploración más agresiva.
   • Estrategia de Muestreo: Las secuencias de control candidatas se generan mediante un enfoque híbrido:
     • Arranque en Caliente: Desplazamiento de secuencias de control anteriores exitosas con ruido añadido para asegurar la continuidad temporal.
     • Muestreo Sesgado por FMM: Mezcla de controles sugeridos por el campo vectorial global FMM con exploración aleatoria para escapar de mínimos locales.

Contribuciones Clave
El artículo afirma cuatro contribuciones principales:

1. Un modelo semi-paramétrico para el movimiento de vehículos tipo automóvil en mallas triangulares irregulares, que combina dinámicas analíticas con aprendizaje residual basado en SGP.
2. Una función de costo para la navegación en mallas derivada de un campo vectorial FMM continuo.
3. Un enfoque de solución que utiliza optimización de restricción $\epsilon$ adaptativa al terreno dentro de un marco de horizonte rodante para generar trayectorias no dominadas que equilibran eficiencia y estabilidad.
4. Un planificador compatible con ROS (Move Base Flex) diseñado para su implementación en tiempo real.

Resultados Experimentales
El método propuesto ($\epsilon$-MPC) se evaluó en simulación (Gazebo) y experimentos del mundo real utilizando un robot AgileX Scout Mini en dos mallas 3D de terreno distintas. Se comparó contra los puntos de referencia Model Predictive Path Integral (MPPI) y Cinodinámico Algorítmico Genético (GAKD).

• Métricas de Rendimiento: El método propuesto logró una tasa de éxito de navegación del 94% en simulación, superando a MPPI (87%) y GAKD (89%).
• Estabilidad: Redujo la desviación máxima de orientación en un 24% en comparación con MPPI y mejoró la métrica de compensación multiobjetivo ($T$) en un 23%.
• Estudios de Ablación:
  • Sesgo FMM: Una fuerza de sesgo FMM de $\alpha_{FMM}=0.7$ produjo resultados óptimos, mejorando las tasas de éxito entre un 11% y un 27% sobre el muestreo puramente aleatorio.
  • Adaptabilidad: El enfoque de restricción adaptativa al terreno eliminó todos los incidentes de vuelco observados con restricciones epsilon fijas y redujo la desviación máxima de orientación en un 32%.
  • Implementación en el Mundo Real: En experimentos físicos, el método mantuvo una tasa de éxito del 80% (vs. 67% para MPPI), navegando con éxito secciones empinadas donde los puntos de referencia fallaron debido a ángulos de alabeo excesivos. Los tiempos de planificación se mantuvieron dentro de las restricciones de tiempo real (~35–48 ms por iteración).

Significado y Afirmaciones
El artículo afirma que su significado principal radica en proporcionar un marco principiado que aborda explícitamente la naturaleza multiobjetivo de la planificación cinodinámica en terrenos irregulares. Al ajustar dinámicamente las restricciones de estabilidad basándose en descriptores locales del terreno, el enfoque logra un equilibrio superior entre la optimalidad de la trayectoria y la seguridad física sin depender de búsquedas evolutivas complejas y computacionalmente costosas (como NSGA-II, que se encontró demasiado lento para su uso en tiempo real). Los autores concluyen que, si bien los métodos de escalarización ponderada pueden optimizar objetivos individuales, el método de restricción $\epsilon$ adaptativo al terreno es más efectivo para garantizar una navegación factible y segura en entornos exteriores no estructurados, ofreciendo una solución práctica que escala desde la simulación hasta la implementación en el mundo real.