Coordinating the motions of multiple robots with kinodynamic constraints

This paper focuses on the coordination of multiple robots with kinodynamic constraints along specified paths. The presented approach generates continuous velocity profiles that avoid collisions and minimize the completion time for the robots. The approach identifies collision segments along each robot's path and then optimizes the motions of the robots along their collision and collision-free segments. For each path segment for each robot, the minimum and maximum possible traversal times that satisfy the dynamics constraints are computed by solving the corresponding two-point boundary value problems. Then the collision avoidance constraints for pairs of robots can be combined to formulate a mixed integer nonlinear programming (MINLP) problem. Since this nonconvex MINLP model is difficult to solve, we describe two related mixed integer linear programming (MILP) formulations that provide schedules that are lower and upper bounds on the optimum; the upper bound schedule is a continuous velocity schedule. The approach is illustrated with robots modeled as double integrators subject to velocity and acceleration constraints. An implementation that coordinates 12 nonholonomic car-like robots is described.