Formation-Aware Planning and Navigation with Corridor Shortest Path Maps

The need to plan motions for agents with variable shape constraints such as under different formations appears in several virtual and real-world applications of autonomous agents. In this work, we focus on planning and execution of formation-aware paths for a group of agents traversing a cluttered environment. The proposed planning framework addresses the trade-off between being able to enforce a preferable formation when traversing the corridors of the environment, versus accepting to switch to alternative formations requiring less clearance in order to utilize narrower corridors that can lead to a shorter overall path to the final destination. At the planning stage, this trade-off is addressed with a multi-layer graph annotated with per-layer navigation costs and formation transition costs, where each layer represents one formation together with its specific clearance requirement. At the navigation stage, we introduce Corridor Shortest Path Maps (CSPMs), which produce a vector field for guiding agents along the solution corridor, ensuring unobstructed in-formation navigation in cluttered environments, as well as group motion along lengthwise-optimal paths in the solution corridor. We also present examples of how our multi-layer planning framework can be applied to other types of multi-modal planning problems.