Hierarchical optimal planning and real-time tracking of parking trajectories based on risk field

In order to improve the safety, smoothness and robustness of the automatic parking trajectory for light commercial vehicles in dynamic environments, this paper designs a novel real-time parking trajectory planning and tracking control architecture. A parking risk field (PRF) is established to reflect the guiding role and constraints of the parking environment on the vehicle. Nonlinear Model Predictive Control (NMPC) incorporating the PRF is proposed to realize the trajectory smooth optimization and real-time dynamic obstacle avoidance while tracking the global trajectory. Firstly, in the global parking trajectory planning layer, the initial trajectory is generated based on hybrid A*, path quadratic smoothing and speed planning algorithms. The parking problem is then formulated as an optimization problem, with the global trajectory planned based on the initial one. Secondly, in the real-time tracking control layer, considering the influence of trajectory guidance, drivable area modeling and dynamic obstacle avoidance, the PRF is constructed, comprising the trajectory attraction field, parking boundary field and vehicle repulsion field. Based on the PRF, the NMPC cost function is formulated. Additionally, vehicle ellipse constraints are designed and work with the boundary field to confine the vehicle within the parking space. Finally, the proposed optimal planning and dynamic tracking method based on the PRF is verified through simulation and real vehicle experiments. Both simulation and experiment results demonstrate that the designed trajectory planning and control architecture can enable the vehicle to park safely, smoothly and accurately in the parking space, while avoiding dynamic obstacles in real time through NMPC.