Automatic Parking Trajectory Planning Based on Warm Start Nonlinear Dynamic Optimization

In this paper, we propose an optimal parking path planning method based on numerical solving, which leverages the concept of the distance between convex sets. The obstacle avoidance constraints were transformed into continuous, smooth nonlinear constraints using the Lagrange dual function. This approach enables the determination of a globally optimal parking path while satisfying vehicular kinematic constraints. To address the inefficiency typically associated with numerical solving, a warm start strategy was employed for the optimization variables: first, the Hybrid A* algorithm was utilized to generate the initial path values; next, a velocity planning problem was formulated to obtain initial velocity values; and finally, converted convex optimization problems were used to compute the initial dual variables. The optimality of the proposed method was validated through a real car test with ACADO as a solver in three typical parking scenarios. The results demonstrate that the proposed method achieved smoother parking paths in real time.