This paper is focused on the cooperative trajectory planning problem for multiple car-like robots in a cluttered and unstructured environment narrowed by static obstacles. The concerned multi-vehicle trajectory planning (MVTP) problem is challenging because i) the scenario is nonconvex and tiny; ii) the vehicle kinematics is nonconvex; and iii) a feasible homotopy class is unavailable a priori. We propose a two-stage MVTP method: Stage 1 identifies a feasible homotopy class, and Stage 2 quickly finds a local optimum based on the identified homotopy class. Numerical optimal control, adaptive scaling, grouping, and trust region construction strategies are integrated into the proposed planner. Our planner is extensively compared in 100 benchmark cases with the state-of-the-art MVTP methods such as incremental sequential convex programming, numerical optimal control, conflict-based search, priority-based trajectory optimizer, and optimal reciprocal collision avoidance. The simulation results demonstrate our method's superiority in runtime and optimality. Experiments with three car-like robots demonstrate the efficiency of our proposed planner. Source codes are in https://github.com/libai1943/MVTP_benchmark.

Road shoulders and slopes often appear in unstructured environments. They make 2.5D vehicle trajectory planning commonly seen in our daily life, which lies on a 2D manifold embedded in a 3D space. The height difference of these terrains brings spatially dependent constraints on vehicle maneuvers, such as the limit on vehicle steering for vehicle tire protection when a vehicle approaches a road shoulder edge. These constraints have an “if-else” structure since they are activated only when the vehicle passes through the local area with a height difference, making the restriction on variables coupled with the judgment of variables. This makes the application of state-of-art optimization-based planners challenging. To solve this problem, we devise an approximation formulation for these constraints in the trajectory planning optimization problem, whose solution depends on a proper initial guess for the optimizer. We propose a two-stage trajectory planning framework, where the first stage improves the hybrid A* algorithm by adding spatially dependent constraints into node expansion to provide the initial guess. Then, the optimization problem with the formulated spatially dependent constraints is solved for further trajectory smoothness and quality. Finally, the simulation results validate the fast and high-quality planning performance of our proposed framework.