Hierarchical Search-Based Cooperative Motion Planning

TL;DR

A leaderless, hierarchical Search-Based Cooperative Motion Planning (SCMP) method, which uses a binary conflict search tree to minimize runtime and can adapt to scenarios featuring multiple groups with different shapes, outlier agents, and elaborate obstacles.

Abstract

Cooperative path planning, a crucial aspect of multi-agent systems research, serves a variety of sectors, including military, agriculture, and industry. Many existing algorithms, however, come with certain limitations, such as simplified kinematic models and inadequate support for multiple group scenarios. Focusing on the planning problem associated with a nonholonomic Ackermann model for Unmanned Ground Vehicles (UGV), we propose a leaderless, hierarchical Search-Based Cooperative Motion Planning (SCMP) method. The high-level utilizes a binary conflict search tree to minimize runtime, while the low-level fabricates kinematically feasible, collision-free paths that are shape-constrained. Our algorithm can adapt to scenarios featuring multiple groups with different shapes, outlier agents, and elaborate obstacles. We conduct algorithm comparisons, performance testing, simulation, and real-world testing, verifying the effectiveness and applicability of our algorithm. The implementation of our method will be open-sourced at https://github.com/WYCUniverStar/SCMP.

Figures (6)

• Figure 1: Ackermann steering model.
• Figure 2: A pipeline of SCMP. The SCMP pipeline showcases cooperative search by green rectangular and blue triangular agent groups, alongside a purple outlier, with start/goal states marked by filled/dashed rectangles and obstacles in gray. Conflict detection leads to the expansion of two sub-nodes and constraint application on the blue group and replan with CSHA*.
• Figure 3: Two batches of CSHA* search. Here, we assume that agent A is the first-agent closest to the goal in the cooperative agent group, and the yellow dotted box is $\mathcal{R}$. (a) is the first-agent's search in the first batch. Since the distance between agents B & C and the corresponding $\mathcal{I}$ (blue box) is too large, the first-agent A can only choose the wait state (green box) from its seven child nodes to maintain the relative position and shape. (b) is the remaining agent's search in the first batch of the group, Agents B & C choose the node closest to $\mathcal{I}$ (green box) among the feasible statuses (although the status in the red box of agent B is closer, it is illegal). (c) is the search conducted by the first-agent in the second batch. At this time, since agents B & C are very close to the last $\mathcal{I}$ (blue box in (a)), agent A searches normally. (d) is the search of the remaining agent in the second batch of the group, still choosing the node closest to $\mathcal{I}$. After two batches of searches, CSHA* changes from the upper part to the lower part of (e). The relative position of the group is closer to given $\mathcal{R}$.
• Figure 4: Performance tests in 300 m $\times$ 300 m scenario
• Figure 5: Critical snapshots of simulation tests of ours and CL-CBSwenCLMAPFMultiAgentPath2022. The figures show obstacles as black areas consisting of dots and bars. In our method ((a) & (b)), 19 agents navigate a 300 m² area in two phases, forming two cooperative groups, each showing four distinct formations. In phase one, groups A (triangle) and B (pentagon) aim for top-left and top-right targets within 84 seconds, overcoming narrow paths and ring obstacles, respectively. The second phase extends the first, with a total time of 165 seconds, marked by shifts in group shapes and sizes. Group A morphs from a triangle to a diamond (now group C), reducing to 8 agents, and group B transforms to a flat diamond (group D), also with 8 agents, both avoiding circular obstacles. CL-CBS results((c) & (d)), without shape constraints, reveal agents' inability to collaborate, focusing solely on reaching endpoints.