Trajectory Tracking Model Predictive Controller Design for Autonomous Vehicles with Updating Constrains of Tire Characteristics

Abstract

In this paper, we address the problem of trajectory tracking control of autonomous vehicles by considering the nonlinear characteristics of tires. By considering the influence of the tires’ dynamics on steering stability, the proposed predictive controller can track the desired trajectory and desired velocity in the presence of road curvature while minimizing the lateral tracking deviation. First of all, a hierarchical control structure is adopted, in which the upper-level controller is used to calculate the desired acceleration and the desired front-wheel angle to maintain the control target, and the lower-level controller realized the command through the corresponding component devices. Moreover, a force estimator is designed based on the radial basis function (RBF) neural network to estimate the lateral force of the tires, which is incorporated into the boundary conditions of the vehicle envelope constraint to improve the adaptability of the controller to the vehicle performance. Finally, the proposed controller is tested by co-simulation of CarSim (a simulation software specifically for vehicle dynamics)/Simulink (a modular diagram environment for multidomain simulation as well as model-based design) and hardware-in-loop simulation system. The co-simulation and experimental results demonstrate the controller safely driving at the vehicle’s handling limits and effectively reduce the slip phenomenon of the vehicle.

1. Introduction

Autonomous vehicles have a great potential in improving traffic flow and safety performance, which makes the technology of autonomous vehicle more and more popular, especially in developed countries and emerging markets [1,2]. As one of the important components of autonomous vehicles, the trajectory tracking control system aims to track the desired trajectory by controlling the actuators of the steering wheel, brake and throttle [3]. The high nonlinearity of the vehicle dynamics will make it difficult for the trajectory tracking controller to ensure the tracking accuracy and stability of the vehicle at the same time [4]. Therefore, a good trajectory tracking control system should fully consider the road information and the nonlinearities characteristics of tires.

In order to address the trajectory tracking problem, much research has been devoted to selecting the suitable control algorithms, such as the linear quadratic regulator (LQR) [5,6], fuzzy control [7,8], the sliding mode control (SMC) [9,10] and the proportional-integrated-differential (PID) control [11]. However, the constrains caused by tires during trajectory tracking cannot be fully considered in these controller design. In recent years, the model prediction control (MPC) methods have been widely used in trajectory tracking control, which is characterized by rolling optimization and dealing with the optimization problem while considering the input and output constraints [12]. A large number of researches indicate that the MPC algorithms are good at dealing with the trajectory tracking control problems with the multi-constraints [13,14]. Some researches converted the trajectory tracking control problem based on the MPC controller that considering vehicle stability and environmental constraints [15] or collision avoidance restraints [16]. Some researches improve trajectory tracking accuracy by adaptively adjusting the weight matrix in the target function [17,18]. In addition, a kinematic cascade MPC-PID controller with side slip angle compensation was designed in Ref. [19] to solve the trajectory tracking control problem of high-speed autonomous vehicles. The authors of [20] proposed a dead-band penalty function to solve the inequality constraints and guarantee smoothness of the solution.

More precise vehicle models can improve the controller’s ability to predict the future behavior of the vehicle. Banginwar proposed a real-time predictive vehicle controller by using Pontryagin’s optimization method in Ref. [21]. Meanwhile, Raigoza enhanced the trajectory tracking method with automatic collision avoidance in Ref. [22]. The authors of [23] designed a MPC controller, which takes the 8-degree-of-freedom vehicle model as the prediction model and the 14-degree-of-freedom vehicle model as the control object to achieve trajectory tracking. The Unscented Kalman Filter (UKF) was used in Ref. [24] to estimate the vehicle states, which were used to judge vehicle stability. Although the MPC method with a prediction function could significantly improve the trajectory tracking accuracy, it needed to repeatedly solve optimization problems at each control step, which leads to a heavy computing burden and potential risks in real-time control. In addition, some constraints have a great impact on the trajectory tracking performance of the vehicle, such as the slip and body deviation [25]. Therefore, a suitable equivalent model of vehicle dynamics should be constructed by considering the reasonable constraints

Ignoring the aerodynamics and gravity, the tire force plays a critical role in improving the vehicle’s handling performance. However, most researches limited the tire model to the linear area [26]. With increasing the vehicle speed, the nonlinearity caused by the tires’ dynamics had an influence on the trajectory tracking accuracy, even the handling stability. In Ref. [27], the piecewise linear approximation of the tire characteristic curve method and root mean square error (RMSE) were used to obtain segmented points in different regions to improve the approximate accuracy of the tire models. By combining regularization with continuous linearization, a nonlinear tire model was established to estimate the lateral force of the tire. In addition, Mammar et al. [28] derived the tire cornering stiffness and slip angle based on the tire characteristics to establish a tire model. However, the piecewise affine (PWA) tire model and tire cornering stiffness estimation are only the approximation of the tire model in a limited area, which could not capture the nonlinearity evident for the trajectory tracking control. Thus, the tires’ dynamics should be fully considered.

It is essential to guarantee the vehicle stability, and it should be mentioned that equilibrium analysis [29] or empirical judgment methods [30] are indeed needed to judge vehicle stability, along with the vehicle stability judged using the envelope constraints [31]. The lateral velocity as a boundary condition for the envelope constraints could not be obtained from the sensors directly. Considering that the lateral velocity can be estimated accurately as presented in Refs. [32,33], the lateral velocity could be gained as a configuration, which is set as the input of the envelope constraint of the vehicle swing.

Motivated by the above discussion, this paper presents a trajectory tracking controller design with consideration of the tires’ dynamics and slip constraints. The innovations of the paper are summarized as follows:

• Proposing a MPC controller by considering the tires’ dynamics and the road curvature for the trajectory tracking control;
• Designing a tire force estimator based on the radial basis function (RBF) neural network to estimate the vehicle driving states, which are used to update the slip constraint.
• Conducting the co-simulation (CarSim/Simulink) and Hardware-in-loop (HIL) platform to validate the performance of the proposed trajectory tracking control system.

The remainder of this paper is organized as follows. In Section 2, the control strategy of the trajectory tracking control system is described. In Section 3, a trajectory tracking model is established based on the trajectory tracking control system. In Section 4, based on the MPC algorithm, the trajectory tracking problem considering the slip constraint is solved. In Section 5.1, the CarSim/Simulink co-simulation platform is constructed to verify the effectiveness of the envelope constraint under extreme driving states and test that the vehicle can still drive effectively beyond the nonlinear region of the tire, and it is compared with the algorithm proposed in Ref. [34]. In Section 5.2, the HIL platform is built, and the effectiveness of the algorithm is verified based on the quintic polynomial function trajectory planning, and it is compared and verified with the co-simulation. Section 6 summarizes the contents of this paper.

2. Proposed Trajectory Tracking Control System

A hierarchical control structure is proposed to construct the controller for the trajectory tracking control system, as shown in Figure 1. The overall goal of the control strategy is to calculate the desired front-wheel steering angle and longitudinal acceleration and implement this control target via the corresponding component devices to eventually tracking the desired trajectory. The control structure consists of two sub-controllers, which are the upper-level controller and the lower-level controller. The functional implementation of the upper-level controller is divided into two parts, including the vehicle trajectory tracking model and control law, which are used to collect the state between the vehicle and the desired trajectory and driver input, and then compute the desired front-wheel steering angle and desired longitudinal acceleration. The lower-level controller realizes the output of the upper-level controller via the steering and driving devices to track the desired trajectory.

3. Trajectory Tracking Model

A valuable vehicle model can improve the smoothness of the vehicle during driving and the handling stability of the vehicle during real-time control of autonomous vehicles. Thus, in this section, the trajectory tracking model is established by considering the force of the tires, as shown in Figure 2.

The model is established based on the following assumptions:

• The influence of road slope is not taken into account;
• The coupling relationship between longitudinal and lateral forces is ignored; only the pure cornering characteristics of the tire is considered;
• The load transfer of the tire is ignored;
• The air resistance is ignored;

The heading error $e_{\phi }$ and longitudinal velocity error $e_v$ are defined as:

$$\{\begin{array}{c}{e}_{\phi }=\phi -{\phi }_r\\ e_v=v_x-v_r\end{array},$$

(1)

where $e_{\phi }$ represents the orientation error of the yaw angle with respect to the desired trajectory, and $e_v$ is the error between the current and the desired longitudinal velocity.

The desired yaw rate is obtained:

$${\dot{\phi }}_r=v_x{C}_R,$$

(2)

where $C_R$ is the road curvature.

Thus, combined Equation (1) with Equation (2):

$$\{\begin{array}{l}{\dot{e}}_{\phi }=r-v_x{C}_R\hfill \\ {\dot{e}}_v={\dot{v}}_x-{\dot{v}}_r\hfill \end{array},$$

(3)

The assumption for the heading error is small; ${\dot{e}}_y$ can be defined as:

$${\dot{e}}_y=v_y+v_x{e}_{\phi },$$

(4)

The vehicle dynamics model can be described by the following differential equations:

$$\{\begin{array}{l}m\left({\dot{v}}_x-r{v}_y\right)=F_{xf}+F_{xr}cos{\delta }_f=m{a}_x\hfill \\ m\left({\dot{v}}_y+r{v}_x\right)=F_{yf}+F_{yr}\hfill \\ I_z\dot{r}=l_f{F}_{yf}-l_r{F}_{yr}\hfill \end{array},$$

(5)

where $a_x$ is the generalized longitudinal acceleration of the vehicle; $l_f$ and $l_r$ are the distance from CG(center of gravity) to the front/real axle; $F_{xf}$ is the longitudinal force of the front wheel; $F_{xr}$ is the longitudinal force of the rear wheel; and ${\delta }_f$ is the front-wheel steering angle. The lateral force of the front/rear wheel $F_{yf}$ and F_yr are functions of the slip angle of the tire, which can be calculated in the following ways:

$$\{\begin{array}{l}{F}_{yf}=f_{yf}\left({\alpha }_f,\mu ,F_{zf}\right)\hfill \\ F_{yr}=f_{yr}\left({\alpha }_r,\mu ,F_{zr}\right)\hfill \end{array},$$

(6)

where $\mu$ is the road friction coefficient, and $F_z$ represents the total vertical load of the vehicle.

The slip angles ${\alpha }_f$ and ${\alpha }_r$ indicate the angle between the wheel velocity and the direction of the wheel itself:

$$\{\begin{array}{l}{\alpha }_f=ta{n}^{-1}\left(\frac{v_y+l_{f}r}{v_x}\right)-{\delta }_f\hfill \\ {\alpha }_r=ta{n}^{-1}\left(\frac{v_y-l_{r}r}{v_x}\right)\hfill \end{array},$$

(7)

The total vertical load of the vehicle is distributed between the front and rear wheels according to the geometry of the car model, described by the parameters $l_f$ and $l_r$:

$$\{\begin{array}{l}{F}_{zf}=\frac{l_{f}mg}{2\left(l_f+l_r\right)}\hfill \\ F_{zr}=\frac{l_{r}mg}{2\left(l_f+l_r\right)}\hfill \end{array}.$$

(8)

By analyzing Equation (6) and considering tire characteristics, we find that there are three main factors affecting the lateral force $F_y$: the vertical load $F_z$, the road friction coefficient $\mu$, and the slip angle α, which is a complex nonlinear time-varying function with three inputs and one output. It is difficult to establish its accurate mathematical model with traditional methods, so this paper establishes a neural network model of the lateral force by the characteristics of the RBF neural network approximation of nonlinear functions as shown in Figure 3. The neural network consists of three layers. The first layer is the input layer, which consists of three input quantities: $F_z, \mu , \alpha$. The second layer is the hidden layer, and the activation function adopted by the hidden layer is a Gaussian function:

$$C_j=\exp \left(-\frac{\Vert \mathrm{X}-C_j\Vert {}^2}{{\beta }_j^2}\right)j=1,2,\cdots ,n,$$

(9)

where ${\beta }_j$ is the center width of the hidden layer; X is the input vector; $C_j$ is the central unit of the jth radial basis function, which is the same as the input vector X dimensionality. The third layer is the output layer, which is the output lateral force $F_y$, and its solution is:

$$F_y={\displaystyle \sum }_{i=1}^n{w}_i{G}_i,$$

(10)

where $w_i$ is the weight.

When the road friction coefficient is fixed, the lateral force is estimated by the RBF neural network as shown in the Figure 4:

As shown in Figure 4, the lateral force estimator based on the RBF neural network can estimate the lateral force of the tire well.

Equations (3)–(10) can be combined to obtain the nonlinear vehicle dynamics equations:

$$\{\begin{array}{l}{\dot{v}}_y=a_{11}{v}_y+a_{11}r+b_1{\delta }_f\hfill \\ \dot{r}=a_{21}{v}_y+a_{22}r+b_2{\delta }_f\hfill \\ {\dot{e}}_y=v_y+v_x{e}_{\phi }\hfill \\ {\dot{e}}_{\phi }=r-C_R\left(e_v+v_{xr}\right)\hfill \\ {\dot{e}}_v=r{v}_y-{\dot{v}}_{xr}+a_x\hfill \end{array},$$

(11)

where, $a_{11}=\frac{1}{m}\left(\frac{\partial f_{yf}}{\partial v_y}+\frac{\partial f_{yr}}{\partial v_y}\right)$, $a_{12}=\frac{1}{m}\left(\frac{\partial f_{yf}}{\partial r}+\frac{\partial f_{yr}}{\partial r}\right)-v_x$, $a_{21}=\frac{1}{I_z}\left(l_f\frac{\partial f_{yf}}{\partial v_y}-l_r\frac{\partial f_{yr}}{\partial v_y}\right)$, $a_{22}=\frac{1}{I_z}\left(l_f\frac{\partial f_{yf}}{\partial r}-l_r\frac{\partial f_{yr}}{\partial r}\right)$, $b_1=\frac{1}{m}\left(\frac{\partial f_{yf}}{\partial {\delta }_f}\right)$, $b_2=\frac{1}{I_z}\left(l_f\frac{\partial f_{yf}}{\partial {\delta }_f}\right)$.

Equation (11) is rewritten as a linear state space equation form:

$$\dot{x}\left(t\right)=A\left(t\right)x\left(t\right)+B_1\left(t\right)u\left(t\right)+B_2\left(t\right)\rho \left(t\right),$$

(12)

with $A\left(t\right)=\left[\begin{array}{ccccc}{a}_{11}& a_{12}& 0& 0& 0\\ a_{21}& a_{22}& 0& 0& 0\\ 1& 0& 0& v_x& 0\\ 0& 1& 0& 0& C_R\\ r& 0& 0& 0& 0\end{array}\right]$, $B_1\left(t\right)=\left[\begin{array}{cc}\begin{array}{c}0\\ 0\\ 0\end{array}& \begin{array}{c}{b}_1\\ b_2\\ 0\end{array}\\ \begin{array}{c}0\\ 1\end{array}& \begin{array}{c}0\\ 0\end{array}\end{array}\right]$, $B_2\left(t\right)=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ -v_{xr}& 0\\ 0& -1\end{array}\right]$, where the state variables is $x\left(t\right)={\left[\begin{array}{ccc}{v}_y& r& \begin{array}{ccc}{e}_y& e_{\phi }& e_v\end{array}\end{array}\right]}^T$, the control input is $u\left(t\right)={\left[\begin{array}{cc}{a}_x& {\delta }_f\end{array}\right]}^T$, the output variable the output variable is $y\left(t\right)={\left[\begin{array}{ccc}{v}_y& r& \begin{array}{ccc}{e}_y& e_{\phi }& e_v\end{array}\end{array}\right]}^T$, and $\rho \left(t\right)={\left[C_R,{\dot{v}}_r\right]}^T$ is defined as the disturbance.

In real-time control applications, the trajectory tracking model is usually applied to the discrete time domain, and then converted to a discrete time domain through zero-order retention (ZOH) discretization, expressed as:

$$\{\begin{array}{l}x\left(k+1\right)=A_{d}x\left(k\right)+B_{u}u\left(k\right)+B_d\rho \left(k\right)\hfill \\ y\left(k\right)=Cx\left(k\right)\hfill \end{array},$$

(13)

where $k$ represents the $k_{th}$ sampling time, $A_d$, $B_u$, $B_d$ and C are system matrices, which can be expressed as:

$$\begin{gathered} A_d={\displaystyle \sum }_{k=0}^{\infty }\frac{{\mathrm{A}}^k{T}_s^k}{k!},B_u={\displaystyle \sum }_{k=0}^{\infty }\frac{{\mathrm{A}}^{k-1}{T}_s^k}{k!}{B}_1, \\ {B}_d={{\displaystyle \sum }}_{k=0}^{\infty }\frac{{\mathrm{A}}^{k-1}{T}_s^k}{k!}{B}_2,C=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right], \end{gathered}$$

(14)

where $T_s$ is sampling time ($T_s=50 \mathrm{ms}$), $C$ is an identity matrix, and y ∈ R⁵ is the state quantity of vehicle trajectory tracking system.

4. Control Law Allocation

This section describes the proposed control law used for calculating the front-wheel angle and the longitudinal acceleration. The control law is designed to solve the MPC-based trajectory tracking control problem. In addition, this control law takes into account the vehicle’s handling stability, which is defined by the envelope constraint.

4.1. Control Objectives

To improve the lateral tracking accuracy and the vehicle’s handling stability, the lateral position deviation, heading error, and longitudinal velocity error of the vehicle should be minimized, and the yaw rate and lateral velocity of the vehicle should be limited by the maximum lateral force and maximum slip angle that the tire can provide. Therefore, the targets of the trajectory tracking control system can be considered as:

• The target of the trajectory tracking behavior can be regarded as the vehicle’s accurate tracking of the desired trajectory:

  $$Objective\left(A\right):\{\begin{array}{c}{e}_y\left(k\right)\to 0\\ e_{\phi }\left(k\right)\to 0\\ e_v\left(k\right)\to 0\end{array},as k\to \infty ,$$

  (15)
• The controller proposed uses the envelope suggested by Ref. [35], and the vehicle stability is judged by the vehicle’s velocity state: lateral velocity $\left(v_y\right)$ and the yaw rate ($r$), and we estimate the lateral force under different driving conditions through the RBF neural network. Using the lateral force estimated by the RBF neural network, the threshold of the slip angle α under different driving conditions is calculated from the tire model, and the lateral velocity $\left(v_y\right)$ limit is defined by limiting the slip angle of the rear tire. The maximum slip angle limit of the rear tire can be transformed into the constraint on the lateral velocity $\left(v_y\right)$ and the yaw rate $\left(r\right)$ of the vehicle by Equation (7):

  $$\left|\frac{v_y-l_{r}r}{v_x}\right|\le {\alpha }_t,$$

  (16)

  where the threshold of the yaw rate can be obtained according to the maximum lateral force and Equation (5). In addition, the maximum lateral force of the tire is estimated by the RBF neural network.

  $$\left|r+\frac{g}{v_x}\phi \right|\le min\left(\frac{F_{yf}\left(1+\frac{l_f}{l_r}\right)}{m{v}_x},\frac{F_{yr}\left(1+\frac{l_r}{l_f}\right)}{m{v}_x}\right)$$

  (17)
  The constraints (16) and (17) form a closed envelope, as shown in Figure 5. Equation (16) limits the control boundary of the yaw rate, and the Equation (17) limits the control boundary of the lateral velocity. When all states are within the envelope, the vehicle stability can be guaranteed.
• In order to achieve the required lateral tracking accuracy and control stability, the steering angle and the physical characteristics of the driving and braking should be considered. Apply the following constraints to the vehicle’s variables:

  $$\{\begin{array}{l}\Delta u_{min}\le \Delta u\left(k\right)\le \Delta u_{max}\hfill \\ u_{min}\le u\left(k\right)\le u_{max}\hfill \end{array},$$

  (18)

  where $u\left(k\right) \mathrm{and} \Delta u\left(k\right)$ are the control input and control input increment of the trajectory tracking control system.

4.2. Trajectory Tracking Controller Based on MPC

The function of the proposed trajectory tracking control system is to calculate the dynamic control command and subsequently manipulate the vehicle to the desired trajectory. This function is achieved with the control law and can be translated into solving an MPC problem. The schematic block diagram of the MPC algorithm is shown in Figure 6.

During the transitional state, the reference is $Y_r\left(k+1\right)$ described as the control objective $(A_d)$, and the disturbances $\rho \left(k\right)$ are the road curvature and the desired longitudinal acceleration. The output $y\left(k+1\right)$ is the lateral velocity, yaw rate, heading error and longitudinal velocity error. The MPC controller consists of three parts: the constraints, the trajectory tracking control model, and the cost function. The trajectory tracking model is designed according to the Equation (11). The constraints are shown in Equations (16)–(18).

To facilitate the design of the controller, the state vector $x\left(k\right)$ and the input increment $\Delta u\left(k\right)$ are usually coupled in a broadening vector that can be expressed as $\tilde{x}\left(k\right)={\left[x\left(k\right), \Delta u\left(k\right)\right]}^T$. Rewriting Equations (13) and (14):

$$\{\begin{array}{l}\tilde{x}\left(k+1\right)=\tilde{A}\tilde{x}\left(k\right)+{\tilde{B}}_u\Delta u\left(k\right)+{\tilde{B}}_d\rho \left(k\right)\hfill \\ \tilde{y}\left(k\right)=\tilde{C}\tilde{x}\left(k\right)\hfill \end{array},$$

(19)

where $\tilde{A}=\left[\begin{array}{cc}{A}_d& B_d\\ 0_{Nu\times Nx}& I_{Nu}\end{array}\right]$, ${\tilde{B}}_u=\left[\begin{array}{c}{B}_u\\ I_{Nu}\end{array}\right], {\tilde{B}}_d=\left[\begin{array}{c}{B}_d\\ I_{Nu}\end{array}\right], \tilde{C}=\left[C,0_{5\times 2}\right],$ N_x is the dimension of the state quantity and N_u means the dimension of the control quantity, and the change of the control input is $\Delta u\left(k\right)=u\left(k\right)-u\left(k-1\right)$. The predicted output performance vector and the sequence of the future incremental inputs at time step $k$ are denoted as $Y\left(\mathrm{k}\right)$ and $\Delta U\left(k\right)$, respectively.

$$\{\begin{array}{l}Y\left(\mathrm{k}\right)={\left[\tilde{y}\left(k+1|k\right),\tilde{y}\left(k+2|k\right),\cdots ,\tilde{y}\left(k+N_p|k\right)\right]}^T\hfill \\ \Delta U\left(k\right)={\left[\Delta u\left(k\right),\Delta u\left(k+1\right),\cdots ,\Delta u\left(k+N_c-1\right)\right]}^T\hfill \end{array},$$

(20)

where $N_p$ is the prediction horizon and $N_c$ means the control horizon, $\tilde{y}\left(k+1\right),\tilde{y}\left(k+2\right),\cdots ,\tilde{y}\left(k+N_p\right)$ are the predictive performance at the step $k$, $\Delta u\left(k\right),\Delta u\left(k+1\right),\cdots ,\Delta u\left(k+N_c-1\right)$ are the control input increments for the control time domain.

The MPC controller can predict the state of the next moment according to the state of the current moment, so the prediction equation can be expressed as:

$$\{\begin{array}{l}\tilde{x}\left(k+1\right)=\tilde{A}\tilde{x}\left(k\right)+{\tilde{B}}_u\Delta u\left(k\right)+{\tilde{B}}_d\rho \left(k\right)\hfill \\ \tilde{x}\left(k+2\right)={\tilde{A}}^2\tilde{x}\left(k\right)+\tilde{A}{\tilde{B}}_u\Delta u\left(k\right)+{\tilde{B}}_u\Delta u\left(k\right)+\tilde{A}{\tilde{B}}_d\rho \left(k\right)+{\tilde{B}}_d\rho \left(k+1\right)\hfill \\ \tilde{x}\left(k+3\right)={\tilde{A}}^3\tilde{x}\left(k\right)+{\tilde{A}}^2{\tilde{B}}_u\Delta u\left(k\right)+\tilde{A}{\tilde{B}}_u\Delta u\left(k+1\right)+\cdots \hfill \\ +{\tilde{B}}_u\Delta u\left(k+2\right)+{\tilde{A}}^2{\tilde{B}}_d\rho \left(k\right)+\tilde{A}{\tilde{B}}_d\rho \left(k+1\right)+{\tilde{B}}_d\rho \left(k+2\right)\hfill \\ ⋮\hfill \\ \tilde{x}\left(k+N_c\right)={\tilde{A}}^{N_c}\left(k\right)\tilde{x}\left(k\right)+{\tilde{A}}^{N_c-1}{\tilde{B}}_u\Delta u\left(k\right)+\cdots \hfill \\ +{\tilde{B}}_u\Delta u\left(k+N_c-1\right)+{\tilde{A}}^{N_c-1}{\tilde{B}}_d\rho \left(k\right)+\cdots +{\tilde{B}}_d\rho \left(k+N_c-1\right)\hfill \\ ⋮\hfill \\ \tilde{x}\left(k+N_p\right)={\tilde{A}}^{N_p}\left(k\right)\tilde{x}\left(k\right)+{\tilde{A}}^{N_p-1}{\tilde{B}}_u\Delta u\left(k\right)+\cdots \hfill \\ +{\tilde{B}}_u\Delta u\left(k+N_p-1\right)+{\tilde{A}}^{N_p-1}{\tilde{B}}_d\rho \left(k\right)+\cdots +{\tilde{B}}_d\rho \left(k+N_p-1\right)\hfill \end{array},$$

(21)

The output vector of the predictive can be expressed in matrix form:

$$Y\left(t\right)={\psi }_t\tilde{x}\left(t\right)+{\theta }_t\mathsf{\Delta }u\left(t\right)+{\tau }_t\varphi \left(t\right),$$

(22)

where ${\psi }_t=\left[\begin{array}{c}\begin{array}{c}\tilde{C}\tilde{A}\\ \tilde{C}{\tilde{A}}^2\\ ⋮\end{array}\\ \begin{array}{c}\tilde{C}{\tilde{A}}^{N_c}\\ ⋮\\ \tilde{C}{\tilde{A}}^{N_p}\end{array}\end{array}\right]$, ${\theta }_t=\left[\begin{array}{cccc}\tilde{C}{\tilde{B}}_u& 0& \cdots & 0\\ \tilde{C}\tilde{A}{\tilde{B}}_u& \tilde{C}{\tilde{B}}_u& \cdots & ⋮\\ ⋮& ⋮& \cdots & 0\\ \tilde{C}{\tilde{A}}^{N_c-1}{\tilde{B}}_u& \tilde{C}{\tilde{A}}^{N_c-2}{\tilde{B}}_u& \cdots & \tilde{C}{\tilde{B}}_u\\ ⋮& ⋮& \ddots & ⋮\\ \tilde{C}{\tilde{A}}^{N_p-1}{\tilde{B}}_u& \tilde{C}{\tilde{A}}^{N_p-2}{\tilde{B}}_u& \cdots & \tilde{C}{\tilde{A}}^{N_p-N_c}{\tilde{B}}_u\end{array}\right]$,

${\tau }_t=\left[\begin{array}{cccc}\tilde{C}{\tilde{B}}_d& 0& \cdots & 0\\ \tilde{C}\tilde{A}{\tilde{B}}_d& \tilde{C}{\tilde{B}}_d& \cdots & ⋮\\ ⋮& ⋮& \cdots & 0\\ \tilde{C}{\tilde{A}}^{N_c-1}{\tilde{B}}_d& \tilde{C}{\tilde{A}}^{N_c-2}{\tilde{B}}_d& \cdots & \tilde{C}{\tilde{B}}_d\\ ⋮& ⋮& \ddots & ⋮\\ \tilde{C}{\tilde{A}}^{N_p-1}{\tilde{B}}_d& \tilde{C}{\tilde{A}}^{N_p-2}{\tilde{B}}_d& \cdots & \tilde{C}{\tilde{A}}^{N_p-N_c}{\tilde{B}}_d\end{array}\right]$.

To calculate the desired control increment, the trajectory tracking problem is converted to solve the quadratic optimal solution problem:

$$\underset{\Delta u,\epsilon }{\min }J\left(x\left(k\right),u\left(k-1\right),\Delta U\left(k\right)\right)={\displaystyle \sum }_{k=1}^{N_p}Q\Vert Y_r\left(k\right)-Y{\left(k\right)\Vert }^2+{\displaystyle \sum }_{k=1}^{N_c-1}R\Vert \Delta u{\left(k\right)\Vert }^2,$$

(23)

$$s.t.\{\begin{array}{l}\begin{array}{l}x\left(k+1\right)=A_{d}x\left(k\right)+B_{u}u\left(k\right)+B_d\rho \left(k\right)\hfill \\ y\left(k\right)=Cx\left(k\right)\hfill \end{array}\hfill \\ \left|H_{sh}y\left(k\right)\right|\le G_{sh}\left(k\right)\hfill \\ \Delta U_{min}\le \Delta U\le \Delta U_{max}\hfill \\ u_{min}\le A\Delta U+U\ll u_{max}\hfill \end{array},$$

(24)

where $Q, R$ is the weight matrix; and $\epsilon$ is the coefficient of relaxation. Combining Equation (20) with Equation (22), and defining $E\left(k\right)=-Y_r\left(k\right)+{\theta }_t\mathsf{\Delta }u\left(k\right)+{\tau }_t\varphi \left(k\right)$, the objective function can be transformed into a quadratic optimization problem combined with the constraints and the following optimization problems can be solved:

$$\begin{gathered} \underset{\Delta u,\epsilon }{\min }J\left(x\left(k\right),u\left(k-1\right),\Delta U\left(k\right)\right)={\left[\Delta U{\left(k\right)}^T,\epsilon \right]}^T{H}_k\left[\Delta U{\left(k\right)}^T,\epsilon \right]+G_k\left[\Delta U{\left(k\right)}^T,\epsilon \right] \\ s.t.\{\begin{array}{l}\begin{array}{l}x\left(k+1\right)=A_{d}x\left(k\right)+B_{u}u\left(k\right)+B_d\rho \left(k\right)\hfill \\ y\left(k\right)=Cx\left(k\right)\hfill \end{array}\hfill \\ \left|H_{sh}y\left(k\right)\right|\le G_{sh}\left(k\right)\hfill \\ \Delta U_{min}\le \Delta U\le \Delta U_{max}\hfill \\ u_{min}\le A\Delta U+U\ll u_{max}\hfill \end{array} \end{gathered}$$

(25)

where $H_k=\left[\begin{array}{cc}{\theta }_t^{T}R{\theta }_t& 0\\ 0& S_{sh}\end{array}\right]$, $G_k=2E^{T}Q{\theta }_t$.

In addition, Equations (12) and (20) transform the slip constraint (16) into a constraint on the control increment $\mathsf{\Delta }u$:

$$\begin{gathered} \left|H_{ssh}Y\left(k\right)\right|=\left|H_{ssh}\left({\psi }_t\tilde{x}\left(t\right)+{\theta }_t\mathsf{\Delta }u\left(t\right)+{\tau }_t\varphi \left(t\right)\right)\right|\le G_{ssh}\left(k\right) \\ \Rightarrow \left|H_{ssh}\left({\theta }_t\mathsf{\Delta }u\left(t\right)\right)\right|\le -\left|H_{ssh}\left({\psi }_t\tilde{x}\left(t\right)+{\tau }_t\varphi \left(t\right)\right)\right|+G_{ssh}\left(k\right), \end{gathered}$$

(26)

where, $H_{ssh}=diag{\left(H_{sh},H_{sh},\cdots H_{sh}\right)}_{Np\times Np}, G_{ssh}\left(k\right)={\left(G_{sh},G_{sh},\cdots G_{sh}\right)}_{Np\times 1}^T$.

4.3. Lower-level Controller Design

In order to verify the effectiveness of the proposed MPC control algorithm, the lower-level controller needs to be established to convert the output of the MPC controller into the input of the actuator [36]. Among them, the control input of the steering system of the actuator is the front-wheel steering angle, and the control input of the driving system is the desired throttle opening $a_{thdes}$ and the desired braking master cylinder pressure $P_{bdes}$. The schematic is shown in Figure 7. The switching logic of the lower-level controller can be simplified to compare $a_x$ with 0, to apply the drive control (drive mode) when $a_x\ge 0$, to compare the desired throttle opening $a_{thdes}$ with the throttle opening threshold $a_{th\_sat}$ and to carry out the PI control. Otherwise, the braking control (braking mode) is applied to compare the desired braking master cylinder pressure $p_{bdes}$ with the braking master cylinder pressure threshold $p_{b\_sat}$ and carry out the PI control. The switching logic can be formulated as:

$${\alpha }_{thdes}=\{\begin{array}{l}{\alpha }_{thdes},\hspace{1em}{a}_x\ge 0\hfill \\ 0,\hspace{1em}\hspace{1em}\hspace{1em}{a}_x<0\hfill \end{array},$$

(27)

$$P_{bdes}=\left\{\begin{array}{l}0,\hspace{1em}\hspace{1em}{a}_x\ge 0\\ P_{bdes},\hspace{1em}{a}_x<0\end{array}\right.,$$

(28)

5. Experimental Platform and Testing

In order to fully demonstrate the advantages of the proposed control algorithm, the double-shift line condition is used as the test environment and builds a co-simulation performed of CarSim and Matlab/Simulink and the HIL platform. The development process is shown in Figure 8. Firstly, the functional analysis of the trajectory tracking system is carried out; secondly, the effectiveness of the MPC algorithm is verified by the co-simulation environment; thirdly, the MPC controller is compiled into the Raspberry Pi through the method of generating C++ code by Matlab, and the Raspberry Pi is used as a controller to control the controlled vehicle of the CarSim. CarSim, as a simulation software for vehicle dynamics, utilizes detailed vehicle characteristic parameters to simulate the dynamic behavior of vehicles and is widely used in the development of modern automotive control systems.

5.1. Simulation

In order to verify the validity of the proposal control algorithm, this section combines the virtual vehicle in CarSim software with the controller proposed in Matlab/Simulink to achieve trajectory tracking. The block diagram of the co-simulation is shown in Figure 9, and the primary parameters of the co-simulation are shown in

Table 1. Symbol and definition of parameters.

Definition	Symbol	Value
Vehicle mass	m (kg)	1723
Inertia moment of the vehicle about yaw axis	$I_z\left(\mathrm{kg}·{\mathrm{m}}^2\right)$	4175
Distance of the front/rear axle from CG	$l_f/l_r$(m)	1.232/1.464
Minimal/maximal yaw rate	$r_{min}/r_{max}\left(rad/s\right)$	−0.5/0.5

.

Furthermore, the RESM performance index was introduced to determine quantitative differences between the plotted data, and the RMSE is formulized as follows:

$$RMSE=\sqrt{\frac{1}{n}{{\displaystyle \sum }}_{i=1}^n{\left(y_i-Y_i\right)}^2},$$

(29)

where $y_i$ is the predicted value at the time $i$, $Y_i$ is the true value at the time $i$.

5.1.1. Accuracy of RBF Neural Network in Estimating Lateral Force

In order to verify the accuracy of the proposed RBF neural network in estimating lateral force, we carried out simulation experiments on ice-covered pavement, and set the adhesion coefficient of the ice-covered pavement to 0.3. The double-shift line condition was tested at $36 \mathrm{km}·{\mathrm{h}}^{-1}$, and the experimental results are shown in Figure 10. The error between the RBF neural network estimate and the true value of the tire is within 9N as shown in Figure 10. The RESM performance index between the RBF neural network estimate and the true value of the tire is 7.1473, which can effectively estimate the lateral force and apply it to the boundary value of the vehicle nonlinear dynamics model and slip constraint.

5.1.2. Influence of Slip Constraints

In order to verify the influence of the proposed algorithm on the operation stability of autonomous vehicles at the vehicle’s handling limits, we carried out simulation experiments on ice-covered pavement, and set the adhesion coefficient of the ice-covered pavement to 0.3. We compared applying the slip constraint and not applying the slip constraint at $50 \mathrm{km}·{\mathrm{h}}^{-1}$, and the experimental results are shown in Figure 11.

Table 2. RMSE of trajectory tracking error on ice-covered pavement.

Regardless of Constraints	Considering the Constraint
5.0016	0.9640

shows the RSME of the trajectory tracking error on the ice-covered pavement, and Figure 11 shows the results of the trajectory tracking and the comparison of the lateral velocity and yaw rate under the ice-covered pavement. Figure 11a,b show the vehicle has a serious skidding phenomenon at $160 \mathrm{m}$ when no slipping constraint was imposed, and the vehicle can still drive effectively when the slip constraint is applied. As shown in Table 2, when the vehicle is in the slip constraint, the tracking performance of the desired trajectory is better, where the RESM of the trajectory tracking error is 0.9406; the front-wheel steering angle under the slip constraint is within the saturation constraint of the actuator as shown in Figure 11c. As shown in Figure 11d, the vehicle seriously exceeds the envelope constraint without the envelope constraint, and the vehicle becomes unstable. In summary, it can be seen that the application of the slip constraint can effectively improve the slip phenomenon of the vehicle and the vehicle is guaranteed to be within the envelope constraint.

5.1.3. Effect of Nonlinear Envelope Constraints

The algorithm proposed for verification can still ensure the driving of the vehicle beyond the nonlinear area of the tire. We carried out the simulation experiments on wet asphalt pavement and set its pavement adhesion coefficient to 0.6. We compared applying the slip constraint and not applying the slip constraint at $75 \mathrm{km}·{\mathrm{h}}^{-1}$. Figure 12 shows the results of the experiment. Figure 12 shows the results of the trajectory tracking and the comparison of the lateral velocity and yaw rate under the ice-covered pavement. The vehicle can still drive efficiently beyond the linear region, as shown in Figure 12a,b, and the tracking error of applying the nonlinear envelope constraint is smaller than the error of applying the linear envelope in the second half of the trajectory. As shown in

Table 3. RMSE of trajectory tracking error.

Linear Region	Nonlinear Regions
1.1974	1.0886

, the nonlinear envelope constraint performed well in tracking the desired trajectory, where the RESM of the trajectory tracking error is 1.0886. The front-wheel steering angle is within the saturation constraint of the actuator as shown in Figure 12c. As shown in Figure 12d, the vehicle is constrained beyond the linear envelope and within the nonlinear envelope constraint, and the vehicle remains stable. In summary, it can be seen that the vehicle is within the constraint of the nonlinear envelope, and the vehicle can still drive effectively.

5.1.4. Comparison of Simulation Result

This section offers a comparison with the MPC trajectory tracking algorithm proposed in the Ref. [34]. In the first scenario, the vehicle has a driving speed of $36 \mathrm{km}·{\mathrm{h}}^{-1}$ in the double-shift line condition as shown in Figure 13. In the second scenario, the vehicle has a driving speed of $45 \mathrm{km}·{\mathrm{h}}^{-1}$ in the double-shift line condition as shown in Figure 14. In the third scenario, the vehicle has a driving speed of $55 \mathrm{km}·{\mathrm{h}}^{-1}$ in the double-shift line condition as shown in Figure 15. The RESM of the trajectory tracking error as shown in

Table 4. RESM of the trajectory tracking error.

	Proposed Controller	MPC-Ref Controller	Improvement
36$\mathrm{km}·{\mathrm{h}}^{-1}$	0.0574	0.0653	12.09%
45$\mathrm{km}·{\mathrm{h}}^{-1}$	0.0490	0.2774	82.33%
55 $\mathrm{km}·{\mathrm{h}}^{-1}$	0.0527	-	-

.

As shown in Figure 13a, the algorithm proposed in Ref. [34] and the algorithm proposed in this paper effectively track the desired trajectory at $36 \mathrm{km}·{\mathrm{h}}^{-1}$. Moreover, the tracking error of the algorithm proposed can be maintained within the range of 0.04 m. The tracking error of the Ref. is within $0.07 \mathrm{m}$ as shown in Figure 13b, but it is still large compared to the algorithm proposed. When the driving speed is $45 \mathrm{km}·{\mathrm{h}}^{-1}$, Ref. [34] has a large tracking error, and the algorithm proposed still maintains a good control effect, and the tracking error is maintained in the range of 0.05m, as shown in Figure 14. As shown in Figure 15, the algorithm proposed can effectively track the desired trajectory, and the tracking error remains within 0.07 m. Compared with the algorithm proposed in Ref. [34], the proposed controller performed well in tracking the desired trajectory, as shown in Table 4. The effectiveness of the trajectory tracking controller proposed is effectively verified, and it can be tracked at higher speeds compared with Ref. [34].

5.2. Hardware-in-loop Platform and Test

5.2.1. Desired Trajectory Setting

The quintile polynomial curve fitting can plan a trajectory with a smooth curvature according to the initial and final states of the vehicle, which is widely used in trajectory tracking. The time-based quintile polynomial curve fitting generates the desired trajectory, and it verifies the effectiveness of the MPC trajectory tracking controller through the Raspberry Pi. The quintile polynomial curve fitting can be described as:

$$\left\{\begin{array}{l}{x}_r\left(t\right)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5\\ y_r\left(t\right)=b_0+b_1{x}_r+b_2{x}_r^2+b_3{x}_r^3+b_4{x}_r^4+b_5{x}_r^5\end{array}\right.,$$

(30)

where $a_0,\cdots ,a_5$, $b_0,\cdots ,b_5$ are designed according to the initial and final state of the vehicle.

The calculation formula for the heading angle ${\phi }_r$ and the curvature road $C_R$ is as follows:

$${\phi }_r\left(t\right)=\arctan \left\{y_r^{\prime }\left[x_r\left(t\right)\right]\right\},$$

(31)

$$C_R\left(t\right)=\frac{y_r^{″}\left[x_r\left(t\right)\right]}{{\left(1+{\left\{y_r^{\prime }\left[x_r\left(t\right)\right]\right\}}^2\right)}^{\frac{3}{2}}},$$

(32)

5.2.2. Hardware-in-loop Platform Setup

In the hardware-in-loop platform design, the Robot Operating System (ROS) system is used as a platform for the operation of the vehicle program, and the MPC trajectory tracking program runs in ROS. As an alternative to a physical car, $\mathrm{CarSim}$ has the same object characteristics as a physical car. In addition, $\mathrm{Matlab}$ implements the information transfer between $\mathrm{CarSim}$ and ROS through topic communication. ROS (MPC trajectory tracking program) obtains the status information of the vehicle: the vehicle position, acceleration, yaw angle and yaw rate, and the quintile polynomial curve fitting to calculate the desired trajectory information and processes, and it calculates that information and obtains the feedback information at the current moment, such as the vehicle’s desired throttle opening, the desired brake master cylinder pressure, and the front-wheel steering angle, where this feedback control information is simultaneously communicated to CarSim (vehicle) to control the vehicle in real time. The control block diagram is shown in Figure 16.

ROS runs on the Raspberry Pi 4B (CPU 64bit 1.5 GHz, 8 GB Sto-rage) with the ubuntu18.4+ros melodic system, and CarSim runs on the same computer as Matlab (CPU i9-10850k 3.60 GHz). The control cycle of ROS (MPC trajectory tracking program) is $T_b$ = $20 \mathrm{ms}$. Four scenarios (low speed, medium speed, medium-high speed, and high speed) are set to verify the effectiveness of the proposed algorithm. The primary parameters of the simulated vehicle are shown in Table 1.

In the first scenario, the vehicle enters the desired trajectory at an initial velocity of 36 km/h, reaches the center point of the desired trajectory at a speed of 54 km/h, and then reaches the end of the desired trajectory to return to the initial speed and continue driving at the initial speed. The description of the other three scenes is similar to the first. The information for the four scenario is shown in

Table 5. Simulation scenarios.

	Initial Velocity	Center Point Velocity	Time of Lane Change
Scenario A	36 km/h	45 km/h	8.3 s
Scenario B	72 km/h	80 km/h	4.8 s
Scenario C	$108$ km/h	112 km/h	3.3 s
Scenario D	$144$ km/h	147.6 km/h	2.2 s

. Scenario A, B, C, and D represent the first, second, third, and fourth scenario, respectively.

5.2.3. Hardware-in-loop Platform Results and Analysis

In this hardware-in-loop platform experiment, dry asphalt pavement was selected and the adhesion coefficient was set to 0.75.

Table 6. RMSE of trajectory tracking error in the three scenarios.

Scenario A	Scenario B	Scenario C
0.0081	0.0191	0.0430

shows the RMSE of trajectory tracking error under the three scenarios. The results of the trajectory tracking under the three scenarios are displayed in Figure 17. Figure 17a,b show that the lateral deviation of unmanned vehicles is always kept within 0.10 m, where the control effect is good under the three scenarios. The lower the velocity, the lower the tracking error. As shown in Table 6, the RMSE of trajectory tracking error is always kept within 0.0430, and the lower the velocity, the smaller the RMSE of the tracking error. Figure 17c shows that the front-wheel steering angle optimized by the controller is within the saturation constraint of the actuator regardless of the velocity. The three scenario errors are small and have better tracking performance as shown in Figure 17d. Figure 17e shows that the longitudinal acceleration of the vehicle is much less than 0.2 g and is always within the saturation constraint. Figure 17f shows that the longitudinal velocity error of the vehicle is 0.15 m/s and has good tracking performance in the two scenarios of Scenario A and Scenario B, but the longitudinal velocity error in Scenarios C is within 0.35 m/s, and the longitudinal velocity error is large.

5.2.4. Comparative Analysis of Co-simulation and Hardware-in-loop Platform Experiments

The comparison verification is carried out in Scenario D, where the dry asphalt pavement was selected and the adhesion coefficient was set to 0.75; the results are shown in Figure 18. The RMSE of the trajectory tracking error is shown in

Table 7. RMSE of trajectory tracking error in Scenario D.

	HIL	CarSim/Simulink	Improvement
Scenario D	0.0705	0.0667	−5.69%

. The results show that the controller proposed in this paper achieves good control effects in both CarSim/Simulink co-simulation and the HIL platform. However, the front-wheel steering angle in the HIL platform is higher than that in co-simulation as shown in Figure 18c. The longitudinal velocity error of the HIL simulation is larger, as shown in Figure 18f. What is more, the co-simulation performed well in tracking the desired trajectory, where the RESM of the trajectory tracking error is 0.0667, as shown in Table 7.

6. Conclusions

In this paper, a control law is designed to improve the trajectory tracking accuracy and vehicle handling stability to achieve safe driving. The control law is based on the model predictive control with the slip constraint, which calculates the desired longitudinal acceleration and the desired front-wheel steering angle. In addition, the proposed control law can satisfy such control objectives and the vehicle handling stability simultaneously during the control period. The research is summarized below:

• In order to improve the tracking accuracy of the vehicle and the stability of the vehicle, a control method, which considers the influence of road curvature and tire nonlinear dynamic characteristics on the trajectory tracking performance, is developed by the control law based on the MPC. Furthermore, the lateral force is estimated based on the RBF neural network, which is incorporated into the boundary conditions of the vehicle envelope constraint to reduce the slip phenomenon of vehicles;
• The trajectory tracking control system with the control law is simulated by Matlab/Simulink and Raspberry Pi combined with the CarSim-based vehicle model. Meanwhile, the development technology route was expounded;
• In co-simulation (CarSim/Simulink), the accuracy of the RBF neural network in estimating the lateral force is verified under actual driving conditions; the slip phenomenon can be effectively reduced after the vehicle is applied to the envelope constraint; and the influence of the vehicle on the driving state after the linear and nonlinear envelope constraint is analyzed. Finally, compared with the Ref. [34], the results show that the proposed controller is better than the controller proposed in the Ref. [34];
• In the HIL platform, we verify the performance of the proposed MPC controller in three different scenarios. Finally, in comparing the co-simulation results with the HIL platform, the results show that the co-simulation is better than the HIL platform;
• In the future research, the study of the tire deflection characteristics based on experimental data would be considered. In addition, on the basis of the establishment of the HIL platform, the ROS-based verification platform is built to verify the effectiveness of the proposed algorithm.