A Coordinated Behavior Planning and Trajectory Planning Framework for Multi-UGVs in Unstructured Narrow Interaction Scenarios

Generating safe, smooth, and efficient trajectories is a fundamental and difficult task for multiple unmanned ground vehicles (MUGVs) in unstructured narrow interaction scenarios (UNIS). In this paper, a coordinated behavior planning and trajectory planning (BPTP) framework is proposed to deal with the MUGVs collision-prone problem in UNIS, which consists of interactive behavior planning layer and an optimization-based trajectory planning layer. The behavior planning layer is designed based on the mixed integer quadratic programming (MIQP) and breadth-first search (BFS) approach to generate collision-free trajectory clusters in the spatio-temporal state. The selected coarse trajectory fully considers the continuity of the trajectory, the non-collision and the driving efficiency between MUGVs. For trajectory planning, a spatio-temporal motion corridor (STMC) approach is proposed based on MIQP and optimization approach to model the constraints posed by complex unstructured environments in a unified way. Based on STMC, a safe and smooth trajectory is optimized, conforming to the decision provided by the behavior planner. Finally, the proposed BPTP framework is validated in both simulations and real-vehicle, and experimental results demonstrate that the proposed framework achieves human-like driving behavior in UNIS safely and smoothly compared to the existing planning methods.