Fast and Optimal Trajectory Planning for Multiple Vehicles in a Nonconvex and Cluttered Environment: Benchmarks, Methodology, and Experiments
Yakun Ouyang, Bai Li, Youmin Zhang, Tankut Acarman, Yuqing Guo, Tantan Zhang
Abstract
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.
BibTeX
@inproceedings{icra2022_fastandoptimaltr,
title = {Fast and Optimal Trajectory Planning for Multiple Vehicles in a Nonconvex and Cluttered Environment: Benchmarks, Methodology, and Experiments},
author = {Yakun Ouyang and Bai Li and Youmin Zhang and Tankut Acarman and Yuqing Guo and Tantan Zhang},
booktitle = {ICRA 2022},
year = {2022}
}