DiMOpt: a distributed multi-robot trajectory optimization algorithm

This paper deals with Multi-robot Trajectory Planning, that is, the problem of computing trajectories for multiple robots navigating in a shared space while minimizing for control energy. Approaches based on trajectory optimization can solve this problem optimally. However, such methods are hampered by complex robot dynamics and collision constraints that couple robot’s decision variables. We propose a distributed multi-robot optimization algorithm (DiMOpt) that addresses these issues by exploiting (1) consensus optimization strategies to tackle coupling collision constraints, and (2) a single-robot sequential convex programming method for efficiently handling non-convexities introduced by dynamics. We compare DiMOpt with a baseline centralized multi-robot sequential convex programming algorithm (SCP). We empirically demonstrate that DiMOpt scales well for large fleets of robots while computing solutions faster and with lower costs. Finally, DiMOpt is an iterative algorithm that finds feasible trajectories before converging to a locally optimal solution, and results suggest the quality of such fast initial solutions is comparable to a converged solution computed via SCP.