Decoupled multiagent path planning via incremental sequential convex programming

This paper presents a multiagent path planning algorithm based on sequential convex programming (SCP) that finds locally optimal trajectories. Previous work using SCP efficiently computes motion plans in convex spaces with no static obstacles. In many scenarios where the spaces are non-convex, previous SCP-based algorithms failed to find feasible solutions because the convex approximation of collision constraints leads to forming a sequence of infeasible optimization problems. This paper addresses this problem by tightening collision constraints incrementally, thus forming a sequence of more relaxed, feasible intermediate optimization problems. We show that the proposed algorithm increases the probability of finding feasible trajectories by 33% for teams of more than three vehicles in non-convex environments. Further, we show that decoupling the multiagent optimization problem to a number of single-agent optimization problems leads to significant improvement in computational tractability. We develop a decoupled implementation of the proposed algorithm, abbreviated dec-iSCP. We show that dec-iSCP runs 14% faster and finds feasible trajectories with higher probability than a decoupled implementation of previous SCP-based algorithms. The proposed algorithm is real-time implementable and is validated through hardware experiments on a team of quadrotors.