TSCDO: A lightweight and optimal trajectory planning algorithm for multiple vehicles in the cluttered warehouse scenario

This paper focuses on lightweight and optimal multi-vehicle trajectory planning (MVTP) problems when vehicles travel in cluttered warehouses with known static obstacles. We formulate this problem as an optimal control problem (OCP) and design a three-stage complete decoupling optimization (TSCDO) algorithm to solve it. In stage 1, the generalized Voronoi graph (GVG) method is used to initialize the map, and an X-Y-T AA* algorithm is proposed to improve the time efficiency of the initial guess generation process. In stage 2, an exclusive 3D collision-free tunnel is built for each vehicle along the initial guess, which simplifies the coupled and intractably scaled collision-avoidance constraints to in-tunnel constraints that are small-scale and independent of environmental complexity. In stage 3, the nominally coupled OCP is decoupled into multiple simple sub-OCPs, and the nonlinear constraints are transformed into external penalty functions. Meanwhile, those sub-OCPs with pure box constraints are addressed in parallel to obtain the optimal trajectories in this stage. Compared to most existing methods, the proposed algorithm demonstrates a relatively effective balance between solution optimality and computational burden. And its effectiveness and efficiency are validated through simulations and experimental results.