Optimal Localized Trajectory Planning of Multiple Non-holonomic Vehicles

We present a trajectory planning method for multiple vehicles to navigate a crowded environment, such as a gridlocked intersection or a small parking area. In these scenarios, decoupled path planning techniques may not produce feasible solutions and a joint planning method is necessary to allow the vehicles to reach their destinations. We use multiple Reeds-Shepp (RS) non-holonomic dynamic models, and combine them into a single higher-dimensional non-holonomic multi-vehicle system. We then search for a simultaneous set of vehicle trajectories, represented by a single choreography curve in the higher-dimensional space, using the RRT* graph-based search method. While variants of RRT are widely used, convergence is known only for the specific cases of holonomic systems and sub-Riemannian non-holonomic systems. We prove the algorithm's convergence in a more general setting and demonstrate the effectiveness of the approach through simulation and experimental studies on multiple robots. The proposed approach with guaranteed optimality can be used to locally resolve collisions among a small subset of vehicles in a large multi-vehicle trajectory planning problem in presence of obstacles.