A hybrid multi-stage optimization approach for multi-vehicle motion planning problems with lane changing

The multi-vehicle motion planning for lane change scenario is one of the frontiers and hotspots of intelligent transportation research. It is usually modeled as a complex and constrained optimal control problem, especially when there are obstacles on the lane, and the collision avoidance restrictions are thereby imposed on the vehicles. It brings great challenges to the optimization of the planning problem. In this work, we propose a multi-stage trajectory optimization framework to handle the constraints generated by the obstacles, the formulation of the optimal control problem is thereby simplified. The constrained optimal control problem is discretized to a nonlinear programming problem based on a hybrid method combining Gauss pseudospectral direct transcription and multiple shooting. On this basis, the first-order sensitivity information is further derived to accelerate the optimization process on nonlinear programming. The simulation results verify that the proposed strategy is advantageous over the classical method, and that the proposed approach is effective in collision avoidance.