 Original Article
 Open access
 Published:
Path Tracking Controller Design of Automated Parking Systems via NMPC with an Instructible Solution
Chinese Journal of Mechanical Engineering volumeÂ 37, ArticleÂ number:Â 65 (2024)
Abstract
Parking difficulties have become a social issue that people have to solve. Automated parking system is practicable for quick par operations without a driver which can also greatly reduces the probability of parking accidents. The paper proposes a Lyapunovbased nonlinear model predictive controller embedding an instructable solution which is generated by the modified rearwheel feedback method (RFLNMPC) in order to improve the overall path tracking accuracy in parking conditions. Firstly, A discretetime RFLNMPC considering the position and attitude of the parking vehicle is proposed to increase the success rate of automated parking effectively. Secondly, the RFLNMPC problem with a multiobjective cost function is solved by the InteriorPoint Optimization, of which the iterative initial values are described as the instructable solutions calculated by combining modified rearwheel feedback to improve the performance of local optimal solution. Thirdly, the details on the computation of the terminal constraint and terminal cost for the linear timevarying case is presented. The closedloop stability is verified via Lyapunov techniques by considering the terminal constraint and terminal cost theoretically. Finally, the proposed RFLNMPC is implemented on a selfdriving Lincoln MKZ platform and the experiment results have shown improved performance in parallel and vertical parking conditions. The Monte Carlo analysis also demonstrates good stability and repeatability of the proposed method which can be applied in practical use in the near future.
1 Introduction
Parking is becoming a difficult task for people who are not good at driving as the number of vehicle is increasing and the parking space is crowded. With the rapid development of the autonomous vehicle technology, automated parking systems (APS), which is attracting a great deal of attention from the research organization and automobile industry, can help drivers park efficiently and accurately. Many autonomous vehicle companies, such as Bosch, Tesla, Momenta, have launched their new generation vehicles equipped with APS. According to the process of parking, the research of APS can be divided into three key components, namely parking slots detection [1], path planning [2, 3], and path tracking control [4]. As the parking space becomes narrower, it is crucial that the vehicle can accurately track the reference path to successfully park. However, it can be difficult to establish a precise mathematical model of APS because the vehicle has the characteristics of nonlinearity, timevarying, and multiple variables, which can cause the path tracking error inevitably. If the attitude and position of the reference path cannot be tracked accurately, especially for the parallel parking condition, it is easy to cause parking failure or vehicle collision. Therefore, a parking path tracking controller is discussed in the paper in order to obtain improved path tracking performance and increase the success rate of automated parking.
The path tracking problems of autonomous vehicles have been studied abundantly in recent years. Control methods in most research usually focus on vehicle stability and path tracking lateral distance error [5,6,7,8,9]. Nonetheless, it is more difficult for autonomous vehicles to park at a specified location and attitude compared with the general path tracking conditions. To overcome the aforementioned issues of the general path tracking controller, many studies have been carried out on the parking path tracking control algorithms which usually involve fuzzy logic control, feedback control with singlepoint preview, modelfree control, and model predictive control (MPC) method. For instance, the fuzzy logic control algorithms can simulate the nonlinear control of the human driver and improve robustness against uncertainties [10]. Nevertheless, if the initial position of the vehicle is uncertain, the use of the fuzzy logic controller may cause parking failure. Philip et al. [11] combined the fuzzy control with the neural network algorithm to realize automated parking, while the fuzzy control needs to establish a fuzzy rule base by the expertise. An imagebased fuzzy controller for an APS was developed in Ref.Â [12], where the parameters of membership functions were optimized by using a genetic algorithm against the complicated tuning of the controller. Although fuzzy logic control can solve the uncertainty effectively in environmental mapping, the performance of controller may be limited to the knowledge of human experts, or may require a relatively long parking space to the vehicle.
The feedback control with singlepoint preview algorithm usually is subject to the accurate system model and has specific control laws. Jeong et al. [13] presented a feedforward and feedback linear quadratic regulator (LQR) optimal lateral control parking algorithm combining the road curvature to avoid peak steering angle during the parking process. To compensate for the uncertainty caused by various vehicle parameters, Song [14] proposed a dynamic surface control to improve the robustness while tracking the desired path in different parking maneuvers Gao et al. [15] combined the switching control algorithm with backstepping theory to guarantee the global exponential convergence rate for automated parking lateral control in real road experiments. Du et al. [16] proposed the modified sliding mode controller to ensure that the vehicle can park along the slot center line accurately without intrusion into adjacent slots. Although the feedback control with singlepoint preview can achieve stable parking path tracking, the singlepoint preview algorithm cannot contain enough future road information, which may cause large tracking errors.
The modelfree control method is divided into proportional integral (PI) control and intelligent control generally. Ballina et al. [17] designed a fuzzy PD+I controller to achieve the path tracking of parallel parking condition, while the control parameters and fuzzy rules need to be determined by the expertise. In addition, intelligent control algorithms are also applied to APS increasingly. For example, Lin et al. and Moon et al. designed the path tracking controller based on artificial neural network (ANN) to overcome the restriction in traditional control algorithms and operate the vehicle like a human driver in a manner that is characterized without path planning [18, 19]. Song et al. [20] presented a dataefficient reinforcement learning (RL) method which is constructed to learn from data using a modelbased method. The algorithm integrates planning and tracking control using RL to maximize the overall performance potentially. Zhang et al. [21] proposed a RLbased endtoend parking algorithm to achieve automated parking, which can achieve a better parking performance than using the path planning and path trackingbased method. However, the intelligent control algorithm will be greatly affected as long as the vehicle or the operating condition changes since the system dynamics are considered as a blackbox. Furthermore, RL and ANN require a large number of interactions and data sets to achieve acceptable performance respectively, which is not conducive to the commercial application of APS.
The MPC method, which has the ability to forecast future road shape and minimize the gap between the reference path and the trajectory anticipated by the vehicle model in a receding horizon, is a proven method applied to linear and nonlinear system for decades. Qiu et al. [22] applied the linear error model to design the MPC in parking trajectory tracking control layer. The control signals including velocity and steering angle are obtained by the quadratic programming (QP) optimization. Ye et al. [23] proposed a linear MPC with softening constraints to improve the accuracy of parking path tracking and the control increment of each cycle is also calculated by the QP. Meng et al. [24] established the vehicle kinematics model and designed a parking path tracking controller based on MPC algorithm which can make the lateral error of simulation in centimeter scale. Ghaffari et al. [25] presented a parallel exit parking method based on MPC using vehicle dynamic model, while it is difficult to estimate the dynamic parameters of the vehicle accurately under low speed condition. Overall, MPC is widely used in parking path tracking control issues for the ability to take control actions accordingly through forecasting the information of future road shape and generate the optimal steering angle by online optimization. However, to reduce the computational burden, most research choose the linearized kinematic model, which can result in inaccurate prediction model due to the linearization error. Besides, the closedloop stability of MPC is generally not taken into account in parking path tracking control system. Last but not the least, few existing MPC algorithms applied in path tracking control system can be verified by vehicle test, which cannot reflect the practicability and effectiveness of the algorithm. To overcome the aforementioned issues, an instructable solutionbased nonlinear model predictive controller (NMPC) embedding the modified rearwheel feedback method (RFLNMPC) is proposed. The main contributions of the paper areÂ as follows.

(1)
In the control design aspect, a discretetime RF LNMPC considering the position and attitude of the parking vehicle is proposed for automated parking system, which is verified by the selfdriving Lincoln MKZ platform. Compared with Ref.Â [26], a scaling factor that punishes more on yaw angle error costs is also introduced to help get rid of shortsighted optimization for better parking.

(2)
The RFLNMPC problem with a multiobjective cost function is solved by the InteriorPoint Optimization whose instructable solution are appropriately calculated by combining the modified rearwheel feedback method to improve the performance of local optimal solution.

(3)
The details on the computation of the terminal constraint and terminal cost for the linear timevarying case is presented, and the closedloop stability and recursive feasibility of RFLNMPC controller is proved via Lyapunov techniques.
The remainder of the paper is organized as follow: the vehicle kinematics model and path tracking model are presented in Section 2. Section 3 designs the RFLNMPC controller. In Section 4 the closedloop stability and feasibility of RFLNMPC scheme are analyzed. The proposed controller is implemented and tested on the selfdriving Lincoln MKZ platform in Section 5. Finally, conclusions are drawn in Section 6.
2 Model of Path Tracking System
2.1 Vehicle Kinematics Model
The singletrack kinematics bicycle model merges the left and right wheels into a pair of single wheels at the center of the front and rear axles. Figure 1 shows the schematic diagram of the vehicle kinematics model. Since the longitudinal speed is low in APS, the slip angles of the front and rear wheel are supposed to be zero in the paper.
The position and attitude of vehicle can be defined as \(({x}_{r},\hspace{0.33em}{y}_{r},\hspace{0.33em}{\varphi }_{r})\) on XOY inertial coordinates.Â \(({x}_{f},{y}_{f})\) and \(({x}_{r},{y}_{r})\) are the coordinates of the center point of front axle and rear axle respectively, \(\varphi\) is the yaw angle of the vehicle. \(\delta\) is the steering angle of the front wheel, \({v}_{r}\) is the speed of the center point of rear axle, and \(L\) represents the wheelbase, \(r\) is the turning radius of vehicle. It is important to note that the rear axis coordinates are applied in the kinematic model in order to control the vehicle attitude better.
When the vehicle is moving, the speed of rear axle center point can be expressed as:
Restricting the model movement in the plane, the nonholonomic constraint equations for the front and rear wheels are:
Using Eqs. (1)â€“(3), we can get:
According to the geometry of the front and rear wheels:
Substituting Eqs. (4) andÂ (5) into Eqs. (2) andÂ (1), the yaw rate of vehicle can be derived:
where \(\omega\) is the yaw rate of the vehicle. Meanwhile, the rear wheel steering radius and the front wheel steering angle are computed using \(\omega\) and \({v}_{r}\):
Referring to Eqs. (4) and (6), the kinematic bicycle model with respect to the axle center of the rear wheels can be described:
The model is discretized using forward differencing method with sample time \(\Delta t\). The discrete statespace equation can be written as [27]:
2.2 Path Tracking Model
As shown in Figure 2, the nearest path point from the center of the rear axle to the path is a datum point in reference. Previewed path points are obtained by selecting an interval of S which starts from the nearest path point. \({s}_{k}\) are previewed path points in the prediction horizon and N is the prediction step. The length S between neighboring previewed points is equal to \({v}_{r}\cdot \Delta t\). It is worth noting that if the remaining previewed path points are not enough for the current prediction step, the prediction step will be redefined to ensure that the last segment of the reference path can be tracked accurately.
If the vehicle deviates from the desired path, the vertical distance between vehicle position and the tangent of previewed point is defined as lateral error which is denoted as in Eq. (10). When the difference of the heading angles between the previewed point and the nearest point is taken into account, the method can evaluate the tracking error more accurately.
where \(({x}_{c},{y}_{c})\) represents one of the way points in the reference path which is nearest to the center point of the rear axle. \({\varphi }_{des}\) is the desired yaw angle that is determined by the reference path. The yaw angle error \({e}_{\varphi }\) is defined as in Eq. (11) which is the difference between current yaw angle of the vehicle and the desired yaw angle of the reference path:
where \(v<0\) represents that the vehicle is moving backward. Otherwise, the vehicle is moving forward. \(\mathit{mod}(\cdot )\) is a remainder operator.
3 RFLNMPC Controller Design
3.1 Problem Formulations and Preliminaries
Before establishing the RFLNMPC controller using the singletrack kinematics bicycle model, the NMPC scheme is briefly introduced. For a nonlinear system, the general form of the discrete model is usually considered. Furthermore, \(\xi (k)\) and \(u(k)\) are employed to denote the state and the control input at time \(k\):
where \(f(\cdot ,\hspace{0.33em}\cdot )\) is the state transition function, \(\xi\) and \(u\) are the state and the control input of the system, respectively, which are subject to the constraints:
where \({\rm X}\subset {\mathbb{R}}^{n}\) is a convex polytope as the constraint of state, \(\Gamma \subset {\mathbb{R}}^{m}\) is a convex polytope as the constraint of the sequence of inputs.
In the control process, the controller combines the current state with the desired state over a finite prediction horizon in the future. A series of control sequences are obtained by solving the optimization problem satisfying the objective function and several constraints in the horizon \([k,k+{N}_{p}]\).
The typical cost function \({J}_{{N}_{p},{N}_{c}}(\cdot ,\hspace{0.33em}\cdot )\) is considered as follows:
where \({N}_{p}\) is the prediction horizon, \({N}_{c}\) is the control horizon, constrained by \({N}_{c}\le \hspace{0.33em}{N}_{p}\). \(Q\) and \(R\) are the weighting matrix. At each sampling time, the NMPC controller solves the optimization problem:
Subject to:
The optimal solution denoted by \({u}^{*}(\cdot )\) of Eqs. (14)â€“(20) is generated, and the control input is, therefore, defined by:
Hence, \(u(k)\) is applied to the system at time \(k\). At the next sampling time, the optimization problem in Eqs. (14)â€“(20) is resolved over the shifted predictive horizon, and the process is thus iterated at each sampling time.
3.2 Parking Path Tracking Controller Design
The process of automated parking control is tracking a series of waypoints on the reference path. In addition, the paper assumes that the reference path has been planned in advance. Automated parking control strategy is shown as in Figure 3.
Firstly, the controller obtains the realtime state of vehicle via various sensors. After obtaining the reference path information, the discrete previewed points are generated and can be applied to construct the path tracking model. Furthermore, the terminal constraint and terminal cost are calculated to ensure closedloop stability and feasibility of the RFLNMPC. The control input can be calculated by the optimization of the function with Interior Point Optimizer (IPOPT) which is an open source software package for largescale nonlinear optimization problems [28]. In order to improve the performance of the local optimal solution, the an instructable value of the optimization model are selected by the modified rearwheel feedback method with singlepoint preview during the prediction horizon. Finally, the first element of the control sequence is selected as the control input for the next moment. The method of solving the optimization problem in finite prediction horizon is used to calculate the latest control variable for path tracking until the end of parking.
Considering a given lateral error and yaw angle error, the nonlinear timediscrete system in state space formulation is represented by:
where \(\xi\) is the state vector of Eq. (21), \(\gamma\) is the vehicle state vector, \(\Delta \delta\) is the control input of system.
In the paper, the main focus is that the proposed controller should not only lead to a small tracking error including lateral distance error and yaw angle error but also guarantee the closedloop stability and recursive feasibility of system in order to increase the success rate of automated parking effectively. Thus, the cost function is defined as follows:
where \(P\) is the terminal penalty matrix. The cost penalizes \(\Delta \delta\) to improve the smoothness of steering. \({\xi }^\text{T}(\gamma (k+{N}_{p}))P\xi (\gamma (k+{N}_{p}))\) is the quadratic terminal cost which penalizes the states at the end of the finite horizon.
The automated parking control problem for the RFLNMPC issue can be formulated as:
Subject to:
where \({\delta }_{min}\) and \({\delta }_{max}\) are the minimum and maximum allowed steering wheel angles, \(\Delta {\delta }_{min}\) and \(\Delta {\delta }_{max}\) are the minimum and maximum allowed increment steering angle in each sample time, \({\rm X}_{f}\) is the terminal region which can be chosen such that it is invariant for the nonlinear system controlled by a local linear state feedback.
3.3 Computation of Terminal Cost and Terminal Constraint
In the NMPC tracking control, it is well known that the closedloop stability cannot be automatically guaranteed by the optimality of the solution since the optimization problem may be solved and an infeasible solution is obtained. In other words, there does not exist a sequence of control inputs for which the constraints are satisfied. Even if the optimization problem is always feasible, the computed optimal solution may not lead to an asymptotically stable closedloop system. In general, feasibility and closedloop stability are not ensured in Eqs. (27)â€“(33). In the paper, the terminal penalty matrix and the terminal constraint are chosen to ensure the closedloop stability and feasibility. The region of attraction and performance bound of the nonlinear system are controlled by a local linear state feedback.
The firstorder Taylor approximation of the system in Eq. (21) is taken into account at the origin point:
where \(A\) and \(B\) are defined as \((\partial f/\partial \xi (\gamma ))(\text{0,0})\) and \((\partial f/\partial \Delta \delta )(\text{0,0})\) respectively. Thus, the parts of Eq. (34) can be expressed as:
where \({\delta }_{0}\) is the current steering angle.
If Eq. (34) is stabilizable, then a linear state feedback, \(\Delta \widetilde{\delta }={K}_{lqr}\widetilde{\xi }\), can be determined by \({A}_{k}=A+B{K}_{lqr}\) which is asymptotically stable. \({K}_{lqr}\) is the feedback gain vector of the linear quadratic regulator (LQR) problem:
where \({P}_{lqr}\) can be obtained by solving the Riccati equation.
Then, we can have the following results. Suppose that the firstorder Taylor approximation of the system (21) at the origin point is stable, if \({Q}^{*}=Q+{K}_{lqr}^\text{T}R{K}_{lqr}\) is the symmetric and positive definite, the Lyapunov equation which is shown in Eq. (36) will have an exclusive symmetric and positivedefinite solution \(P\) [29]:
where \(\varepsilon \in [0,\infty )\) satisfies \(\varepsilon <{\lambda }_{max}({A}_{K})\).
Therefore, the terminal penalty matrix \(P\) can be calculated online by solve Eqs. (38) and (36), which make the terminal cost contain the timevarying variable such as the yaw angle error, vehicle speed. The formula for solving the Lyapunov equation is given in Ref.Â [30].
Similarly, assume that the firstorder Taylor approximation of the system (21) at the origin point is stable, one neighborhood \({\rm X}_{f}\) of the origin point is as follows:
where \(\alpha \in (0,\infty )\) is the constant. \({\rm X}_{f}\) which is defined as a terminal region must satisfy three conditions.
(1) \(\text{X}_{f}\subset \text{X},\)
(2) \({K}_{lqr}\xi \in \Gamma\), for all \(\xi \in \text{X}_{f}\), \(\Gamma =\left[\Delta {\delta }_{min},\Delta {\delta }_{max}\right],\)
(3) \(\text{X}_{f}\) is invariant for the nonlinear system (21) controlled by the local linear feedback \(\Delta \delta ={K}_{lqr}\xi\).
In order to determine the terminal region, the largest possible \(\alpha\) need to be found by solving an iterative optimization problem [29]:
where \(\phi (\xi )\) equals to \(f(\xi (\gamma ),{K}_{lqr}\xi (\gamma )){A}_{K}\xi (\gamma )\). The \(\mathit{max}\{\cdot \}\) is a function that returns the largest element within the brace.
According to the selected value of \(\varepsilon\) and initial value of \({\alpha }_{1}\), the offline iteration starts from \({\alpha }_{1}\) until the iterative optimization solution become nonpositive. By following the method to find a suitable \(\alpha\), the terminal region and the terminal penalty matrix can give a set of values shown in Eq. (37).
3.4 The Instructable Solution of Optimization Model
The IPOPT which is an open source software package for largescale nonlinear optimization problems is applied to efficiently solve the addressed LNMPC problem in the paper. The IPOPT implements an interior point line search filter method that aims to find a local optimal solution. The mathematical details of the algorithm can be found in several publications [26].
In the IPOPT, the types of variables are divided into state variables \(({x}_{r},{y}_{r},\varphi )\) and control variables \(\delta\). The number of all variables and constraints are \(4{N}_{p}\) and \(4{N}_{p}+1\). The number of nonzero entries in the Jacobian matrix and Hessian matrix are \(11{N}_{p}4\) and \(6{N}_{p}+1\). The iterative initial values of state variables can be determined by the current vehicle state. In order to improve the performance of local optimal solution, the iterative initial values of the control variables are described as the instructable solutions which are calculated by the modified rearwheel feedback method with singlepoint preview during the prediction horizon, which can make the local optimal solution more feasible under the current parking condition.
After the nearest path point and previewed path points are determined, the previewed singlepoint can be selected by the current speed feedback dynamically:
where \({d}_{p}\) is the preview distance determined according to the speed feedback, \({k}_{p}\) is the proportional coefficient.
According to the kinematic model based on rear wheel position, the yaw rate of vehicle can be described as follows:
where \({c}_{r}\) is the road curvature which should be distinguished to be positive or negative in the parking control process.
The yaw angle error rate \({\dot{\varphi }}_{e}\) provides local asymptotic convergence to twice continuously differentiable paths [31]:
where \({g}_{1}({e}_{r},{\varphi }_{e})\) and \({k}_{e}\) are positive constants.
According to Ackermann's steering and small angle approximation theorem, the final control law applied in the calculation of instructable solution can be described as follows:
The procedure for determining the control gains has been derived in Ref.Â [32]. The two gains can be described:
where \(\zeta\) is the damping factor which can suppress overshoot, \(a\) is the positive constant which can make the error converge.
4 Feasibility and Closedloop Stability Analysis
After the terminal cost and terminal constraint are determined, the closedloop stability of the proposed RFLNMPC scheme can be proved by using the Lyapunov techniques.
For simplicity, the following assumptions are made.
(1) There is no model mismatch between the prediction model and the actual model.
(2) The sets \({\rm X}\), \({\rm X}_{f}\) , and \(\Gamma\) are closed and contain the origin point in their interior.
(3) \({\rm X}_{f}\) is invariant for the nonlinear system controlled by a local linear state feedback \(\Delta \delta ={K}_{lqr}\xi\).
(4) \(Q\), \(R\) and \(P\) are symmetric positive definite.
4.1 Feasibility of RFLNMPC Scheme
To prove the closedloop stability of RFLNMPC controller under the terminal cost and terminal constraint, it is necessary to ensure recursive feasibility of the controller. In other words, the calculated control sequence \(\Delta {\delta }_{k+1}^{*}\) is feasible at time \(k+1\) whenever the local optimal solution at time \(k\) is feasible.
In order to distinguish the variable of system clearly, a bar in \(\left( {\overline{\xi },\overline{u}} \right)\) represents the prediction value and an asterisk in \(({\xi }^{*},{u}^{*})\) indicates the optimal value. So, the feasible local optimal solution at time \(k\) is described as:
Eq. (45) satisfies the constraint (31) and its corresponding sequence of states in prediction horizon \([k+1,k+{N}_{p}]\) can be obtained by:
Then, Eq. (46) satisfies the state constraint (28) and terminal constraint (33). According to the principle of NMPC, the first element obtained by the openloop control is applied to the system (21), whose state can be acquired \(\xi (k+1)\) at time \(k+1\). Without considering disturbance and model mismatch, the state \(\xi (k+1)\) can be described:
Setting \(\xi (k+1)\) as the initial state, the local optimal candidate solution can be chosen:
Due to \({K}_{lqr}\xi \in \Gamma ,\forall \xi \in {X}_{f}\) and \(\overline{\xi }^{*} (k + N_{p} k) \in X_{f}\), Eq. (48) satisfies the constraint (31). The state prediction sequence corresponding to local optimal candidate solution (48) can be obtained by:
It can be noticed from Eq. (49) that the first \({N}_{p}1\) steps satisfy the state constraint (32) shown as Eq. (50). Meanwhile, \({\rm X}_{f}\) is invariant for the nonlinear system \(\xi \left(k+1\right)=f\left(\xi \left(k\right),{K}_{lqr}\xi \left(k\right)\right),\) which indicates the state prediction sequence (49) satisfies the terminal constraint simultaneously shown in Eq.Â (51):
To sum up, \(\Delta \delta (k+1)\) is the feasible solution of the proposed LNMPC controller at time \(k+1\).
4.2 Closedloop Stability of RFLNMPC Scheme
The following theorem discusses the closedloop stability of the RFLNMPC scheme. The cost function is nonincreasing, which is crucial for the asymptotical stability proof.
The optimal value of cost function can be calculated by Eqs. (45) and (46) at time \(k\):
where \({J}_{k}^{*}\) is defined to be continuous and equal to 0 at \(\xi =0\).
By using the feasible solution (48) and state prediction sequence (49), the optimal value of cost function at time \(k+1\) can be described as:
Through further calculations, Eq. (53) can be written as:
Considering Eq. (52), the optimal value of cost function (54) is formulated as:
If \({\rm X}_{f}\) is the terminal region for the nonlinear system, the positive definite function \(E(\xi )\) must satisfy the HamiltonJacobiBellman inequality for all \(\xi \in {\rm X}_{f}\), which can be described as [29]:
where \(F(\cdot ,\cdot )\) is the cost function for the nonlinear system.
Combining Eq. (52) with Eq.Â (56), an inequality can be formulated as:
Then Eq. (55) is transformed into the following inequality:
Since the optimal solution is not worse than the feasible solution, Eq. (58) becomes:
Therefore, the cost function is nonincreasing due to \({J}^{*}\ge 0\). The righthand side is clearly negative definite due to the positive definiteness of \(Q\) and \(R\). The Lyapunov techniques are used to show that the closedloop system (21) is asymptotically stable. Eq. (59) is sufficient to ensure that the state of RFLNMPC converges to the origin point as \(k\to \infty\) if the initial state lies in \({\rm X}\).
5 Experiment Validation
A selfdriving Lincoln MKZ platform shown in Figure 4, is used to implement and test the proposed RFLNMPC controller. The platform is equipped with an integrated Differential Global Position System (DGPS) and an Inertial Measurement Unit (IMU). These sensors can measure the vehicle position and yaw angle which are used to calculate the lateral distance error and yaw angle error. The vehicle can also realize the bywire control of the throttle, brake, steering angle and gear shifting system. The LNMPC parking controller is implemented in C++ under ROS system. Generally, the selfdriving platform should contain two modules: trajectory planning [3] and trajectory tracking [33]. But trajectory planning is not the focus of the paper. In this paper, the precollected waypoints are regarded as desired trajectory.
To verify the superiority of the presented RFLNMPC controller, the experiment results among the RFLNMPC, LNMPC (The iterative initial values of steering in IPOPT is zero.) and the NMPC are given in the section using the typical parallel and vertical parking scenarios. For a fair comparison, the parameters of three parking controllers are set to be the same. The parameters of vehicle and parking controller are shown in Table 1. The vehicle longitudinal speed is controlled by the PID controller.
Remark: our goal is to make the vehicle track the reference path as accurately as possible, which can avoid collision or parking failure caused by excessive lateral distance error or yaw angle error. In this sense, the experiment results mainly illustrate the performance of lateral distance error and yaw angle error.
The proposed RFLNMPC controller can finish the parallel parking condition with a small yaw angle error and lateral distance error shown in enlarged pictures of Figure 5. The boxes in Figure 5 represent the outline of the vehicle. It can be seen that the tracking effect of RFLNMPC is the best and the corresponding trajectory of the vehicle is more consistent with the reference path at the parking termination state. The curvature of reference path and the vehicle speed are shown in Figure 11. The maximum curvature is 0.2Â m^{âˆ’1} which is already close to the maximum turning radius of the vehicle. The maximum speed in turning is 3.6Â km/h. As the vehicle enters the second curve, the speed drops to 0.54Â km/h.
The lateral distance error and yaw angle error are shown in Figures 7 and 8, respectively. The yellow dotted lines in Figures 7 and 8 represent the boundary of lateral distance error and yaw angle error. The two boundaries are set to 0.05Â m and 2Â°. Due to the influence of the initial error and acceleration, all three controllers have the lateral error which is not more than 0.08Â m after the vehicle enters the first curve. The lateral distance error of RFLNMPC reduces to less than 0.05Â m at 6 s and remains at 0.03 m as the speed decreases. It is also worth noting that the yaw angle error of RFLNMPC is no more than 2Â° during the parking process. On the contrary, the lateral distance error of LNMPC and NMPC keep increasing as the vehicle enters the second curve. When the vehicle is parked to the destination, the errors of the LNMPC and NMPC are 0.08Â m and 0.18Â m, respectively. According to Figure 8, even though the yaw angle error of LNMPC converges to 0 at 22Â s, there is still an increased yaw angle error due to the tradeoff of factors in the cost function of LNMPC. However, the final experiment results of LNMPC including position and attitude of the vehicle are within the acceptable ranges. The terminal constraint and terminal cost are not considered in NMPC, of which the lateral error and the angle error does not converge to the acceptable ranges. The yaw angle error of NMPC has reached 3.8Â° in the destination, which may cause the parking failure. To sum up, the proposed RFLNMPC parking controller performs much better in the parallel parking condition.
Figure 9 shows the actual steeringbywire feedback values and the instructable solutions calculated by RFLNMPC. It can be seen that the steering angle of RFLNMPC is relatively smooth, with a maximum of 33Â°. The iterative initial value of LNMPC in IPOPT is always zero, so the local optimal value of steering angle at curve is worse than that of RFLNMPC in the performance of parking path tracking. For the NMPC, even though the steering angle of the vehicle have reached the limit value, there is still a large lateral distance error and yaw angle error in the parking process, which may cause the vehicle collision.
It can be seen from Figure 10 that the tracking effect of the RFLNMPC and LNMPC controller is better than that of the NMPC controller which does not consider the terminal constraint and terminal cost in vertical parking condition. Since the curvature of reference path remains basically unchanged, it can be seen that the effects of the BFLNMPC and LNMPC are similar at the parking termination state. The curvature of reference path and the vehicle speed are shown in Figure 11. The maximum curvature is 0.195Â m^{âˆ’1} which is already close to the maximum turning radius of the vehicle. The maximum speed in turning is 3.2Â km/h. As the vehicle enters the parking spot, the speed drops to 1.3Â km/h.
The lateral distance error and yaw angle error are shown in Figures 12 and 13, respectively. The boundary of lateral distance error and yaw angle error are set to 0.05 m and 1Â° shown as the yellow dotted line. Similar to the experiment results under the parallel parking condition, the proposed RFLNMPC controller can maintain a small yaw angle error and lateral distance error in the parking process. The peak value of the yaw angle error is about 0.84Â° and the peak value of the distance error is 0.04Â m as shown in the red line. Although the lateral distance error and yaw angle error of the LNMPC controller converges to about 0.01Â m and 0.2Â° at the end of parking, the maximum lateral distance error and yaw angle error are 0.05Â m and 1.5Â° which are much larger than that of RFLNMPC in the parking process. The lateral distance error and yaw angle error between LNMPC and RFLNMPC are only 0.01Â m and 0.3Â° which are negligible error. In practice, the performance of the two controllers is almost the same in the final position of vertical parking condition. However, accurate path following around turns is tricky for vertical parking. It can also be found that the maximum error occurs in the place where the path curvature is the largest. Therefore, the maximum error is described as the advantage of RFLNMPC in the vertical parking experiment. On the other hand, it can also be shown that the error of RFLNMPC is smaller than that of LNMPC when the curvature is larger. In addition, the performance of NMPC is even worse compared to LNMPC, which can be demonstrated that the maximum yaw angle error and lateral distance error of NMPC has reached 2.14Â° and 0.12Â m. According to the aforementioned results, the proposed RFLNMPC parking controller performs much better in the vertical parking condition.
Figure 14 shows the actual steeringbywire feedback values in the vertical parking condition and the instructable solutions calculated by RFLNMPC. it is observed that the vehicle controlled by the proposed RFLNMPC controller can turn earlier when the curvature of the reference path is increasing. Similar to the experiment results under the parallel parking condition, the local optimal value of LNMPC at curve is worse than that of RFLNMPC in the performance of parking path tracking.
In order to present the parking effect of three controllers more intuitional, three groups of experiment photos under parallel parking condition are selected for comparison as shown in Figure 15. The path tracking experiment in Figure 15 corresponds to the parallel parking results in Figure 5. More detailed tracking performance can be observed from the Figures 7 and 8. By taking the white line as the reference, we can clearly observe that the proposed RFLNMPC controller enables the vehicle to park into the parking space and maintain a good attitude in the final position compared with the LNMPC and NMPC controller.
Due to the disturbance in the experimental condition, the proposed RFLNMPC algorithm may be caused to be random, which implies that the results obtained by a single experiment cannot fully reflect the comprehensive characteristics of the algorithm. To solve the aforementioned problem and make our experiment more convincing, the Monte Carlo method is applied to prove the effectiveness of the proposed controller by analyzing ten groups of the experimental data. The lateral distance error and yaw angle error of each experimental group are used to calculate the mean and standard deviation. Furthermore, in order to further illustrate the stability of the proposed controller, the standard deviation is also calculation as follows:
where \(N\) is the number of data for each experiment, \({f}_{i}\) is the lateral distance error or yaw angle error of data for each experiment, \(\mu\) is the mean value of data for each experiment.
Ten groups of experimental data in two parking scenarios are analyzed by the standard deviation and mean value as shown in Tables 2 and 3. The mean value of the yaw angle errors and lateral distance errors caused by proposed RFLNMPC under different parking conditions is lower than those of the LNMPC and NMPC. More importantly, the experimental data of the proposed RFLNMPC controller has smaller standard deviation value, which indicates that the proposed RFLNMPC controller has better stability and repeatability in practical use. Therefore, through the analysis of experimental data using the Monte Carlo analysis, the effect of initial lateral distance error can be excluded. The results obtained by the Monte Carlo method prove the effectiveness and practicability of the proposed RFLNMPC algorithm in another aspect.
6 Conclusions
The paper presents an implementable RFLNMPC controller for APS in order to improve the overall path tracking accuracy in parking condition. A discretetime RFLNMPC considering the position and attitude of vehicle is proposed to increase the success rate of automated parking effectively. Furthermore, the modified rearwheel feedback method is applied to calculate the instructable solution for the IPOPT, which can improve the performance of local optimal solution. The paper details how to compute the terminal constraint and terminal cost for the linear timevarying case. Then, the closedloop stability and recursive feasibility of the proposed RF LNMPC controller are verified via Lyapunov techniques by adding a terminal state constraint and a terminal cost theoretically. Finally, the proposed LNMPC is implemented on the selfdriving Lincoln MKZ platform and the experiment results have shown improved performance in the parallel and vertical parking conditions. The Monte Carlo analysis also demonstrates good stability and repeatability of LNMPC which can be applied in practical use in the near future.
Data availability
The data that support the findings of this study are available from the corresponding author, upon reasonable request.
References
J K Suhr, H G Jung. Endtoend trainable onestage parking slot detection integrating global and local information. IEEE Transactions on Intelligent Transportation Systems, 2021, 23(5): 45704582.
Z Qin, X Chen, M Hu, et al. A novel path planning methodology for automated valet parking based on directional graph search and geometry curve. Robotics and Autonomous Systems, 2020, 132: 103606.
X Chen, Z Qin, J Fan, et al. An efficient path planning methodology based on the starting region selection. SAE International Journal of Advances and Current Practices in Mobility, 2020, 2(2020010118): 30723082.
H Vorobieva, S Glaser, N MinoiuEnache, et al. Automatic parallel parking in tiny spots: path planning and control. IEEE Transactions on Intelligent Transportation Systems, 2014, 16(1): 396410.
C Hu, R Wang, F Yan. Integral sliding modebased composite nonlinear feedback control for path following of fourwheel independently actuated autonomous vehicles. IEEE Transactions on Transportation Electrification, 2016, 2(2): 221230.
Z Qin, L Chen, M Hu, et al.Â Lateral and longitudinal dynamics control framework of autonomous vehicles based on multiparameter joint estimation. IEEE Transactions on Vehicular Technology, 2022, 71(6): 58375852.
L Hu, J Ou, J Huang, et al. Safety evaluation of pedestrianvehicle interaction at signalized intersections in Changsha, China. Journal of Transportation Safety & Security, 2021: 126.
H Lin, X Bao, M Lin, et al. Research on risky driving behavior evaluation model based on CIDAS real data. Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering, 2021, 235(8): 21762187.
L Chen, Z Qin, W Kong, et al. Lateral control using LQR for intelligent vehicles based on the optimal fronttire lateral force. Journal of Tsinghua University (Science and Technology), 2021, 61(9): 906912.
Y Zhao, E G Collins. Fuzzy parallel parking control of autonomous ground vehicles in tight spaces. Proceedings of the 2003 IEEE International Symposium on Intelligent Control, IEEE, 2003: 811816.
A K Philip, M Sackey, P OwusuAnsah, et al. Automatic parking control algorithms and simulation research based on fuzzy controller. SAE Technical Paper, 2020.
B Pluymers, J A Rossiter, J A Suykens, et al. The efficient computation of polyhedral invariant sets for linear systems with polytopic uncertainty. Proceedings of the 2005, American Control Conference, IEEE, 2005: 804808.
Y Jeong, S Kim, K Yi, et al. Design and implementation of parking control algorithm for autonomous valet parking. SAE Technical Paper, 2016.
B Song. Cooperative lateral vehicle control for autonomous valet parking. International Journal of Automotive Technology, 2013, 14(4): 633640.
H Gao, J Zhu, X Li, et al. Automatic parking control of unmanned vehicle based on switching control algorithm and backstepping. IEEE/ASME Transactions on Mechatronics, 2022, 27(3): 12331243.
X Du, K Tan. Autonomous reverse parking system based on robust path generation and improved sliding mode control. IEEE Transactions on Intelligent Transportation Systems, 2014, 16(3): 12251237.
E Ballinas, O Montiel, O Castillo, et al. Automatic parallel parking algorithm for a carlike robot using fuzzy PD+I control. Engineering Letters, 2018, 26(4).
Y L Lin, L Li, X Y Dai, et al. Master general parking skill via deep learning. Proceedings of 2017 IEEE Intelligent Vehicles Symposium (IV), IEEE, 2017: 941946.
J Moon, I Bae, S Kim. Automatic parking controller with a twin artificial neural network architecture. Mathematical Problems in Engineering, 2019, 2019(18).
S Song, H Chen, H Sun, et al. Data efficient reinforcement learning for integrated lateral planning and control in automated parking system. Sensors, 2020, 20(24): 7297.
P Zhang, L Xiong, Z Yu, et al. Reinforcement learningbased endtoend parking for automatic parking system. Sensors, 2019, 19(18): 3996.
D Qiu, D Qiu, B Wu, et al. Hierarchical control of trajectory planning and trajectory tracking for autonomous parallel parking. IEEE Access, 2021, 9: 9484594861.
H Ye, H Jiang, S Ma, et al. Linear model predictive control of automatic parking path tracking with soft constraints. International Journal of Advanced Robotic Systems, 2019, 16(3): 1729881419852201.
X Meng, J Wu, R He, et al. A path planning and model predictive control for automatic parking system. SAE Technical Paper, 2020010121.
A Ghaffari, M YarAhmadi, H Rahmanei. Parallel exit parking based on model predictive control in intelligent vehicles. Proceedings of the 28th Annual International Conference of Iranian Society of Mechanical EngineersISME2020, 2020: 2729.
C Yin, B Xu, X Chen, et al. Nonlinear model predictive control for path tracking using discrete previewed points. Proceedings of 2020 IEEE 23rd International Conference on Intelligent Transportation Systems (ITSC), IEEE, 2020: 16.
Z Qin, L Chen, J Fan, et al. An improved realtime slip model identification method for autonomous tracked vehicles using forward trajectory prediction compensation. IEEE Transactions on Instrumentation and Measurement, 2021, 70: 7501012.
A WÃ¤chter. Short tutorial: Getting started with ipopt in 90 minutes, 2019. https://projects.coinor.org/CoinBinary/export/837/CoinAll/trunk/Installer/files/doc/Short%20tutorial%20Ipopt.pdf.
H Chen, F AllgÃ¶wer. A quasiinfinite horizon nonlinear model predictive control scheme with guaranteed stability. Automatica, 1998, 34(10): 12051217.
W X Han. A formula for solving the Lyapunov matrix equation. Journal of Tianjin University (Science and Technology), 1988, 34(03): 408409.
B Paden, M ÄŒÃ¡p, S Z Yong, et al. A survey of motion planning and control techniques for selfdriving urban vehicles. IEEE Transactions on Intelligent Vehicles, 2016, 1(1): 3355.
C Samson. Timevarying feedback stabilization of carlike wheeled mobile robots. The International Journal of Robotics Research, 1993, 12(1): 5564.
J Huang, Y Chen, X Peng, et al. Study on the driving style adaptive vehicle longitudinal control strategy. IEEE/CAA Journal of Automatica Sinica, 2020, 7(4): 11071115.
Acknowledgements
Not applicable.
Funding
Supported by National Key R&D Program of ChinaÂ (Grant No.Â 2021YFB2501800), National NaturalÂ Science Foundation of China (Grant No. 52172384), Science and Technology Innovation Program of Hunan Province of ChinaÂ (Grant No. 2021RC3048), and State Key Laboratory of Advanced Design and Manufacturing Technology for Vehicle of ChinaÂ (Grant No. 72275004).
Author information
Authors and Affiliations
Contributions
CL wrote the manuscript; QZB and HMJ were in charge of the whole structure of the manuscript; BYG and PXY assisted with experimental analysis. All authors read and approved the final manuscript.
Corresponding author
Ethics declarations
Competing Interests
The authors declare no competing financial interests.
Rights and permissions
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.
About this article
Cite this article
Chen, L., Qin, Z., Hu, M. et al. Path Tracking Controller Design of Automated Parking Systems via NMPC with an Instructible Solution. Chin. J. Mech. Eng. 37, 65 (2024). https://doi.org/10.1186/s10033024010424
Received:
Revised:
Accepted:
Published:
DOI: https://doi.org/10.1186/s10033024010424