Resumen Técnico: DynoJEPP – Estimación Conjunta, Predicción y Planificación en Entornos Dinámicos

1. Planteamiento del Problema

La autonomía robótica en entornos del mundo real se ve fundamentalmente desafiada por la presencia de objetos dinámicos con movimientos complejos e impredecibles. Si bien los avances recientes han unificado con éxito la estimación del estado del robot y la planificación de trayectorias dentro de marcos basados en grafos de factores para entornos estáticos, la extensión de estos enfoques a escenas dinámicas sigue siendo limitada.

Las soluciones existentes suelen enfrentar una de dos limitaciones:

1. Suposición de Conocimiento Perfecto: Asumen que las trayectorias futuras de los objetos son conocidas a priori, lo cual es poco realista en entornos dinámicos.
2. Estrategias Reactivas: Se basan en una evasión puramente reactiva, careciendo de la previsión que ofrece la predicción de trayectorias.

Un problema crítico, a menudo pasado por alto en los marcos de optimización simultánea, es el flujo bidireccional de información. En las formulaciones conjuntas convencionales, la información de los módulos de planificación y predicción retroalimenta al módulo de estimación de estado. Este bucle de retroalimentación corrompe las estimaciones de estado con sesgos "optimistas" derivados de planes futuros, lo que conduce a una precisión de estimación degradada, comportamientos no deseados y, en última instancia, planes inseguros y colisiones.

2. Metodología: El Marco DynoJEPP

Los autores proponen DynoJEPP, un marco unificado basado en grafos de factores que optimiza simultáneamente la estimación del estado del robot y de los objetos, la predicción de trayectorias y la planificación local. La innovación central reside en la introducción de un factor dirigido novedoso para controlar el flujo de información.

A. Factores Dirigidos

Para evitar que la planificación y la predicción degraden la estimación, DynoJEPP introduce un factor dirigido que impone un flujo de información unidireccional:

• Estimación $\to$ Predicción/Planificación: Los resultados de la estimación (pose actual del robot y movimientos de los objetos) se propagan hacia adelante para informar la predicción y la planificación.
• Predicción/Planificación $\nrightarrow$ Estimación: Se impide que los estados futuros y las restricciones de planificación influyan en la estimación de estados actuales o pasados.

Técnicamente, esto se logra mediante la manipulación de la Jacobiana. Al anular las entradas de la Jacobiana con respecto a la variable fuente (el estado de estimación) en las ecuaciones normales ($J^\top J \delta = -J^\top r$), el marco asegura que el gradiente del costo de planificación/predicción no actualice las variables de estimación. Esto desacopla estructuralmente el subgrafo de estimación del subgrafo de planificación, manteniendo al mismo tiempo la dependencia necesaria para que la planificación utilice las estimaciones actuales.

B. Componentes de la Formulación Conjunta

El marco integra tres módulos en un único grafo de factores:

1. Estimación de Estado: Se basa en SLAM Dinámico Híbrido para estimar la pose del robot ($W X_k$) y los movimientos de cuerpos rígidos SE(3) de los objetos dinámicos ($W eH_k$). Utiliza mediciones de puntos, odometría y factores de suavizado de movimiento para rastrear los centros de masa de los objetos.
2. Predicción de Movimiento de Objetos: Utiliza los movimientos SE(3) estimados para pronosticar las trayectorias futuras de los objetos en un horizonte $N$. Emplea un modelo de velocidad constante (impuesto por factores de suavizado), pero relaja esta suposición cerca de obstáculos estáticos utilizando factores conscientes de la escena estática (basados en ESDF).
3. Planificación Local: Emplea un Control Predictivo Basado en Modelos (MPC) basado en grafos de factores para optimizar las poses, velocidades y aceleraciones futuras del robot. Incorpora:
   • Conciencia de Escena Estática: Utiliza Campos de Distancia Signada Euclídea (ESDF) para evitar obstáculos estáticos.
   • Conciencia de Escena Dinámica: Utiliza factores de obstáculos dinámicos dirigidos para penalizar la proximidad a objetos en movimiento basándose en sus trayectorias predichas.

C. Extensión Cooperativa (C-DynoJEPP)

El artículo introduce DynoJEPP Cooperativo (C-DynoJEPP), que permite al robot egoísta incorporar comportamientos cooperativos en su planificación. En esta extensión, los factores de obstáculos dinámicos se dirigen desde la planificación hacia la predicción. Esto crea un ciclo dirigido entre la predicción y la planificación, modelando la influencia mutua donde el robot reacciona al objeto, y se espera que el objeto reaccione a la trayectoria prevista del robot. Esto permite una navegación menos conservadora y más asertiva en escenarios interactivos.

3. Contribuciones Clave

1. Marco DynoJEPP: El primer marco basado en grafos de factores que estima conjuntamente el movimiento de objetos dinámicos, predice su evolución e incorpora estas predicciones en las acciones de control del robot dentro de un único problema de optimización.
2. Factor Dirigido Novedoso: Un mecanismo para especificar la dirección de la influencia en los grafos de factores, permitiendo un flujo de información controlado entre estimación, predicción y planificación. Esto previene la "corrupción" de las estimaciones de estado por las restricciones de planificación futuras.
3. Optimización Cooperativa: Una extensión (C-DynoJEPP) que parametriza el nivel de cooperación de los objetos dinámicos, permitiendo una influencia mutua entre el plan del robot y la predicción del objeto sin comprometer la independencia de la estimación de estado.

4. Resultados Experimentales

Los autores evaluaron el marco en un entorno de almacén simulado (Gazebo) con robots de tracción diferencial y obstáculos dinámicos. Compararon tres configuraciones: No Dirigida (flujo bidireccional), Dirigida (DynoJEPP) y Desacoplada (estimación separada seguida de planificación/predicción conjunta).

• Seguridad y Tasa de Éxito: En 10 experimentos de navegación que involucraban obstáculos dinámicos, la configuración No Dirigida falló en la mayoría de los casos (colisionando en 8 de cada 10 ensayos dinámicos) debido a una planificación excesivamente optimista que corrompía las estimaciones. En contraste, la configuración Dirigida logró una tasa de éxito del 100% en todos los ensayos.
• Precisión de Estimación: El método Dirigido mantuvo una precisión de estimación comparable a la línea base Desacoplada, superando significativamente al enfoque No Dirigido. El método No Dirigido mostró una degradación significativa tanto en las estimaciones de la trayectoria del robot como en la de los objetos.
• Navegación Cooperativa: En escenarios con obstáculos dinámicos reactivos, C-DynoJEPP demostró la capacidad de navegar de manera más asertiva que el DynoJEPP estándar. Alcanzó los objetivos más rápido (82 pasos frente a 92 pasos para la ejecución más rápida de DynoJEPP) aprovechando pesos cooperativos para influir en las predicciones.

5. Significado y Afirmaciones

El artículo afirma que controlar la direccionalidad del flujo de información es crítico para una operación segura en entornos dinámicos. Sin factores dirigidos, el acoplamiento de planificación y estimación conduce a un bucle de retroalimentación donde los objetivos de planificación distorsionan las estimaciones de estado, resultando en comportamientos inseguros.

Los autores enfatizan que, si bien la estimación no se beneficia inherentemente de la optimización conjunta con la planificación, la predicción y la planificación sí se benefician de ser optimizadas conjuntamente con la estimación, particularmente al modelar comportamientos interactivos. El marco propuesto proporciona una solución robusta que permite una optimización simultánea segura sin sacrificar la integridad de la estimación de estado.

El trabajo concluye señalando que la investigación futura se centrará en implementar una versión eficiente, incremental y multihilo de DynoJEPP para lograr un rendimiento en tiempo real, estableciendo puntos de referencia frente a estándares de la industria como Nav2.