One-Step Model Predictive Path Integral for Manipulator Motion Planning Using Configuration Space Distance Fields

Resumen Técnico: MPPI de un solo paso para la planificación de movimiento de manipuladores utilizando campos de distancia en el espacio de configuración

Planteamiento del problema
La planificación de movimiento para manipuladores robóticos enfrenta una dicotomía entre los métodos basados en optimización y los basados en muestreo. Los enfoques de optimización clásicos (p. ej., CHOMP, TrajOpt) dependen de Campos de Distancia con Signo (SDF) en el espacio de trabajo para imponer restricciones de evitación de colisiones. Sin embargo, estos métodos sufren de mínimos locales y fallan cuando los gradientes de los SDF se desvanecen cerca de los obstáculos. Además, la distribución de los valores de los SDF en el espacio de articulaciones suele ser no uniforme. Por el contrario, los métodos libres de gradiente como el control de Integral de Trayectoria Predictiva de Modelo (MPPI) evitan el desvanecimiento de gradientes mediante el uso de despliegues de trayectoria de largo horizonte para explorar implícitamente los obstáculos. Aunque son efectivos, el MPPI estándar es computacionalmente costoso debido a la necesidad de miles de muestras de trayectoria, comprobaciones de colisión repetidas y la dificultad de diseñar funciones de costo con unidades físicas heterogéneas (p. ej., mezclando distancia, velocidad y torque).

Metodología
Los autores proponen un marco que integra Campos de Distancia en el Espacio de Configuración (CDF) con el control de Integral de Trayectoria Predictiva de Modelo (MPPI) para permitir la navegación directa en el espacio de configuración del robot.

1. Representación CDF: A diferencia de los SDF en el espacio de trabajo, los CDF ($f_c$) modelan las distancias directamente en el espacio de configuración. Estos satisfacen la ecuación de Eikonal ($\|\nabla_q f_c\| = 1$) casi en todas partes, proporcionando información de gradiente confiable y no desvanecida que apunta hacia la dirección de escape óptima de los obstáculos.
2. Función de Costo Basada en Ángulos: Para unificar los términos de costo y eliminar las unidades heterogéneas, los autores introducen una función de costo basada en ángulos. El costo $c(\theta_1, \theta_2)$ se define como:
   • $\theta_1$: El ángulo entre el vector de movimiento y el gradiente del CDF (normal del obstáculo). Esto penaliza el movimiento hacia los obstáculos.
   • $\theta_2$: El ángulo entre el vector de movimiento y el vector de la meta. Esto penaliza el movimiento lejos de la meta.
   • Lógica Condicional: El costo del obstáculo ($\theta_1$) se establece en cero si el robot se aleja del obstáculo, si la distancia al obstáculo supera un umbral ($d_{act}$), o si la distancia al obstáculo es mayor que la distancia a la meta. Esto evita que el robot circule innecesariamente alrededor de los obstáculos.
3. MPPI de Horizonte de un Solo Paso: Aprovechando las pistas de seguridad geométrica local proporcionadas por el gradiente del CDF, el método reduce el horizonte de predicción de MPPI a un solo paso ($H=1$). En lugar de simular trayectorias de largo horizonte para acumular costos futuros, el algoritmo muestrea entradas de control de un solo paso, evalúa el costo angular inmediato y actualiza la política de distribución de muestreo.
4. Manejo de Límites de Articulación: Las entradas de control se proyectan sobre el conjunto factible definido por los límites de las articulaciones utilizando una estrategia de recorte elemento a elemento antes de la ejecución.

Contribuciones Clave

• Integración de CDF y MPPI: El artículo combina la fiabilidad del gradiente de los CDF con la robustez de muestreo de MPPI, eliminando la necesidad de despliegues de largo horizonte.
• Función de Costo Unificada: Al expresar todos los costos como ángulos, el método simplifica el ajuste de pesos y elimina el problema de comparar unidades físicas heterogéneas.
• Horizonte de un Solo Paso: El uso de gradientes de CDF permite una reducción drástica del horizonte de predicción de $H \gg 1$ a $H=1$, reduciendo significativamente la carga computacional mientras se mantiene la evitación de colisiones.
• Evitación de Mínimos Locales: Al eliminar el resolvedor de optimización iterativo (que es propenso a mínimos locales en espacios de alta dimensión) y depender de las actualizaciones de distribución de MPPI, el método mejora la robustez en entornos congestionados.

Resultados Experimentales
El método fue evaluado en un robot de 2-DoF de dos eslabones y un manipulador Franka Emika de 7-DoF en simulación.

• Robot de 2-DoF: El método propuesto logró una tasa de éxito del 99.6%, superando significativamente al MPPI original (71.2%) y a un método de optimización basado en QP (64.1%). Aunque la longitud de la trayectoria fue ligeramente mayor que las de los baselines, el método evitó con éxito los óptimos locales que atrapaban al resolvedor QP.
• Robot Franka de 7-DoF:
  • Escenario 1 (60 obstáculos): Tasa de éxito del 100% con una frecuencia de control promedio de 776 Hz.
  • Escenario 2 (123 obstáculos): Tasa de éxito del 86% con una frecuencia de control promedio de 718 Hz.
  • Comparación: El baseline de QP logró solo ~13-14% de tasa de éxito y operó a ~220 Hz. Una comparación con una implementación estándar de MPPI (cuRobo) mostró que el método propuesto corre a ~700+ Hz frente a ~28-61 Hz del MPPI estándar, atribuido al horizonte simplificado de un solo paso y al uso eficiente de CDF.

Significancia y Reivindicaciones
El artículo afirma que el marco CDF-MPPI ofrece una solución práctica para la planificación de movimiento en alta dimensión en entornos congestionados. Al aprovechar los gradientes de CDF para proporcionar pistas de seguridad local directa, el método logra:

1. Alta Eficiencia: Frecuencias de control que superan los 750 Hz, superando sustancialmente tanto a los resolvedores basados en optimización como al MPPI estándar.
2. Alta Fiabilidad: Tasas de éxito casi perfectas en 2D y tasas de éxito consistentemente altas en escenarios complejos de 7-DoF.
3. Simplicidad: Una función de costo unificada que simplifica el ajuste y elimina la carga computacional de las simulaciones de largo horizonte.

Los autores señalan que los fallos ocurren principalmente cuando el robot comienza extremadamente cerca de los obstáculos (dejando tiempo insuficiente para la adaptación de la distribución) o debido a errores de estimación de CDF en configuraciones cercanas a los obstáculos. Identifican la mejora de la fidelidad del CDF cerca de los obstáculos y la extensión de la representación a regiones esféricas como direcciones para el trabajo futuro.