An Optimal Control Based Trajectory Planning Method for Cooperative Merging Maneuvers

Due to the prediction errors and diverse driving styles of surrounding vehicles, traffic congestion and even accidents can occur in the traffic bottleneck area where vehicles merge into the main road. To ensure that connected and automated vehicles (CAVs) can quickly and efficiently merge into the main road, this paper proposes a cooperative trajectory planning method based on optimal control for vehicle cut-in scenarios. A optimal control model is constructed, and a quintic polynomial is used to generate the initial solution of the optimal control problem to accelerate the solution process and improve the success rate. Finally, The obtained merging trajectory is then synchronized with the movement of vehicles on the main road to optimize their actions and ensure a safe merging process. Joint simulation with Robot Operating System (ROS) and RVIZ is utilized, and the results show that the proposed algorithm can quickly obtain cooperative results, the locally successful merging trajectory meets the requirements of smoothness and safety, and if the solution does not allow merging, the merging vehicle is instructed to terminate the execution.