Path Tracking Controller Design of Automated Parking Systems via NMPC with an Instructible Solution

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 Lyapunov-based nonlinear model predictive controller embedding an instructable solution which is generated by the modified rear-wheel feedback method (RF-LNMPC) in order to improve the overall path tracking accuracy in parking conditions. Firstly, A discrete-time RF-LNMPC considering the position and attitude of the parking vehicle is proposed to increase the success rate of automated parking effectively. Secondly, the RF-LNMPC problem with a multi-objective cost function is solved by the Interior-Point Optimization, of which the iterative initial values are described as the instructable solutions calculated by combining modified rear-wheel 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 time-varying case is presented. The closed-loop stability is verified via Lyapunov techniques by considering the terminal constraint and terminal cost theoretically. Finally, the proposed RF-LNMPC is implemented on a self-driving 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, time-varying, 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 single-point preview, model-free 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 image-based 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 single-point 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 single-point preview can achieve stable parking path tracking, the single-point preview algorithm cannot contain enough future road information, which may cause large tracking errors.

The model-free 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 data-efficient reinforcement learning (RL) method which is constructed to learn from data using a model-based method. The algorithm integrates planning and tracking control using RL to maximize the overall performance potentially. Zhang et al. [21] proposed a RL-based end-to-end parking algorithm to achieve automated parking, which can achieve a better parking performance than using the path planning and path tracking-based 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 black-box. 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 closed-loop 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 solution-based nonlinear model predictive controller (NMPC) embedding the modified rear-wheel feedback method (RF-LNMPC) is proposed. The main contributions of the paper are as follows.

• (1) In the control design aspect, a discrete-time RF- LNMPC considering the position and attitude of the parking vehicle is proposed for automated parking system, which is verified by the self-driving 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 short-sighted optimization for better parking.

• (2) The RF-LNMPC problem with a multi-objective cost function is solved by the Interior-Point Optimization whose instructable solution are appropriately calculated by combining the modified rear-wheel 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 time-varying case is presented, and the closed-loop stability and recursive feasibility of RF-LNMPC 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 RF-LNMPC controller. In Section 4 the closed-loop stability and feasibility of RF-LNMPC scheme are analyzed. The proposed controller is implemented and tested on the self-driving 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 single-track 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.

Figure 1

The single-track kinematics bicycle model

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:

$${v}_{r}={\dot{x}}_{r}\text{cos}\varphi +{\dot{y}}_{r}\text{sin}\varphi.$$

(1)

Restricting the model movement in the plane, the nonholonomic constraint equations for the front and rear wheels are:

$${\dot{x}}_{f}\text{sin}\left(\varphi +\delta \right)-{\dot{y}}_{f}\text{cos}\left(\varphi +\delta \right),$$

(2)

$${\dot{x}}_{r}\text{sin}\varphi +{\dot{y}}_{r}\text{cos}\varphi =0.$$

(3)

Using Eqs. (1)–(3), we can get:

$$\left\{\begin{array}{c}{\dot{x}}_{r}={v}_{r}\text{cos}\varphi, \\ {\dot{y}}_{r}={v}_{r}\text{sin}\varphi. \end{array}\right.$$

(4)

According to the geometry of the front and rear wheels:

$$\left\{\begin{array}{c}{x}_{f}={x}_{r}+L\text{cos}\varphi, \\ {y}_{f}={y}_{r}+L\text{sin}\varphi. \end{array}\right.$$

(5)

Substituting Eqs. (4) and (5) into Eqs. (2) and (1), the yaw rate of vehicle can be derived:

$$\omega =\frac{{v}_{r}\text{tan}\delta }{L},$$

(6)

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}$:

$$\left\{\begin{array}{l}r=\frac{{v}_{r}}{\omega },\\ \delta =\text{arctan}(\frac{L}{r}).\end{array}\right.$$

(7)

Referring to Eqs. (4) and (6), the kinematic bicycle model with respect to the axle center of the rear wheels can be described:

$$\left[\begin{array}{c}{\dot{x}}_{r}\\ {\dot{y}}_{r}\\ \dot{\varphi }\end{array}\right]=\left[\begin{array}{c}\text{cos}\varphi \\ \text{sin}\varphi \\ \frac{\text{tan}\delta }{L}\end{array}\right]{v}_{r}.$$

(8)

The model is discretized using forward differencing method with sample time $\Delta t$. The discrete state-space equation can be written as [27]:

$$\left[\begin{array}{c}{x}_{r}(k+1)\\ {y}_{r}(k+1)\\ \varphi (k+1)\end{array}\right]=\left[\begin{array}{c}{x}_{r}(k)+{v}_{r}\Delta t\text{cos}\varphi (k)\\ {y}_{r}(k)+{v}_{r}\Delta t\text{sin}\varphi (k)\\ \varphi (k)+\frac{{v}_{r}\Delta t\text{tan}\delta (k)}{L}\end{array}\right].$$

(9)

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.

Figure 2

Previewed path points

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.

$${e}_{y}=\left({x}_{r}-{x}_{c}\right){\text{sin}{\varphi }}_{\mathit{des}}-\left({y}_{r}-{y}_{c}\right){\text{cos}{\varphi }}_{\mathit{des}},$$

(10)

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:

$$\left\{\begin{array}{c}{e}_{\varphi }=\mathit{mod}((\varphi -{\varphi }_{des})-2\pi )-\pi, \hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}v<0,\\ {e}_{\varphi }=\mathit{mod}(3\pi (\varphi -{\varphi }_{des})-2\pi )-\pi, \hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}v>0,\end{array}\right.$$

(11)

where $v<0$ represents that the vehicle is moving backward. Otherwise, the vehicle is moving forward. $\mathit{mod}(\cdot )$ is a remainder operator.

3 RF-LNMPC Controller Design

3.1 Problem Formulations and Preliminaries

Before establishing the RF-LNMPC controller using the single-track 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$:

$$\xi \left(k+1\right)=f\left(\xi \left(k\right),u\left(k\right)\right),$$

(12)

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:

$$\xi \left(k\right)\in {\rm X},u\left(k\right)\in \Gamma ,$$

(13)

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:

$$\begin{aligned}{J}_{{N}_{p},{N}_{c}}\left(\xi \left(k\right),u\left(k\right)\right)={\sum }_{i=0}^{{N}_{p}-1}{\xi }^\text{T}\left(k+i\right)Q\xi \left(k+i\right)\\+{\sum }_{i=0}^{{N}_{c}-1}{u}^\text{T}(k+i)Ru(k+i),\end{aligned}$$

(14)

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:

$$\underset{\xi ,\hspace{0.33em}\hspace{0.33em}u}{min}\hspace{0.33em}\hspace{0.33em}{J}_{{N}_{p},{N}_{c}}(\xi (k),\hspace{0.33em}u(k)).$$

(15)

Subject to:

$$\xi \left(k+i+1\right)=f\left(\xi \left(k+i\right),u\left(k+i\right)\right),i=0,\ldots ,{N}_{p}-1,$$

(16)

$$\xi \left(k+i\right)\hspace{0.33em}\in \hspace{0.33em}{\rm X},i=0,\hspace{0.33em}\ldots ,\hspace{0.33em}{N}_{p}-1,$$

(17)

$$u(k+i)\hspace{0.33em}\in \hspace{0.33em}\Gamma ,\hspace{0.33em}i=0,\hspace{0.33em}\ldots ,\hspace{0.33em}{N}_{c}-1.$$

(18)

The optimal solution denoted by ${u}^{*}(\cdot )$ of Eqs. (14)–(20) is generated, and the control input is, therefore, defined by:

$${u}^{*}(\cdot )=\left\{{u}^{*}(k),\hspace{0.33em}{u}^{*}(k+1),\hspace{0.33em}\ldots ,{u}^{*}(k+{N}_{c}-1)\right\},$$

(19)

$$u\left(k\right)={u}^{*}\left(k\right).$$

(20)

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.

Figure 3

Automated parking RF-LNMPC strategy

Firstly, the controller obtains the real-time 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 closed-loop stability and feasibility of the RF-LNMPC. 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 large-scale 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 rear-wheel feedback method with single-point 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 time-discrete system in state space formulation is represented by:

$$\xi \left(k+1\right)=f\left(\xi \left(\gamma \left(k\right)\right),\Delta \delta \left(k\right)\right),$$

(21)

$$\gamma \left(k+1\right)=\xi \left(\gamma \left(k\right),\delta \left(k\right)\right),$$

(22)

where $\xi$ is the state vector of Eq. (21), $\gamma$ is the vehicle state vector, $\Delta \delta$ is the control input of system.

$$\xi ={\left[\begin{array}{cc}{e}_{y}& {e}_{\varphi }\end{array}\right]}^\text{T},$$

(23)

$$\gamma ={\left[\begin{array}{ccc}{x}_{r}& {y}_{r}& \varphi \end{array}\right]}^\text{T},$$

(24)

$$\Delta \delta =\delta \left(k+i+1\right)-\delta \left(k+i\right),i=0,\hspace{0.33em}\ldots ,\hspace{0.33em}{N}_{c}-1.$$

(25)

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 closed-loop 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:

$$\begin{aligned}&{J}_{{N}_{p},{N}_{c}}(\xi (\gamma (k)), \Delta \delta (k))\\&=\sum\limits_{i=0}^{{N}_{p}-1}{\xi }^\text{T}(\gamma (k+i))Q\xi (\gamma (k+i))\\&+ \sum \limits_{i=0}^{{N}_{c}-1}\Delta {\delta }^\text{T}(k+i)R\Delta \delta (k+i)\\&+{\xi }^\text{T}(\gamma (k+{N}_{p}))P\xi (\gamma (k+{N}_{p})),\end{aligned}$$

(26)

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 RF-LNMPC issue can be formulated as:

$$\underset{\xi , \Delta \delta }{min} {J}_{{N}_{p},{N}_{c}}\left(\xi \left(\gamma \left(k\right)\right),\Delta \delta \left(k\right)\right).$$

(27)

Subject to:

$$\begin{aligned}& \xi (k + 1 + i) = f(\xi (\gamma (k + i)),\Delta \delta (k + i)) = \left[ {\begin{array}{*{20}c} {e_{y} (k + i)} \\ {e_{\varphi } (k + i)} \\ \end{array} } \right] \hfill \\ &= \left[ {\begin{array}{*{20}l} {\left( {x_{r} (k + i) - x_{c} (i)} \right)\sin \varphi_{des} - \left( {y_{r} (k + i) - y_{c} (i)} \right)\cos \varphi_{des} (i)} \\ {mod ((\varphi (k + i) - \varphi_{des} (i)) - 2\pi ) - \pi } \\ \end{array} } \right], \hfill \\& \quad i = 0, \; \ldots , \; N_{p} - 1 \; , \hfill \\ \end{aligned}$$

(28)

$$\begin{aligned} & \gamma (k + 1 + i) = \xi (\gamma (k + i), \delta (k + i)) \hfill \\ &= \left[ {\begin{array}{*{20}c} {x_{r} (k + i) + v_{r} \times \Delta t \times \cos \varphi (k + i)} \\ {y_{r} (k + i) + v_{r} \times \Delta t \times \sin \varphi (k + i)} \\ {\varphi (k + i) + {{v_{r} \times \Delta t \times \tan \delta (k + i)} \mathord{\left/ {\vphantom {{v_{r} \times \Delta t \times \tan \delta (k + i)} L}} \right. \kern-0pt} L}} \\ \end{array} } \right], i = 0, \; \ldots , \; N_{p} - 1 \; , \hfill \\ \end{aligned}$$

(29)

$${\delta }_{min}\le \delta (k+i)\le {\delta }_{max}\hspace{0.33em},\hspace{0.33em}i=0,\hspace{0.33em}\ldots ,\hspace{0.33em}{N}_{c}-1,$$

(30)

$$\Delta {\delta }_{min}\le \Delta \delta \left(k+i\right)\le \Delta {\delta }_{max}\hspace{0.33em},i=0,\hspace{0.33em}\ldots ,\hspace{0.33em}{N}_{c}-1,$$

(31)

$$\xi \left(k+i\right)\hspace{0.33em}\in \hspace{0.33em}{\rm X},i=0,\hspace{0.33em}\ldots ,\hspace{0.33em}{N}_{p}-1,$$

(32)

$$\xi \left(\gamma \left(k+{N}_{p}\right)\right)=\left[\begin{array}{c}{e}_{y}\left(k+{N}_{p}\right)\\ {e}_{\varphi }\left(k+{N}_{p}\right)\end{array}\right]\in {\rm X}_{f},$$

(33)

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 closed-loop 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 closed-loop system. In general, feasibility and closed-loop stability are not ensured in Eqs. (27)–(33). In the paper, the terminal penalty matrix and the terminal constraint are chosen to ensure the closed-loop stability and feasibility. The region of attraction and performance bound of the nonlinear system are controlled by a local linear state feedback.

The first-order Taylor approximation of the system in Eq. (21) is taken into account at the origin point:

$$\widetilde{\xi }\left(\gamma \left(k+1\right)\right)=A\widetilde{\xi }\left(\gamma \left(k\right)\right)+B\Delta \widetilde{\delta }\left(k\right),$$

(34)

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:

$$\widetilde{\xi }=\left[\begin{array}{c}{e}_{y}\\ {e}_{\varphi }\end{array}\right],\quad \Delta \widetilde{\delta }=\Delta \delta , \quad A=\left[\begin{array}{cc}1& {v}_{r}\Delta t{\text{cos}\varphi }_{e}\\ 0& 1\end{array}\right],$$

$$B=\left[\begin{array}{c}0\\ {v}_{r}\Delta t/L{\text{cos}}^{2}{\delta }_{0}\end{array}\right],$$

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:

$${K}_{lqr}=(R+{B}^\text{T}{P}_{lqr}B{)}^{-1}{B}^\text{T}{P}_{lqr}A,$$

(35)

where ${P}_{lqr}$ can be obtained by solving the Riccati equation.

Then, we can have the following results. Suppose that the first-order 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 positive-definite solution $P$ [29]:

$${Q}^{*}=-({A}_{k}+\varepsilon I{)}^\text{T}P-P\left({A}_{k}+\varepsilon I\right),$$

(36)

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 time-varying 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 first-order Taylor approximation of the system (21) at the origin point is stable, one neighborhood ${\rm X}_{f}$ of the origin point is as follows:

$$\text{X}_{f}=\left\{\xi \in {R}^{n}{\left|\xi \right.}^\text{T}P\xi \le \alpha \right\},$$

(37)

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]:

$$\underset{\xi }{max}\left\{{\xi }^\text{T}P\phi \left(\xi \right)-\varepsilon {\xi }^\text{T}P\xi |{\xi }^\text{T}P\xi \le \alpha \right\},$$

(38)

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 non-positive. 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 large-scale 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 rear-wheel feedback method with single-point 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 single-point can be selected by the current speed feedback dynamically:

$${d}_{p}={k}_{p}{v}_{r}\Delta t,$$

(39)

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:

$$\omega =\frac{{c}_{r}{v}_{r}\text{cos}({\varphi }_{e})}{1-{c}_{r}{e}_{r}}-{\dot{\varphi }}_{e},$$

(40)

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]:

$$\omega =\frac{{c}_{r}{v}_{r}\text{cos}({\varphi }_{e})}{1-{c}_{r}{e}_{r}}-{g}_{1}({e}_{r},{\varphi }_{e}){\varphi }_{e}-{k}_{e}{v}_{r}\frac{\text{sin}({\varphi }_{e})}{{\varphi }_{e}}{e}_{r},$$

(41)

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:

$${\delta }_{initial}= \text{arctan} \left\{L\left(\frac{{c}_{r}\text{cos}({\varphi }_{e})}{1-{c}_{r}{e}_{r}}-{k}_{\varphi }{\varphi }_{e}-{k}_{e}{e}_{r}\right)\right\}.$$

(42)

The procedure for determining the control gains has been derived in Ref. [32]. The two gains can be described:

$${k}_{\varphi }=2\zeta a,$$

(43)

$${k}_{e}={a}^{2},$$

(44)

where $\zeta$ is the damping factor which can suppress overshoot, $a$ is the positive constant which can make the error converge.

4 Feasibility and Closed-loop Stability Analysis

After the terminal cost and terminal constraint are determined, the closed-loop stability of the proposed RF-LNMPC 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 RF-LNMPC Scheme

To prove the closed-loop stability of RF-LNMPC 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:

$$\Delta \delta^{ * } = \left[ {\begin{array}{*{20}c} {\Delta \overline{\delta }^{ * } (k|k)} \\ {\Delta \overline{\delta }^{ * } (k + 1|k)} \\ \vdots \\ {\Delta \overline{\delta }^{ * } (k + N_{p} - 1|k)} \\ \end{array} } \right].$$

(45)

Eq. (45) satisfies the constraint (31) and its corresponding sequence of states in prediction horizon $[k+1,k+{N}_{p}]$ can be obtained by:

$$\xi^{ * } (k) = \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\overline{\xi }^{ * } (k + 1|k)} \\ {\overline{\xi }^{ * } (k + 2|k)} \\ \end{array} } \\ \vdots \\ {\overline{\xi }^{ * } (k + N_{p} - 1|k)} \\ {\overline{\xi }^{ * } (k + N_{p} |k)} \\ \end{array} } \right].$$

(46)

Then, Eq. (46) satisfies the state constraint (28) and terminal constraint (33). According to the principle of NMPC, the first element obtained by the open-loop 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:

$$\xi (k+1)={\xi }^{*}(k+1|k).$$

(47)

Setting $\xi (k+1)$ as the initial state, the local optimal candidate solution can be chosen:

$$\Delta \delta (k + 1) = \left[ {\begin{array}{*{20}c} {\Delta \overline{\delta }^{ * } (k + 1|k)} \\ \vdots \\ {\Delta \overline{\delta }^{ * } (k + N_{p} - 1|k)} \\ {K_{lqr} \overline{\xi }^{ * } (k + N_{p} |k)} \\ \end{array} } \right].$$

(48)

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:

$$\left\{ {\begin{array}{*{20}l} {\overline{\xi }(k + i|k + 1) = \overline{\xi }^{ * } (k + i|k),\quad i \in [1,N_{p} ]}, \\ \begin{aligned} &\overline{\xi }(k + N_{p} + 1|k + 1) = f(\overline{\xi }^{ * } (k + N_{p} |k),\; \\& K_{lqr} \overline{\xi }^{ * } (k + N_{p} |k)),\quad i = N_{p} + 1. \hfill \\ \end{aligned} \\ \end{array} } \right.$$

(49)

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):

$$\overline{\xi }(k + i|k + 1) \in {\rm X},\;\;i = \left[ {1,N_{p} } \right],$$

(50)

$$\overline{\xi }\left( {k + N_{p} + 1|k + 1} \right) \in {\rm X}_{f}.$$

(51)

To sum up, $\Delta \delta (k+1)$ is the feasible solution of the proposed LNMPC controller at time $k+1$.

4.2 Closed-loop Stability of RF-LNMPC Scheme

The following theorem discusses the closed-loop stability of the RF-LNMPC scheme. The cost function is non-increasing, 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$:

$$\begin{aligned} J_{k}^{*} =& \mathop \sum \limits_{i = 0}^{{N_{p} - 1}} \xi^\text{*T} \left( {k + i{|}k} \right)Q\xi^{*} \left( {k + i{|}k} \right) \\&+\hspace{0.33em}\sum \limits_{i=0}^{{N}_{c}-1}\Delta {{\delta }^{*}}^\text{T}(k+i|k)R\Delta {\delta }^{*}(k+i|k)\\&+{{\xi }^{*}}^\text{T}\left(k+{N}_{p}|k\right)P{\xi }^{*}\left(k+{N}_{p}|k\right),\end{aligned}$$

(52)

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:

$$\begin{aligned}J_{k + 1} =& \sum\limits_{i = 0}^{{N_{p} - 1}} {\overline{\xi }^\text{T} (k + i + 1|k + 1)} Q\overline{\xi }(k + i + 1|k + 1) \hfill \\ & + \;\sum\limits_{i = 0}^{{N_{c} - 1}} {\Delta \overline{\delta }^\text{T} (k + i + 1|k + 1)} R\Delta \overline{\delta }(k + i + 1|k + 1) \hfill \\ & + \overline{\xi }^\text{T} (k + N_{p} + 1|k + 1)P\overline{\xi }(k + N_{p} + 1|k + 1) \hfill \\ =& \sum\limits_{i = 0}^{{N_{p} - 2}} {\overline{\xi }^\text{ * T} (k + i + 1|k)} Q\overline{\xi }^{ * } (k + i + 1|k) \hfill \\ & + \;\sum\limits_{i = 0}^{{N_{c} - 2}} {\Delta \overline{\delta }^\text{ * T} (k + i + 1|k)} R\Delta \overline{\delta }^{ * } (k + i + 1|k) \hfill \\ & + \overline{\xi }^\text{T} (k + N_{p} |k + 1)Q\overline{\xi }(k + N_{p} |k + 1) \hfill \\ & + \Delta \overline{\delta }^\text{T} (k + N_{c} |k + 1)R\Delta \overline{\delta }(k + N_{c} |k + 1) \hfill \\ & + \overline{\xi }^\text{T} (k + N_{p} + 1|k + 1)P\overline{\xi }(k + N_{p} + 1|k + 1)\;. \hfill \\ \end{aligned}$$

(53)

Through further calculations, Eq. (53) can be written as:

$$\begin{aligned} J_{k + 1}^{{}} =& \sum\limits_{i = 0}^{{N_{p} - 1}} {\overline{\xi }^\text{ * T} k + i|k)} Q\overline{\xi }^{ * } (k + i|k) \hfill \\ & + \;\sum\limits_{i = 0}^{{N_{c} - 1}} {\Delta \overline{\delta }^\text{ * T} (k + i|k)} R\Delta \overline{\delta }^{ * } (k + i|k) \hfill \\ & + \overline{\xi }^\text{ * T} (k + N_{p} |k)P\overline{\xi }^{ * } (k + N_{p} |k) \hfill \\ & - \overline{\xi }^\text{ * T} (k|k)Q\overline{\xi }^{ * } (k|k) - \Delta \overline{\delta }^{ * T} (k|k)R\Delta \overline{\delta }^{ * } (k|k) \hfill \\ & + \overline{\xi }^\text{T} (k + N_{p} |k + 1)Q\overline{\xi }(k + N_{p} |k + 1) \hfill \\ & + \Delta \overline{\delta }^\text{T} (k + N_{c} |k + 1)R\Delta \overline{\delta }(k + N_{c} |k + 1) \hfill \\ & + \overline{\xi }^\text{T} (k + N_{p} + 1|k + 1)P\overline{\xi }(k + N_{p} + 1|k + 1) \hfill \\ & - \overline{\xi }^\text{ * T} (k + N_{p} |k)P\overline{\xi }^{ * } (k + N_{p} |k). \hfill \\ \end{aligned}$$

(54)

Considering Eq. (52), the optimal value of cost function (54) is formulated as:

$$\begin{aligned} J_{k + 1}^{{}} =&J_{k}^{ * } - \overline{\xi }^\text{ * T} (k|k)Q\overline{\xi }^{ * } (k|k) - \Delta \overline{\delta }^\text{ * T} (k|k)R\Delta \overline{\delta }^{ * } (k|k) \hfill \\ & + \overline{\xi }^\text{T} (k + N_{p} |k + 1)Q\overline{\xi }(k + N_{p} |k + 1) \hfill \\ & + \Delta \overline{\delta }^\text{T} (k + N_{c} |k + 1)R\Delta \overline{\delta }(k + N_{c} |k + 1) \hfill \\ & + \overline{\xi }^\text{T} (k + N_{p} + 1|k + 1)P\overline{\xi }(k + N_{p} + 1|k + 1) \hfill \\ & - \overline{\xi }^\text{ * T} (k + N_{p} |k)P\overline{\xi }^{ * } (k + N_{p} |k). \hfill \\ \end{aligned}$$

(55)

If ${\rm X}_{f}$ is the terminal region for the nonlinear system, the positive definite function $E(\xi )$ must satisfy the Hamilton-Jacobi-Bellman inequality for all $\xi \in {\rm X}_{f}$, which can be described as [29]:

$$E\left( {\xi \left( s \right)} \right) - E\left( {\xi \left( j \right)} \right) \le - \mathop \sum \limits_{i = j}^{s} F\left( {\xi \left( i \right),u\left( i \right)} \right),\;N_{p} \ge s \ge j \ge 0,$$

(56)

where $F(\cdot ,\cdot )$ is the cost function for the nonlinear system.

Combining Eq. (52) with Eq. (56), an inequality can be formulated as:

$$\begin{aligned} &\overline{\xi }^\text{T} (k + N_{p} |k + 1)Q\overline{\xi }(k + N_{p} |k + 1) \\&+ \Delta \overline{\delta }^\text{T} (k + N_{c} |k + 1)R\Delta \overline{\delta }(k + N_{c} |k + 1) \\&+ \overline{\xi }^\text{T} \left( {k + N_{p} + 1{|}k + 1} \right)P\overline{\xi }\left( {k + N_{p} + 1{|}k + 1} \right) \hfill \\ &- \overline{\xi }^\text{*T} (k + N_{p} |k)P\overline{\xi }^{*} (k + N_{p} |k) \le 0. \hfill \\ \end{aligned}$$

(57)

Then Eq. (55) is transformed into the following inequality:

$$\begin{aligned}J_{k + 1} \le &J_{k}^{*} - \overline{\xi }^\text{*T} (k|k)Q\overline{\xi }^{*} (k|k)\\&- \Delta \overline{\delta }^\text{*T} (k|k)R\Delta \overline{\delta }^{*} (k|k).\end{aligned}$$

(58)

Since the optimal solution is not worse than the feasible solution, Eq. (58) becomes:

$$\begin{aligned} J_{k + 1}^{*} - J_{k}^{*} \le &- \overline{\xi }^\text{*T} (k|k)Q\overline{\xi }^{*} (k|k) \\& - \Delta \overline{\delta }^\text{*T} (k|k)R\Delta \overline{\delta }^{*} (k|k).\end{aligned}$$

(59)

Therefore, the cost function is non-increasing due to ${J}^{*}\ge 0$. The right-hand side is clearly negative definite due to the positive definiteness of $Q$ and $R$. The Lyapunov techniques are used to show that the closed-loop system (21) is asymptotically stable. Eq. (59) is sufficient to ensure that the state of RF-LNMPC converges to the origin point as $k\to \infty$ if the initial state lies in ${\rm X}$.

5 Experiment Validation

A self-driving Lincoln MKZ platform shown in Figure 4, is used to implement and test the proposed RF-LNMPC 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 by-wire control of the throttle, brake, steering angle and gear shifting system. The LNMPC parking controller is implemented in C++ under ROS system. Generally, the self-driving 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 pre-collected waypoints are regarded as desired trajectory.

Figure 4

The Lincoln MKZ platform used in the experiment

To verify the superiority of the presented RF-LNMPC controller, the experiment results among the RF-LNMPC, 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.

Table 1 Parameters of vehicle and controller for experiment

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 RF-LNMPC 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 RF-LNMPC 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⁻¹ 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.

Figure 5

Trajectory comparison in parallel parking

Figure 6

Vehicle speed and the curvature of reference path in parallel parking

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 RF-LNMPC 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 RF-LNMPC 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 RF-LNMPC parking controller performs much better in the parallel parking condition.

Figure 7

The lateral distance error comparison in parallel parking

Figure 8

The yaw angle error comparison in parallel parking

Figure 9 shows the actual steering-by-wire feedback values and the instructable solutions calculated by RF-LNMPC. It can be seen that the steering angle of RF-LNMPC 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 RF-LNMPC 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.

Figure 9

The steering angle comparison in parallel parking

It can be seen from Figure 10 that the tracking effect of the RF-LNMPC 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⁻¹ 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.

Figure 10

Trajectory comparison in vertical parking

Figure 11

Vehicle speed and the curvature of reference path in vertical parking

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 RF-LNMPC 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 RF-LNMPC in the parking process. The lateral distance error and yaw angle error between LNMPC and RF-LNMPC 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 RF-LNMPC in the vertical parking experiment. On the other hand, it can also be shown that the error of RF-LNMPC 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 RF-LNMPC parking controller performs much better in the vertical parking condition.

Figure 12

The lateral distance error comparison in vertical parking

Figure 13

The yaw angle error comparison in vertical parking

Figure 14 shows the actual steering-by-wire feedback values in the vertical parking condition and the instructable solutions calculated by RF-LNMPC. it is observed that the vehicle controlled by the proposed RF-LNMPC 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 RF-LNMPC in the performance of parking path tracking.

Figure 14

The steering angle comparison in vertical parking

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 RF-LNMPC 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.

Figure 15

The experiment photos under the parallel parking condition

Due to the disturbance in the experimental condition, the proposed RF-LNMPC 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:

$$\sigma =\sqrt{\frac{1}{N}{\sum }_{i=1}^{N}({f}_{i}-\mu {)}^{2}},$$

(60)

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 RF-LNMPC under different parking conditions is lower than those of the LNMPC and NMPC. More importantly, the experimental data of the proposed RF-LNMPC controller has smaller standard deviation value, which indicates that the proposed RF-LNMPC 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 RF-LNMPC algorithm in another aspect.

Table 2 The data analysis of lateral distance error (m)

Table 3 The data analysis of yaw angle error (°)

6 Conclusions

The paper presents an implementable RF-LNMPC controller for APS in order to improve the overall path tracking accuracy in parking condition. A discrete-time RF-LNMPC considering the position and attitude of vehicle is proposed to increase the success rate of automated parking effectively. Furthermore, the modified rear-wheel 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 time-varying case. Then, the closed-loop 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 self-driving 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.