Graphs of Space-Time Convex Sets (ST-GCS)

Best-First Search (BFS) Solver on ST-GCS

To solve spatiotemporal motion planning on ST-GCS efficiently, we develop a BFS solver over path-indexed partial trajectories. Unlike ordinary graph search, a node is indexed by its full prefix path, because extending the path can change the optimal continuous states along earlier segments. Each node stores its prefix path, optimized trajectory, and an ordering key summing up the cost-to-come and a possibly inflated heuristic. The heuristic is an admissible estimate of the remaining cost to reach the target. When a node is expanded, the solver extends it to adjacent unrevisited convex sets and calls PathOptimize, which fixes the discrete path in the above program for ST-GCS and turns the mixed-integer convex program into a path-conditioned convex optimization over entry/exit states and segment times. Feasible children are inserted into OPEN unless upper-bound pruning removes them using the incumbent cost, or a dominance check prunes them against accepted nodes reaching the same convex set. The left animation visualizes this search process, while the right animation shows the optimized trajectory returned on the ST-GCS.

Exact Convex Decomposition (ECD)

ECD reserves the space-time occupancy of a moving object directly in ST-GCS of any dimension. Given a piecewise-linear trajectory and a clearance radius, ECD removes the swept occupancy from every intersected space-time convex set, slices the remaining free space into convex pieces, and rebuilds the graph connectivity among the updated sets. This produces a new ST-GCS whose vertices still represent space-time collision-free convex regions, but now exclude the reserved trajectory. As a result, dynamic obstacles and previously planned robots can be handled in the same way: later planning queries simply search through the remaining collision-free space-time. The following animations show an example in 2D Space.

Prioritized Planning Frameworks for Multi-Robot Motion Planning (MRMP)

For MRMP, we use the BFS solver on ST-GCS as the low-level single-robot planner and prioritized planning as the high-level coordination framework. After a robot trajectory is planned, ECD reserves its endpoint-augmented space-time occupancy so later low-level queries search only through the remaining collision-free ST-GCS. To improve scalability, we apply this coordination in a receding-horizon manner: each window plans with near-term reservations, commits a short execution prefix, and then replans from the updated robot states. This is the Windowed-PBS + BFS framework shown in the animation below.

Performance Comparison

We conduct extensive experiments comparing MRMP performance across representative instances from the three benchmark domains: rand, maze, and iris. Each planner is run with a 30-second runtime budget, and each video reports sum-of-costs (SoC), makespan, and runtime. These examples highlight domain-dependent tradeoffs across randomly connected convex regions, maze-like bottlenecks, and IRIS-generated decompositions. In iris, the convex sets only partially cover the full collision-free space, so ST-GCS-based planners may produce higher-cost trajectories than planners searching directly in continuous free space.

The following animations compare qualitative comparison in representative instances. Each planner is run with a 30-second runtime budget.

rand domain 10-Robot MRMP instance

maze domain 10-Robot MRMP instance

iris domain 16-Robot MRMP instance

Large-Scale Demonstration with Trajectory Optimization Postprocessing

The following simulations show the proposed Windowed-PBS + BFS scaling to highly congested MRMP instances with tens to one hundred robots. After computing feasible piecewise-linear trajectories, we optionally apply trajectory optimization as a smoothing postprocessing step using fixed-time spatial samples. The resulting convex quadratic program improves smoothness while preserving start/goal states, velocity limits, assigned collision-free regions, and sampled robot-robot separation.

48-UAV MRMP in Village Scene: Windowed-PBS + BFS (225.0s); Trajectory Optimization Postprocessing (163.2s)

Real Robots Deployment

We deploy the same planning pipeline on nine differential-drive mobile robots in an indoor rearrangement task. For system robustness, we set velocity limits below the robots’ physical limits and use an inter-robot clearance larger than twice the robot radius. The smoothed trajectories are converted into time-indexed references, which the robots track using segment-wise linear and angular velocities between fixed-duration state samples.

BibTeX Citation