Collision-free time-optimal path parameterization for multi-robot teams

TL;DR

The paper tackles collision-free time-optimal coordination of multiple car-like robots along fixed geometric paths in cluttered environments. It introduces TOPPCar, a time-parameterization method that uses a square-speed profile $h(s)=(ds/dt)^2$, a path-based dynamics formulation, and a priority-queue-driven strategy to account for inter-robot collisions within a spatiotemporal $(s,t)$ framework, while enforcing state-dependent actuation bounds. Key contributions include a single-agent TOPP formulation adapted to car-like dynamics, a systematic collision-occupancy construction in $(s,s)$ and $(s,t)$ spaces with rectangle-based over-approximations, and a practical multi-agent planning pipeline that yields $10-20\%$ reductions in makespan relative to state-of-the-art baselines, validated in simulation and hardware. The approach enables faster, safer coordination for robot teams in static obstacle-rich settings, while acknowledging the limitations imposed by the priority-queue homotopy choices and proposing future work on optimizing those choices and removing reliance on fixed homotopy classes.

Abstract

Coordinating the motion of multiple robots in cluttered environments remains a computationally challenging task. We study the problem of minimizing the execution time of a set of geometric paths by a team of robots with state-dependent actuation constraints. We propose a Time-Optimal Path Parameterization (TOPP) algorithm for multiple car-like agents, where the modulation of the timing of every robot along its assigned path is employed to ensure collision avoidance and dynamic feasibility. This is achieved through the use of a priority queue to determine the order of trajectory execution for each robot while taking into account all possible collisions with higher priority robots in a spatiotemporal graph. We show a 10-20% reduction in makespan against existing state-of-the-art methods and validate our approach through simulations and hardware experiments.

Figures (6)

• Figure 1: A set of collision-free time-optimal TOPPcar trajectories (red, blue, green) for a three-agent team tracked by RC cars in the a VICON motion capture space. The solid lines have already been traveled, while the dotted lines are still to be traversed.
• Figure 2: Diagram of the Car Model
• Figure 3: Depiction of the collision-avoidance constraint generation pipeline. a) Two discretized paths. (blue, orange) All points within a collision distance are marked (red. black) The blue trajectory has the higher priority. b) The points of collision plotted as collision obstacles (red) in ($s-s$) space. The blue trajectory is plotted along $s_1$, the horizontal axis, and the orange trajectory along $s_2$, the vertical axis. c) The points of collision plotted in ($s-t$) as collision obstacles (yellow) the orange trajectory must avoid. d) The unreachable spaces in ($s-t$) (teal), due to the monotonicity of time, completed for each obstacle. e) Rectangles overapproximated for each obstacle (yellow). f) TOPPcar trajectories computed with (dark blue) and without (white) collision constraints plotted in ($s-t$).
• Figure 4: Illustration of time constraints due to a collision obstacle in $s-t$ with a slight abuse of notation. Two possible trajectories (green, blue) in $s-t$ space are depicted that avoid the collision obstacle (grey).
• Figure 5: Obstacle environments for the Decentralized comparison. The starting (green) and ending (red) points of each trajectory are marked with colored dots. The paths are colorized by traversal time. Sample trajectories from three environments are depicted: A mostly-empty map with a) TOPPCar trajectories and b) Decentralized Planner trajectories. A structured lightly-cluttered map with c) TOPPCar trajectories. and d) Decentralized Planner trajectories. An unstructured densely-cluttered map with e) TOPPCar trajectories and f) Decentralized Planner trajectories