A Model Predictive Controller with Longitudinal Speed Compensation for Autonomous Vehicle Path Tracking

Abstract

Autonomous vehicle path tracking accuracy faces challenges in being accomplished due to the assumption that the longitudinal speed is constant in the prediction horizon in a model predictive control (MPC) control frame. A model predictive control path tracking controller with longitudinal speed compensation in the prediction horizon is proposed in this paper, which reduces the lateral deviation, course deviation, and maintains vehicle stability. The vehicle model, tire model, and path tracking model are described and linearized using the small angle approximation method and an equivalent cornering stiffness method. The mechanism of action of longitudinal speed changed with state vector variation, and the stability of the path tracking closed-loop control system in the prediction horizon is analyzed in this paper. Then the longitudinal speed compensation strategy is proposed to reduce tracking error. The controller designed was tested through simulation on the CarSim-Simulink platform, and it showed improved performance in tracking accuracy and satisfied vehicle stability constrains.

1. Introduction

In recent years, with the development of artificial intelligence, big data and information processing technology, autonomous vehicles have received more and more attention. Autonomous vehicle technology aims to improve driving safety, driving comfort, and its economy, as well as reduce traffic accident rates [1,2,3]. As the basic part of autonomous vehicle motion control module, path tracking is desired to follow the reference path accurately. It is one of the research hotspots in recent research on autonomous vehicles [4,5]. This paper mainly focuses on path tracking control for autonomous vehicles.

As shown in Figure 1, a path tracking controller was designed based on Ackerman’s steering geometry method. Ref. [6] adopted the pure pursuit method, path tracking controllers with deviation-based adaptive preview distance velocity were built and compared with lateral position deviation-based controllers. Ref. [7] proposed a path tracking fuzzy controller based on the clothoid curve-fitting reference path. A preview Stanley path tracking controller was designed in [8]. The vehicle kinematics model was also built to design path tracking controller based on pure pursuit method in [9], shown in Figure 2. This type of method is simple and works well in many situations. However, it is only suitable for the conditions where vehicle speed is low, and the path curvature and curvature change rate are small, due to the lack of consideration of vehicle dynamics. The control accuracy of these methods is obviously dependent on the preview information and the scenario applied.

In order to improve the tracking accuracy, Ref. [10,11,12] designed controllers using the PID method based on state deviation of vehicle dynamics. The control accuracy and robustness of the PID method performs relatively better and is easily applied. However, the parameters of the PID controller are not always optimal; it requires additional design to self-tune parameters to satisfy control accuracy. Its robustness to time-varying system parameters and external disturbance is not strong due to the control parameters being determined using trial-and-error. The authors of references [13,14,15] proposed path tracking controllers in the frame of sliding model control (SMC) based on state feedback information. The radial basis function neural network was used to optimize the sliding mode control output chattering phenomenon in [16,17]. An enhanced state observer-based SMC strategy was proposed to achieve the control purpose and maintain the lane keeping errors in [18]. The SMC method has strong robustness to model uncertainty and external disturbance. However, the workload and complexity of the path tracking SMC controller are also huge due to eliminating the chattering phenomenon and designing the observer.

Under high-speed conditions, the vehicle dynamics information and road curvature information have a great influence on tracking accuracy. The feedforward-feedback control method was used to improve the path tracking accuracy. Ref. [19] considered the influence of rear tires on steering control, and a feedforward-feedback controller was designed to realize the steering yaw stability control under the limited condition of force saturation of rear tires. In [20,21], the vehicle’s steady-state sideslip angle was introduced into the feedback control law. In order to overcome the external disturbance and the uncertainty of model parameters, a feedforward-feedback control method was proposed in [22]. A robust H_∞ output-feedback control strategy was proposed for the path following ability of autonomous ground vehicles in [23]. This type of method requires precise vehicle dynamics information for designing the feedback control law. Therefore, it requires a more accurate sensor to obtain the state information of vehicle dynamics. It is relatively difficult to apply in real time due to the collection and processing of the feedback information. Some studies have designed path tracking controllers based on the linear quadratic regulator (LQR) method [24,25]. However, the robustness of the control system is difficult to guarantee when the system needs to handle model uncertainty and external disturbance.

Model predictive control (MPC) has the capability of handling system constraints and future prediction in the design process. It minimizes the gap between the reference path and the actual path by the vehicle dynamics model in a prediction horizon, and it has become a popular method in the control of autonomous vehicles [26,27,28,29,30]. A differential evolution algorithm was introduced into the MPC path tracking controller in order to improve the computational efficiency [31]. The influences caused by the parameters such as prediction horizon, control horizon, and sampling time on tracking accuracy and computational efficiency were analyzed, and a parameter adjustment method for an MPC controller was proposed in [32]. Ref. [33] studied the control incremental modeling method based on Laguerre function to simplify parameter complexity in prediction horizon. Ref. [34,35] proposed a hierarchical control framework based on the combination of MPC control and other methods.

In existing research on path tracking in MPC control frames, it is assumed that the vehicle longitudinal speed in the prediction horizon is constant, aiming to ensure the optimization problem is a convex optimization problem. Most of the previous literature has only considered the dynamics on the lateral and yaw directions, and generally neglected the longitudinal dynamics in the prediction horizon. However, the speed fluctuates under actual driving conditions in the prediction horizon, which causes the prediction error of the vehicle states in the prediction horizon, thus affecting the control accuracy. In particular, it will cause greater prediction errors and change the stability constraint of the closed-loop control system, when the vehicle is at high speed and rapid acceleration or deceleration. Then control system produces control deviation and affect path tracking accuracy. In order to minimize the control deviation caused by speed change in the predicted horizon in the MPC control frame, an MPC path tracking controller, with longitudinal speed compensation in the prediction horizon to reduce the prediction error and tracking error, is proposed in this paper, as shown in Figure 3. The main contributions of this paper include: (1) the mechanism of action of longitudinal speed changes upon the state vector variation and the stability of the path tracking closed-loop control system in prediction horizon is analyzed; (2) a longitudinal speed compensation strategy is proposed to reduce tracking error; and (3) the MPC controller with longitudinal speed compensation is designed and tested on the platform of CarSim-Simulink.

The structure of this paper is as follows. The vehicle dynamics model, tire model and path tracking model are presented in Section 2. The MPC path tracking controller is presented and longitudinal speed compensation strategy is proposed in Section 3. The simulation is conducted in Section 4. In Section 5, the paper concludes with a brief discussion of the results.

2. Modeling

2.1. Vehicle Lateral Dynamics Model

In this paper, we focus on the vehicle states and path tracking performance during tracking of the reference path in the two-dimensional space. For the convenience of vehicle control, we assume that the left and right tires have the same velocity and steering angle. The single-track bicycle model is used in this paper, which has two velocity states and three position states, as illustrated in Figure 4. The front steering angle δ is the only controlled variable. In the existing research on vehicle path tracking using the MPC method, the longitudinal speed v_x is not allowed to be a variable in prediction horizon to make the optimization problem of δ convex optimization. For the method proposed in this paper, the longitudinal speed v_x is compensated in prediction horizon.

The vehicle dynamics model is described by the equations of motion:

$$\{\begin{array}{l}m({\dot{v}}_y+v_{\mathrm{x}}r)=F_{\mathrm{yf}}\cos (\delta )+F_{\mathrm{yr}}\\ I_{\mathrm{Z}}\dot{r}=a{F}_{\mathrm{yf}}\cos (\delta )-b{F}_{\mathrm{yr}}\\ \dot{Y}=v_{\mathrm{x}}\sin (\theta )+v_y\cos (\theta )\\ X=v_{\mathrm{x}}\cos (\theta )+v_y\sin (\theta )\end{array}$$

(1)

where v_x and r are the lateral speed and yaw rate, respectively; F_yf and F_yr are the lateral tire force of the front and rear axles, respectively; δ and θ are the front steering angle and heading angle, respectively; a and b are the distance from the center of mass O to the front and rear axles, respectively.

With the small angle assumption approximation method, the nonlinear vehicle dynamics model can be expressed as:

$$\{\begin{array}{l}{\dot{v}}_y\approx (F_{\mathrm{yf}}+F_{\mathrm{yr}})/m-v_{\mathrm{x}}r\\ \dot{r}\approx (a{F}_{\mathrm{yf}}-b{F}_{\mathrm{yr}})/I_{\mathrm{Z}}\end{array}$$

(2)

2.2. Tire Model

The lateral tire force is described as the function of slip angle using the Fiala brush tire model [36]:

$$F_{\mathrm{tire}}(\alpha )=\{\begin{array}{l}-C_{\alpha }\tan \alpha +\frac{C_{\alpha }^2}{3\mu F_{\mathrm{Z}}}\left|\tan \alpha \right|\tan \alpha -\frac{C_{\alpha }^3}{27{\mu }^2{F}_{\mathrm{Z}}^2},\left|\alpha \right|<\arctan (\frac{3\mu F_{\mathrm{Z}}}{C_{\alpha }})\\ -\mu F_Z\mathrm{sgn}(\alpha ),\mathrm{otherwise}\end{array}$$

(3)

where F_tire(α) and F_Z are lateral tire force and vertical force, respectively; μ is the road surface coefficient of friction; α and C_α are the slip angle and cornering stiffness, respectively;

The slip angle of the front and rear tires, with small angle approximation, can be expressed as:

$$\{\begin{array}{l}{\alpha }_{\mathrm{f}}=\frac{ar}{v_{\mathrm{x}}}+\frac{v_{\mathrm{y}}}{v_{\mathrm{x}}}-\delta \\ {\alpha }_{\mathrm{r}}=\frac{v_{\mathrm{y}}}{v_{\mathrm{x}}}-\frac{br}{v_{\mathrm{x}}}\\ \delta =\beta +\frac{ar}{v_{\mathrm{x}}}-F_{\mathrm{tire}}^{-1}(F_{\mathrm{yf}})\end{array}$$

(4)

Considering the nonlinear characteristics of the tire under limited operating conditions and the online calculation burden of the MPC control, the affine approximation linearization method is applied to the lateral rear tire force in prediction horizon [26,27,30]. The linear tire model can be expressed as:

$$\{\begin{array}{l}{F}_{\mathrm{yr}}={\overline{F}}_{\mathrm{r}}-{\overline{C}}_{\mathrm{r}}({\alpha }_r-{\overline{\alpha }}_{\mathrm{r}})\\ {\overline{C}}_{\mathrm{r}}=\frac{({\overline{F}}_{\mathrm{r},ss}-{\overline{F}}_{\mathrm{r}})}{{\overline{\alpha }}_{\mathrm{r},ss}-{\overline{\alpha }}_{\mathrm{r}}}\end{array}$$

(5)

where ${\overline{\alpha }}_{\mathrm{r}}$ and ${\overline{\alpha }}_{\mathrm{r},ss}$ are the initial tire slip angle in prediction horizon and stead-state tire slip angle, respectively; ${\overline{F}}_{\mathrm{r}}$ and ${\overline{F}}_{\mathrm{r},ss}$ are initial tire lateral force in prediction horizon and stead-state tire lateral force, respectively.

2.3. Path-Tracking Model

The path tracking model is shown in Figure 5, illustrating the relationship between the lateral deviation e, the heading deviation θ_e, and the distance s along the path. In most of the existing path-tracking controllers, the lateral deviation e, the heading deviation θ_e are chosen as the reference states [28,29,32,33], solving the optimization problem by minimizing e and θ_e. However, path tracking lateral deviation is minimized when vehicle sideslip is held tangent to the desired path at all times [19,20,37]. Therefore, the course deviation Δφ is chosen as the reference state in this paper, and the path tracking model can be expressed as:

$$\{\begin{array}{l}\dot{e}=v_{\mathrm{y}}+v_{\mathrm{x}}{\theta }_{\mathrm{e}}\\ {\dot{\theta }}_{\mathrm{e}}=r-v_{\mathrm{x}}k(s)\\ \dot{s}=v_{\mathrm{x}}\cos ({\theta }_{\mathrm{e}})-v_{\mathrm{y}}\sin ({\theta }_{\mathrm{e}})\\ \Delta \phi ={\theta }_{\mathrm{e}}+\beta =\theta -{\theta }_{\mathrm{d}}+\beta \end{array}$$

(6)

where k(s) is the curvature of the reference path at the position of s.

3. MPC Controller Design and Longitudinal Speed Compensation

Existing research has held the assumption that the vehicle longitudinal speed is a constant in prediction horizon [25,26,27,28,29], so that the optimization problem becomes convex optimization; thus it becomes easy for the optimization problem to obtain its solution that satisfies the constraints. In fact, the speed of the vehicle is constantly changing, which will produce corresponding error during predicting vehicle states in prediction horizon. Therefore, in this section, an MPC controller with speed compensation is constructed.

3.1. Control Model

The state space model of path tracking system can be derived from Equations (2), (5) and (6) as:

$$\{\begin{array}{l}\dot{x}=Ax(t)+Bu(t)+Ek(s)+d\\ y=Cx+f\end{array}$$

(7)

$A=\left[\begin{array}{cccc}0& a_{12}& a_{13}& 0\\ 0& 0& 0& a_{24}\\ 0& 0& a_{33}& a_{34}\\ 0& 0& a_{43}& a_{44}\end{array}\right]$, $B=\left[\begin{array}{c}0\\ 0\\ b_{31}\\ b_{41}\end{array}\right]$, $E=\left[\begin{array}{c}0\\ 0\\ -v_{\mathrm{x}}\\ 0\end{array}\right]$, $d=\left[\begin{array}{c}0\\ 0\\ \frac{2({\overline{F}}_{\mathrm{r}}+{\overline{C}}_{\mathrm{r}}{\overline{\alpha }}_{\mathrm{r}})}{m}\\ -\frac{2b({\overline{F}}_{\mathrm{r}}+{\overline{C}}_{\mathrm{r}}{\overline{\alpha }}_{\mathrm{r}})}{I_{\mathrm{Z}}}\end{array}\right]$, $C={\left[\begin{array}{cc}0& 1\\ 1& 0\\ 0& 0\\ 0& 0\end{array}\right]}^T$, $f=\left[\begin{array}{c}0\\ \beta \end{array}\right]$, $a_{12}=v_{\mathrm{x}}$, $a_{13}=1$, $a_{14}=1$, $a_{33}=-\frac{2{\overline{C}}_{\mathrm{r}}}{m{v}_{\mathrm{x}}}$, $a_{34}=\frac{2b{\overline{C}}_{\mathrm{r}}}{m{v}_{\mathrm{x}}}-v_{\mathrm{x}}$, $a_{43}=\frac{2b{\overline{C}}_{\mathrm{r}}}{I_{\mathrm{Z}}{v}_{\mathrm{x}}}$, $a_{44}=-\frac{2b^2{\overline{C}}_{\mathrm{r}}}{I_{\mathrm{Z}}{v}_{\mathrm{x}}}$, $b_{31}=\frac{2}{m}$, $b_{41}=\frac{2a}{I_{\mathrm{Z}}}$.

where x = [e, θ_e, v_y, r]^T is the state vector, y = [e, Δφ]^T is the path tracking error chosen as the output vector, u = [F_yf] is the lateral tire force of the front axle chosen as the control input vector.

To facilitate controller design and implementation, the continuous time state space model is converted into a discrete time system using the zero-order hold method, which is expressed as:

$$\{\begin{array}{l}x(k+1)=A_{c}x(k)+B_{c}u(k)+E_{c}k(s)+d_c\\ y(k)=Cx(k)+f\end{array}$$

(8)

3.2. Vehicle Stability Constraints

The vehicle stability constraints are defined by the bounds of two vital indicators of β and r. The bounds of the β and r reflect the maximum capabilities of the tire, under the assumptions of steady-state cornering and the given tire model [19,30].

The maximum steady-state yaw rate and the vehicle β can be expressed as:

$$\{\begin{array}{l}{r}_{\max }=\frac{\mu g}{v_{\mathrm{x}}}\\ {\beta }_{\max }={\alpha }_{\mathrm{r},\mathrm{sat}}+\frac{br}{v_{\mathrm{x}}}\\ {\alpha }_{\mathrm{r},\mathrm{sat}}=\arctan (\frac{3\mu mg}{C_{\alpha }{}_{\mathrm{r}}}\frac{a}{a+b})\end{array}$$

(9)

where g is the gravity, α_r,sat is the rear slip angle that produces maximal lateral force of rear tire.

The vehicle stability constraints defined by Equation (9) can be expressed as the matrix inequality.

$$\left|H_{\mathrm{v}}x\right|\le G_{\mathrm{v}}$$

(10)

3.3. MPC Formation

The control objective of the path tracking MPC controller is to solve a convex optimization problem of at each time step. Then the first optimal value of control sequence is used as the future input, and the lateral force sequence is as follows.

$$F_{\mathrm{yf}}(k)=[F_{\mathrm{yf},1},F_{\mathrm{yf},2},\cdots ,F_{\mathrm{yf},k},\cdots F_{\mathrm{yf},N_c-1}]$$

(11)

The objective function can be expressed as:

$$\underset{U(k)}{\min }{J}_{Np}={\displaystyle \sum _{i=k}^{k+Np-1}{y}^{T}Qy}+{\displaystyle \sum _{i=k}^{k+Nc-1}\Delta u^{T}R\Delta u}+W\epsilon$$

(12)

$$s.t.\left[H_{\mathrm{v}}{x}^i\right]\le G_{\mathrm{v}},i=0,\cdots ,k,\cdots Nc-1$$

(13)

$$\left|\Delta u(k+i)\right|\le \Delta u_{\max },i=0,\cdots ,k,\cdots ,Nc-1$$

(14)

$$\Delta u(k+i),i=Nc,Nc+1,\cdots ,Np-1$$

(15)

$$\left|u(k+i)\right|\le u_{\max },i=0,\cdots ,k,\cdots ,Nc-1$$

(16)

where U(k) = [u(k), ε], and ε is a non-negative slack variable used to ensure the optimization problem is always feasible. N_p and N_c are the prediction horizon and control horizon, respectively. Q, R, and W are weighting matrices.

3.4. Longitudinal Speed Compensation

3.4.1. Influence on Prediction Error

In order to guarantee that the optimization problem is a convex problem, existing research has assumed that the longitudinal speed is not a variable in prediction horizon of the MPC controller. The prediction equation of vehicle states in the prediction horizon can be expressed as:

$$\{\begin{array}{l}\overline{X}={\overline{\mathsf{\psi }}}_{\mathrm{t}}x(t)+{\overline{H}}_{\mathrm{t}}\overline{U}+{\overline{\Omega }}_{\mathrm{t}}K+{\overline{\Theta }}_{\mathrm{t}}\overline{D}\\ \overline{Y}=C\overline{X}+f\end{array}$$

(17)

$$\begin{gathered} \overline{X}=\left[\begin{array}{c}{x}_{\mathrm{t}}^1\\ x_{\mathrm{t}}^2\\ ⋮\\ x_{\mathrm{t}}^{\mathrm{Nc}}\\ ⋮\\ x_{\mathrm{t}}^{\mathrm{Np}}\end{array}\right], \overline{U}=\left[\begin{array}{c}{u}_{\mathrm{t}}^1\\ u_{\mathrm{t}}^2\\ ⋮\\ u_{\mathrm{t}}^{\mathrm{Nc}}\\ ⋮\\ u_{\mathrm{t}}^{\mathrm{Nc}}\end{array}\right], K=\left[\begin{array}{c}{k}_{\mathrm{t}}^1(s)\\ k_{\mathrm{t}}^2(s)\\ ⋮\\ k_{\mathrm{t}}^{\mathrm{Nc}}(s)\\ ⋮\\ k_{\mathrm{t}}^{\mathrm{Np}}(s)\end{array}\right], \overline{D}=\left[\begin{array}{c}{d}_{\mathrm{c}}\\ d_{\mathrm{c}}\\ ⋮\\ d_{\mathrm{c}}\\ ⋮\\ d_{\mathrm{c}}\end{array}\right], {\overline{\mathsf{\psi }}}_{\mathrm{t}}=\left[\begin{array}{c}{A}_{\mathrm{c}}\\ A_{\mathrm{c}}^2\\ ⋮\\ A_{\mathrm{c}}^{\mathrm{Nc}}\\ ⋮\\ A_{\mathrm{c}}^{\mathrm{Np}}\end{array}\right], \\ {\overline{H}}_{\mathrm{t}}=\left[\begin{array}{cccccc}{B}_{\mathrm{c}}& 0& \cdots & 0& 0& 0\\ A_{\mathrm{c}}{B}_{\mathrm{c}}& B_{\mathrm{c}}& 0& \cdots & 0& 0\\ ⋮& \cdots & \cdots & 0& 0& 0\\ A_{\mathrm{c}}^{\mathrm{Nc}-1}{B}_{\mathrm{c}}& \cdots & B_{\mathrm{c}}& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ A_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}{B}_{\mathrm{c}}& \cdots & A_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-\mathrm{k}-1}{B}_{\mathrm{c}}& \cdots & A_{\mathrm{c}}{B}_{\mathrm{c}}& B_{\mathrm{c}}\end{array}\right], \\ {\overline{\Omega }}_{\mathrm{t}}=\left[\begin{array}{cccccc}{E}_{\mathrm{c}}& 0& \cdots & 0& 0& 0\\ A_{\mathrm{c}}{E}_{\mathrm{c}}& E_{\mathrm{c}}& 0& \cdots & 0& 0\\ ⋮& \cdots & \cdots & 0& 0& 0\\ A_{\mathrm{c}}^{\mathrm{Nc}-1}{E}_{\mathrm{c}}& \cdots & E_{\mathrm{c}}& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ A_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}{E}_{\mathrm{c}}& \cdots & A_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-\mathrm{k}-1}{E}_{\mathrm{c}}& \cdots & A_{\mathrm{c}}{E}_{\mathrm{c}}& E_{\mathrm{c}}\end{array}\right], \\ {\overline{\Theta }}_{\mathrm{t}}=\left[\begin{array}{cccccc}1& 0& \cdots & 0& 0& 0\\ A_{\mathrm{c}}& 1& 0& \cdots & 0& 0\\ ⋮& \cdots & \cdots & 0& 0& 0\\ A_{\mathrm{c}}^{\mathrm{Nc}-1}& \cdots & A_{\mathrm{c}}& 1& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ A_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}& \cdots & A_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-\mathrm{k}-1}& \cdots & A_{\mathrm{c}}& 1\end{array}\right] \end{gathered}$$

The optimal sequence of future input can be solved using Equations (12)–(16) and expressed as:

$$\Delta \overline{u}*={[\Delta {\overline{F}}_1,\Delta {\overline{F}}_2,\cdots ,\Delta {\overline{F}}_{\mathrm{Nc}-k},\cdots ,\Delta {\overline{F}}_{\mathrm{Nc}}]}^T$$

(18)

However, the longitudinal speed is a variable in the prediction horizon. So, without loss of generality, the longitudinal speed varied with constant acceleration, thus the longitudinal velocity in prediction horizon at time t can be expressed as:

$$v_{\mathrm{x}}(t)=v_{\mathrm{x}}+T\cdot a_{\mathrm{x}}=v_{\mathrm{x}}+\Delta v_{\mathrm{x}}$$

(19)

$$v_{\mathrm{x}}(t)=[v_{\mathrm{x}}^1,v_{\mathrm{x}}^2,\cdots ,v_{\mathrm{x}}^{\mathrm{k}},\cdots ,v_{\mathrm{x}}^{\mathrm{Np}}]$$

(20)

$$v_{\mathrm{x}}(t)=[v_{\mathrm{x}},(v_{\mathrm{x}}+\Delta v_{\mathrm{x}}),\cdots ,(v_{\mathrm{x}}+k\cdot \Delta v_{\mathrm{x}}),\cdots ,v_{\mathrm{x}}+(Np-1)\cdot \Delta v_{\mathrm{x}}]$$

(21)

where v_x is the initial value at time t in prediction horizon, and T is the sampling time.

The state space model of path tracking system Equation (7) can be expressed as:

$$\{\begin{array}{l}\dot{x}(t)=\widehat{A}x(t)+\widehat{B}u(t)+\widehat{E}k(s)+\widehat{d}\\ y=Cx+f\end{array}$$

(22)

$$\widehat{A}=\left[\begin{array}{cccc}0& (v_{\mathrm{x}}+k\cdot \Delta v_{\mathrm{x}})& 1& 0\\ 0& 0& 0& 1\\ 0& 0& -\frac{2{\overline{C}}_{\mathrm{r}}}{m(v_{\mathrm{x}}+k\cdot \Delta v_{\mathrm{x}})}& \frac{2b{\overline{C}}_{\mathrm{r}}}{m(v_{\mathrm{x}}+k\cdot \Delta v_{\mathrm{x}})}\\ 0& 0& \frac{2b{\overline{C}}_{\mathrm{r}}}{I_{\mathrm{Z}}(v_{\mathrm{x}}+k\cdot \Delta v_{\mathrm{x}})}& \frac{2b^2{\overline{C}}_{\mathrm{r}}}{I_{\mathrm{Z}}(v_{\mathrm{x}}+k\cdot \Delta v_{\mathrm{x}})}\end{array}\right], \widehat{E}=\left[\begin{array}{c}0\\ 0\\ -(v_{\mathrm{x}}+k\cdot \Delta v_{\mathrm{x}})\\ 0\end{array}\right]$$

$\widehat{B}=B$, $\widehat{d}=d$, k = 0, …, k, …, N_p − 1.

The discrete time system using the zero-order hold method is expressed as:

$$\{\begin{array}{l}x(k+1)={\widehat{A}}_{c}x(k)+{\widehat{B}}_{c}u(k)+{\widehat{E}}_{c}k(s)+{\widehat{d}}_c\\ y(k)=Cx(k)+f\end{array}$$

(23)

The prediction equation of the state variable in the prediction horizon can be expressed as:

$$\{\begin{array}{l}X={\mathsf{\psi }}_{\mathrm{t}}x(t)+H_{\mathrm{t}}U+{\Omega }_{\mathrm{t}}K+{\Theta }_{\mathrm{t}}D\\ Y=CX+f\end{array}$$

(24)

$$\begin{gathered} X=\left[\begin{array}{c}{x}_{\mathrm{t}}^1\\ x_{\mathrm{t}}^2\\ ⋮\\ x_{\mathrm{t}}^{\mathrm{Nc}}\\ ⋮\\ x_{\mathrm{t}}^{\mathrm{Np}}\end{array}\right], U=\left[\begin{array}{c}{u}_{\mathrm{t}}^1\\ u_{\mathrm{t}}^2\\ ⋮\\ u_{\mathrm{t}}^{\mathrm{Nc}}\\ ⋮\\ u_{\mathrm{t}}^{\mathrm{Nc}}\end{array}\right], K=\left[\begin{array}{c}{k}_{\mathrm{t}}^1(s)\\ k_{\mathrm{t}}^2(s)\\ ⋮\\ k_{\mathrm{t}}^{\mathrm{Nc}}(s)\\ ⋮\\ k_{\mathrm{t}}^{\mathrm{Np}}(s)\end{array}\right], D=\left[\begin{array}{c}{d}_{\mathrm{c}}\\ d_{\mathrm{c}}\\ ⋮\\ d_{\mathrm{c}}\\ ⋮\\ d_{\mathrm{c}}\end{array}\right], {\overline{\mathsf{\psi }}}_{\mathrm{c}}=\left[\begin{array}{c}{\widehat{A}}_{\mathrm{c}}\\ {\widehat{A}}_{\mathrm{c}}^1{\widehat{A}}_{\mathrm{c}}\\ ⋮\\ {\widehat{A}}_{\mathrm{c}}^{\mathrm{Nc}-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{Nc}-2}\cdots {\widehat{A}}_{\mathrm{c}}\\ ⋮\\ {\widehat{A}}_{\mathrm{c}}^{\mathrm{Np}-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{Np}-2}\cdots {\widehat{A}}_{\mathrm{c}}\end{array}\right], \\ {H}_{\mathrm{t}}=\left[\begin{array}{cccccc}{\widehat{B}}_{\mathrm{c}}& 0& \cdots & 0& 0& 0\\ {\widehat{A}}_{\mathrm{c}}^1{B}_{\mathrm{c}}& {\widehat{B}}_{\mathrm{c}}& 0& \cdots & 0& 0\\ ⋮& \cdots & \cdots & 0& 0& 0\\ {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{c}-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{c}-2}\cdots {\widehat{A}}_{\mathrm{c}}^1{B}_{\mathrm{c}}& \cdots & {\widehat{B}}_{\mathrm{c}}& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-2}\cdots {\widehat{A}}_{\mathrm{c}}^1{B}_{\mathrm{c}}& \cdots & {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-k-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-k-2}\cdots {\widehat{A}}_{\mathrm{c}}^1{B}_{\mathrm{c}}& \cdots & {\widehat{A}}_{\mathrm{c}}^1{B}_{\mathrm{c}}& {\widehat{B}}_{\mathrm{c}}\end{array}\right], \\ {\Omega }_{\mathrm{t}}=\left[\begin{array}{cccccc}{\widehat{E}}_{\mathrm{c}}& 0& \cdots & 0& 0& 0\\ {\widehat{A}}_{\mathrm{c}}^1{\widehat{E}}_{\mathrm{c}}& {\widehat{E}}_{{}_{\mathrm{c}}}^1& 0& \cdots & 0& 0\\ ⋮& \cdots & \cdots & 0& 0& 0\\ {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{c}-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{c}-2}\cdots {\widehat{A}}_{\mathrm{c}}^1{\widehat{E}}_{\mathrm{c}}& \cdots & {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{c}-1}{\widehat{E}}_{{}_{\mathrm{c}}}^{\mathrm{N}\mathrm{c}-2}& {\widehat{E}}_{{}_{\mathrm{c}}}^{\mathrm{N}\mathrm{c}-1}& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-2}\cdots {\widehat{A}}_{\mathrm{c}}^1{\widehat{E}}_{\mathrm{c}}& \cdots & {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}\cdots {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-k-1}{\widehat{E}}_{{}_{\mathrm{c}}}^{\mathrm{N}\mathrm{p}-k-1}& \cdots & {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}{\widehat{E}}_{{}_{\mathrm{c}}}^{\mathrm{N}\mathrm{p}-2}& {\widehat{E}}_{{}_{\mathrm{c}}}^{\mathrm{N}\mathrm{p}-1}\end{array}\right], \\ {\Theta }_{\mathrm{t}}=\left[\begin{array}{cccccc}1& 0& \cdots & 0& 0& 0\\ {\widehat{A}}_{\mathrm{c}}& 1& 0& \cdots & 0& 0\\ ⋮& \cdots & \cdots & 0& 0& 0\\ {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{c}-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{c}-2}\cdots {\widehat{A}}_{\mathrm{c}}& \cdots & {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{c}-1}& 1& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}{\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-2}\cdots {\widehat{A}}_{\mathrm{c}}& \cdots & A_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}\cdots A_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-k-1}& \cdots & {\widehat{A}}_{\mathrm{c}}^{\mathrm{N}\mathrm{p}-1}& 1\end{array}\right] \end{gathered}$$

The optimal sequence of input can be solved by minimizing the objective function (12):

$$\Delta u*={[\Delta F_1,\Delta F_2,\cdots ,\Delta F_{Nc-k},\cdots ,\Delta F_{Nc}]}^T$$

(25)

$$\vartheta =\Delta u*-\Delta \overline{u}*$$

(26)

Therefore, the MPC controller produces a output difference ϑ (lateral tire force of front axle) due to the difference of state matrices in the prediction horizon, which further leads to vehicle tracking error.

3.4.2. Influence on Control System Stability

We assume the feedback gain is g(k) in prediction horizon at time t according to Equation (7), and it is defined as:

$$\begin{array}{l}u(k+1)=g(k)x(k)\\ g=[{\mathrm{k}}_1,{\mathrm{k}}_2,{\mathrm{k}}_3,{\mathrm{k}}_4]\end{array}$$

(27)

The transfer function of the state feedback control system can be expressed as:

$$W_G(s)=C{[sI-(A+Bg)]}^{-1}B$$

(28)

Therefore, the state matrix of the closed-loop control system can be expressed as:

$$\begin{array}{l}f(s)=\left|sI-(A_{\mathrm{c}}+B_{\mathrm{c}}g)\right|\\ =s^2\left|\begin{array}{cc}s-(T{a}_{33}+1+k_{3}T{b}_{31})& T{a}_{34}+k_4{b}_{31}\\ -T{a}_{43}-T{k}_3{b}_{41}& s-T{a}_{44}+1+T{k}_4{b}_{41}\end{array}\right|\end{array}$$

(29)

$$\begin{array}{l}f(s)=s^2(s^2+s\chi +\gamma )\\ \chi =T{a}_{33}+T{a}_{44}+T{k}_3{b}_{31}+T{k}_4{b}_{41}+2\\ \gamma =(T{a}_{33}+T{k}_3{b}_{31}+1)(T{a}_{44}+T{k}_4{b}_{41}+1)+(T{a}_{43}+T{k}_3{b}_{41})(T{a}_{34}+T{k}_4{b}_{31})\end{array}$$

The closed-loop system is nominally progressively stable, and the characteristic roots are all within the unit circle [38]. Therefore, the system stability constraints are expressed as:

$$\{\begin{array}{l}{\chi }^2-4\gamma >0\\ \left|\frac{-\chi \pm \sqrt{{\chi }^2-4\gamma }}{2}\right|<1\end{array},or\{\begin{array}{l}{\chi }^2-4\gamma <0\\ \left|\chi \right|<2\\ 0<4\gamma -{\chi }^2<4\\ 0<\gamma <1\end{array}$$

(30)

When v_x becomes (v_x + k·Δv_x ) (k = 0, ... k, …N_p − 1) in the prediction horizon, the system stability constraints will produce difference.

$$\{\begin{array}{l}{(\chi +\Delta \chi )}^2-4(\gamma +\Delta \gamma )>0\\ \left|\frac{-(\chi +\Delta \chi )\pm \sqrt{{(\chi +\Delta \chi )}^2-4(\gamma +\Delta \gamma )}}{2}\right|<1\end{array},or\{\begin{array}{l}{(\chi +\Delta \chi )}^2-4(\gamma +\Delta \gamma )<0\\ \left|(\chi +\Delta \chi )\right|<2\\ 0<4(\gamma +\Delta \gamma )-{(\chi +\Delta \chi )}^2<4\\ 0<(\gamma +\Delta \gamma )<1\end{array}$$

(31)

where Δχ and Δγ are the produced difference due to (k·Δv_x).

3.4.3. Longitudinal Speed Compensation

A large number of existing studies have taken the steering angle or the lateral force of the front tire as the control output. The above analysis shows that when the longitudinal speed changes in the prediction horizon, the vehicle states produce prediction errors in the prediction horizon in each optimization cycle, and the stability constraints of the closed-loop system also change. Thus, the difference in control output is produced, which further generates the tracking error throughout the path tracking process. Therefore, systematically considering the convex optimization problem and the variation of longitudinal speed in prediction horizon, a speed compensation strategy is proposed to reduce tracking error. The longitudinal speed compensation model can be expressed as:

$$\{\begin{array}{l}{v}_{\mathrm{x}}*=f(v_{\mathrm{x}},v_{\mathrm{x},\max },a_{\mathrm{x}},T_{\mathrm{T}},\tau )\\ f=\min (v_{\mathrm{x}},v_{\mathrm{x},\max },a_{\mathrm{x}},T_{\mathrm{T}},\tau )\\ v_{\mathrm{x},\max }=\sqrt{\mu g/k(s)}\\ a_{\mathrm{x}}\le \frac{\sqrt{{(\mu g)}^2+{[{(v_{\mathrm{x}}*)}^{2}k(s)]}^2}}{v_{\mathrm{x}}*}\end{array}$$

(32)

where τ is the compensation factor, T_T is the time length of prediction horizon, a_x is the longitudinal acceleration.

In order to accurately capture the propagation of v_y and r at high frequency, sampling time T is set to be 0.02 s, considering the accuracy of the vehicle information captured. Note that the value of the control horizon Nc and prediction horizon Np are set to be Nc = 15 and Np = 50, based on the consideration of higher control accuracy and calculation efficiency. The compensation factor τ is set to be 0.5. This choice is made because in the high-speed driving, the speed of compensation cannot exceed the actual speed of vehicle. The weighting matrices are obtained by iteratively tuning by the principles mentioned in [26,27], R = 1, Q= diag{1000, 5}, W = [10, 10, 10, 10].

3.4.4. Asymptotic Stability of MPC Control System

To prove the asymptotic stability of closed-loop discrete system formed by Equation (8), we assume the final target can be achieved, which is the error between the final predicted output and reference states become zero. The Lyapunov function V(x_k) is defined by the objective function formed by Equation (12) at the sample time k [38,39], which can be expressed as

$$V(x_k)={\displaystyle \sum _{i=1}^{Np}{y}_{k+i}^T{Q}_1{y}_{k+i}}+{\displaystyle \sum _{i=1}^{Nc}\Delta u_{k+i}^T{R}_1\Delta u_{k+i}}$$

(33)

where Q₁ and R₁ are the nonnegative weighting coefficient.

The Lyapunov function V(x_k+1) at the next sample time k + 1 can be expressed as

$$V(x_{k+1})={\displaystyle \sum _{i=1}^{Np}{y}_{k+1+i}^T{Q}_1{y}_{k+1+i}}+{\displaystyle \sum _{i=1}^{Nc}\Delta u_{k+1+i}^T{R}_1\Delta u_{k+1+i}}$$

(34)

An intermediate function V_in is defined, which is obtained by shifting the optimal inputs sequence of V(x_k) one step forward and setting its last input Δu_k+Nc as zero. The intermediate function is a non-optimal objective function comparing with V(x_k+1), so we can obtain the inequality

$$V(x_{k+1})\le V_{\mathrm{in}}$$

(35)

$$V(x_{k+1})-V(x_k)\le V_{\mathrm{in}}-V(x_k)$$

(36)

According to definition of the intermediate function, V_in shares the same future inputs sequence and the predictive outputs sequence with V(x_k) for the sample time k + 1, ..., k + N_c − 1, the difference between these two functions can be expressed as

$$V_{\mathrm{in}}-V(x_k)=y_{k+Np}^T{Q}_1{y}_{k+Np}-y_{k+1}^T{Q}_1{y}_{k+1}-\Delta u_k^T{R}_1\Delta u_k$$

(37)

The error between the final predicted output and reference states is zero. Then, it can be obtained that

$$V_{\mathrm{in}}-V(x_k)=-y_{k+1}^T{Q}_1{y}_{k+1}-\Delta u_k^T{R}_1\Delta u_k$$

(38)

$$V(x_{k+1})-V(x_k)\le -y_{k+1}^T{Q}_1{y}_{k+1}-\Delta u_k^T{R}_1\Delta u_k\le 0$$

(39)

Then the closed-loop system is asymptotic stable.

4. Simulation and Results

4.1. Simulation Vehicle

The proposed controller was verified using a simulation platform, which was conducted via MATLAB/Simulink and CarSim, and it included a validated high-fidelity full-vehicle dynamics model. The parameters of the vehicle and path are listed in

Table 1. Parameters of the vehicle and path.

Parameter	Symbol	Value	Units
Vehicle mass	m	1230	kg
Yaw inertia	IZ	1343.1	kg·m2
Front axle-O distance	a	1.04	m
Rear axle-O distance	b	1.56	m
Front cornering stiffness	Caf	48,840	N/rad
Rear cornering stiffness	Car	32,887	N/rad
Friction coefficient	μ	1	n/a

.

4.2. Reference Path

In order to verify the performance of the proposed controller, we designed the reference path, which is parameterized as a curvature profile and position profile, as shown in Figure 6. The path generation methods were proposed in [22,29]. The longitudinal speed tracking was achieved using a PID controller, the longitudinal speed fluctuated between 55–65 (km/h) in order to verify the proposed controller.

4.3. Search Algorithm for Optimal Reference Point

In order to improve the computational efficiency, the dichotomy was applied in the optimal reference point search process. The search process is divided into two layers, the first layer uses a large step search to obtain the approximate position, then a small step search is used to obtain the precise reference point position, as shown in Figure 7.

Search Algorithm:

Step 1: The reference point P(t) of t previous moment t is the initial point P_D(1) of current moment (t + 1), calculating the distance D(1) between the vehicle mass center O(t + 1) and P_D(1).

Step 2: Obtain the point P_D(n) on the path with the large step s_D, calculating the distance D(n) between O(t) and P_D(n), if D(n) > D(n − 1), then stop.

Step 3: The reference point P_D(n − 1) is the initial point P_d(n), obtain the point P_d(h) on the path with small step s_d = s_D/2, if d(h) > d(h − 1), then stop.

Step 4: Return to step 3 until s_d < ε (ε is the precision value).

Step 5: The point P(t + 1) is the obtained optimal reference point, and expressed as:

$$\{\begin{array}{l}e(t+1)=\left|O(t+1)\mathrm{P}(t+1)\right|\cdot \mathrm{sign}(\mathrm{P}(t),\mathrm{P}(t+1))\\ \Delta \phi (t+1)=\theta -\arctan (\frac{y_h-y_{h-1}}{x_h-x_{h-1}})+\beta (t+1)\end{array}$$

(40)

where sign indicates direction of the tracking error.

4.4. Controller Performance

The first controller was an MPC controller without speed compensation (Original), which is indicated by a blue dotted line, and the second controller was an MPC controller with longitudinal speed compensation (proposed), which is indicated by a red solid line.

Figure 8 shows the simulation results of the tracked path of different controllers. The vehicle longitudinal speed curves are highly similar under the action of two controllers. The deviation occurred near the position where the curvature changes greatly. Both controllers can track the reference path precisely. Figure 9 shows the simulation results of the lateral deviation and the course deviation of different controllers, and the statistics for the results are shown in

Table 2. Comparison of different controllers.

Controller	$\overline{\left|\mathit{e}\right|}\hspace{1em}(\mathbf{m})$	$\mathit{M}\mathit{S}\mathit{E}(\left|\mathit{e}\right|)\hspace{1em}(\mathbf{m})$	$\overline{\left|\Delta \mathit{\phi }\right|}\hspace{1em}(\mathbf{deg})$	$\mathit{M}\mathit{S}\mathit{E}(\left|\Delta \mathit{\phi }\right|)\hspace{1em}(\mathbf{deg})$	$\mathit{M}\mathit{a}\mathit{x}(\left|\mathit{e}\right|)\hspace{1em}(\mathbf{m})$
Original	0.1624	0.1367	0.6678	1.1014	0.5292
Proposed	0.1544	0.1289	0.6374	1.0571	0.5022

where $\overline{\left|e\right|}$ is the average value of absolute lateral deviation; $MSE(\left|e\right|)$ is the mean squared error of absolute lateral deviation; $\overline{\left|\Delta \phi \right|}$ is the average value of absolute course deviation; $MSE(\left|\Delta \phi \right|)$ is the mean squared error of absolute lateral deviation; $Max(\left|e\right|)$ is the maximum of absolute lateral deviation.

. The maximum absolute lateral deviation occurred near the position where the curvature changes greatly. When the reference path curvature changed greatly, the vehicle tracking performance became worse due to the controller, vehicle inertia and other factors. The proposed controller performed better; the performance difference analyzed by these indicators allowed us to find the average value of absolute lateral deviation, the average value of absolute course deviation, the mean squared error of absolute lateral deviation and the mean squared error of absolute course deviation. The proposed controller reduced the average value of absolute lateral deviation by 5% and reduced the average value of absolute course deviation by 4.5%. The tracking error was relatively small for both controllers when the path changed gently, shown in Figure 6 and Figure 9. The tracking error increased significantly when the path changed suddenly. However, the proposed controller performed better, especially when the path curvature varies suddenly.

Figure 10 shows the frequency distribution histogram of lateral deviation and course deviation of a different controller, the small error frequency increased through the speed compensation strategy, and the larger the error frequency decreased, the results indicates that proposed method is effective.

Figure 11 shows the lateral force of the front tire of different controllers. The maximum value of lateral force occurred near the position where the curvature changed greatly and the tracking error appeared to be maximal, as shown in Figure 6 and Figure 9. The proposed controller output was relatively small compared to the original controller, which improved the economy of control. The vehicle sideslip angle fluctuated from −5° to 5° of controllers, and the vehicle yaw rate ranged from −0.8 rad/s to 0.8 rad/s, the results indicate that the vehicle maintained stability, as shown in Figure 12. Thus, the simulation results show that the proposed controller exhibits better tracking performance.

5. Conclusions

An MPC path tracking controller with longitudinal speed compensation that can reduce the tracking error and maintain vehicle stability for an autonomous vehicle is designed. Under the condition that longitudinal speed is varied in the prediction horizon, the influence on controller performance caused by speed changes is discussed, by analyzing the variations of the state vector and the stability constraints of closed-loop system in prediction horizon, and results indicates that the speed fluctuation will produce corresponding tracking error and change the stability constraints. Based on the analyzed results, an MPC path tracking controller with longitudinal speed compensation is proposed. Simulation results show the proposed controller reduces the average value of absolute lateral deviation by 5% and the average value of absolute course deviation by 4.5%. This paper aims to verify the effectiveness of path tracking MPC controller with speed compensation under high-speed condition. Future work will study the adaptive compensation strategy under different speed conditions.