Adaptive Cost-Map-based Path Planning in Partially Unknown Environments with Movable Obstacles

Resumen Técnico: Planificación de Trayectorias Basada en Mapas de Costos Adaptativos en Entornos Desconocidos con Obstáculos Móviles

Planteamiento del Problema
La navegación autónoma fiable en escenarios de respuesta ante desastres (por ejemplo, edificios colapsados, pasillos llenos de humo) requiere que los robots operen en entornos no estructurados donde los obstáculos pueden ser móviles. Los planificadores tradicionales suelen asumir obstáculos estáticos o dependen de una percepción rica en RGB-D y mapas semánticos para clasificar objetos. Sin embargo, estos enfoques con alta carga visual son frágiles en zonas de desastre debido al polvo, el humo y la iluminación deficiente, y a menudo exceden los presupuestos computacionales de los robots de Búsqueda y Rescate (SAR) con recursos limitados. El desafío central abordado es permitir que un robot determine, en tiempo real y sin visión ni conocimiento semántico previo, si un obstáculo desconocido puede ser desplazado para alcanzar un objetivo, y ajustar su plan de navegación en consecuencia dentro de un stack estándar de ROS 2.

Metodología
Los autores proponen un marco de mapa de costos adaptativo basado únicamente en LiDAR, integrado en el stack Nav2 de ROS 2. El sistema evita el preentrenamiento y los complejos pipelines de visión, basándose en lugar de ello en escaneos planares y computación a nivel de CPU. La arquitectura consta de dos plugins personalizados principales:

1. Capa de Obstáculos Móviles: Esta capa compara los escaneos LiDAR entrantes con un mapa estático SLAM preexistente. Los retornos que no coinciden con las estructuras estáticas conocidas se clasifican inicialmente como "móviles" y se les asigna un costo de trayectoria reducido ("ligero"). El sistema utiliza el etiquetado de componentes conectados para mantener la identidad del obstáculo incluso durante la oclusión parcial. Un filtro de distancia a la pared evita falsos positivos manteniendo como letales los obstáculos adyacentes a paredes estáticas. La capa admite una inflación suave ponderada, lo que permite al robot preferir la evasión pero intentar el empuje cuando no existe una ruta libre de colisiones.
2. Verificador de Progreso de Pose Lenta (Slow-Pose Progress Checker): Este módulo monitorea la relación entre la velocidad comandada y la velocidad real derivada de la odometría. Distingue entre maniobras lentas intencionales (por ejemplo, giros) y una obstrucción genuina.
   • Escalada de Costo: Si el robot se ralentiza significativamente mientras empuja, el costo del obstáculo más cercano se escala de "ligero" a "pesado". Si el robot se detiene por completo, el costo se escala a "letal".
   • Replanificación: Estas actualizaciones de costo activan al planificador global (NavFn) para retroceder y re-enrutar.
   • Ejecución: El planificador local utiliza el controlador de Integración de Trayectoria Predictiva de Modelo (MPPI), seleccionado por su capacidad para realizar maniobras complejas, incluyendo el movimiento hacia atrás, para reposicionar al robot para una interacción óptima con el obstáculo.

Contribuciones Clave

• Formulación NAMO Nativa de ROS 2: Un enfoque de Navegación entre Obstáculos Móviles (NAMO) que elimina la necesidad de visión o requisitos semánticos previos, diseñado para la integración directa con el stack Nav2.
• Plugin de Mapa de Costos Adaptativo: Una "Capa de Obstáculos Móviles" que etiqueta los objetos detectados por LiDAR con costos de trayectoria adaptativos, actualizándolos dinámicamente según la retroalimentación de la interacción.
• Bucle de Retroalimentación Basado en Velocidad: Un "Verificador de Progreso de Pose Lenta" personalizado que envía señales de tiempo real de "pesado" o "letal" a la capa del mapa de costos basada en el rendimiento del movimiento, permitiendo una respuesta graduada a la resistencia del obstáculo sin el abandono prematuro de objetos empujables.

Resultados Experimentales
El sistema fue evaluado en simulaciones de Gazebo utilizando un robot Scout Mini a través de tres escenarios: obstáculos aislados, pasillos con desorden de movilidad mixta y obstáculos de doble fila complejos.

• Tasas de Éxito: El sistema adaptativo logró tasas de alcance de objetivo significativamente más altas en comparación con un planificador de obstáculos estáticos base, el cual falló por completo en escenarios que requerían interacción con obstáculos (por ejemplo, 100% de éxito en escenarios de pasillo frente a "imposible" para el modelo base).
• Eficiencia: Los tiempos de trayectoria fueron generalmente mejores o comparables a los del modelo base. Por ejemplo, en el Escenario 1-a, el sistema adaptativo fue un 33% más rápido (27.79s vs. 41.71s). En casos que involucraban obstáculos inamovibles (1-c), el sistema aceptó tiempos de trayectoria más largos para evitar intentos de empuje fútiles.
• Precisión: La estimación de la movilidad fue altamente precisa para objetos ligeros (100%) e inamovibles (95%). La precisión disminuyó para obstáculos "pesados" (60–70%), resaltando la dificultad de distinguir objetos empujables pero difíciles bajo condiciones de aproximación variables.
• Limitaciones Observadas: El estudio señaló desafíos con obstáculos agrupados cerca de las paredes que causan una clasificación ambigua debido a la oclusión del LiDAR, y una deriva ocasional de la localización que provoca desalineaciones en el mapa de costos. El verificador basado en velocidad a veces tuvo dificultades para distinguir entre maniobras legítimas de baja velocidad y fallos de empuje.

Significancia y Reivindicaciones
El artículo afirma que los mapas de costos conscientes de la interacción proporcionan una extensión ligera y nativa de ROS 2 para navegar en entornos no estructurados. La significancia radica en su capacidad para permitir que los robots de SAR naveguen entre obstáculos potencialmente móviles sin depender de pipelines de visión frágiles o inferencia intensiva de GPU. Al depender únicamente de escaneos planares y stacks de navegación estándar, el método es adecuado para plataformas con recursos limitados y puede integrarse en sistemas heterogéneos con un mínimo de ingeniería. Los autores posicionan esto como un paso práctico hacia la robótica de respuesta ante desastres robusta, reconociendo que, aunque el método no es perfecto para todos los tipos de obstáculos (particularmente los pesados), ofrece una alternativa robusta y de bajo cómputo a las soluciones NAMO dependientes de la visión. La implementación completa se ha publicado como código abierto.