Fast and Optimal Trajectory Planning for Multiple Vehicles in a Nonconvex and Cluttered Environment: Benchmarks, Methodology, and Experiments

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.