Motion planning for hundreds of floating robots

Resumen Técnico: Planificación de Movimiento para Cientos de Robots Flotantes

Planteamiento del Problema
El artículo aborda el desafío de planificar trayectorias libres de colisiones y dinámicamente factibles para grandes flotas de robots omnidireccionales flotantes (robots de superficie) que operan en un plano de dos dimensiones. A diferencia de los drones aéreos, que pueden utilizar maniobras verticales para evitar conflictos en transiciones densas, los robots de superficie están limitados a 2D, lo que hace que la coordinación y la evitación de colisiones sean intrínsecamente más difíciles. El contexto de planificación es el diseño de espectáculos interactivos, donde un diseñador especifica fotogramas clave dispersos (configuraciones de inicio y fin) y requiere una retroalimentación casi inmediata (segundos) para generar trayectorias que pueden durar minutos y miles de pasos temporales.

La dificultad central reside en la escalabilidad de la evitación de colisiones. A medida que el tamaño de la flota ($N$) aumenta, el acoplamiento entre agentes crece rápidamente (a menudo de forma cuadrática), lo que genera un número de restricciones de colisión por pares que escala como $\binom{N}{2}K$ (donde $K$ es el número de pasos temporales). Para valores grandes de $N$ (por ejemplo, 500) y $K$ (por ejemplo, 1000), las formulaciones estándar de Programación Convexa Secuencial (SCP) resultan en Programas Cuadráticos (QP) con cientos de millones de entradas no nulas, lo que las vuelve computacionalmente intratables para los ciclos de diseño iterativos.

Metodología
Los autores proponen un flujo de trabajo de planificación jerárquico y escalable que descompone el problema globalmente acoplado en subproblemas más pequeños e independientes. El flujo consta de cuatro etapas principales:

1. Inicialización y Asignación:

   • El proceso comienza con un problema de asignación de costo mínimo para emparejar robots con posiciones objetivo. Para los robots "etiquetados" (aquellos con objetivos definidos por el usuario), el costo es cero o infinito para imponer coincidencias específicas. Para los robots "no etiquetados", el costo es la distancia euclidiana, minimizando el desplazamiento total para reducir intercambios innecesarios.
   • Se genera una trayectoria inicial mediante interpolación lineal entre las posiciones de inicio y fin. Esta inicialización generalmente no está libre de colisiones.

2. Construcción del Grafo de Colisiones:

   • Se construye un grafo espacio-temporal donde los nodos representan pares robot-tiempo.
   • Las aristas espaciales conectan robots en el mismo paso de tiempo si se encuentran dentro de un radio de seguridad $R$.
   • Las aristas temporales conectan a un robot consigo mismo a través de pasos de tiempo consecutivos si ocurre una colisión en cualquiera de ellos.
   • Los componentes conectados de este grafo definen "clústeres de colisión". Los autores utilizan una estructura de datos KD-tree para detectar violaciones por pares de manera eficiente, logrando una aceleración de casi 2000× en comparación con la comprobación por pares ingenua.

3. Agrupamiento (Clustering) y Descomposición:

   • Para proporcionar al optimizador la libertad de planificar desvíos, la ventana temporal de cada clúster se expande (se amortiza) mediante un hiperparámetro $B$ (establecido en 3 segundos).
   • Los clústeres amortizados que se traslapan se fusionan utilizando un procedimiento de unión-conjunto (union-find). Para evitar la creación de subproblemas demasiado grandes (lo que anularía los beneficios de la paralelización), se impone un límite de complejidad. Si la fusión excede un umbral de entradas no nulas en las matrices del QP, el grupo se divide mediante un mecanismo de puntuación que minimiza la complejidad máxima de los subproblemas resultantes.

4. Resolución de Subproblemas y Fusión:

   • Cada clúster se resuelve de forma independiente y en paralelo utilizando un marco de trabajo SCP.
   • Mejora de la Formulación: El artículo introduce una formulación de "transcripción directa" donde el vector de decisión incluye tanto posiciones como velocidades ($\chi = [p, v]$). Esto convierte la dinámica en restricciones de igualdad locales y mejora significativamente la dispersión de la matriz de restricciones (reduciendo la complejidad de $O(N^2K^2)$ a $O(N^2K)$) en comparación con la formulación de "disparo directo" (direct shooting) utilizada en trabajos previos.
   • Robustez: Los mecanismos incluyen la perturbación de la inicialización para configuraciones simétricas, el refinamiento adaptativo de la discretización si una resolución es infactible, y un paso de pulido ligero.
   • Detección de Ciclos: El flujo de trabajo detecta dependencias cíclicas donde la fusión de clústeres conduce a patrones de colisión oscilantes. Tras la detección, los subproblemas en conflicto se bloquean en un único subproblema conjunto para las iteraciones posteriores.
   • Finalmente, las soluciones locales se fusionan en una trayectoria global, y el bucle se repite hasta encontrar una solución libre de colisiones.

Contribuciones Clave

• Flujo de Trabajo Jerárquico Escalable: Un método que construye un grafo de colisiones, descompone el problema en clústeres de interacción y los resuelve en paralelo, permitiendo la planificación de cientos de robots en segundos.
• Mecanismos de Descomposición Robustos: Técnicas para manejar patologías comunes, incluyendo actualizaciones rápidas de grafos, estrategias de amortiguación (buffering), límites de complejidad para evitar la explosión de subproblemas y detección de ciclos para prevenir la oscilación.
• Reformulación de la Optimización: Un cambio hacia una formulación de transcripción directa para la optimización de trayectorias a nivel de clúster, lo que mejora la dispersión del QP y la robustez del solver, particularmente para configuraciones con altas fracciones de robots etiquetados.

Resultados y Validación
El sistema fue validado en simulaciones y en dos despliegues en el mundo real:

• Simulación: El flujo de trabajo generó con éxito trayectorias libres de colisiones para flotas de hasta 500 robots durante 1000 pasos temporales. En un caso de estudio con 500 robots, el solver logró una tasa de convergencia del 100% para hasta un 30% de robots etiquetados y del 99% para porcentajes superiores, con tiempos de computación típicamente inferiores a unos pocos segundos.
• Despliegue Real 1 (Lago de Zúrich): Una flota de 24 robots fue coreografiada para transiciones que duraron entre 1 y 2 minutos (más de 400 pasos temporales). Los diseñadores pudieron ajustar iterativamente los fotogramas clave y regenerar las trayectorias en cuestión de segundos.
• Despliegue Real 2 (Bienal de Venecia 2025): Una flota de 8 robots realizó transiciones que abarcaron más de 2000 pasos temporales. Una trayectoria de 2491 pasos fue computada en 1.23 segundos.

Significado
El artículo sostiene que este flujo de trabajo enfocado en la ingeniería aborda el cuello de botella fundamental de la escalabilidad en la planificación de movimiento multi-robot en superficies. Al descomponer el problema y optimizar la formulación del solver, el enfoque es órdenes de magnitud más rápido que los flujos de trabajo tradicionales de SCP monolíticos. Esta velocidad permite una experiencia de usuario fluida e interactiva para el diseño de coreografías, permitiendo a los diseñadores ajustar iterativamente fotogramas clave dispersos y recibir retroalimentación inmediata sobre trayectorias libres de colisiones y dinámicamente factibles, una capacidad previamente limitada por las restricciones computacionales en entornos de superficie 2D.