Coordinating Multiple Robots with Kinodynamic Constraints Along Specified Paths

Abstract

This paper focuses on the collision-free coordination of multiple robots with kinodynamic constraints along specified paths. We present an approach to generate continuous velocity profiles for multiple robots; these velocity profiles satisfy the dynamics constraints, avoid collisions, and minimize the completion time. The approach, which combines techniques from optimal control and mathematical programming, consists of identifying collision segments along each robot’s path, and then optimizing the robots’ velocities along the collision and collision-free segments. First, 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. The collision avoidance constraints for pairs of robots can then 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, which provide schedules that give lower and upper bounds on the optimum; the upper bound schedule is designed to provide continuous velocity trajectories that are feasible. The approach is illustrated with coordination of multiple robots, modeled as double integrators subject to velocity and acceleration constraints. An application to coordination of nonholonomic car-like robots is described, along with implementation results for 12 robots.