DSMPC-DCBF: A Hierarchical Parking Trajectory Optimization Method Based on MPC

Planning the trajectory of self-driving vehicles in complex parking scenarios is challenging, and parking needs to consider safety, efficiency and accuracy. Due to the expression of obstacle constraints, the speed of the iterative method will have a complex impact on the final optimized trajectory, so it is not easy to plan a reliable trajectory. To address the issues of traditional trajectory optimization methods being unable to balance feasibility and safety, as well as imprecise environmental modeling, this paper proposes the Discrete-time Control Barrier Function (DCBF) obstacle avoidance function based on accurate environment modeling and the parking trajectory optimization algorithm based on Model Predictive Control-Discrete-time Control Barrier Function with a Slack Variable (SMPC-DCBF). In order to further accelerate the solution of trajectory optimization algorithm, this paper proposes a hierarchical parking optimization algorithm, that is, SMPC-DCBF with double warm start (DSMPC-DCBF), such as we can further accelerate the solution of trajectory optimization algorithm.