1 Introduction

With the acceleration of urbanization and the increasing number of car ownership, while bringing convenience to transportation, the issue of “difficult parking” has become increasingly prominent (Tang et al., 2019). As a key part of autonomous driving technologies, automatic parking technology can not only effectively solve the occurrence of collisions, scrapes, and other accidents caused by lack of experience or technology of drivers, improve parking safety, but also alleviate the stress of drivers when parking, significantly enhancing parking efficiency and driving comfort (Li et al., 2020). Therefore, it is of great significance to conduct in-depth research on automatic path planning and path tracking technology.

Parking path planning methods are primarily categorized into four groups: geometric curve methods (Chen et al., 2023), graph search methods (Cao et al., 2023), random sampling methods (Zhang et al., 2021), and intelligent optimization methods (Su et al., 2019). In the geometric curve method, Helene et al. (2012) analyzed narrow parking spaces based on the clothoid curve and designed parking paths suitable for narrow parking spaces. However, the design of the clothoid curve was relatively complex requiring a large amount of computation. In the graph search method, Ren et al. (2022) proposed an automatic parking path planning method based on the Hybrid A* algorithm and RS curves. They optimized the path using Bezier curves and gradient descent. The results demonstrated that this method had improved path search capability and flexibility in complex scenarios. Although the method solved the problem of discontinuous curvature, the planned path was not smooth enough and the tracking effect was poor. In the random sampling method, Zhang et al. (2022) proposed a set of principles for selecting reference points and updating path nodes that conformed to the kinematic constraints of vehicles, improving the Rapidly Exploring Random Tree algorithm. The results showed that the proposed method was able to safely guide a car to complete parking tasks. However, the quality of the paths planned by this method was poor, and they were difficult to track. In the intelligent optimization method, Chen et al. (2021) proposed a staged training method based on curriculum learning using the coordinates of the center of the rear axle and the front wheel steering angle as state variables, achieved a planning success rate of 90.6% while satisfying comfort based on deep reinforcement learning. However, it required a large amount of data and had high real-time requirements.

Currently, most research on path planning for automatic parking can satisfy the curvature constraints and obstacle avoidance constraints during the parking process. However, the planned paths are complex, which is not conducive to real-time tracking control.

Common parking path tracking methods include pure pursuit (PP) algorithm (Shin et al., 2022), fuzzy control (Wang et al., 2022), and model predictive control (MPC) algorithm (Liu, 2022). In the pure pursuit algorithm, Erno et al. (2019) proposed an improved pure pursuit algorithm. By selecting multiple target points, they adjusted the forward distance based on curvature and lateral deviation, realizing dynamic optimization of the preview distance. Experimental verification showed good feasibility in practical applications and better tracking performance. In the pure fuzzy control, Naitik et al. (2018) proposed a parallel parking method based on hybrid fuzzy inference, which considered various parking scenarios, including sudden obstacle appearance during parking, the experimental results showed that the method could successfully complete parking in different parking scenarios, but its tracking accuracy was relatively poor. In the model predictive control algorithm, Yu et al. (2023) established a parking path tracking control method based on model predictive control, which tracked paths consisting of clothoid curves, arcs, and straight lines. The results indicated that the method had high tracking accuracy, with a maximum tracking error of 0.023 m.

The simulated annealing algorithm, which boasts high accuracy, strong universality, and flexibility in application, is widely utilized in research. Zhao et al., (2022a, 2022b) optimized the designed path planning method for moving targets through the simulated annealing algorithm. The results showed that the designed method was able to generate multi-UAV collaborative verification paths for moving ships that met time constraints. Zhao et al., (2022a, 2022b) proposed an artificial potential field method optimized by the simulated annealing algorithm. The results showed that this method enabled the mobile robot to escape from local minima positions, successfully reaching the target position with shorter time and greater stability.

To reduce the complexity of path planning, ensure the continuity of path curvature, and enhance parking efficiency, this paper proposes a parallel parking path planning method based on a hybrid superimposed curve of the quintic polynomial curve and improved sigmoid function. The contributions of this paper are threefold:

  1. (1)

    The method proposed in this paper effectively addresses the issue of sudden changes in curvature that occur in conventional parallel parking path planning methods.

  2. (2)

    The method utilizes a simulated annealing algorithm to optimize the maximum curvature and the curvature at both endpoints of the parking path, satisfying the maximum curvature constraint and solving the problem of the vehicle not being parallel to the parking space at the end of the parking maneuver.

  3. (3)

    The path planned by the method proposed in this paper is easy to track, and its lateral tracking error is small, which has been verified by an optimized MPC algorithm.

In this paper, first, a vehicle kinematic model is established. Second, considering the position, front wheel angle, and yaw angle constraints during the parking process, a hybrid superimposed curve is designed. The parking path planning problem is converted into an optimal control problem, with the maximum curvature and curvature at both ends as objective functions, and the parameters are optimized using the simulated annealing algorithm. Finally, virtual simulation experiments and real-vehicle tests are conducted using the MPC algorithm optimized with the simulated annealing algorithm, verifying the effectiveness of the proposed path planning method.

2 Model Establishment and Vehicle Constraints

2.1 The Establishment of Vehicle Kinematics Model

During parking, vehicle speed is low, and side slip caused by tire lateral force can be ignored. The tires only perform pure rolling. According to the Ackerman steering principle, the entire vehicle is modeled as a rigid body, and a kinematic model of the vehicle is established, as shown in Fig. 1.

Fig. 1
figure 1

Kinematic model of vehicle

Full size image

Here, (xf, yf) and (xr, yr) are the coordinates of the center points of the vehicle’s front and rear axles; vf and vr are the speeds of the vehicle’s front and rear axles centers; lf, l, and lr are the lengths of the vehicle’s front suspension, wheelbase, and rear suspension, respectively; θ is the vehicle’s yaw angle; δ is the vehicle’s front wheel steering angle; a, b, c, and d are the four vertices of the vehicle’s outline. Taking the center of the vehicle’s rear axle as the reference point, a differential equation for the vehicle’s kinematics is established:

$$\left\{ \begin{gathered} \dot{x} = v_{{\text{r}}} \cdot \cos \theta \hfill \\ \dot{y} = v_{{\text{r}}} \cdot \sin \theta \hfill \\ \dot{\theta } = v_{{\text{r}}} \cdot \tan \delta /l \hfill \\ \end{gathered} \right.$$
(1)

Based on geometric relationships and the location of the vehicle’s rear axle center point, the coordinates of the four vertices of the vehicle’s outline can be calculated:

$$\left\{ \begin{gathered} x_{{\text{a}}} = x_{{\text{r}}} { + }\left( {l + l_{{\text{f}}} } \right)\cos \theta - \frac{W\sin \theta }{2} \hfill \\ y_{{\text{a}}} = y_{{\text{r}}} { + }\left( {l + l_{{\text{f}}} } \right)\sin \theta + \frac{W\cos \theta }{2} \hfill \\ x_{{\text{b}}} = x_{{\text{r}}} { - }l_{{\text{r}}} \cos \theta - \frac{W\sin \theta }{2} \hfill \\ y_{{\text{b}}} = y_{{\text{r}}} { - }l_{{\text{r}}} \sin \theta + \frac{W\cos \theta }{2} \hfill \\ x_{{\text{c}}} = x_{{\text{r}}} { - }l_{{\text{r}}} \cos \theta + \frac{W\sin \theta }{2} \hfill \\ y_{{\text{c}}} = y_{{\text{r}}} { - }l_{{\text{r}}} \sin \theta - \frac{W\cos \theta }{2} \hfill \\ x_{{\text{d}}} = x_{{\text{r}}} { + }\left( {l + l_{{\text{f}}} } \right)\cos \theta + \frac{W\sin \theta }{2} \hfill \\ y_{{\text{d}}} = y_{{\text{r}}} { + }\left( {l + l_{{\text{f}}} } \right)\sin \theta - \frac{W\cos \theta }{2} \hfill \\ \end{gathered} \right.$$
(2)

Here, W is the vehicle width.

2.2 Vehicle Self-Constraints

During the automatic parking process, to ensure the traceability of the parking path, it is necessary to meet the corresponding constraints of the vehicle itself:

$$\left\{ \begin{gathered} \left| v \right| \le v_{\max } \hfill \\ \left| a \right| \le a_{\max } \hfill \\ \left| j \right| \le j_{\max } \hfill \\ \left| \delta \right| \le \delta_{\max } \hfill \\ \left| w \right| \le w_{\max } \hfill \\ \end{gathered} \right.$$
(3)

Here, vmax, amax, jmax, δmax, and wmax are the maximum allowable speed, maximum acceleration, maximum acceleration rate, maximum front wheel angle, and maximum front wheel angle rate during parking, respectively.

3 Parallel Parking Path Planning and Optimization

3.1 Parallel Parking Path Planning Based on Quintic Polynomial Curve

During the parking process, curvature constraints need to be satisfied to avoid the occurrence of tire scrubbing. At the same time, the yaw angle and curvature at the starting and ending points of parking should be as small as possible, so as to ensure that the vehicle body is parallel to the lane lines and the steering wheel is in the neutral position at these two points. The quintic polynomial curve, which has continuous curvature and small computational complexity, satisfies the above requirements. Therefore, a quintic polynomial curve is used to plan the parallel parking path (Yu et al., 2022).

The expression of the quintic polynomial curve:

$$f(x) = a_{0} + a_{1} x + a_{2} x^{2} + a_{3} x^{3} + a_{4} x^{4} + a_{5} x^{5}$$
(4)

Here, a0 ~ a5 are the coefficients of the quintic polynomial curve.

The coordinates of the parallel parking starting point, P1, are given as (x1, y1), and the coordinates of the ending point, P2, are given as (x2, y2). Based on the coordinates of these two points, a vehicle position constraint equation is established:

$$y_{1} = a_{0} + a_{1} x_{1} + a_{2} x_{1}^{2} + a_{3} x_{1}^{3} + a_{4} x_{1}^{4} + a_{5} x_{1}^{5}$$
(5)
$$y_{2} = a_{0} + a_{1} x_{2} + a_{2} x_{2}^{2} + a_{3} x_{2}^{3} + a_{4} x_{2}^{4} + a_{5} x_{2}^{5}$$
(6)

The body of the parallel parking vehicle should be parallel to the lane line at both the starting and ending points. This implies that the first derivative of the path curve at these two points is zero. The vehicle yaw angle constraint equation is established as follows:

$$0 = a_{1} + 2a_{2} x_{1} + 3a_{3} x_{1}^{2} + 4a_{4} x_{1}^{3} + 5a_{5} x_{1}^{4}$$
(7)
$$0 = a_{1} + 2a_{2} x_{2} + 3a_{3} x_{2}^{2} + 4a_{4} x_{2}^{3} + 5a_{5} x_{2}^{4}$$
(8)

The front wheel angle at the starting and ending points of parallel parking is expected to be aligned, implying that the second derivative of the path curve at these two points is required to be zero. A constraint equation for the vehicle front wheel angle is established as follows:

$$0 = 2a_{2} + 6a_{3} x_{1} + 12a_{4} x_{1}^{2} + 20a_{5} x_{1}^{3}$$
(9)
$$0 = 2a_{2} + 6a_{3} x_{2} + 12a_{4} x_{2}^{2} + 20a_{5} x_{2}^{3}$$
(10)

A parallel parking space is set up with a length of Lw = 7 m and a width of Lp = 2.5 m. The starting and ending points of the parking are presented as shown in Fig. 2. The distances from the rear end and left end of the vehicle outline at the parking ending point to the left side and top side of the parking space are designated as e1 and e2. The distance from the right end of the vehicle outline at the parking starting point to the parking space line is known as e3. In consideration of safety and curvature variations during the parking process, e1, e2, and e3 are set at 0.25 m, 0.2 m, and 0.3 m, respectively.

Fig. 2
figure 2

Selection of parallel parking starting point and ending point

Full size image

The starting point P1 coordinate of the quintic polynomial curve is set as (8.15 m, 3.60 m), and the ending point P2 coordinate is set as (1.20 m, 1.48 m). Equations (5) through (10) are combined to calculate the parallel parking quintic polynomial curve:

$$\begin{gathered} f(x) = 0.00078445x^{5} - 0.0183x^{4} + 0.1399x^{3} \hfill \\ \quad \quad \quad - 0.3587x^{2} + 0.3752x + 1.3407 \hfill \\ \end{gathered}$$
(11)

As shown in Fig. 3, collisions between the trajectory of the vehicle’s rear axle center and the surrounding lane lines and parking space lines are avoided, thereby complying with the obstacle avoidance constraints. As shown in Fig. 4, the parking path and curvature, planned via the quintic polynomial curve, are smooth and continuous, without any sudden changes. Moreover, compliance with the constraint set by the vehicle’s structural parameters is maintained for the maximum curvature.

Fig. 3
figure 3

Track of the rear axle center of the vehicle

Full size image
Fig. 4
figure 4

Parking path and curvature of quintic polynomial curve

Full size image

During the actual parking process, the vehicle may not be able to accurately reach the starting point due to positioning errors. Figure 5 shows the variations in parking path curvature when positioning errors in all four directions at the parking starting point are 0.1 m. When the parking starting points are positioned at (8.05 m, 3.60 m) and (8.15 m, 3.70 m), it is observed that the maximum curvature of the path exceeds the curvature constraint, which increases the risk of collision with surrounding obstacles during the parking process. Consequently, this may result in the vehicle’s inability to complete the parking smoothly.

Fig. 5
figure 5

Comparison of curvature at different parking starting points

Full size image

3.2 Parallel Parking Path Planning Based on Improved Sigmoid Function

The quintic polynomial curve exhibits the problem of excessive curvature, and when the parking starting point is not properly selected, it fails to meet the curvature constraint conditions. Consequently, an improved Sigmoid function is used to plan the parallel parking path (Xiong et al., 2017).

The expression of the improved Sigmoid function:

$$g(x) = \frac{{b_{0} }}{{1 + e^{{(b_{1} x + b_{2} )}} }} + b_{3} x$$
(12)

Here, b0 ~ b3 are the coefficients of the improved Sigmoid function.

The position constraints and yaw angle constraints should be satisfied by the improved Sigmoid function:

$$\left\{ \begin{gathered} g(x_{1} ) = y_{1} ,g(x_{2} ) = y_{2} \hfill \\ \dot{g}(x_{1} ) = 0,\dot{g}(x_{2} ) = 0 \hfill \\ \end{gathered} \right.$$
(13)

Taking the parking starting point (8.05 m, 3.60 m) and the ending point (1.20 m, 1.48 m) as an example, Eq. (13) should be solved to calculate the expression of the improved Sigmoid function:

$$g(x) = \frac{12.7444}{{1 + e^{( - 0.4156x + 1.9219)} }} - 0.8286x$$
(14)

As shown in Fig. 6, the parking path and curvature planned by the improved Sigmoid function are compared with the path curvature planned by the above quintic polynomial curve. Although it satisfies the maximum curvature constraint, the curvature at the starting and ending points is too large, which leads to the occurrence of tire steering in place.

Fig. 6
figure 6

Parking path and curvature of improved sigmoid function

Full size image

3.3 Parallel Parking Path Planning Based on Superposition Curve Optimized by Simulated Annealing Algorithm

To solve the problem of the maximum curvature of the quintic polynomial curve exceeding the defined value and the excessive curvature of the endpoint of the improved Sigmoid function, the two curves are superimposed according to a certain ratio:

$$h(x) = w_{1} f(x) + (1 - w_{1} )g(x)$$
(15)

Here, w1 is the proportional coefficient.

The parking path must satisfy the conditions of continuous curvature and maximum curvature constraints. The magnitude of the parking path curvature is closely related to ride comfort; the smaller the maximum curvature of the path, the higher the comfort. To improve ride comfort, the maximum curvature of the parking path, kmax, should be made as small as possible. When the maximum curvature kmax is smaller, that is, w1 is smaller, the front wheel angle of the vehicle during path tracking is also smaller, thereby improving the smoothness and ride comfort during parking. However, the corresponding curvatures at the parking starting and ending points are too large, which increases tire wear. When w1 is larger, it solves the problem of tire steering in place better, but the maximum curvature also increases. To balance the relationship between the maximum curvature and the curvatures at the parking starting and ending points, a corresponding objective function is designed:

$$J_{1} = w_{2} (k_{{{\text{start}}}} + k_{{{\text{goal}}}} ) + w_{3} k_{\max }$$
(16)

Here, kstart is the curvature corresponding to the parking starting point; kgoal is the curvature corresponding to the parking ending point; w2 and w3 are the weighting coefficients for the curvatures at the parking starting and ending points and the maximum curvature. To increase the impact of the maximum curvature on the objective function, w2 is set to 2 and w3 is set to 13.

The simulated annealing algorithm, which is easy to implement and robust, is a general probabilistic optimization algorithm inspired by the annealing process of solids. The algorithm starts at a high initial temperature and gradually anneals, performing local search at each temperature until reaching the optimal solution (Xu et al., 2024). The acceptance probability based on the Metropolis criterion is:

$$P = \left\{ \begin{aligned} 1, \,& \quad E_{t + 1} < E_{t} \\ e^{{\frac{{ - (E_{t + 1} - E_{t} )}}{\alpha T}}} , \,& \quad E_{t + 1} \ge E_{t} \\ \end{aligned} \right.$$
(17)

Here, Et is the system energy at time t, which is the value of the objective function; Et+1 is the system energy at time t + 1; α is the temperature decay coefficient; T is the initial temperature.

As shown in Fig. 7, the flowchart of the simulated annealing algorithm is presented. The main parameter settings are listed in Table 1.

Fig. 7
figure 7

Flowchart of simulated annealing algorithm

Full size image
Table 1 Settings of simulated annealing algorithm parameters
Full size table

As shown in Fig. 8, the iterative process for solving the proportional coefficient w1 using the simulated annealing algorithm is presented. When the iteration reaches the 30th time, the optimal solution is obtained, characterized by a fitness value of 3.105708, and the proportional coefficient w1 has a value of 0.887. When the parking starting point (8.05 m, 3.60 m) is used as an example and substituted into Eq. (15), the optimized parallel parking path expression is derived:

$$h\left( x \right) = 0.887\;f\left( x \right) + 0.113\;g\;\left( x \right)$$
(18)
Fig. 8
figure 8

Iteration process of simulated annealing algorithm

Full size image

As shown in Fig. 9, the curvature of the parallel parking path after optimization using the simulated annealing algorithm is presented. The problems of the quintic polynomial curve and the improved Sigmoid function are solved by the optimized path, which improves ride comfort while avoiding steering tire in place. The optimized maximum curvature kmax is less than 1/Rmin, satisfying the maximum curvature constraint.

Fig. 9
figure 9

Parking path and curvature at different starting points

Full size image

The path planning method mentioned has validated its effectiveness for specific starting and ending points. To ensure the safety of the parking process, the coordinates of the ending position and the longitudinal coordinate of the starting position are kept constant. The study examines the extreme values of the starting point’s horizontal coordinate under the conditions that meet both obstacle avoidance and curvature constraints. As shown in Fig. 10, the curvature constraint is not satisfied when the horizontal coordinate of the starting point is less than 8.01 m, thus the minimum value for the starting point’s horizontal coordinate is established at 8.01 m. Figure 11 shows that when the horizontal coordinate of the starting point is 8.41 m, it satisfies the curvature constraint while just meeting the requirements for obstacle avoidance, thereby setting the maximum value for the starting point’s horizontal coordinate at 8.41 m.

Fig. 10
figure 10

The track and curvature of the vehicle when the parking starting point is (8.01 m, 3.60 m)

Full size image
Fig. 11
figure 11

The track and curvature of the vehicle when the parking starting point is (8.41 m, 3.60 m)

Full size image

4 Parking Path Tracking Optimized by Simulated Annealing Algorithm

4.1 The Design of Model Predictive Controller

The model predictive control algorithm comprises three parts: a predictive model, a rolling optimization, and a feedback correction. The core idea of it is the prediction of the system’s behavior based on input information, which allows for adjustments to the system’s state to achieve targeted control (Gong et al., 2020).

Based on the kinematic model of the vehicle, the state quantity is given as \(\xi = \left( {x,y,\theta } \right)\), and the control quantity as \(u=\left(\delta ,v\right)\). Consequently, the reference system’s state quantity and control quantity are expressed as follows:

$$\dot{\xi } = f(\xi ,u)$$
(19)

The state quantity and control quantity of the reference point are to be substituted at any time:

$$\dot{\xi }_{r} = f(\xi_{r} ,u_{r} )$$
(20)

Here, \({\xi }_{r}\) and \({u}_{r}\) are state quantity and control quantity at any point.

Based on the kinematics model, the state-space equation is a nonlinear system, which does not meet the requirements of model predictive control. Therefore, Taylor series expansion is used to linearize the equation at point (20) of Eq. (19), ignoring the higher order terms and retaining the first-order terms:

$$\begin{aligned} \dot{\xi } &= f(\xi_{r} ,u_{r} ) + \frac{\partial f(\xi ,u)}{{\partial \xi }}(\xi - \xi_{r} ) \\ & \quad + \frac{\partial f(\xi ,u)}{{\partial u}}(u - u_{r} ) \\ \end{aligned}$$
(21)

Equation (20) is to be subtracted from Eq. (21):

$$\dot{\xi } - \dot{\xi }_{r} = \frac{\partial f(\xi ,u)}{{\partial \xi }}(\xi - \xi_{r} ) + \frac{\partial f(\xi ,u)}{{\partial u}}(u - u_{r} )$$
(22)

\(\Delta \xi =\xi -{\xi }_{{\text{r}}},\Delta u=u-{u}_{{\text{r}}}\) are defined, and Eq. (22) is rearranged to obtain:

$$\Delta \dot{\xi } = A\Delta \xi + B\Delta u$$
(23)

Here, \(A=\left[\begin{array}{ccc}0& 0& -{v}_{{\text{r}}}{\text{sin}}{\theta }_{{\text{r}}}\\ 0& 0& {v}_{{\text{r}}}{\text{cos}}{\theta }_{{\text{r}}}\\ 0& 0& 0\end{array}\right]\)

$$B=\left[\begin{array}{cc}{\text{cos}}{\theta }_{{\text{r}}}& 0\\ {\text{sin}}{\theta }_{{\text{r}}}& 0\\ {\text{tan}}{\delta }_{{\text{r}}}/l& {v}_{{\text{r}}}/\left(l{{\text{cos}}}^{2}{\delta }_{{\text{r}}}\right)\end{array}\right]$$

Equation (23) is to be discretized:

$$\Delta \dot{\xi } = \frac{\Delta \xi (k + 1) - \Delta \xi (k)}{T} = A\Delta \xi (k) + B\Delta u(k)$$
(24)

Here, T is the sampling time, k is the current sampling instant, and k + 1 is the subsequent sampling instant.

Based on Eq. (24), the discrete linear state-space equation can be derived as:

$$\tilde{\xi }(k + 1) = A_{k} \tilde{\xi }(k) + B_{k} \tilde{u}(k)$$
(25)

Here, \({A}_{k}=T*A+E\), \({B}_{k}=T*B\).

Equation (25) is processed for the next step:

$$\Phi (k|t) = \left[ \begin{gathered} \tilde{\xi }(k|t) \hfill \\ \tilde{u}(k - 1|t) \hfill \\ \end{gathered} \right]$$
(26)
$$\eta (k|t) = C_{k} \tilde{\xi }(k)$$
(27)

Here, \({C}_{k}=[1 \quad 0]\).

It can be deduced that:

$$\Phi (k + 1|t) = \tilde{A}_{k} \Phi (k|t) + \tilde{B}_{k} \Delta u(k|t)$$
(28)
$$\eta (k|t) = \tilde{C}_{k} \Phi (k|t)$$
(29)

Here, \({\widetilde{A}}_{k}=\left[\begin{array}{cc}{A}_{k}& {B}_{k}\\ {0}_{m\times n}& {I}_{m}\end{array}\right]\), \({\widetilde{B}}_{k}=\left[\begin{array}{c}{B}_{k}\\ {I}_{m}\end{array}\right]\), \({\widetilde{C}}_{k}=\left[{C}_{k} 0\right]\), m is the control dimension, n is the state dimension.

The predictive horizon and control horizon of the system are designated as Np and Nc, respectively. The state of the system is predicted based on Eqs. (28) and (29):

$$\begin{gathered} \Phi (k + N_{p} |t) = \tilde{A}_{k}^{{N_{p} }} \Phi (k|t) + \tilde{A}_{k}^{{N_{p} - 1}} \tilde{B}_{k} \Delta u(k|t) \hfill \\ \quad \quad \quad \quad \quad \quad+ \cdots + \tilde{A}_{k}^{{N_{p} - N_{c} - 1}} \tilde{B}_{k} \Delta u(k + N_{c} |t) \hfill \\ \end{gathered}$$
(30)
$$\begin{aligned} \eta (k + N_{p} |t) = \tilde{C}_{k} \tilde{A}_{k}^{{N_{p} }} \Phi (k|t) + \tilde{C}_{k} \tilde{A}_{k}^{{N_{p} - 1}} \tilde{B}_{k} \Delta u(k|t) \\ \quad \quad \quad \quad \quad \quad + \cdots + \tilde{C}_{k} \tilde{A}_{k}^{{N_{p} - N_{c} - 1}} \tilde{B}_{k} \Delta u(k + N_{c} |t) \\ \end{aligned}$$
(31)

The output of the whole system in the future moment is expressed in the form of a matrix:

$$Y(k) = \Psi \Phi (k|t) + \Theta \Delta U(k)$$
(32)

Here, \(Y\left(k\right)=\left[\begin{array}{c}\eta \left(k+1|t\right)\\ \eta \left(k+2|t\right)\\ \cdot \cdot \cdot \\ \eta \left(k+{N}_{c}|t\right)\\ \cdot \cdot \cdot \\ \eta \left(k+{N}_{p}|t\right)\end{array}\right]\), \(\Psi =\left[\begin{array}{c}{\widetilde{C}}_{k}{\widetilde{A}}_{k}\\ {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{2}\\ \cdot \cdot \cdot \\ {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{c}}\\ \cdot \cdot \cdot \\ {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{p}}\end{array}\right],\)

$$\Delta U\left(k\right)=\left[\begin{array}{c}\Delta u(k|t)\\ \Delta u(k+1|t)\\ \cdot \cdot \cdot \\ \Delta u(k+{N}_{c}|t)\end{array}\right],$$
$$\Theta =\left[\begin{array}{cccc}{\widetilde{C}}_{k}{\widetilde{B}}_{k}& 0& 0& 0\\ {\widetilde{C}}_{k}{\widetilde{A}}_{k}{\widetilde{B}}_{k}& {\widetilde{C}}_{k}{\widetilde{B}}_{k}& 0& 0\\ \cdots & \cdots & \ddots & \cdots \\ {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{c}-1}{\widetilde{B}}_{k}& {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{c}-2}{\widetilde{B}}_{k}& \cdots & {\widetilde{C}}_{k}{\widetilde{B}}_{k}\\ {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{c}}{\widetilde{B}}_{k}& {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{c}-1}{\widetilde{B}}_{k}& \cdots & {\widetilde{C}}_{k}{\widetilde{A}}_{k}{\widetilde{B}}_{k}\\ \vdots & \vdots & \ddots & \vdots \\ {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{p}-1}{\widetilde{B}}_{k}& {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{p}-2}{\widetilde{B}}_{k}& \cdots & {\widetilde{C}}_{k}{{\widetilde{A}}_{k}}^{{N}_{p}-{N}_{c}-1}{\widetilde{B}}_{k}\end{array}\right].$$

In MPC, the objective function is designed to optimize control performance. It can be established as follows:

$$\begin{aligned} \min J(k) & = \sum\limits_{i = 1}^{{N_{p} }} {||\eta (k + i|t) - \eta_{r} (k + i|t)||}_{Q}^{2} \\ & \quad + \sum\limits_{i = 1}^{{N_{c} - 1}} {||\Delta U(k + i|t)||}_{R}^{2} + \rho \varepsilon^{2} \\ \end{aligned}$$
(33)

Here, \({\eta }_{r}(k+1|t)\) is the reference output; Q is the state weighting matrix; R is the control weighting matrix; \(\rho\) is the weighting coefficient; \(\varepsilon\) is the relaxation factor.

In the objective function, the system’s ability to follow the desired path is represented by the first term, the constraint on control increment to avoid sudden changes in control inputs that might affect the continuity of control increments is represented by the second term, and the relaxation factor, which can prevent the situation of not obtaining optimal solutions during the solution process, is represented by the third term.

The objective function is transformed into a standard quadratic form:

$$\begin{aligned} \min J(\xi (k),u(k - 1),\Delta U(k)) \\ & =[\Delta U(k)^{T} ,\varepsilon ]^{T} H[\Delta U(k)^{T} ,\varepsilon ] + G[\Delta U(k)^{T} ,\varepsilon ] \\ \end{aligned}$$
(34)

Here, \(H=\left[\begin{array}{cc}{\Theta }^{T}Q\Theta +R& 0\\ 0& \rho \end{array}\right]\), \(G=\left[\begin{array}{cc}2{e}^{T}Q\Theta & 0\end{array}\right]\), \(e=\left[\eta \left(k+1|t\right)-{\eta }_{r}\left(k+1|t\right)\right]\) is the tracking error within the prediction horizon.

The system’s safety, stability, and efficiency can be improved by setting the control input constraints and control increment constraints in MPC reasonably:

$$u_{\min } \le u(k) \le u_{\max }$$
(35)
$$\Delta u_{\min } \le \Delta u(k) \le \Delta u_{\max }$$
(36)

4.2 Model Predictive Control Algorithm Prediction Time Horizon Optimization

In MPC, the prediction time horizon Np determines the time span of future state prediction by the controller. When Np is larger, the controller can predict farther future states, which helps to improve system stability. However, larger Np also increases computational complexity and computation workload, and may introduce greater prediction errors. When Np is smaller, the computational workload is relatively smaller, but it may reduce system stability and control effectiveness. This is because a smaller prediction time horizon may not fully consider future influencing factors, leading to less optimized control actions.

With the parking speed set to 1 m/s, and based on experience, the control time horizon Nc is determined as 20% of the prediction time horizon Np. By selecting different Np values, the optimized path from the starting point (8.05 m, 3.60 m) is tracked. Figure 12 shows the comparison of tracking errors under different prediction time horizons. As Np increases, the maximum tracking error first decreases and then increases. To find the optimal Np, the simulated annealing algorithm is used to optimize the prediction time horizon. The parameter selection is shown in Table 1.

Fig. 12
figure 12

Comparison of tracking errors under different prediction time horizons

Full size image

When employing the simulated annealing algorithm for optimization, it is necessary to design a reasonable and effective fitness function to solve for the optimal parameters. In the process of parking, to reduce the path tracking error, the designed optimization fitness function is as follows:

$$J_{2} = \int_{0}^{t} {\sqrt {\left( {\frac{e(t)}{{\hat{E}}}} \right)^{2} } } dt$$
(37)

Here, J2 is the fitness value; t is the total parking time; e(t) is the lateral error during tracking; \(\widehat{E}\) is the lateral error threshold, which is set to 0.35 m.

Based on the simulation results and experience, the optimization range of the prediction time horizon Np is set to [25, 55], and the constraint conditions are set to:

$${\text{s}}.{\text{t}}.\left\{ \begin{gathered} N_{{\text{p}}} \in {\mathbb{R}} \cup N_{{\text{p}}} \in [25,55] \hfill \\ N_{{\text{c}}} = {\text{round}}\,(0.2*N_{{\text{p}}} ) \hfill \\ \end{gathered} \right.$$
(38)

When the simulated annealing algorithm reaches the 24th iteration of optimization, the optimal fitness value obtained is 4.021, and the optimized prediction time horizon Np is 46, with the corresponding control time horizon Nc being 9. Table 2 shows the comparison of the maximum tracking error and the total tracking error before and after optimization when the sampling time is 0.05 s. The analysis indicates that both the maximum tracking error and total tracking error for the optimized Np are lower than those for the initial Np, verifying the reliability of the Np optimized through the simulated annealing algorithm.

Table 2 Comparison of the maximum tracking error and total tracking error before and after optimization
Full size table

4.3 Pure Pursuit Algorithm Preview Distance Optimization

To verify the superiority of the MPC algorithm in path tracking, a simple, easy-to-use, and highly stable pure pursuit algorithm is utilized for comparison. In the case of the pure pursuit algorithm, the selection of the preview distance is identified as an important parameter influencing tracking accuracy:

$$\delta = {\text{arctan}}\left(\frac{{{2}le}}{{l_{{\text{d}}}^{{2}} }}\right)$$
(39)

Here, e is the lateral deviation between the vehicle axis and the preview point; ld is the preview distance.

When the desired path is being tracked, if ld is assigned a larger value, significant vibrations are not experienced by the vehicle, and the steering action tends to be relatively smooth. However, during the process of approaching the desired path, an increase in local tracking errors may be experienced. With the assignment of a smaller ld, a lack of predictive tracking might occur, which can easily result in vibrations and may lead to deviations in vehicle tracking or even failures.

The objective function is designed to consider the magnitude of tracking error and the amplitude of front wheel angle changes. The smaller the total tracking error of the path, the higher the precision of the vehicle in following the desired path; the smaller the variance of the front wheel angle, the more gradual the changes in the front wheel angle, indicating a smoother and more stable vehicle operation. The objective function for optimizing the preview distance in the pure pursuit algorithm is designed as follows:

$$J_{3} = \min (w_{4} E_{{{\text{tot}}}} + w_{5} \sigma_{\delta } )$$
(40)

Here, Etot is the total path tracking error; \({\sigma }_{\delta }\) is the variance of the front wheel angle; w4 and w5 are the weighting coefficients for Etot and \({\sigma }_{\delta }\). To increase impact of tracking error on the objective function, w4 is set to 10, w5 is set to 1.

By considering the size of the tracking error and the amplitude of the front wheel angle change, the optimal preview distance is obtained as -0.5 m through simulated annealing algorithm optimization.

5 Experimental Verification

5.1 Simulation Experiment

A certain vehicle model is selected as the research object, and its vehicle parameters are shown in Table 3.

Table 3 Vehicle parameters
Full size table

In the parallel parking simulation scenario, the parking space length is 7 m and the width is 2.5 m. The parking speed is set to 1 m/s, the prediction time horizon NP is 46, the control time horizon NC is 9, the sampling time is 0.05 s, and the starting coordinates for parallel parking are (8.05 m, 3.60 m), and the ending coordinates are (1.20 m, 1.48 m).

Automatic parking speed tracking requires the combined action of the drive system and the braking system. Based on the PID algorithm, longitudinal speed tracking is achieved to facilitate rapid and accurate control of the reference parking speed. As shown in Fig. 13, this method enables quick and precise tracking control of the target vehicle speed with minimal error.

Fig. 13
figure 13

Parallel parking speed tracking process

Full size image

The optimized MPC algorithm, enhanced with simulated annealing algorithm for path tracking, is compared with the optimized PP algorithm. As shown in Fig. 14, the process of tracking the parallel parking path through the use of both algorithms manages to accurately follow the desired path. This is achieved without any collisions with road boundaries, obstacles in front of or behind the parking space, etc., during the tracking process. Upon completion of parking, the vehicle maintains its position inside the parking space, adhering to its obstacle avoidance constraints and endpoint position constraints.

Fig. 14
figure 14

Parallel parking path tracking process

Full size image

As shown in Fig. 15a, both control algorithms can quickly and accurately reach the target point from the starting position while satisfying the obstacle avoidance constraints. From the local enlarged view, it can be seen that the tracking effect of MPC algorithm is better than that of PP algorithm. As shown in Fig. 15b, the maximum front wheel angle of MPC algorithm is 0.5173 rad, which does not exceed the maximum constraint value of front wheel angle of 0.5546 rad. At the same time, MPC algorithm has good tracking performance for front wheel angle, and the change of front wheel angle is smooth, without obvious mutation. Compared with PP algorithm, the maximum front wheel angle of PP algorithm is 0.5262 rad, which has good tracking performance at small front wheel angles. As shown in the local enlarged view, PP algorithm has limited tracking ability for large angles, and the front wheel angle will vibrate during tracking. As shown in Fig. 15c, the comparison between the desired yaw angle and the actual yaw angle is presented. At the parking endpoint, the yaw angles tracked by the MPC algorithm and the PP algorithm are − 0.0054 rad and − 0.0059 rad, respectively, both approaching 0, which indicates that the vehicle is parallel to the lane line at the parking endpoint. Overall, from the entire curve, the fitting degree of the yaw angle tracked based on the MPC algorithm to the desired yaw angle is higher than that of the yaw angle tracked based on the PP algorithm.

Fig. 15
figure 15

Parallel parking path tracking results

Full size image

As shown in Fig. 16a, the maximum and average tracking errors of MPC algorithm in the tracking process are 0.0072 m and 0.0043 m, while the maximum and average tracking errors of PP algorithm are 0.0087 m and 0.0056 m. The lateral errors at the parking end of the two algorithms are 0.0036 m and 0.0045 m. Therefore, MPC algorithm has higher lateral tracking accuracy compared to PP algorithm. As shown in Fig. 16b, the maximum and average front wheel angle errors of MPC algorithm in the tracking process are 0.0685 rad and 0.0361 rad, while the maximum and average front wheel angle errors of PP algorithm are 0.2002 rad and 0.0596 rad. The front wheel angle errors at the parking end of the two algorithms are – 0.0035 rad and 0.0745 rad. The front wheel angle error change tracked by MPC algorithm is more gentle and smooth compared to PP algorithm, so MPC algorithm has higher front wheel angle tracking accuracy compared to PP algorithm. As shown in Fig. 16c, the maximum and average yaw angle errors of MPC algorithm in the tracking process are 0.0339 rad and 0.0111 rad, while the maximum and average yaw angle errors of PP algorithm are 0.0367 rad and 0.0291 rad. Therefore, MPC algorithm has higher yaw angle tracking accuracy compared to PP algorithm.

Fig. 16
figure 16

Parallel parking path tracking error

Full size image

The MPC algorithm optimized by simulated annealing algorithm has better tracking performance in all aspects compared to the optimized PP algorithm. However, the MPC algorithm has a large amount of computation and requires a high amount of computing power on the processing unit.

When the actual vehicle parameters exceed those listed in Table 3, the planned path may not satisfy the obstacle avoidance constraints. To meet the obstacle avoidance constraints and enhance the universality of the proposed planning method, the maximum vehicle dimensions should be defined. Multiple simulation experiments have confirmed that when the vehicle length does not exceed 4.45 m and the vehicle width does not exceed 1.84 m, the obstacle avoidance constraints can be satisfied. Figure 17 shows the path tracking process of the MPC algorithm with a vehicle length of 4.45 m and a vehicle width of 1.84 m, where the right profile of the vehicle overlaps with the parking space line of the valet parking space, thereby validating the correctness of the maximum vehicle dimensions.

Fig. 17
figure 17

The path tracking process of the MPC algorithm with a vehicle length of 4.45 m and a vehicle width of 1.84 m

Full size image

5.2 Real Vehicle Experiment

As shown in Fig. 18, the actual vehicle of the Apollo self-driving developer kit is equipped with hardware devices such as LiDAR, ultrasonic radar, camera, GPS + IMU combined inertial navigation, and other sensors.

Fig. 18
figure 18

Apollo autonomous driving developer kit

Full size image

The dimensions of the Apollo Autonomous Driving Developer Kit are 1.43 × 0.89 m. Based on the size ratio between the self-driving vehicle and the parking space in the simulation environment, the parallel parking space is set to be 3.0 m × 1.5 m. As shown in Fig. 19, a parallel parking space is set up in an open field, and yellow and black warning tape is used to simulate the parking space lines.

Fig. 19
figure 19

Parallel parking experiment scene construction

Full size image

In the Dreamview’s Module Controller interface, the Prediction, Routing, Planning, Control, and Canbus communication modules are started. The experimental vehicle is driven to the starting position of the parking lot, the parking destination is selected, the parking request is sent, and the parking process is started. As shown in Fig. 20, the parking process is displayed on the Dreamview interface. The red rectangle is the target parking space, and the blue solid line is the planned parking path.

Fig. 20
figure 20

Dreamview interface parking process

Full size image

At the beginning of the parking process, the Record module is turned on to record relevant data of the Apollo Autonomous Driving Developer Kit during the parking process. The parallel parking path tracking results are shown in Fig. 21. The Apollo Autonomous Driving Developer Kit can perform good tracking control on the desired path, with a maximum tracking error of 0.057 m, which is within the allowable error range. The parking process is safe, smooth, and successfully completed.

Fig. 21
figure 21

Parallel parking path tracking results

Full size image

6 Conclusion

A parallel parking path planning method based on a hybrid superposition curve is proposed to address the issues of maximum curvature exceeding the defined value for quintic polynomial curves and excessive curvature at the endpoints of the improved Sigmoid function. Based on the establishment of a kinematic model that satisfies position, curvature, and yaw angle constraints, the method optimizes the curvature at the start and end points as well as the maximum curvature during parking using simulated annealing algorithm. In addition, the simulated annealing algorithm is employed to optimize the prediction time horizon in the model predictive control algorithm. In simulation validation, the maximum parking tracking error is 0.0072 m, and the front wheel angle error and yaw angle error do not exceed 0.0685 rad and 0.0339 rad, respectively. A comparison with the optimized pure pursuit algorithm demonstrates the higher accuracy and better robustness of the model predictive control algorithm. The feasibility and effectiveness of the proposed path planning method are further verified through real-vehicle testing, where the maximum tracking error is 0.057 m.

In the future, research should be conducted on path planning methods for perpendicular parking and diagonal parking, as well as on real-time obstacle avoidance control methods for automatic parking.