Efficient Kinodynamic Multi-Robot Replanning in Known Workspaces

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.