Implan: Scalable Incremental Motion Planning for Multi-Robot Systems

We consider the collision-free motion planning problem for a group of robots using a library of motion primitives. To cope with the complexity of the problem, we introduce an incremental algorithm based on an SMT solver, where we divide the robots into small groups based on a priority assignment algorithm. The priority assignment algorithm assigns priorities to the robots in such a way that the robots do not block the cost-optimal trajectories of the other robots. While the priority assignment algorithm attempts to assign distinct priorities to the robots, the algorithm ends up with assigning the same priority to some robots due to the dependencies among themselves. The algorithm includes the robots with the same priority in the same group. Our incremental algorithm then considers the robot groups one by one based on their priority and synthesizes the trajectories for the group of robots together. While synthesizing the trajectories for the robots in one group, the algorithm considers the higher priority robots as dynamic obstacles, and introduces a minimal delay in executing the cost-optimal trajectories to avoid collision with the higher priority robots. We apply our method to synthesize trajectories for a group of quadrotors in our lab space. Experimental results show that we can synthesize trajectories for tens of robots with complex dynamics in a reasonable time.