Motion planning for hundreds of floating robots

Technical Summary: Motion Planning for Hundreds of Floating Robots

Problem Statement
The paper addresses the challenge of planning collision-free, dynamically feasible trajectories for large fleets of omnidirectional floating robots (surface robots) operating on a planar manifold. Unlike aerial drones, which can utilize vertical maneuvers to deconflict dense transitions, surface robots are constrained to 2D, making coordination and collision avoidance intrinsically harder. The planning context is interactive show design, where a designer specifies sparse keyframes (start and end configurations) and requires near-immediate feedback (seconds) to generate trajectories that may span minutes and thousands of time steps.

The core difficulty lies in the scalability of collision avoidance. As fleet size ($N$) increases, the inter-agent coupling grows rapidly (often quadratically), leading to a number of pairwise collision constraints scaling as $\binom{N}{2}K$ (where $K$ is the number of time steps). For large $N$ (e.g., 500) and $K$ (e.g., 1000), standard Sequential Convex Programming (SCP) formulations result in Quadratic Programs (QP) with hundreds of millions of non-zero entries, rendering them computationally intractable for iterative design loops.

Methodology
The authors propose a scalable, hierarchical planning pipeline that decomposes the globally coupled problem into smaller, independent subproblems. The pipeline consists of four main stages:

1. Initialization and Allocation:

   • The process begins with a minimum-cost assignment problem to match robots to target positions. For "labelled" robots (those with fixed user-defined targets), the cost is zero or infinite to enforce specific matches. For "unlabelled" robots, the cost is the Euclidean distance, minimizing total displacement to reduce unnecessary swaps.
   • An initial trajectory is generated via linear interpolation between start and end positions. This initialization is generally not collision-free.

2. Collision Graph Construction:

   • A spatio-temporal graph is built where nodes represent robot-time pairs.
   • Spatial edges connect robots at the same timestep if they are within a safety radius $R$.
   • Temporal edges connect a robot to itself across consecutive timesteps if a collision occurs in either.
   • Connected components of this graph define "collision clusters." The authors utilize a KD-tree data structure to detect pairwise violations efficiently, achieving a speedup of nearly 2000× compared to naive pairwise checking.

3. Clustering and Decomposition:

   • To provide the optimizer freedom to plan detours, the temporal window of each cluster is expanded (buffered) by a hyperparameter $B$ (set to 3 seconds).
   • Overlapping buffered clusters are merged using a union-find procedure. To prevent the creation of subproblems that are too large (negating parallelization benefits), a complexity cap is enforced. If merging exceeds a threshold of non-zero entries in the QP matrices, the group is split using a scoring mechanism that minimizes the maximum complexity of the resulting subproblems.

4. Subproblem Solving and Merging:

   • Each cluster is solved independently and in parallel using an SCP framework.
   • Formulation Improvement: The paper introduces a "direct transcription" formulation where the decision vector includes both positions and velocities ($\chi = [p, v]$). This converts dynamics into local equality constraints and significantly improves the sparsity of the constraint matrix (reducing complexity from $O(N^2K^2)$ to $O(N^2K)$) compared to the "direct shooting" formulation used in prior work.
   • Robustness: Mechanisms include perturbing initialization for symmetric configurations, adaptively coarsening discretization if a solve is infeasible, and a lightweight polishing step.
   • Cycle Detection: The pipeline detects cyclic dependencies where merging clusters leads to oscillating collision patterns. Upon detection, conflicting subproblems are locked into a single joint subproblem for subsequent iterations.
   • Finally, local solutions are merged into a global trajectory, and the loop repeats until a collision-free solution is found.

Key Contributions

• Scalable Hierarchical Pipeline: A method that builds a collision graph, decomposes the problem into interaction clusters, and solves them in parallel, enabling planning for hundreds of robots in seconds.
• Robust Decomposition Mechanisms: Techniques for handling common pathologies, including fast graph updates, buffering strategies, complexity caps to prevent subproblem explosion, and cycle detection to prevent oscillation.
• Optimization Reformulation: A shift to a direct transcription formulation for cluster-level trajectory optimization, which improves QP sparsity and solver robustness, particularly for configurations with high fractions of labelled robots.

Results and Validation
The system was validated in simulations and two real-world deployments:

• Simulation: The pipeline successfully generated collision-free trajectories for fleets of up to 500 robots over 1000 time steps. In a case study with 500 robots, the solver achieved a 100% convergence rate for up to 30% labelled robots and 99% for higher percentages, with computation times typically under a few seconds.
• Real-World Deployment 1 (Lake Zürich): A fleet of 24 robots was choreographed for transitions spanning 1–2 minutes (400+ time steps). Designers could iteratively adjust keyframes and regenerate trajectories within seconds.
• Real-World Deployment 2 (Venice Biennale 2025): A fleet of 8 robots performed transitions spanning over 2000 time steps. A trajectory of 2491 steps was computed in 1.23 seconds.

Significance
The paper claims that this engineering-focused pipeline addresses the core scalability bottleneck of multi-robot motion planning on surfaces. By decomposing the problem and optimizing the solver formulation, the approach is orders of magnitude faster than traditional monolithic SCP pipelines. This speed enables a smooth, interactive user experience for choreography design, allowing designers to iteratively adjust sparse keyframes and receive immediate feedback on collision-free, dynamically feasible trajectories, a capability previously limited by computational constraints in 2D surface environments.