Vehicle Trajectory Adaptive Tracking Control Based on Variable Prediction Horizon

Abstract

The design of intelligent vehicle trajectory tracking controllers still has some problems, such as parameter uncertainty and time consumption. To improve the tracking accuracy of the trajectory tracking controller and reduce its computational complexity, an adaptive MPC trajectory tracking control method with a variable prediction horizon is proposed. Firstly, a three-degree-of-freedom vehicle dynamics model is constructed, and the design is improved based on the ordinary MPC controller. Secondly, several groups of different constant vehicle speeds are selected to compare the tracking effect of the ordinary MPC and the improved controller. Then, low speed (30 km/h) and high speed (100 km/h) are selected as representative speeds to solve the calculation time of the controller. The relationship between vehicle speed and prediction horizon is analyzed, and curve fitting is carried out. An adaptive trajectory tracking controller is designed. Finally, it is verified by CarSim and MATLAB/Simulink co-simulation. The results show that compared with ordinary MPC, the improved adaptive trajectory tracking controller can maintain good tracking accuracy and stability according to the speed change and improve the computational efficiency of the controller.

1. Introduction

At present, intelligent vehicles have become an important breakthrough in the automotive field for the future and have received public attention and recognition with many unique advantages. Intelligent vehicle technology consists of three key components: environmental perception, autonomous decision-making, and control execution, among which trajectory tracking control research is particularly critical, which is not only a key part of control execution but also an indispensable part of intelligent transportation systems [1]. Therefore, in the face of a variety of road and driving conditions, whether the trajectory tracking controller can accurately and stably track the trajectory is of great significance and function for the safe driving of intelligent vehicles. Trajectory tracking control can accurately track the expected trajectory given by the planning layer through the active steering control of the steering wheel and maintain the stability and ride comfort of the vehicle [2]. At present, the methods mainly used for trajectory tracking include PID [3], fuzzy control [4], pure tracking algorithm [5], linear quadratic regulator (LQR) [6], model predictive control (MPC) [7], etc. In addition, there are PI control and adaptive control and other advanced control strategies combined. Zhang D et al. [8] proposed a networked modeling and PI control method for direct drive wheel systems based on wireless network environments, which significantly improved the stability and dynamic performance of the system by optimizing the controller design. Model predictive control forecasts the future through predictive models, determines the current optimal control input by rolling optimization, and responds to the difference between actual and predicted by feedback correction so as to effectively control complex dynamic systems and improve system performance. MPC not only has outstanding advantages in dealing with multi-objective constrained optimization problems but also has the ability to predict the subsequent driving conditions of vehicles, so it is widely used in the field of unmanned driving.

In recent years, the application of model predictive control (MPC) in path tracking control has made remarkable progress. Liang J et al. [9] proposed a robust predictive control method based on a polyhedron model for path tracking of autonomous vehicles. By introducing a robust optimization technique, the stability and tracking accuracy of the system under an uncertain environment are improved significantly. Zhiheng You [10] studied the trajectory tracking problem of unmanned vehicles, analyzed the traditional trajectory tracking control method based on pre-sight, developed the vehicle lateral control algorithm based on pure tracking theory and the vehicle longitudinal speed following algorithm based on expert PID control, and solved the trajectory tracking control problem of unmanned vehicles under complex road surfaces such as high speed and low adhesion. When the ground adhesion condition is good, the control effect is better, but when the ground adhesion condition is slightly poor, the vehicle can not track the reference track well and even appears to sideslip and run off. Lu Y et al. [11] used multi-agent systems to build a general framework and proposed an MPC-based AFS control strategy. AFS and DYC worked together as agents and showed through virtual simulation that the proposed control architecture could effectively maintain vehicle stability and reduce driver workload. Huiling He [12] established a model prediction controller based on vehicle dynamics. For path tracking and longitudinal velocity tracking, the horizontal and longitudinal errors were used as state variables to establish state equations, and a comparison experiment between the model prediction controller and the pure tracking controller was designed. The experimental results showed that the designed model prediction controller exhibits better tracking performance than the pure tracking controller. Finally, particle swarm optimization is introduced to improve the tracking accuracy of the controller.

Feng Mingwang [13] designed the trajectory tracking controller based on the model predictive control algorithm and established the predictive model based on the three-degree-of-freedom vehicle dynamics model. The front wheel Angle, center of mass side deflection Angle and tire side deflection Angle are the constraint conditions, and the front wheel Angle increment is the control quantity. The optimization objective function is established and transformed into the standard quadratic programming problem, and the steering Angle increment in the control horizon is obtained by solving it so as to complete the trajectory tracking. It is found that the vehicle speed has a significant impact on the trajectory tracking results under constraints. Then, the tracking controller is optimized with fuzzy control, and a trajectory tracking system with integrated speed and direction control is established. The simulation verifies that the trajectory tracking system has strong robustness. Based on the two-degree-of-freedom dynamics model, literature [14] proposed a linear time-varying MPC trajectory tracking controller to improve the accuracy of vehicle trajectory tracking. Literature [15] also considers multiple state constraints and proposes a trajectory tracking controller based on multi-constraint model predictive control to achieve multi-objective optimization. The proposed comprehensive control method can realize obstacle avoidance paths more effectively, and the path tracking controller has dynamic tracking and operability. In literature [16], a robust model predictive control method with a finite horizon is proposed to improve the traditional MPC so that the vehicle has better path-tracking accuracy.

For the MPC-based vehicle trajectory tracking methods mentioned above, fixed prediction horizon and control horizon are adopted, but the selection of horizon has a great influence on the trajectory tracking control effect and body attitude control. Under different vehicle speed conditions, the fixed horizon cannot meet the requirements of high-precision tracking control [17]. Literature [18] proposes to automatically adjust the MPC controller to predict the horizon according to the change of the curvature of the reference trajectory, which reduces the operation time and ensures the real-time tracking control. However, the kinematic model cannot be applied under the conditions of high speed and large lateral acceleration. In literature [19], based on the three-degree-of-freedom dynamic model, a genetic algorithm was used to optimize the prediction horizon and control horizon of the model prediction controller, but the ride comfort decreased. In literature [20], a parameter estimator was added to the MPC controller for real-time estimation of vehicle driving state parameters. The simulation results show that the controller can well realize the trajectory tracking of intelligent vehicles at constant speed, but the control effect is not ideal when the speed changes with fixed control parameters.

Based on this, an adaptive MPC trajectory tracking control method with a variable prediction horizon is proposed in this paper. The overall logical framework for the article is shown in Figure 1. The main contribution of this paper: Based on the improved design of the ordinary MPC controller, the computational efficiency of the model prediction controller is improved, and the prediction horizon is updated according to the real-time vehicle speed on the premise of meeting the vehicle trajectory tracking accuracy and stability so that the vehicle can maintain good adaptive performance at different speeds.

2. Construction of Vehicle Three-Degree-of-Freedom Dynamic Model

An accurate vehicle dynamics model is the basis of the MPC algorithm. The dynamic model is used to study the force condition of the vehicle during operation, and the model equation is constructed by Newton’s second law of motion. The model can describe the motion state of the vehicle more accurately when the vehicle is traveling at high speed or on a road with large curvature. In view of the high requirements for tracking control reliability when vehicles are running at high speeds, the vehicle dynamics model is used as the basis of path tracking research in this study in order to improve the reliability of tracking control when vehicles are running at high speeds. This paper adopts the B-Class vehicle model in CarSim as the control object. A three-degree-of-freedom (3-DOF) vehicle dynamics model is employed, encompassing longitudinal, lateral, and yaw motions. To balance computational efficiency and control accuracy, the control object is simplified to include only lateral dynamics and steering inputs, neglecting secondary factors such as rigid suspension and aerodynamic drag. The simplified 3-DOF vehicle dynamics model is established as shown in Figure 2, and its dynamic equations are expressed in Equation (1) [21]. The controller is designed based on a three-degree-of-freedom vehicle model and implemented in MATLAB R2022b and validated through co-simulation with CarSim 2019.0. By embedding CarSim’s vehicle dynamics equations into the MPC optimization problem, the controller optimizes the steering input sequence in real time based on real-time vehicle state feedback, achieving precise tracking of the target trajectory.

Figure 1 above: F_lf and F_lr are the longitudinal forces on the front and rear wheels of the vehicle, respectively. F_cf and F_cr are the lateral forces on the front and rear wheels of the vehicle, respectively. F_xf and F_xr are the forces on the x-axis on the front and rear wheels of the vehicle, respectively. F_yf and F_yr are the forces on the y-axis on the front and rear wheels of the vehicle, respectively, and v is the center speed of the front axle of the vehicle.

$$\left\{\begin{array}{l}m{a}_y=-m\dot{x}\dot{\phi }+2\left[C_{\mathrm{cf}}({\delta }_f-{\displaystyle \frac{\dot{y}+a\dot{\phi }}{\dot{x}}})+C_{\mathrm{cr}}({\displaystyle \frac{b\dot{\phi }-\dot{y}}{\dot{x}}})\right]\hfill \\ m{a}_x=m\dot{y}\dot{\phi }+2[C_{\mathrm{lf}}{s}_f+C_{\mathrm{cf}}({\delta }_f-{\displaystyle \frac{\dot{y}+a\dot{\phi }}{\dot{x}}}){\delta }_f+C_{\mathrm{lr}}{s}_r]\hfill \\ I_z\ddot{\phi }=2\left[a{C}_{\mathrm{cf}}({\delta }_f-{\displaystyle \frac{\dot{y}+a\dot{\phi }}{\dot{x}}})-b{C}_{\mathrm{cr}}({\displaystyle \frac{b\dot{\phi }-\dot{y}}{\dot{x}}})\right]\hfill \\ \dot{Y}=\dot{x}sin\phi +\dot{y}cos\phi \hfill \\ \dot{X}=\dot{x}cos\phi -\dot{y}sin\phi \hfill \end{array}\right.$$

(1)

In Formula (1), m is the vehicle maintenance mass, the unit is kg; a and b are the distances between the vehicle’s center of mass and the front and rear axles, respectively, in unit m; $\phi$ and $\dot{\phi }$ are the yaw angle and yaw speed of the vehicle, in units rad and rad/s, respectively; ${\delta }_f$ is the front wheel steering Angle, the unit is °; I_z is the moment of inertia of the vehicle around the z axis, and the unit is kg⋅m². $\dot{X}$ and $\dot{Y}$ are respectively the transverse and longitudinal velocity of the vehicle in OXY coordinate system, $\dot{x}$ and $\dot{y}$ are respectively the longitudinal and lateral velocity of the vehicle in oxy coordinate system, in unit km/h; C_cf and C_cr are the lateral stiffness of front and rear wheels, respectively. The unit is N·rad⁻¹. C_lf and C_lr are the longitudinal stiffness of the front and rear wheels, respectively, expressed in N⸳m⁻¹. S_f and S_r are the slip rates of the front and rear wheels, respectively.

3. Design of Model Predictive Trajectory Tracking Controller

According to the vehicle dynamics equation above, the state quantity in the system is selected as $\zeta ={\left[\dot{y} \dot{x} \phi \dot{\phi } Y X\right]}^T$, and the control quantity is $u={\delta }_f$. The status can be expressed as

$$\dot{\zeta }=f(\zeta ,u)$$

(2)

Assuming that the sampling time is T, the forward Euler method is adopted to discretize (2) at time k to obtain

$$\zeta (k+1)=F\left[\zeta (k),u(k)\right]$$

(3)

Linearization is carried out with the method of state trajectory, and by applying a constant control quantity to Formula (3) at the operating point, it is obtained:

$${\zeta }_0(k+1)=F\left[{\zeta }_0(k),u_0(k)\right]$$

(4)

The nonlinear Formula (3) is expanded by Taylor at the working point $({\zeta }_0 u_0)$, omitting the higher-order items to get

$$\begin{array}{c}\zeta (k+1)=F\left[{\zeta }_0(k),u_0(k)\right]+{\displaystyle \frac{\partial F}{\partial \zeta }}\left|{\zeta }_0(k),u_0(k)\right.\left[\zeta (k)-{\zeta }_0(k)\right]\\ + {\displaystyle \frac{\partial F}{\partial u}}\left|{\zeta }_0(k),u_0(k)\right.\left[u(k)-u_0(k)\right]\end{array}$$

(5)

By diverting Equation (4) from Equation (5), we get the following expression for the discrete linear state space and output η(k):

$$\left\{\begin{array}{l}\zeta (k+1)=A_k\zeta (k)+B_{k}u(k)+d(k)\\ \eta (k)=C_k\zeta (k)\end{array}\right.$$

(6)

In the above formula, A_k and B_k are self-defined matrices. In order to be able to write state space equations, it is convenient to express. $A_k=\left[\begin{array}{cccccc}{a}_{11}& a_{12}& 0& a_{14}& 0& 0\\ a_{21}& a_{22}& 0& a_{24}& 0& 0\\ 0& 0& 1& T& 0& 0\\ a_{41}& a_{42}& 0& a_{44}& 0& 0\\ a_{51}& a_{52}& a_{53}& 0& 1& 0\\ a_{61}& a_{62}& a_{63}& 0& 0& 1\end{array}\right]$, $B_k={\left[b_{11} b_{21} 0 b_{41} 0 0\right]}^T$, $C_k=\left[\begin{array}{cccccc}0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]$, the expression of the element as

$$\begin{array}{c}{a}_{11}={\displaystyle \frac{-2(C_{\mathrm{cf}}+C_{\mathrm{cr}})T}{m{\dot{x}}_k}}+1, a_{12}={\displaystyle \frac{2T{C}_{\mathrm{cf}}({\dot{y}}_k+a{\dot{\phi }}_k)+2C_{\mathrm{cr}}({\dot{y}}_k-b{\dot{\phi }}_k)}{m{\dot{x}}_k^2}}-T{\dot{\phi }}_k,\hfill \\ a_{14}=-T{\dot{x}}_k+{\displaystyle \frac{2bT{C}_{\mathrm{cr}}-2aT{C}_{\mathrm{cf}}}{m{\dot{x}}_k}}, a_{21}={\displaystyle \frac{-2T{C}_{\mathrm{cf}}+T{C}_{\mathrm{cr}}}{m{\dot{x}}_k}}, a_{22}={\displaystyle \frac{2T{C}_{\mathrm{cf}}({\dot{y}}_k+a{\dot{\phi }}_k){\delta }_f}{m{\dot{x}}_k^2}},\hfill \\ a_{24}={\displaystyle \frac{T{\dot{y}}_k-2aT{C}_{\mathrm{cf}}{\delta }_f}{m{\dot{x}}_k}}, a_{41}={\displaystyle \frac{2T(b{C}_{\mathrm{cr}}-a{C}_{\mathrm{cf}})}{I_z{\dot{x}}_k}}, a_{51}=Tcos{\phi }_k, a_{52}=Tsin{\phi }_k,\hfill \\ a_{53}=T{\dot{x}}_{k}cos{\phi }_k-T{\dot{y}}_{k}sin{\phi }_k, a_{42}={\displaystyle \frac{2aT{C}_{\mathrm{cf}}({\dot{y}}_k+a{\dot{\phi }}_k)-2b{C}_{\mathrm{cr}}({\dot{y}}_k-b{\dot{\phi }}_k)}{I_z{\dot{x}}_k^2}},\hfill \\ b_{11}={\displaystyle \frac{2T{C}_{\mathrm{cf}}}{m}}, a_{61}=-Tsin{\phi }_k, a_{62}=Tcos{\phi }_k, a_{63}=-T{\dot{x}}_{k}sin{\phi }_k-T{\dot{y}}_{k}cos{\phi }_k,\hfill \\ a_{44}={\displaystyle \frac{-2(a^2{C}_{\mathrm{cf}}+2b^2{C}_{\mathrm{cr}})T}{I_z{\dot{x}}_k}}+1, b_{21}={\displaystyle \frac{2T{C}_{\mathrm{cf}}({\delta }_f-\frac{{\dot{y}}_k+a{\dot{\phi }}_k}{{\dot{x}}_k})}{m}}, b_{41}={\displaystyle \frac{2aT{C}_{\mathrm{cf}}}{I_z}}.\hfill \end{array}$$

Taking the difference between the state variable expressions at two consecutive time instants in Equation (6), it can be obtained:

$$\begin{array}{c}\zeta \left(k+1\right)-\zeta \left(k\right)=A_k\left[\zeta \left(k\right)-\zeta \left(k-1\right)\right]+B_k\left[u\left(k\right)-u\left(k-1\right)\right]\\ + d(k)-d(k-1)\end{array}$$

(7)

Let Δζ(k) = ζ(k) − ζ(k−1), control increment Δu(k) = u(k) − u(k−1), and deviation increment Δd(k) = d(k) − d(k−1); then Equation (7) can be expressed as

$$\zeta (k+1)=\zeta (k)+A_k\Delta \zeta (k)+B_k\Delta u(k)+\Delta d(k)$$

(8)

According to Equations (6) and (8), the output equation of the system can be obtained as follows:

$$\eta (k+1)=\eta (k)+C_k{A}_k\Delta \zeta (k)+C_k{B}_k\Delta u(k)+C_k\Delta d(k)$$

(9)

If $\xi (k)=\left[\begin{array}{c}\Delta \zeta (k)\\ \eta (k)\end{array}\right]$ is set, the following new extended state space expression is obtained:

$$\left\{\begin{array}{l}\xi (k+1)={\tilde{A}}_k\xi (k)+{\tilde{B}}_k\Delta u(k)+{\tilde{D}}_k\Delta d(k)\\ \eta (k)={\tilde{C}}_k\xi (k)\end{array}\right.$$

(10)

In the above formula, ${\tilde{A}}_k=\left[\begin{array}{cc}{A}_k& 0_{N_x\times N_o}\\ C_k{A}_k& I_{N_y\times N_o}\end{array}\right], {\tilde{B}}_k=\left[\begin{array}{c}{B}_k\\ C_k{B}_k\end{array}\right], {\tilde{C}}_k=\left[\begin{array}{cc}{0}_{N_o\times N_x}& I_{N_o\times N_o}\end{array}\right]$, ${\tilde{D}}_k=\left[\begin{array}{c}{I}_{N_x\times N_x}\\ C_k\end{array}\right]$, N_x and N_o are the number of state quantities and the number of control quantities, respectively.

Assuming that the prediction horizon is Np and the control horizon is Nc, according to Equation (10), the predicted values of the system state variables over future time steps are as follows:

$$\left\{\begin{array}{c}\xi (k+1)={\tilde{A}}_k\xi (k)+{\tilde{B}}_k\Delta u(k)+{\tilde{D}}_k\Delta d(k)\hfill \\ \xi (k+2)={\tilde{A}}_k^2\xi (k)+{\tilde{A}}_k{\tilde{B}}_k\Delta u(k)+{\tilde{B}}_k\Delta u(k+1)+{\tilde{A}}_k{\tilde{D}}_k\Delta d(k)+{\tilde{D}}_k\Delta d(k+1)\hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}⋮\hfill \\ \xi (k+N_c)={\tilde{A}}_k^{N_c}\xi (k)+{\tilde{A}}_k^{N_c-1}{\tilde{B}}_k\Delta u(k)+\cdots +{\tilde{B}}_k\Delta u(k+N_c-1)+{\tilde{A}}_k^{N_c-1}{\tilde{D}}_k\Delta d(k)+\cdots \hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+{\tilde{D}}_k\Delta d(k+N_c-1) \hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}⋮\hfill \\ \xi (k+N_p)={\tilde{A}}_k^{N_p}\xi (k)+{\tilde{A}}_k^{N_p-1}{\tilde{B}}_k\Delta u(k)+\cdots +{\tilde{A}}_k^{N_p-N_c}{\tilde{B}}_k\Delta u(k+N_c-1)+{\tilde{A}}_k^{N_p-1}{\tilde{D}}_{k}d(k)\hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+\cdots +{\tilde{D}}_{k}d(k+N_p-1)\hfill \end{array}\right.$$

(11)

Similarly, according to Equation (11), the predicted value of the output variable over the future Np time steps can be calculated as follows:

$$\left\{\begin{array}{c}\eta (k+1)={\tilde{C}}_k{\tilde{A}}_k\xi (k)+{\tilde{C}}_k{\tilde{B}}_k\Delta u(k)+{\tilde{C}}_k{\tilde{D}}_k\Delta d(k)\hfill \\ \eta (k+2)={\tilde{C}}_k{\tilde{A}}_k^2\xi (k)+{\tilde{C}}_k{\tilde{A}}_k{\tilde{B}}_k\Delta u(k)+{\tilde{C}}_k{\tilde{B}}_k\Delta u(k+1)+{\tilde{C}}_k{\tilde{A}}_k{\tilde{D}}_k\Delta d(k)+{\tilde{C}}_k{\tilde{D}}_k\Delta d(k+1)\hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}⋮\hfill \\ \eta (k+N_c)={\tilde{C}}_k{\tilde{A}}_k^{N_c}\xi (k)+{\tilde{C}}_k{\tilde{A}}_k^{N_c-1}{\tilde{B}}_k\Delta u(k)+\cdots +{\tilde{C}}_k{\tilde{B}}_k\Delta u(k+N_c-1)+ {\tilde{C}}_k{\tilde{A}}_k^{N_c-1}{\tilde{D}}_k\Delta d(k)\hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+\cdots +{\tilde{C}}_k{\tilde{D}}_k\Delta d(k+N_c-1)\hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}⋮\hfill \\ \eta (k+N_p)={\tilde{C}}_k{\tilde{A}}_k^{N_p}\xi (k)+{\tilde{C}}_k{\tilde{A}}_k^{N_p-1}{\tilde{B}}_k\Delta u(k)+\cdots +{\tilde{C}}_k{\tilde{A}}_k^{N_p-N_c}{\tilde{B}}_k\Delta u(k+N_c-1)+{\tilde{C}}_k{\tilde{A}}_k^{N_p-1}{\tilde{D}}_k\hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}d(k)+\cdots +{\tilde{C}}_k{\tilde{D}}_{k}d(k+N_p-1)\hfill \end{array}\right.$$

(12)

Equation (12) can be expressed in the following matrix form:

$$F(k)={\Psi }_k{\xi }_k(k)+{\Theta }_k\Delta U(k)+{\Gamma }_k\Phi (k)$$

(13)

$$\begin{array}{c}F(k)=\left[\begin{array}{c}\eta (k+1)\\ ⋮\\ \eta (k+N_p)\end{array}\right], {\Psi }_k=\left[\begin{array}{c}{\tilde{C}}_k{\tilde{A}}_k\\ ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_p}\end{array}\right], {\xi }_k(k)=\left[\begin{array}{c}\xi (k)\\ ⋮\\ \xi (k+N_p)\end{array}\right],\Delta U(k)=\hfill \\ \left[\begin{array}{c}\Delta u(k)\\ ⋮\\ \Delta u(k+N_c-1)\end{array}\right], {\Theta }_k=\left[\begin{array}{cccc}{\tilde{C}}_k{\tilde{B}}_k& 0& 0& 0\\ {\tilde{C}}_k{\tilde{A}}_k{\tilde{B}}_k& {\tilde{C}}_k{\tilde{B}}_k& 0& 0\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_c-1}{\tilde{B}}_k& {\tilde{C}}_k{\tilde{A}}_k^{N_c-1}{\tilde{B}}_k& \cdots & {\tilde{C}}_k{\tilde{B}}_k\\ {\tilde{C}}_k{\tilde{A}}_k^{N_c}{\tilde{B}}_k& {\tilde{C}}_k{\tilde{A}}_k^{N_c}{\tilde{B}}_k& \cdots & {\tilde{C}}_k{\tilde{A}}_k{\tilde{B}}_k\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_p-1}{\tilde{B}}_k& {\tilde{C}}_k{\tilde{A}}_k^{N_p-1}{\tilde{B}}_k& \cdots & {\tilde{C}}_k{\tilde{A}}_k^{N_p-N_c}{\tilde{B}}_k\end{array}\right],\hfill \\ {\Gamma }_k=\left[\begin{array}{cccc}{\tilde{C}}_k{\tilde{D}}_k& 0& \cdots & 0\\ {\tilde{C}}_k{\tilde{A}}_k{\tilde{D}}_k& {\tilde{C}}_k{\tilde{D}}_k& \dots & ⋮\\ ⋮& ⋮& \ddots & 0\\ {\tilde{C}}_k{\tilde{A}}_k^{N_p-1}{\tilde{D}}_k& \cdots & {\tilde{C}}_k{\tilde{A}}_k{\tilde{D}}_k& {\tilde{C}}_k{\tilde{D}}_k\end{array}\right], \Phi (k)=\left[\begin{array}{c}\Delta d(k)\\ ⋮\\ \Delta d(k+N_p-1)\end{array}\right].\hfill \end{array}$$

The control objective function is obtained:

$$J={[F_{ref}(k)-F(k)]}^T\overline{Q}[F_{ref}(k)-F(k)]+\Delta U{(k)}^T\overline{R}\Delta U(k)+{\epsilon }^T\rho \epsilon$$

(14)

$F_{\mathrm{ref}}(k)=\left[\begin{array}{c}{\eta }_{\mathrm{ref}}(k+1)\\ ⋮\\ {\eta }_{\mathrm{ref}}(k+N_p)\end{array}\right]$,$\overline{Q}=\left[\begin{array}{ccc}Q& 0& 0\\ 0& \ddots & 0\\ 0& 0& Q\end{array}\right]$,$\overline{R}=\left[\begin{array}{ccc}R& 0& 0\\ 0& \ddots & 0\\ 0& 0& R\end{array}\right]$, $\overline{Q},\overline{R}$ is the weight coefficient matrix, ρ is the weight coefficient, and ε is the relaxation factor. We tested the values of Q and R. By increasing Q, we placed greater emphasis on tracking the system states, aiming to enable the system to converge more quickly to the reference states and reduce state errors, and increasing R imposed stricter constraints on changes in control inputs in order to reduce fluctuations in control inputs and thereby minimize energy consumption.

By combining Equation (13), let $E=F_{ref}(k)-{\Psi }_k{\xi }_k(k)-{\Gamma }_k\Phi (k)$, be obtained $F_{ref}(k)=E+{\Psi }_k{\xi }_k(k)+{\Gamma }_k\Phi (k)$ and $F_{ref}(k)-F(k)=E-{\Theta }_k\Delta U(k)$, and substitute the equation according to the given variable expressions; Equation (14) can be recalculated and reorganized into the following form:

$$J=\Delta U{(k)}^T({\Theta }_k\overline{Q}{\Theta }_k+\overline{R})\Delta U(k)-2E^T\overline{Q}{\Theta }_k\Delta U(k)+E^T\overline{Q}E+{\epsilon }^T\rho \epsilon$$

(15)

The control quantity over the future Nc time steps is

$$\left\{\begin{array}{l}u(k)=u(k-1)+\Delta \mathrm{u}(k)\\ u(k+1)=u(k-1)+\Delta \mathrm{u}(k)+\Delta \mathrm{u}(k+1)\\ ⋮\\ u(k+N_c-1)=u(k-1)+\Delta \mathrm{u}(k)+\Delta \mathrm{u}(k+1)+\cdots \Delta \mathrm{u}(k+N_c-1)\end{array}\right.$$

(16)

Formula (16) is expressed in matrix form:

$$U(k)=U_t+A_I\Delta U(k)$$

(17)

$U(k)=\left[\begin{array}{c}u(k)\\ u(k+1)\\ ⋮\\ u(k+N_c-1)\end{array}\right]$,$U_t=\left[\begin{array}{c}u(k-1)\\ u(k-1)\\ ⋮\\ u(k-1)\end{array}\right]$,$A_I=$$\left[\begin{array}{cccc}{I}_{N_o}& 0& 0& 0\\ I_{N_o}& I_{N_o}& 0& 0\\ ⋮& 0& \ddots & 0\\ I_{N_o}& I_{N_o}& \cdots & I_{N_o}\end{array}\right]$, $I_{N_o}$ is the identity matrix of N₀ × N₀.

According to Equation (17), the control constraint can be expressed in the following form:

$$\begin{gathered} \left\{\begin{array}{l}{A}_I\Delta U(k)\le U_{max}-U_t\\ A_I\Delta U(k)\le -U_{min}+U_t\end{array}\right. \\ {U}_{max}={[u_{max } u_{max }\dots u_{max}]}^T, U_{\min }={[u_{\min } u_{\min }\dots u_{\min }]}^T \end{gathered}$$

(18)

To reduce online computational burden and improve real-time efficiency, we avoid introducing slack variables. Instead, hard output constraints directly balance tracking error and control effort, preventing slack-induced performance loss. To prevent the empty admissible set, a safe state region is predefined with a control law ensuring states stay within it via optimization, avoiding constraint violations and empty sets. According to Equation (13), the output constraint can be expressed as

$$\begin{gathered} \left\{\begin{array}{l}{\Theta }_k\Delta U_k(k)\le F_{max}-{\Psi }_k{\xi }_k(k)-{\Gamma }_k\Phi (k)\\ -{\Theta }_k\Delta U_k(k)\le -F_{min}+{\Psi }_k{\xi }_k(k)+{\Gamma }_k\Phi (k)\end{array}\right. \\ {F}_{max}={[{\phi }_{max } Y_{max }\dots {\phi }_{max} Y_{max}]}^T,F_{\min }={[{\phi }_{\min } Y_{\min }\dots {\phi }_{\min } Y_{\min }]}^T \end{gathered}$$

(19)

According to Equations (15), (18) and (19), the objective function and constraints are transformed into the following standard form of quadratic programming:

$$\left\{\begin{array}{l}J=min{\displaystyle \frac{1}{2}} x^{T}Hx+f^{T}x\\ A_{\mathrm{cons}}x=b_{\mathrm{cons}}\end{array}\right.$$

(20)

In the above formula, $x=\left[\begin{array}{c}\Delta U\\ \epsilon \end{array}\right]$, $H=\left[\begin{array}{cc}2{\Theta }_k^T\overline{Q}{\Theta }_k+2\overline{R}& 0\\ 0& 2\rho \end{array}\right]$, $f^T=$$\left[\begin{array}{cc}-2E^T\overline{Q}{\Theta }_k& 0\end{array}\right]$, $A_{\mathrm{cons}}=\left[\begin{array}{cc}{A}_I& 0_{N_c{N}_o\times 1}\\ -A_I& 0_{N_c{N}_o\times 1}\\ \Theta & 0_{N_y{N}_p\times 1}\\ -\Theta & 0_{N_y{N}_p\times 1}\end{array}\right]$, $b_{\mathrm{cons}}=\left[\begin{array}{c}{U}_{max}-U_t\\ -U_{min}+U_t\\ F_{max}-{\Psi }_k{\xi }_k(k)-{\Gamma }_k\Phi (k)\\ -F_{min}+{\Psi }_k{\xi }_k(k)+{\Gamma }_k\Phi (k)\end{array}\right]$.

According to Formula (20), a series of control increments in the control horizon at the current time can be obtained, namely:

$$\Delta U={\left[\begin{array}{cccc}\Delta u(k)& \Delta u(k+1)& \cdots & \Delta u(k+N_c-1)\end{array}\right]}^T$$

(21)

According to the above controller design, it can be known that the improved MPC is based on the ordinary MPC to extend the equation of the state variable: the output variable is returned to the state variable to form new feedback, which will make the trajectory continuously approach the desired trajectory, so that the control algorithm of MPC has faster efficiency and higher accuracy. The main difference between the two lies in the equations of the state variables, which we refer to as ordinary MPC and the improved MPC in the following.

4. Simulation Test and Result Analysis

The co-simulation platform of MATLAB/Simulink and CarSim was established to analyze the influence of different constant vehicle speeds and different prediction horizons Np on the trajectory tracking effect, and the simulation test was carried out for the improved model prediction trajectory tracking controller. The main parameters of the vehicle in the simulation are shown in

Table 1. Main parameters of vehicle model.

Parameter Name	Unit	Numerical Value
Sprung mass	kg	1732
Yaw moment of inertia Iz	kg·m2	4175
Front wheel side stiffness Cf	N·rad−1	−66,900
Rear wheel side stiffness Cr	N·rad−1	−62,700
Distance a from center of mass to front axis	m	1.232
Distance b from center of mass to rear axis	m	1.468

:

In the simulation test, the working condition adopted is the double-shift working condition, and the road adhesion coefficient is 0.8. The double shift reference track is automatically generated by the driving scene designer in MATLAB according to the double shift standard test road.

4.1. Simulation Analysis of the Influence of Different Constant Vehicle Speeds on the Controller’s Tracking Control Effect

Firstly, the tracking effect of the improved controller is simulated, and four sets of simulation tests are carried out at different constant speeds, which are 30 km/h, 60 km/h, 80 km/h, and 100 km/h, respectively, and the comparison with the ordinary MPC is carried out.

The simulation results of 30 km/h and 60 km/h trajectory tracking are shown in (a) and (b) in Figure 3. Due to the low speed, the tracking process is relatively stable, the overshoot is small, and the trajectory tracking deviation is small. It can be seen from (a) and (b) in Figure 4 that the fluctuation of the yaw Angle is also within a reasonable range. Compared with the ordinary MPC, the improved vehicle has better stability. It can be seen from (c) and (d) in Figure 3 that with the increase in vehicle speed, when the vehicle speed is 80 and 100 km/h, the ordinary MPC fails to track at the longitudinal displacement of about 90 m. However, although the tracking error of the improved MPC at 40–90 m is slightly larger than that at low speed, it can still track the desired trajectory after 90 m. It can be seen from (c) and (d) in Figure 4 that the yaw Angle of the ordinary MPC fluctuates greatly after the longitudinal displacement of 80 m, putting the vehicle in a dangerous state. In the process of the change of the yaw Angle of the improved MPC, the oscillation amplitude is weakened, and the MPC is more stable, with better tracking accuracy and stability. Through the analysis of the above simulation results, the effectiveness of the improved MPC controller is verified.

The calculation time is also an important index to evaluate the performance of the controller. The calculation time of the controller is solved and analyzed by a simulation test. The calculation time of the controller is compared with that of ordinary MPC at low speed (30 km/h) and high speed (100 km/h), respectively, and the performance of the controller is further evaluated.

Through simulation tests, the calculation time of the controller and the percentage reduction of calculation time are shown in Figure 5. It can be seen from (b) and (d) that the calculation efficiency of the controller is higher when the vehicle is at low speed than at high speed, and the calculation time of the improved controller is shorter, and the calculation performance is significantly improved. However, there are still some time points in the simulation process that take longer to calculate than the ordinary model prediction controller. The reason is that the change of step length in the horizon leads to the influence of the rolling optimization results at the previous time on the rolling optimization results at the next time and thus affects the calculation and solution time. It can be seen from

Table 2. Comparison of calculation time of controllers at 30 km/h.

Parameter	The Improved MPC	Ordinary MPC
The maximum computation time of a single time/ms	4.29	6.96
Average computation time/ms	2.15	3.94

and

Table 3. Comparison of calculation time of controllers at 100 km/h.

Parameter	The Improved MPC	Ordinary MPC
The maximum computation time of a single time/ms	7.10	7.27
Average computation time/ms	4.08	4.76

that the average calculation time of the improved MPC controller at low speed and high speed is 2.15 ms and 4.08 ms, respectively, and the maximum single calculation time is 4.29 ms and 7.10 ms, respectively. The average calculation time of an ordinary MPC controller is 3.94 ms and 4.76 ms, respectively, and the maximum single calculation time is 6.96 ms and 7.27 ms, respectively. Compared with the conventional MPC controller, the average calculation time of the improved MPC controller is reduced by 45.43% and 14.29% at low speed and high speed, respectively, and the maximum single calculation time is reduced by 38.36% and 2.34%.

4.2. Simulation Analysis of the Influence of Vehicle Speed and Prediction Horizon on Tracking Effect

The prediction horizon has a great influence on the performance of the model prediction controller. The prediction horizon is similar to the distance of pre-viewing a future time in the driver model. Under the working conditions of different speeds, different pre-viewing distances will produce different control effects. According to previous research experience, when the vehicle is at low speed, reducing the pre-sight distance will produce a relatively good control effect, and increasing the pre-sight distance will reduce the tracking accuracy and vehicle stability. On the contrary, when the vehicle is at high speed, increasing the pre-sight distance can improve the control effect and will not cause problems such as reduced tracking accuracy and steering jitter due to too small a pre-sight distance, so as to improve the stability of the vehicle. Therefore, the joint simulation platform of CarSim and MATLAB/Simulink was built on the basis of the improved MPC, and the road adhesion coefficient was set to 0.8 under the condition of tracking double shift, and the tracking effect of different speeds and prediction horizons was analyzed. The speed was selected as 30 km/h and 100 km/h, and the prediction horizon Np was selected as 8, 15, 20, 26, and 32, respectively.

When the vehicle speed is 30 km/h, the simulation results of different prediction horizons are compared as shown in Figure 6. Because of the low speed, it is reasonable to select the first 4 groups of prediction horizon values for comparison. Through the analysis of (a) and (b), it can be seen that the whole tracking process is relatively stable, the tracking deviation of the trajectory is small, and the variation of the yaw Angle is also within a reasonable range. When the prediction horizon Np is equal to 8, the lateral tracking deviation and yaw Angle of the trajectory are the best. When the prediction horizon Np is equal to 26, the lateral tracking deviation is the largest. From the analysis of the above simulation results, it can be seen that with the increase in the prediction horizon Np, both lateral error and heading error become larger, resulting in a decrease in vehicle tracking accuracy.

When the vehicle speed is 100 km/h, the simulation results of different prediction horizons are compared as shown in Figure 7. Because the vehicle is in high-speed working condition, the stability of the vehicle will be greatly reduced if the prediction horizon is smaller, so it is reasonable to select the last four groups of prediction horizon values for analysis. Through the analysis of (a) and (b), it can be seen that when the vehicle runs at a higher speed, the smaller prediction horizon can no longer meet the requirements of path tracking accuracy and driving stability. This is because the dynamic characteristics of the vehicle change greatly at high speed, resulting in a sharp decline in the stability of the vehicle’s running, making the vehicle unable to quickly reach the desired corner. When the prediction horizon Np is equal to 15, the vehicle path tracking fails and completely deviates from the expected trajectory. The fluctuation of the yaw Angle of the vehicle is large, which has exceeded the limit of the stability requirements, and the vehicle is seriously unstable. When the prediction horizon Np is equal to 26 or 32, the deviation of trajectory tracking is moderate, and the yaw velocity changes relatively smoothly.

4.3. Design and Simulation Analysis of Adaptive MPC Controller Based on Variable Prediction Horizon

Through the analysis of the above simulation results, it can be seen that under the condition of low speed (constant speed), the tracking process of the vehicle in the forecasting horizon is relatively stable and the tracking accuracy of the vehicle trajectory is relatively high, while the tracking process in the forecasting horizon will increase the pre-sight distance and slow down the expected response trajectory. Although the tracking process is stable, the deviation is relatively large. Under high-speed (constant speed) conditions, the larger prediction horizon can stably track the expected path, but due to the increase in the pre-sight distance, there is a certain tracking bias, while the smaller prediction horizon will sacrifice the stability of the vehicle, and it is very dangerous at high-speed operation and should be avoided as far as possible. If a constant prediction horizon value is used, it cannot meet the requirements of trajectory tracking at low speed and high speed. Therefore, it is necessary to study the prediction horizon Np and update the prediction horizon value in real time according to the current vehicle speed.

Firstly, we determine the design goal of the adaptive trajectory tracking controller, which is to achieve high-precision trajectory tracking under the condition of vehicle dynamics parameter uncertainty and external interference. The core task of the controller is to adjust the control parameters online, adapt to the dynamic changes of the system, and ensure the tracking performance. A parameter adaptive module (variable prediction horizon controller) is added to the model prediction controller, and a parameter adaptive mechanism based on the recursive least squares method (RLS) is adopted to estimate vehicle dynamics parameters online. The vehicle status information is collected in real time, and the vehicle dynamics parameters are updated by the RLS algorithm and fed back to the MPC optimization problem. Based on the updated parameters, re-solve the MPC optimization problem and generate optimal control inputs.

According to the stability theory of MPC, the selection of the prediction horizon needs to meet certain conditions to ensure the stability of the closed-loop system. Based on Lyapunov stability theory and robust control theory, we derive the lower and upper bounds of the prediction horizon to ensure the stability of the system under uncertainty and external interference. In the process of dynamically updating the prediction horizon, we ensure that the prediction horizon still meets the constraints of the system after each update. Through the introduction of feasibility analysis, the solution of the optimization problem still exists in each update, and the stable operation of the system can be guaranteed.

Based on the above stability analysis, the control strategy for real-time optimization of prediction horizon parameters is given, as shown in Figure 8. According to the changes of the initial state of the vehicle and the future reference trajectory information, it is input into the model prediction controller; the controller generates the control quantity and inputs it into the CarSim vehicle model to control the trajectory tracking of the vehicle, and the CarSim vehicle model feeds back the state quantity to the prediction horizon controller. The predictive horizon controller estimates the vehicle dynamics parameters online, adjusts the controller parameters in real time, updates the prediction horizon online, determines the corresponding prediction horizon values, and generates the optimal control input to the MPC model predictive controller, thus forming an adaptive trajectory tracking controller based on the variable prediction horizon.

Through the above simulation analysis of low-speed and high-speed conditions, the value of the forecast horizon has a certain reference basis. In order to more accurately analyze the influence of the prediction horizon on the controller tracking effect under different vehicle speeds, CarSim and MATLAB/Simulink were used for joint simulation, the vehicle speed was set at 24~108 km/h, the prediction horizon Np was set at 6~35, and multiple sets of simulation tests were conducted based on the above selected prediction horizon values. At the same time, in order to ensure the effectiveness of the controller parameters and prevent the occurrence of vehicle control failure, the prediction horizon parameters are selected according to the following requirements: (1) The controller has no over-harmonic oscillation and can successfully track the trajectory path effectively. (2) The solution time of the controller is less than the sampling time [22].

Finally, valid simulation results were obtained. In the trajectory tracking research of autonomous vehicles, lateral error and heading error serve as important metrics for evaluating the controller’s performance. Therefore, we used smaller lateral error and heading error as evaluation criteria and employed the entropy weight method to determine their weights as 0.5623 and 0.4377, respectively. This ensures that the weights are determined based on the objective variability of the data rather than subjective judgment. Additionally, the method of the approximation of the ideal solution ordering (TOPSIS) was applied to obtain the optimal values of the prediction horizon Np under different vehicle speeds, as shown in

Table 4. Corresponding the prediction horizon Np under different vehicle speeds.

Vehicle Speed (km/h)	Prediction Horizon
24	8
30	8
60	15
80	20
100	26
108	26

. This approach facilitates the achievement of optimal path-tracking performance at different speeds.

According to the above simulation analysis and the results in Table 4, considering the accuracy and stability of trajectory tracking and the real-time requirements of the controller under high-speed working conditions, the minimum prediction horizon is 8, and the maximum prediction horizon is 26. The relationship between the prediction horizon and the longitudinal velocity change is obtained through the selection of 4 groups of parameters and the rounding of the third-degree polynomial fitting in MATLAB. As shown in Equation (22), its fitting curve is shown in Figure 9a. Among them, 4 points are the 4 groups of parameters in Table 4, so it can be seen that the curve after fitting and rounding can well represent the 4 groups of parameters.

$$Np=\left\{\begin{array}{l}8,v\le 30\\ int(-0.00004287v^3+0.01157v^2-0.6944v+20),30<v\le 100\\ 26,v>100\end{array}\right.$$

(22)

In order to better test the performance of the improved trajectory tracking controller (the following simulation results are called adaptive), the road adhesion coefficient is set at 0.8, and the vehicle is simulated for trajectory tracking under variable speed conditions, as shown in Figure 9b. Vehicle parameters and controller parameters are consistent with the above simulation. Finally, the simulation results are compared and analyzed, and the validity of the variable prediction horizon adaptive trajectory tracking controller is verified.

The simulation results under variable speed conditions are shown in Figure 10. It can be seen from (a) and (b) that when the prediction horizon Np is equal to 8, the vehicle speed continues to increase, and the longitudinal displacement is about 80 m. At this time, the vehicle dynamic characteristics change greatly, which can no longer meet the constraint requirements, resulting in the failure of tracking the expected trajectory and expected yaw Angle at the exit under the double-shift condition. When the prediction horizon is large, the tracking accuracy is poor, and the adaptive vehicle can accurately track the reference track. Although the yaw Angle deflects with the increase in speed, it can quickly adjust and converge to the expected value. Through the analysis of (c) and (d), it can be seen that when the prediction horizon Np is equal to 8, the vehicle tracking failure leads to large lateral error and heading error, and the adaptive vehicle tracking lateral error is the smallest, and the corresponding heading error is small. The heading error in the prediction horizon of 32 is the smallest at medium and high speeds, but the tracking lateral error is larger. This also indicates that the vehicle stability is better when the prediction horizon value is large at high speed, but the tracking lateral error will be larger. According to (e) and (f), it can be seen that when the prediction horizon Np is small, the yaw velocity will fluctuate greatly; when the prediction horizon Np is equal to 8, the vehicle will become seriously unstable; when the prediction horizon Np is large, the vehicle will have good stability; and the changes of the lateral declination Angle and yaw velocity of the vehicle’s center of mass are smooth and natural using adaptive methods, indicating that the vehicle trajectory tracking process is stable.

To further evaluate the goodness of fit of the model, we calculated the SSE (Sum of Squared Errors) for both the fixed and adaptive prediction horizons in (a) and (b) of Figure 10. In Figure 10a, under different prediction horizons Np, the SSE values are as follows: SSE = 473.55 (Np = 8), SSE = 2.26 (Np = 15), SSE = 2.27 (Np = 20), SSE = 4.11 (Np = 26), and SSE = 6.89 (Np = 32), while the adaptive prediction horizon yields SSE = 1.39. When Np = 8, the system exhibited significant fluctuations during the tracking of the desired trajectory and even deviated from it, leading to a very large SSE value. These results indicate that the adaptive prediction horizon offers superior goodness of fit, smaller prediction errors, and the ability to more effectively track the desired trajectory. In Figure 10b, under different prediction horizons Np, the SSE values are as follows: SSE = 14.82 (Np = 8), SSE = 0.0096 (Np = 15), SSE = 0.034 (Np = 20), SSE = 0.081 (Np = 26), and SSE = 0.17 (Np = 32), while the adaptive prediction horizon yields SSE = 0.0074. When Np = 8, the SSE value was the largest, accompanied by substantial yaw angle fluctuations and a significant deterioration in vehicle stability. These findings demonstrate that the adaptive prediction horizon achieves higher goodness of fit, smaller prediction errors, smoother yaw angle variations, and stable vehicle tracking performance.

Figure 11 shows the change of adaptive prediction horizon under variable speed. It can be seen that with the constant change of vehicle speed, the controller can update the prediction horizon in real time so as to predict the output at the next moment, ensuring that the variable horizon tracking controller has good adaptive performance under different vehicle speeds.

5. Conclusions

Taking vehicle trajectory tracking control as the research direction, this paper establishes a three-degree-of-freedom vehicle dynamics model, improves the controller on the basis of ordinary MPC, selects different speeds and prediction horizons for simulation, compares and analyzes the optimal prediction horizon values corresponding to different speeds, and fits the functional relationship curves between prediction horizon and vehicle speed. A variable prediction horizon adaptive trajectory tracking controller is designed. The prediction horizon is updated in real time according to the vehicle speed, the output value of the next moment is predicted, and the joint simulation platform of CarSim and MATLAB/Simulink is built for verification. The main conclusions are as follows:

(1)

The improved MPC can maintain good trajectory tracking accuracy and stability at both low and high speeds (a specific speed), and the average calculation time is reduced by 45.43% and 14.29%, respectively, compared with the ordinary MPC.

(2)

The simulation results show that when the vehicle is at a low speed, the larger prediction horizon will increase the lateral error of trajectory tracking and increase the computational burden of the controller. Using the smaller prediction horizon, the tracking effect is better when the vehicle speed is low. However, in the practical application process, the complex and changeable working conditions will lead to large speed variation amplitude, and the use of a small prediction horizon will lead to the decline of vehicle trajectory tracking stability. The adaptive trajectory tracking controller can update the prediction horizon value online according to the speed change, ensuring that the vehicle can maintain good adaptive performance at different speeds.