Efficient Kinodynamic Multi-Robot Replanning in Known Workspaces

Abstract

In this work, we consider the problem of online centralized kinodynamic multi-robot replanning (from potentially non-stationary initial states) and coordination in known and cluttered workspaces. Offline state lattice reachability analysis is leveraged to decouple the planning problem into two sequential graph searches-one in the explicit geometric graph of the environment and the other in the graph of the higher-order derivatives of the robot's state-in a manner such that the intermediate vertices of a safe set of geometric paths are guaranteed to have a feasible assignment of higher-order derivatives. Without additional iterative refinement procedures, the resulting time parameterized polynomial trajectories are dynamically feasible and collision-free. Planning results with up to 20 robots in two and three dimensional workspaces suggest the suitability of the proposed approach for multi-robot replanning in known environments.