Hold Or take Optimal Plan (HOOP): A quadratic programming approach to multi-robot trajectory generation

Abstract

In this work, we present Hold Or take Optimal Plan (HOOP), a centralized trajectory generation algorithm for labeled multi-robot systems operating in obstacle-free, two-dimensional, continuous workspaces. Given a team of N robots, each with nth-order dynamics, our algorithm finds trajectories that navigate vehicles from their start positions to non-interchangeable goal positions in a collision-free manner. The algorithm operates in two phases. In the motion planning step, a geometric algorithm finds a collision-free, piecewise-linear trajectory for each robot. In the trajectory generation step, each robot’s trajectory is refined into a higher-order piecewise polynomial with a quadratic program. The novelty of our method is in this problem decomposition. The motion plan, through abstracting away robots’ dynamics, can be found quickly. It is then subsequently leveraged to construct collision avoidance constraints for N decoupled quadratic programs instead of a single, coupled optimization problem, decreasing computation time. We prove that this method is safe, complete, and generates smooth trajectories that respect robots’ dynamics. We demonstrate the algorithm’s practicality through extensive quadrotor experiments.