Recursive Constraint Manifold Subsearch for Multirobot Path Planning with Cooperative Tasks

Abstract

The Cooperative Path Planning (CPP) problem seeks to determine a path for a group of robots which form temporary teams to perform tasks. The multi-scale effects of simultaneously coordinating many robots distributed across the workspace while also tightly coordinating the members of teams increases the difficulty of planning. Previous research produced the Constraint Manifold Subsearch (CMS) algorithm that can find minimal length paths to the CPP problem. However, CMS as currently formulated cannot handle more general cost functions, such as minimizing energy expenditure, and cannot handle task schedules that require multiple input teams to merge to form a set of multiple output teams. Furthermore, as CMS must couple planning for all interacting teams, it does not scale well to very large environments. In this paper, we rederive the CMS algorithm using a task graph to reason about inter-team dependencies, allowing the use of more general cost functions and task schedules. We then introduce the recursive CMS (rCMS) algorithm that exploits the reformulation to split the CPP into independent subproblems, significantly reducing computational complexity. Simulation studies show that rCMS can solve substantially larger problems than CMS.