CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning

Abstract

This paper presents an efficient algorithm, naming Centralized Searching and Decentralized Optimization (CSDO), to find feasible solution for large-scale Multi-Vehicle Trajectory Planning (MVTP) problem. Due to the intractable growth of non-convex constraints with the number of agents, exploring various homotopy classes that imply different convex domains, is crucial for finding a feasible solution. However, existing methods struggle to explore various homotopy classes efficiently due to combining it with time-consuming, precise trajectory solution finding. CSDO, addresses this limitation by separating them into different levels and integrating an efficient Multi- Agent Path Finding (MAPF) algorithm to search homotopy classes. It first searches for a coarse initial guess using a large search step, identifying a specific homotopy class. Subsequent decentralized Sequential Quadratic Programming (SQP) refinement processes this guess, resolving minor collisions efficiently. Experimental results demonstrate that CSDO outperforms existing MVTP algorithms in large-scale, high-density scenarios, achieving up to a 95&#x0025; success rate in 50 m &#x00D7; 50 m random scenarios around one second. Source codes are released in <uri>https://github.com/YangSVM/CSDOTrajectoryPlanning</uri>.