Optimal Multilayered Motion Planning for Multiple Differential Drive Mobile Robots with Hierarchical Prioritization (OM-MP)

TL;DR

This work addresses multi-robot trajectory planning in dynamic environments by decomposing MAPF into MATP and introducing a two-layer OM-MP framework. A global planner using Augmented Graph Search with NURBS and a local planner using Control Barrier Functions and oblique space-time cylinders enables fast, safe, and scalable planning with hierarchical prioritization and optional replanning. Key contributions include the multilayer architecture, hierarchical local prioritization, 3D space-time obstacle bounding, safe corridors via AGS, and a replanning mechanism to approach global optimality. The approach yields real-time feasible trajectories for large fleets, outperforming CBS/ECBS in capacity, scalability, and overall task optimality in simulations and experiments.

Abstract

We present a novel framework for addressing the challenges of multi-Agent planning and formation control within intricate and dynamic environments. This framework transforms the Multi-Agent Path Finding (MAPF) problem into a Multi-Agent Trajectory Planning (MATP) problem. Unlike traditional MAPF solutions, our multilayer optimization scheme consists of a global planner optimization solver, which is dedicated to determining concise global paths for each individual robot, and a local planner with an embedded optimization solver aimed at ensuring the feasibility of local robot trajectories. By implementing a hierarchical prioritization strategy, we enhance robots' efficiency and approximate the global optimal solution. Specifically, within the global planner, we employ the Augmented Graph Search (AGS) algorithm, which significantly improves the speed of solutions. Meanwhile, within the local planner optimization solver, we utilize Control Barrier functions (CBFs) and introduced an oblique cylindrical obstacle bounding box based on the time axis for obstacle avoidance and construct a single-robot locally aware-communication circle to ensure the simplicity, speed, and accuracy of locally optimized solutions. Additionally, we integrate the weight and priority of path traces to prevent deadlocks in limiting scenarios. Compared to the other state-of-the-art methods, including CBS, ECBS and other derivative algorithms, our proposed method demonstrates superior performance in terms of capacity, flexible scalability and overall task optimality in theory, as validated through simulations and experiments.

Figures (13)

• Figure 1: Interactions between different robots in radius and communication with host and others.
• Figure 2: Illustration of pose and control variables of the robot.
• Figure 3: Schematic illustration of a trajectory covered by bubbles bounding box that represent a local free space corridor in (a), (b) and (c), (d) is the mapping distribution of obstacle and planning trajectory point weights coefficients in 2D.
• Figure 4: The overview of this framework. Where the hollow serial numbers depict the sequential passage of signals from the global planner to the execution terminal, while solid serial numbers signify the sequential feedback of signals from the terminal back to the planner. And the solid and dotted lines indicate the wired and wireless transmission of the signal, respectively.
• Figure 5: Illustration of the priority order, the orange arrows stand for the trajectory points of higher priority robots, which are considered as dynamic obstacles by lower priority ones.

Theorems & Definitions (3)

• Definition 1
• Definition 2
• Definition 3