Multi-robot navigation in formation via sequential convex programming

This paper presents a method for navigating a team of robots in formation in 2D and 3D environments with static and dynamic obstacles. The method is local and computes the optimal parameters for the formation within a neighborhood of the robots, allowing for reconfigurations, when required, by considering a set of target formations. The method consists of first computing the largest collision-free convex polytope in a neighborhood of the robots, followed by a constrained optimization via sequential convex programming where the optimal parameters for the formation are obtained. The robots navigate towards the target collision-free formation with individual local planners that account for their dynamics. The approach is efficient and scalable with the number of robots and performed well in simulations with a large team of quadrators and in experiments with two mobile manipulators carrying a rigid object.