# Trajectory Tracking Control of Intelligent Driving Vehicles Based on MPC and Fuzzy PID

Authors: Can Yang; Jie Liu
Journal: Mathematical Problems in Engineering
 (2023)

---

## Abstract

To improve the stability and accuracy of quintic polynomial trajectory tracking, an MPC (model predictive control) and fuzzy PID (proportional-integral-difference)- based control method are proposed. A lateral tracking controller is designed by using MPC with rule-based horizon parameters. The lateral tracking controller controls the steering angle to reduce the lateral tracking errors. A longitudinal tracking controller is designed by using a fuzzy PID. The longitudinal controller controls the motor torque and brake pressure referring to a throttle/brake calibration table to reduce the longitudinal tracking errors. By combining the two controllers, we achieve satisfactory trajectory tracking control. Relative vehicle trajectory tracking simulation is carried out under common scenarios of quintic polynomial trajectory in the Simulink/Carsim platform. The result shows that the strategy can avoid excessive trajectory tracking errors which ensures a better performance for trajectory tracking with high safety, stability, and adaptability.


---

## Body

## 1. Introduction

Autonomous vehicles have brought huge benefits to traffic safety, reducing traffic congestion, environment protection, and productivity. As a key part of autonomous driving, trajectory tracking control has a high demand on the safety and stability of the trajectory tracking. To meet this challenge, it is urgent to propose a combined lateral and longitudinal tracking strategy that ensures the tracking performance [1]. Active front steering (AFS) and electronic stability control (ESC) have been proven effective for enhancing vehicle stability and decreasing vehicle accidents [2]. By combining the two actuators using a proper control strategy, more advantages can be obtained such as solving the problems related to vehicle motion control and trajectory tracking control [3].To achieve the lateral tracking control capability of intelligent vehicles, a number of studies and advancements have been made. Guo et al. [4] developed a fuzzy logic-based lateral controller with parameters tuned through a genetic algorithm. Norouzi et al. [5] adopted the Lyapunov theory for the lateral control, and the sliding mode algorithm and backstepping algorithm were also used to achieve the lateral control. He et al. [6] developed a lateral controller with two layers. Its upper controller was designed based on a linear time-varying MPC algorithm with parameters improved by the PSO algorithm, and its lower controller was designed based on a neural network algorithm to track the desired steering angle quickly. Liu et al. [7] developed a lateral controller with a steering angle compensation method and adaptively changing prediction horizon parameters. Brown et al. [8] developed a framework for an intelligent vehicle motion control system. It included path planning and tracking functions using an MPC algorithm and a multiobjective optimization index to track trajectory safely. Referring to more literature [9–11] and MPC algorithm’s advantages to solve the vehicle motion control problems with constraints, the MPC algorithm has been developed and applied more in the lateral tracking control.To achieve the longitudinal tracking control capability of intelligent vehicles, many proven methods have been developed. Sun et al. [12] developed a vehicle longitudinal speed controller with the hybrid MPC algorithm. The switching strategy of the operation modes and the torques desired were tuned by mixed-integer quadratic programming. El Majdoub et al. [13] proposed a nonlinear controller based on Lyapunov theory to realize the longitudinal control. Kim et al. [14] developed a method for adaptive time-varying controller parameter and throttle and brake control for speed tracking. The method can be used for different types of vehicles without knowing the details of the drive and brake systems. Kebbati et al. [15] developed two longitudinal controllers, respectively, using genetic algorithm and neural network algorithm and compared the performance of each controller.Since there are strong couplings between the lateral control and the longitudinal control of autonomous vehicles, it is inappropriate to focus on only one of them. It is essential to propose a combined lateral and longitudinal trajectory tracking control method. Aiming at this problem, Attia et al. [16] developed a combined longitudinal and lateral control method including a steering strategy based on MPC and a nonlinear speed tracking algorithm considering the powertrain dynamics. Zhang and Li [17] developed a combined controller based on the LQR algorithm and dual-PID algorithm which considers feedforward control and compensation for steering angle. Xu et al. [18] developed a coupled lateral and longitudinal control method integrating efficient controllers unsensitive to system parameters. The longitudinal controller was designed to be a model reference adaptive control system, and the lateral controller was designed based on the SMC algorithm.Given this, based on previous studies, a combined lateral and longitudinal control method is developed. We propose a combined lateral and longitudinal controller with prediction function and parameter structure with adaptive changes. The strategy decouples the lateral and longitudinal tracking problems, makes it possible to design each controller separately, and adopts the quintic polynomial curve as the reference trajectory. The lateral controller is designed based on MPC with a rule-based horizon parameters selection strategy. The longitudinal control method is based on fuzzy PID and inverse dynamics methods. By combining the two controllers with adaptive features, we achieve satisfactory trajectory tracking control. Relative simulation tests’ results show a satisfactory performance of the proposed controller.


## 2. Vehicle Dynamics Model

Mathematical models of vehicle are particularly important in the vehicle motion control. In this study, a 2DOF vehicle model is applied to construct the trajectory tracking control system to increase computing efficiency and ensure the tracking accuracy.The vehicle model is shown in Figure1, and it is created according to relative assumptions as follows [19]:(1)
We assume that the vehicle and the suspension are rigid bodies and neglect the suspension dynamics(2)
We assume that the vehicle uses front wheels to steer, and the two wheels are steered at the same angle(3)
We assume that each axle’s wheels are combined as a single wheel along the vehicle’s centerlineFigure 1
2DOF vehicle dynamics model.Letx be the longitudinal direction of the vehicle, y be the lateral direction of the vehicle, X be the longitudinal direction of the absolute coordinate system, and Y be the lateral direction of the absolute coordinate system.By analyzing the force in they-axis, we can get the following equation:(1)Fyfcosδf+Fyr=may,where Fyf is the front wheels’ cornering force, Fyr is the rear wheels’ cornering force, m is the vehicle weight, and ay is the acceleration in the y-axis.(2)ay=y¨+φ˙x˙,where y¨ is the acceleration in the y-axis caused by the vehicle motion, x˙ is the speed in the x-axis, φ is the yaw angle, and φ˙ is the yaw rate.By substituting equation (2) into equation (1), we can get the following equation:(3)Fyfcosδf+Fyr=my¨+φ˙x˙.According to the rotation law, we can get the following equation:(4)Izφ¨=aFyfcosδf−bFyr,where a is the distance between the vehicle’s center of gravity and the front axle and b is the distance between the vehicle’s center of gravity and the rear axle.According to the cornering force equation, we can get the following equation:(5)Fyf=2Cαfαf,Fyr=2Cαrαr,where Cαf is the front tires’ cornering stiffness, Cαr is the rear tires’ cornering stiffness, αf is the front tire’s slip angle, and αr is the rear tires’ slip angle.By analyzing the vehicle motion status, we can get the following equation:(6)αf=θ−δf=φ˙a+y˙x˙−δf,αr=y˙−φ˙bx˙,where θ is the vehicle’s heading angle.According to equations (1)–(6), we can get the following equation:(7)y¨=2mCαf+Cαrx˙y˙+aCαf−bCαrx˙φ˙−Cαfδf−x˙φ˙,φ¨=2IzaCαf−bCαrx˙y˙+a2Cαf+b2Cαrx˙φ˙−aCαfδf,where Iz is the vehicle’s rotational inertia along z-axis and δf is the front wheels’ steering angle.We rewrite equation (7) to get the following matrix form:(8)y˙y¨φ˙φ¨=01000Cαf+Cαrmx˙0aCαf−bCαrmx˙−x˙00010aCαf−bCαrIzx˙0a2Cαf+b2CαrIzx˙yy˙φφ˙−0Cαfm0aCαfIzδf.Relative vehicle model’s structural parameters are listed in Table1.Table 1
Vehicle model’s parameters.
ParametersValuem (kg)1400a (m)1.015b (m)1.895Cαf (N/rad)−108000Cαr (N/rad)−108000Iz (kg· m2)1502.9


## 3. Reference Trajectory

In the trajectory planning algorithm, the reference path is derived as discrete points in a finite time domain, and the curve fitting method is the primary method for processing the points [20].Since the vehicle’s position, yaw angle, and acceleration are continuous, the trajectory curve should be continuous, and the curve should be continuous at both the first and second orders [21–23]. In this study, quintic polynomial curves are implemented to meet the constraints and test the controller’s performance.The trajectory curve is a function of timet and can be written as follows:(9)Xt=a1+a2t+a3t2+a4t3+a5t4+a6t5,YX=b1+b2X+b3X2+b4X3+b5X4+b6X5,θrt=arctanY'Xt,krt=Y″Xt1+Y'Xt3/2,where a1,a2,a3,a4,a5,a6,b1,b2,b3,b4,b5 are coefficients to be determined.According to equation (9), the following parameters are required to get the reference trajectory curve [21]:(10)Xstart=X0,X˙0,X¨0;Ystart=Y0,Y′0,Y″0;Xend=Xttotal,X˙ttotal,X¨ttotal;Yend=YXend,Y′Xend,Y″Xend,where ttotal is total time interval for the reference trajectory and Xend is the vehicle’s longitudinal position at the end point along the reference trajectory.


## 4. Lateral Control

MPC algorithm is applied to the lateral controller design according to its advantages of predictive control and solving control problems with constraints [24]. The controller ensures accurate lateral tracking control through real-time roll optimization of the front wheel steering angle. The principle of the lateral controller is demonstrated in Figure 2. Y is the vehicle position’s projection on Y-axis of the absolute coordinate system, and X is the vehicle position’s projection on X-axis of the absolute coordinate system.Figure 2
Block diagram of lateral control algorithm.


### 4.1. Analysis of Vehicle Lateral Tracking Error

In this section, we discuss the lateral tracking errors under the Frenet coordinate system, and the diagram of corresponding errors is shown in Figure3. Point A and Point B are, respectively, the actual and reference position of the vehicle at the current moment in the absolute coordinate system. Point C is obtained by projecting point A onto the curve.Figure 3
Diagram of trajectory tracking error.By observing Figure3, we can get the following equation:(11)de=x−xr,y−yr,ηr=−sinθr,cosθr,τr=cosθr,sinθr,ed=deTηr,es=deTτr,θp=θm+eskr,eθ=θ−θp,where de is the vector difference between Point A and Point B, τr is the tangent vector of point B on the curve, ηr is the normal vector of point B on the curve, ed is the lateral position error, es is the longitudinal position error, and eθ is the heading angle error.Furthermore, the following equation can be obtained:(12)ed=deTηr,ed˙=y˙cosφ−θp+x˙sinφ−θp,ed¨=y¨+eφ˙x˙,eφ=φ−θp%2pi,eφ˙=φ˙−krs˙,eφ¨=φ¨−θp¨,where eφ is the yaw angle error, eφ˙ is the rate of yaw angle error change, s˙ is the projection of Point A’s speed onto Point C, θp is the heading angle of Point B, and θr is the heading angle of Point C.


### 4.2. Lateral Tracking Controller Design

A predictive model is necessary to estimate the dynamic system’s future states. In the prediction time domain, the MPC controller performs rolling optimization for the input with constraints to ensure that the cost function is minimized.In this section, a linear time-varying model predictive control method is used for the design of the controller based on equation (9). We can get the trajectory tracking state error matrix as follows:(13)ed˙ed¨eφ˙eφ¨=Aeded˙eφeφ˙+Bδf,where(14)A=01000Cαf+Cαrmx˙−Cαf+CαrmaCαf−bCαrmx˙00010aCαf−bCαrIzx˙−aCαf−bCαrIza2Cαf+b2CαrIzx˙,B=0−Cαfm0−aCαfIz.According to equation (11), we can get the following equation [25]:(15)χ˙=Aχ+Bδ,where (16)χ˙=ed˙,ed¨,eφ,˙eφ¨T,χ=ed,ed˙,eφ,eφ˙T,δ=δf.By discretizing equation (12), the following equation can be obtained:(17)χk+1=Adχk+Bdδk,χk+1=Adχk+Bdδk−1+Δδk,where Ad is the discrete period matrix of A and Bd is the discrete period matrix of B.Furthermore,(18)ξk+1=Ad˙ξk+Bd˙Δδk,where(19)ξk=χkδk−1,Ad˙=AdBd0I,Bd˙=BdI.Let(20)γk=Cd˙ξk,where γ=ed,eφT,Cd˙=10100.According to equation (15), the MPC controller’s cost function can be written as follows:(21)J=ΔminΔUt∑i=1Npγt+i|t−γrt+i|tQ2+∑i=1Nc−1Δδt+i|tR2+ρε2,Δδmin≤Δδt≤Δδmax,δmin≤δt≤δmax,ε>0,where γr=0,0T, Np is the prediction horizon, Nc is the control horizon, δmin is the minimum of δf, δmax is the maximum of δf, Δδmin is the minimum of Δδ, Δδmax is the maximum of Δδ, ε is the relaxation factor, Q is the weighting matrix of the tracking effect, and R is the weighting matrix of the input energy.


### 4.3. Rule-Based Horizon Parameters Selection

The determinations of the MPC controller’s cost function, weighting matrices, and horizon parameters are of great importance. Due to the complex driving conditions of the vehicle, if the MPC controller only uses fixed horizon parameters, it may not be able to obtain a satisfactory trajectory tracking performance [26]. For different trajectories, the optimal horizon parameter selection may be different. In order to avoid that the horizon parameters matching workload is too large, the MPC controller adopts a rule-based method to match horizon parameters Np and Nc adaptively according to the vehicle speed v (km/h).By referring to the simulation results, the horizon parameters are adjusted to ensure the tracking errors are acceptable. The matching rules for the horizon parameters are created as follows:(22)Np,Nc=15,1,v≤10,20,2,10<v≤60,25,22,60<v.


### 4.4. Relative Parameters Setting

The lateral controller’s parameters are shown as follows:(23)Ts=20ms,−0.17rad≤u≤0.17rad,−0.015rad≤Δu≤0.015rad,Q=10000030,R=10,ρ=1000,where Ts is sample time.


### 4.5. Simulation Results

The effect of the lateral controller is evaluated through simulations under two scenarios, and the changing trend in lateral errors during the tracking process is discussed.Scenario 1 (double lane-change maneuver): To test the controller’s performance under critical work condition, we adopt double lane-change maneuvers with different speeds as the test scenarios and LQR lateral controller are adopted as a control group. The simulation processes of test 1∼test 3 and relative results are shown in Figures4–7 and Tables 2 and 3.Scenario 2 (guidance of a highway exit): To test the controller’s performance under large steering condition, we conduct simulation tests under this scenario with different speeds. The simulation processes of test 4∼test 6 and relative results are shown in Figures8–10 and Table 4.Scenario 1 (double lane-change maneuver):(1)
Test 1. By observing Figure4, Tables 2 and 3, and Figure 7, during Test1 using the proposed MPC controller, the average lateral position error is within a satisfactory range of 0.5 cm, and the average heading angle error is less than 0.007 rad. When compared to the test with LQR controller, the average lateral position error decreases by 5.25 cm.(2)
Test 2. By observing Figure5, Tables 2 and 3, and Figure 7, during Test2 using the proposed MPC controller, as the vehicle speed increases, the lateral position error reaches a maximum of 3.28 cm, and the average lateral position error reaches 0.83 cm. The average heading angle error is basically unchanged compared with Test1 using the MPC controller. When compared to the test with LQR controller, the average lateral position error decreases by 30.55 cm.As the LQR controller mainly controls the steering angle according to the current tracking errors without prediction, the lateral position error increases to 33.83 cm.(3)
Test 3. By observing Figures6 and 7, and Tables 2 and 3, during Test1 using the proposed MPC controller, the lateral position error is less than 0.85 cm, and the heading angle error slightly increases as vehicle speed increases. However, the maximum lateral position error increases to 51.71 cm under the LQR control, which represents the loss of safety.Scenario 2 (guidance of a highway exit):Figure 4
Simulation process of lateral tracking under the double lane-change maneuver at 10 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 5
Simulation process of lateral tracking under the double lane-change maneuver at 30 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 6
Simulation process of lateral tracking under the double lane-change maneuver at 60 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 7
Lateral tracking errors of the simulation tests: (a) maximum lateral position errors; (b) average lateral position errors; (c) maximum heading angle errors; (d) average heading angle errors.
(a)(b)(c)(d)Figure 8
Simulation process of lateral tracking under the guidance of a highway exit at constant speed 10 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 9
Simulation process of lateral tracking under the guidance of a highway exit at constant speed 30 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 10
Simulation process of lateral tracking under the guidance of a highway exit at constant speed 50 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Table 2
The results obtained with the LQR controller.
ParametersTest 1Test 2Test 3edavg (cm)5.728.3612.72edmax (cm)21.0933.8351.71eθavg (rad)0.00890.00910.0107eθmax (rad)0.04510.05220.0599Table 3
The results obtained with the MPC controller.
ParametersTest 1Test 2Test3edavg (cm)0.470.830.23edmax (cm)1.843.280.84eθavg (rad)0.00670.00660.0077eθmax (rad)0.02650.02590.0333Table 4
The results obtained with the MPC controller.
ParametersTest 4Test 5Test 6edavg (cm)0.81.451.09edmax (cm)1.52.862.46eθavg (rad)0.01150.01140.012eθmax (rad)0.02120.02120.022By observing the results of Test 4∼Test 6 shown in Figure8∼Figure 10 and Table 4, it can be seen that during the simulation tests using the proposed lateral controller, the lateral position error and heading angle error remain within reasonable ranges of 3 cm and 0.03 rad.According to the above analysis, we can know that the proposed lateral controller has a good control effect and is conducive to improving the control effect of lateral tracking.


## 4.1. Analysis of Vehicle Lateral Tracking Error

In this section, we discuss the lateral tracking errors under the Frenet coordinate system, and the diagram of corresponding errors is shown in Figure3. Point A and Point B are, respectively, the actual and reference position of the vehicle at the current moment in the absolute coordinate system. Point C is obtained by projecting point A onto the curve.Figure 3
Diagram of trajectory tracking error.By observing Figure3, we can get the following equation:(11)de=x−xr,y−yr,ηr=−sinθr,cosθr,τr=cosθr,sinθr,ed=deTηr,es=deTτr,θp=θm+eskr,eθ=θ−θp,where de is the vector difference between Point A and Point B, τr is the tangent vector of point B on the curve, ηr is the normal vector of point B on the curve, ed is the lateral position error, es is the longitudinal position error, and eθ is the heading angle error.Furthermore, the following equation can be obtained:(12)ed=deTηr,ed˙=y˙cosφ−θp+x˙sinφ−θp,ed¨=y¨+eφ˙x˙,eφ=φ−θp%2pi,eφ˙=φ˙−krs˙,eφ¨=φ¨−θp¨,where eφ is the yaw angle error, eφ˙ is the rate of yaw angle error change, s˙ is the projection of Point A’s speed onto Point C, θp is the heading angle of Point B, and θr is the heading angle of Point C.


## 4.2. Lateral Tracking Controller Design

A predictive model is necessary to estimate the dynamic system’s future states. In the prediction time domain, the MPC controller performs rolling optimization for the input with constraints to ensure that the cost function is minimized.In this section, a linear time-varying model predictive control method is used for the design of the controller based on equation (9). We can get the trajectory tracking state error matrix as follows:(13)ed˙ed¨eφ˙eφ¨=Aeded˙eφeφ˙+Bδf,where(14)A=01000Cαf+Cαrmx˙−Cαf+CαrmaCαf−bCαrmx˙00010aCαf−bCαrIzx˙−aCαf−bCαrIza2Cαf+b2CαrIzx˙,B=0−Cαfm0−aCαfIz.According to equation (11), we can get the following equation [25]:(15)χ˙=Aχ+Bδ,where (16)χ˙=ed˙,ed¨,eφ,˙eφ¨T,χ=ed,ed˙,eφ,eφ˙T,δ=δf.By discretizing equation (12), the following equation can be obtained:(17)χk+1=Adχk+Bdδk,χk+1=Adχk+Bdδk−1+Δδk,where Ad is the discrete period matrix of A and Bd is the discrete period matrix of B.Furthermore,(18)ξk+1=Ad˙ξk+Bd˙Δδk,where(19)ξk=χkδk−1,Ad˙=AdBd0I,Bd˙=BdI.Let(20)γk=Cd˙ξk,where γ=ed,eφT,Cd˙=10100.According to equation (15), the MPC controller’s cost function can be written as follows:(21)J=ΔminΔUt∑i=1Npγt+i|t−γrt+i|tQ2+∑i=1Nc−1Δδt+i|tR2+ρε2,Δδmin≤Δδt≤Δδmax,δmin≤δt≤δmax,ε>0,where γr=0,0T, Np is the prediction horizon, Nc is the control horizon, δmin is the minimum of δf, δmax is the maximum of δf, Δδmin is the minimum of Δδ, Δδmax is the maximum of Δδ, ε is the relaxation factor, Q is the weighting matrix of the tracking effect, and R is the weighting matrix of the input energy.


## 4.3. Rule-Based Horizon Parameters Selection

The determinations of the MPC controller’s cost function, weighting matrices, and horizon parameters are of great importance. Due to the complex driving conditions of the vehicle, if the MPC controller only uses fixed horizon parameters, it may not be able to obtain a satisfactory trajectory tracking performance [26]. For different trajectories, the optimal horizon parameter selection may be different. In order to avoid that the horizon parameters matching workload is too large, the MPC controller adopts a rule-based method to match horizon parameters Np and Nc adaptively according to the vehicle speed v (km/h).By referring to the simulation results, the horizon parameters are adjusted to ensure the tracking errors are acceptable. The matching rules for the horizon parameters are created as follows:(22)Np,Nc=15,1,v≤10,20,2,10<v≤60,25,22,60<v.


## 4.4. Relative Parameters Setting

The lateral controller’s parameters are shown as follows:(23)Ts=20ms,−0.17rad≤u≤0.17rad,−0.015rad≤Δu≤0.015rad,Q=10000030,R=10,ρ=1000,where Ts is sample time.


## 4.5. Simulation Results

The effect of the lateral controller is evaluated through simulations under two scenarios, and the changing trend in lateral errors during the tracking process is discussed.Scenario 1 (double lane-change maneuver): To test the controller’s performance under critical work condition, we adopt double lane-change maneuvers with different speeds as the test scenarios and LQR lateral controller are adopted as a control group. The simulation processes of test 1∼test 3 and relative results are shown in Figures4–7 and Tables 2 and 3.Scenario 2 (guidance of a highway exit): To test the controller’s performance under large steering condition, we conduct simulation tests under this scenario with different speeds. The simulation processes of test 4∼test 6 and relative results are shown in Figures8–10 and Table 4.Scenario 1 (double lane-change maneuver):(1)
Test 1. By observing Figure4, Tables 2 and 3, and Figure 7, during Test1 using the proposed MPC controller, the average lateral position error is within a satisfactory range of 0.5 cm, and the average heading angle error is less than 0.007 rad. When compared to the test with LQR controller, the average lateral position error decreases by 5.25 cm.(2)
Test 2. By observing Figure5, Tables 2 and 3, and Figure 7, during Test2 using the proposed MPC controller, as the vehicle speed increases, the lateral position error reaches a maximum of 3.28 cm, and the average lateral position error reaches 0.83 cm. The average heading angle error is basically unchanged compared with Test1 using the MPC controller. When compared to the test with LQR controller, the average lateral position error decreases by 30.55 cm.As the LQR controller mainly controls the steering angle according to the current tracking errors without prediction, the lateral position error increases to 33.83 cm.(3)
Test 3. By observing Figures6 and 7, and Tables 2 and 3, during Test1 using the proposed MPC controller, the lateral position error is less than 0.85 cm, and the heading angle error slightly increases as vehicle speed increases. However, the maximum lateral position error increases to 51.71 cm under the LQR control, which represents the loss of safety.Scenario 2 (guidance of a highway exit):Figure 4
Simulation process of lateral tracking under the double lane-change maneuver at 10 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 5
Simulation process of lateral tracking under the double lane-change maneuver at 30 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 6
Simulation process of lateral tracking under the double lane-change maneuver at 60 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 7
Lateral tracking errors of the simulation tests: (a) maximum lateral position errors; (b) average lateral position errors; (c) maximum heading angle errors; (d) average heading angle errors.
(a)(b)(c)(d)Figure 8
Simulation process of lateral tracking under the guidance of a highway exit at constant speed 10 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 9
Simulation process of lateral tracking under the guidance of a highway exit at constant speed 30 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Figure 10
Simulation process of lateral tracking under the guidance of a highway exit at constant speed 50 km/h: (a) position tracking; (b) lateral position error; (c) heading angle error; (d) steering angle.
(a)(b)(c)(d)Table 2
The results obtained with the LQR controller.
ParametersTest 1Test 2Test 3edavg (cm)5.728.3612.72edmax (cm)21.0933.8351.71eθavg (rad)0.00890.00910.0107eθmax (rad)0.04510.05220.0599Table 3
The results obtained with the MPC controller.
ParametersTest 1Test 2Test3edavg (cm)0.470.830.23edmax (cm)1.843.280.84eθavg (rad)0.00670.00660.0077eθmax (rad)0.02650.02590.0333Table 4
The results obtained with the MPC controller.
ParametersTest 4Test 5Test 6edavg (cm)0.81.451.09edmax (cm)1.52.862.46eθavg (rad)0.01150.01140.012eθmax (rad)0.02120.02120.022By observing the results of Test 4∼Test 6 shown in Figure8∼Figure 10 and Table 4, it can be seen that during the simulation tests using the proposed lateral controller, the lateral position error and heading angle error remain within reasonable ranges of 3 cm and 0.03 rad.According to the above analysis, we can know that the proposed lateral controller has a good control effect and is conducive to improving the control effect of lateral tracking.


## 5. Longitudinal Control

The fuzzy PID algorithm is applied to the longitudinal controller design. The longitudinal controller receives the real-time speed error and position error and then corrects the vehicle acceleration demand. Based on the inverse dynamics method, the optimal motor torque and brake pressure are solved in real time to achieve longitudinal control. The longitudinal tracking control flow is shown in Figure11.Figure 11
Block diagram of the longitudinal control algorithm.


### 5.1. Analysis of Vehicle Longitudinal Tracking Error

In this subsection, the longitudinal tracking problem mainly considers the longitudinal speed error and position error.According to Figure3 and equations (9) and (10), the following equation is obtained:(24)es=τrdeT,vτp=x˙coseφ−y˙sineφ1−kr∗ed,ev=vr−vτp,where vτp is the projection of point A’s speed in the tangent direction of point C and ev is the longitudinal speed error.


### 5.2. Throttle/Brake Calibration

The throttle-brake calibration is the foundation to ensure precise longitudinal tracking control of the vehicle. When the longitudinal tracking control system receives acceleration and deceleration signals at a specific speed, the control module solves the accurate throttle opening Thr and brake pressure P by looking up the throttle/brake calibration tableZ=vlookupa,v.The throttle-brake calibration table is created through the cosimulation based on Carsim and Matlab.(1)
When input throttle opening Thr increasing from 0 to 100% to the vehicle model in Carsim, changing speedv and acceleration a are recorded in the process of accelerating from 0 km/h to the 144 km/h speed corresponding to each throttle opening Thr into the Matlab workspace.(2)
When input brake pressure P increasing from 0 to 7 MPa to the vehicle model in Carsim, changing speedv and acceleration a are recorded in the process of decelerating from 144 km/h to 0 km/h corresponding to each brake pressure P into the Matlab workspace.By integrating data obtained previously, the throttle/brake calibration table can be obtained as shown in Figure12.Figure 12
Throttle/brake calibration table.The longitudinal tracking controller based on the inverse dynamics method receives reference acceleration signals and solves the accurate throttle opening Thr and brake pressureP referring to Figure 12. According to motor speed, the Thr/P values and simplified motor speed-torque curve are shown in Figure 13, and the motor torque demand can be obtained to achieve real-time longitudinal tracking control.Figure 13
Simplified motor speed-torque curve.


### 5.3. Longitudinal Tracking Controller Design

Since the tracking control system itself and multiple table lookups may lead to tracking errors, only controlling the real-time acceleration may lead to the longitudinal speed and position to deviate from the reference trajectory. We adopt the fuzzy PID algorithm in the controller to reduce the tracking errors. The longitudinal controller adjusts the required acceleration and brake pressure to ensure the accuracy of the longitudinal tracking control.The parametersKp,Ki and Kd completely determine the characteristics of a conventional PID controller, and this may lead to the unsatisfactory dynamic performance of nonlinear control systems. However, the fuzzy PID controller tunes the relative parameters adaptively referring to the rules. The common structure of a fuzzy PID controller can be depicted as shown in Figure 14.Figure 14
Fuzzy PID control flowchart.From Figure14, we can see that the fuzzy controller adopts the value of Kp,Ki, and Kd adaptively according to e and ec. The center of gravity method is applied as the defuzzification method and considers the errors of longitudinal speed, longitudinal position, and their rate of change.Fuzzy rules are set and adjusted referring to the following principles [27] and the actual e and ec in the simulation process:(1)
Whene and ec are large, a large kp should be taken to speed up the system response; a small ki should be taken to avoid large system overshoot; a medium kd should be taken to avoid derivative saturation.(2)
Whene and ec are medium, a small kp should be taken to avoid system overshoot; medium ki and kd should be taken to guarantee system responsiveness.(3)
Whene is small, kp and ki should be increased to improve system stability. To avoid the oscillation phenomenon and improve the anti-interference performance of the system, relatively large kd should be taken when ec is small and relatively small kd should be taken when ec is large.According to the theories, simulation analysis, and expert experience [28∼31], relative membership functions are created as shown in Figures 15 and 16, and the fuzzy rules are created as shown in Tables 5–10.Figure 15
Membership functions corresponding to vehicle speed: (a) longitudinal speed error’s membership function; (b) change rate of longitudinal speed error’s membership function; (c)Kp’s membership function; (d)Ki’s membership function; (e)Kd’s membership function.
(a)(b)(c)(d)(e)Figure 16
Membership functions corresponding to vehicle longitudinal position: (a) longitudinal position error’s membership function; (b) change rate of longitudinal position error’s membership function; (c)Kp’s membership function; (d)Ki’s membership function; (e)Kd’s membership function.
(a)(b)(c)(d)(e)Table 5
Fuzzy rules forKp.
eceNBNSZOPSPBNBPBZONBMBPBZOPBPBNBNBPBPBPBPBNBPBPBTable 6
Fuzzy rules forKi.
eceNBNSZOPSPBNBNBNBNBNBNBZOZONBNBNBNBPBNBNBZOZOPBTable 7
Fuzzy rules forKd.
eceNBNSZOPSPBNBNBNBNBNBNBZONBNBNBNBZOPBZONBZONBPBTable 8
Fuzzy rules ofKp.
eceNBNSZOPSPBNBPBZONBMBPBZOPBPBNBNBPBPBPBPBNBPBPBTable 9
Fuzzy rules ofKi.
eceNBNSZOPSPBNBNBNBNBNBNBZOZONBNBNBNBPBNBNBZOZOPBTable 10
Fuzzy rules ofKd.
eceNBNSZOPSPBNBPBZONBMBPBZOPBPBNBNBPBPBPBPBNBPBPB


### 5.4. Simulation Results

The effect of the longitudinal controller is evaluated through the following simulation.A part of the CLTC work condition is used to test the performance of the baseline dual PID controller and the proposed controller. The longitudinal tracking control process and relative results are shown in Figure17 and Table 11.Figure 17
Longitudinal tracking process: (a) speed tracking process; (b) longitudinal position tracking process; (c) longitudinal speed error; (d) longitudinal position error.
(a)(b)(c)(d)Table 11
Results of the longitudinal control tests.
ParametersDual PIDFuzzy PIDesavg (cm)6.230.53esmax (cm)36.084.57evavg (m/s)0.03550.0076evmax (m/s)0.67920.2011According to Figure17 and Table 11, it can be seen that the reference speed and longitudinal position are well tracked by using the proposed controller, the longitudinal position error remains within a reasonable range of 5 cm, and the average speed error is less than 0.008 m/s.However, the simulation test using the dual PID controller reaches a longitudinal position error of 36.08 cm and a longitudinal speed error of 0.6792 m/s, which are unacceptable.Figures17(c) and 17(d) show that the longitudinal errors can be corrected automatically by the longitudinal controller based on fuzzy PID while the position error and speed error cannot be corrected in time by using the dual PID strategy.The proposed controller can tune the parameters adaptively referring to the fuzzy rules to achieve more accurate trajectory tracking, which is different from the baseline dual PID controller.According to the above analysis, we can know that the proposed longitudinal control method reaches a satisfactory control effect of longitudinal tracking.


## 5.1. Analysis of Vehicle Longitudinal Tracking Error

In this subsection, the longitudinal tracking problem mainly considers the longitudinal speed error and position error.According to Figure3 and equations (9) and (10), the following equation is obtained:(24)es=τrdeT,vτp=x˙coseφ−y˙sineφ1−kr∗ed,ev=vr−vτp,where vτp is the projection of point A’s speed in the tangent direction of point C and ev is the longitudinal speed error.


## 5.2. Throttle/Brake Calibration

The throttle-brake calibration is the foundation to ensure precise longitudinal tracking control of the vehicle. When the longitudinal tracking control system receives acceleration and deceleration signals at a specific speed, the control module solves the accurate throttle opening Thr and brake pressure P by looking up the throttle/brake calibration tableZ=vlookupa,v.The throttle-brake calibration table is created through the cosimulation based on Carsim and Matlab.(1)
When input throttle opening Thr increasing from 0 to 100% to the vehicle model in Carsim, changing speedv and acceleration a are recorded in the process of accelerating from 0 km/h to the 144 km/h speed corresponding to each throttle opening Thr into the Matlab workspace.(2)
When input brake pressure P increasing from 0 to 7 MPa to the vehicle model in Carsim, changing speedv and acceleration a are recorded in the process of decelerating from 144 km/h to 0 km/h corresponding to each brake pressure P into the Matlab workspace.By integrating data obtained previously, the throttle/brake calibration table can be obtained as shown in Figure12.Figure 12
Throttle/brake calibration table.The longitudinal tracking controller based on the inverse dynamics method receives reference acceleration signals and solves the accurate throttle opening Thr and brake pressureP referring to Figure 12. According to motor speed, the Thr/P values and simplified motor speed-torque curve are shown in Figure 13, and the motor torque demand can be obtained to achieve real-time longitudinal tracking control.Figure 13
Simplified motor speed-torque curve.


## 5.3. Longitudinal Tracking Controller Design

Since the tracking control system itself and multiple table lookups may lead to tracking errors, only controlling the real-time acceleration may lead to the longitudinal speed and position to deviate from the reference trajectory. We adopt the fuzzy PID algorithm in the controller to reduce the tracking errors. The longitudinal controller adjusts the required acceleration and brake pressure to ensure the accuracy of the longitudinal tracking control.The parametersKp,Ki and Kd completely determine the characteristics of a conventional PID controller, and this may lead to the unsatisfactory dynamic performance of nonlinear control systems. However, the fuzzy PID controller tunes the relative parameters adaptively referring to the rules. The common structure of a fuzzy PID controller can be depicted as shown in Figure 14.Figure 14
Fuzzy PID control flowchart.From Figure14, we can see that the fuzzy controller adopts the value of Kp,Ki, and Kd adaptively according to e and ec. The center of gravity method is applied as the defuzzification method and considers the errors of longitudinal speed, longitudinal position, and their rate of change.Fuzzy rules are set and adjusted referring to the following principles [27] and the actual e and ec in the simulation process:(1)
Whene and ec are large, a large kp should be taken to speed up the system response; a small ki should be taken to avoid large system overshoot; a medium kd should be taken to avoid derivative saturation.(2)
Whene and ec are medium, a small kp should be taken to avoid system overshoot; medium ki and kd should be taken to guarantee system responsiveness.(3)
Whene is small, kp and ki should be increased to improve system stability. To avoid the oscillation phenomenon and improve the anti-interference performance of the system, relatively large kd should be taken when ec is small and relatively small kd should be taken when ec is large.According to the theories, simulation analysis, and expert experience [28∼31], relative membership functions are created as shown in Figures 15 and 16, and the fuzzy rules are created as shown in Tables 5–10.Figure 15
Membership functions corresponding to vehicle speed: (a) longitudinal speed error’s membership function; (b) change rate of longitudinal speed error’s membership function; (c)Kp’s membership function; (d)Ki’s membership function; (e)Kd’s membership function.
(a)(b)(c)(d)(e)Figure 16
Membership functions corresponding to vehicle longitudinal position: (a) longitudinal position error’s membership function; (b) change rate of longitudinal position error’s membership function; (c)Kp’s membership function; (d)Ki’s membership function; (e)Kd’s membership function.
(a)(b)(c)(d)(e)Table 5
Fuzzy rules forKp.
eceNBNSZOPSPBNBPBZONBMBPBZOPBPBNBNBPBPBPBPBNBPBPBTable 6
Fuzzy rules forKi.
eceNBNSZOPSPBNBNBNBNBNBNBZOZONBNBNBNBPBNBNBZOZOPBTable 7
Fuzzy rules forKd.
eceNBNSZOPSPBNBNBNBNBNBNBZONBNBNBNBZOPBZONBZONBPBTable 8
Fuzzy rules ofKp.
eceNBNSZOPSPBNBPBZONBMBPBZOPBPBNBNBPBPBPBPBNBPBPBTable 9
Fuzzy rules ofKi.
eceNBNSZOPSPBNBNBNBNBNBNBZOZONBNBNBNBPBNBNBZOZOPBTable 10
Fuzzy rules ofKd.
eceNBNSZOPSPBNBPBZONBMBPBZOPBPBNBNBPBPBPBPBNBPBPB


## 5.4. Simulation Results

The effect of the longitudinal controller is evaluated through the following simulation.A part of the CLTC work condition is used to test the performance of the baseline dual PID controller and the proposed controller. The longitudinal tracking control process and relative results are shown in Figure17 and Table 11.Figure 17
Longitudinal tracking process: (a) speed tracking process; (b) longitudinal position tracking process; (c) longitudinal speed error; (d) longitudinal position error.
(a)(b)(c)(d)Table 11
Results of the longitudinal control tests.
ParametersDual PIDFuzzy PIDesavg (cm)6.230.53esmax (cm)36.084.57evavg (m/s)0.03550.0076evmax (m/s)0.67920.2011According to Figure17 and Table 11, it can be seen that the reference speed and longitudinal position are well tracked by using the proposed controller, the longitudinal position error remains within a reasonable range of 5 cm, and the average speed error is less than 0.008 m/s.However, the simulation test using the dual PID controller reaches a longitudinal position error of 36.08 cm and a longitudinal speed error of 0.6792 m/s, which are unacceptable.Figures17(c) and 17(d) show that the longitudinal errors can be corrected automatically by the longitudinal controller based on fuzzy PID while the position error and speed error cannot be corrected in time by using the dual PID strategy.The proposed controller can tune the parameters adaptively referring to the fuzzy rules to achieve more accurate trajectory tracking, which is different from the baseline dual PID controller.According to the above analysis, we can know that the proposed longitudinal control method reaches a satisfactory control effect of longitudinal tracking.


## 6. Combined Longitudinal and Lateral Controller

We design the lateral controller and longitudinal controller in the previous sections. To verify the reference trajectory tracking performance of proposed controllers, this section shows the details of the structure of the combined lateral and longitudinal trajectory tracking controller. The tracking results of the simulation tests are analyzed to verify the performance of the combined controller in this section.


### 6.1. Global Tracking Control Architecture

The goal of the combined lateral and longitudinal controller is to realize tracking control accurately. Based on the previous controller designs, the lateral controller and the longitudinal controller are combined in the architecture as shown in Figure18.Figure 18
Block diagram of the combined lateral and longitudinal control algorithm.In the global architecture,(1)
The quintic polynomial curve illustrated in Section3 is selected as the fitting curve of reference trajectory input to the combined controller(2)
Based on the reference trajectory, the steering angle, the motor torque, and the brake pressure are calculated to ensure the efficient vehicle tracking control


### 6.2. Trajectory Tracking Controller Performance Analysis

To verify the performance of the proposed combined controller proposed, the simulation tests using the MPC-fuzzy PID controller and LQR-dual PID controller are carried out under the common scenarios of quintic polynomial trajectory.The representative parking and lane-changing driving scenarios of the quintic polynomial curve planning trajectory are selected for simulation tests.The lateral and longitudinal position error, velocity error, and heading angle error are regarded as evaluation criteria to evaluate the effect of the trajectory tracking.


#### 6.2.1. Simulation Analysis of Parking Scenario

Settings of the parking scenario: Based on equation (9), the initial vehicle state is set to Xstart=0,0,0,Ystart=0,0,0, the end state of the vehicle is set to Xend=150,0,0,Yend=12,0,0, and the simulation duration is set to 30 s. The quintic polynomial trajectory corresponds to the side parking scenario where the vehicle first accelerates and then decelerates to stop. The simulation process and relative results are shown in Figures 19 and 20 and Table 12.Figure 19
Simulation process of trajectory tracking under parking scenario. (a) position tracking process; (b) speed tracking process; (c) speed error; (d) lateral position error; (e) longitudinal position error; (f) heading angle error; (g) steering angle.
(a)(b)(c)(d)(e)(f)(g)Figure 20
Trajectory tracking errors under parking scenario: (a) lateral tracking errors of trajectory tracking tests; (b) longitudinal tracking errors of trajectory tracking tests.
(a)(b)Table 12
Simulation results of the trajectory tracking under parking scenario.
ParametersLQR-dual PIDMPC-fuzzy PIDedmax (cm)2.960.41edavg (cm)1.420.18eθmax (rad)0.00810.0081eθavg (rad)0.00420.0041evmax (m/s)0.07310.0349evavg (m/s)0.01110.0026esmax (cm)11.231.39esavg (cm)2.60.35As can be seen in Figures19 and 20 and Table 12, in the simulation using MPC-fuzzy PID controller, the lateral position error does not exceed 0.5 cm, and the heading error is less than 0.009 rad under parking scenario. The maximum and average lateral position errors are reduced by 2.55 cm and 1.24 cm, respectively, compared with the simulation using the LQR-dual PID controller. The data shown previously demonstrate that the lateral controller based on MPC has satisfactory performance for lateral tracking accuracy. The maximum longitudinal position error is 1.39 cm, the average longitudinal position error is 0.35 cm, the maximum longitudinal speed error is 0.0349 m/s, and the average longitudinal speed error is 0.0026 m/s. The maximum and average longitudinal position error is reduced by 9.84 cm and 2.25 cm, respectively, compared with the simulation using the LQR-dual PID controller. The data shown previously demonstrate that the longitudinal controller based on fuzzy PID has satisfactory performance for longitudinal tracking accuracy.Moreover, the evaluation criteria shown previously in the simulation test using MPC-fuzzy PID controller change stably remain within a reasonable range. It can be inferred that the trajectory tracking controller proposed in this paper has more satisfactory performance than the LQR-dual PID controller for trajectory tracking.


#### 6.2.2. Simulation Analysis of Lane-Changing Scenario

Settings of the lane-changing scenario: According to equation (9), the initial state of the vehicle is set to Xstart=0,0,0,Ystart=0,0,0, the end state of the vehicle is set to Xend=400,25,0,Xend=30,0,0, and the simulation duration is set to 30 s. The quintic polynomial trajectory corresponds to the lane-changing scenario where the vehicle accelerates first and then changes into a far-side high-speed lane. The simulation process is shown in Figures 21 and 22. The simulation results are shown in Table 13.Figure 21
Simulation process of trajectory tracking under the lane-changing scenario: (a) position tracking process; (b) speed tracking process; (c) speed error; (d) lateral position error; (e) longitudinal position error; (f) heading angle error; (g) steering angle.
(a)(b)(c)(d)(e)(f)(g)Figure 22
Trajectory tracking errors under the lane-changing scenario: (a) lateral tracking errors; (b) longitudinal tracking errors.
(a)(b)Table 13
Simulation results of the trajectory tracking under the lane-changing scenario.
ParametersLQR-dual PIDMPC-fuzzy PIDedmax (cm)2.790.16edavg (cm)1.030.0645eθmax (rad)0.00210.0023eθavg (rad)0.000720.00079evmax (m/s)0.08720.0352evavg (m/s)0.01390.0022esmax (cm)7.741.9esavg (cm)3.890.62As can be seen in Figures21 and 22 and Table 13, in the simulation using the MPC-fuzzy PID controller, the lateral position error does not exceed 0.2 cm, and the heading angle error remains within an acceptable range of 0.0023 rad under the lane-changing scenario. The lateral position error’s maximum and average are, respectively, reduced by 2.63 cm and 0.9655 cm compared with the control group. The data shown previously demonstrate that the lateral controller based on MPC has satisfactory performance for lateral tracking accuracy. The longitudinal position error does not exceed 1.9 cm, and the longitudinal speed error is less than 0.04 m/s. The longitudinal position error’s maximum and average are reduced by 5.84 cm and 3.27 cm, respectively, compared with the control group. The data shown previously demonstrate that the longitudinal controller based on fuzzy PID has satisfactory performance for longitudinal tracking accuracy.Moreover, the evaluation criteria shown previously in the simulation test using MPC-fuzzy PID controller change stably remain within a reasonable range. It can be inferred that the proposed controller has more effective performance than LQR-dual PID controller for trajectory tracking.According to the above analysis, it can be inferred that the proposed MPC-fuzzy PID controller achieves a more satisfactory trajectory tracking effect with the global architecture as proposed in Section6.


## 6.1. Global Tracking Control Architecture

The goal of the combined lateral and longitudinal controller is to realize tracking control accurately. Based on the previous controller designs, the lateral controller and the longitudinal controller are combined in the architecture as shown in Figure18.Figure 18
Block diagram of the combined lateral and longitudinal control algorithm.In the global architecture,(1)
The quintic polynomial curve illustrated in Section3 is selected as the fitting curve of reference trajectory input to the combined controller(2)
Based on the reference trajectory, the steering angle, the motor torque, and the brake pressure are calculated to ensure the efficient vehicle tracking control


## 6.2. Trajectory Tracking Controller Performance Analysis

To verify the performance of the proposed combined controller proposed, the simulation tests using the MPC-fuzzy PID controller and LQR-dual PID controller are carried out under the common scenarios of quintic polynomial trajectory.The representative parking and lane-changing driving scenarios of the quintic polynomial curve planning trajectory are selected for simulation tests.The lateral and longitudinal position error, velocity error, and heading angle error are regarded as evaluation criteria to evaluate the effect of the trajectory tracking.


### 6.2.1. Simulation Analysis of Parking Scenario

Settings of the parking scenario: Based on equation (9), the initial vehicle state is set to Xstart=0,0,0,Ystart=0,0,0, the end state of the vehicle is set to Xend=150,0,0,Yend=12,0,0, and the simulation duration is set to 30 s. The quintic polynomial trajectory corresponds to the side parking scenario where the vehicle first accelerates and then decelerates to stop. The simulation process and relative results are shown in Figures 19 and 20 and Table 12.Figure 19
Simulation process of trajectory tracking under parking scenario. (a) position tracking process; (b) speed tracking process; (c) speed error; (d) lateral position error; (e) longitudinal position error; (f) heading angle error; (g) steering angle.
(a)(b)(c)(d)(e)(f)(g)Figure 20
Trajectory tracking errors under parking scenario: (a) lateral tracking errors of trajectory tracking tests; (b) longitudinal tracking errors of trajectory tracking tests.
(a)(b)Table 12
Simulation results of the trajectory tracking under parking scenario.
ParametersLQR-dual PIDMPC-fuzzy PIDedmax (cm)2.960.41edavg (cm)1.420.18eθmax (rad)0.00810.0081eθavg (rad)0.00420.0041evmax (m/s)0.07310.0349evavg (m/s)0.01110.0026esmax (cm)11.231.39esavg (cm)2.60.35As can be seen in Figures19 and 20 and Table 12, in the simulation using MPC-fuzzy PID controller, the lateral position error does not exceed 0.5 cm, and the heading error is less than 0.009 rad under parking scenario. The maximum and average lateral position errors are reduced by 2.55 cm and 1.24 cm, respectively, compared with the simulation using the LQR-dual PID controller. The data shown previously demonstrate that the lateral controller based on MPC has satisfactory performance for lateral tracking accuracy. The maximum longitudinal position error is 1.39 cm, the average longitudinal position error is 0.35 cm, the maximum longitudinal speed error is 0.0349 m/s, and the average longitudinal speed error is 0.0026 m/s. The maximum and average longitudinal position error is reduced by 9.84 cm and 2.25 cm, respectively, compared with the simulation using the LQR-dual PID controller. The data shown previously demonstrate that the longitudinal controller based on fuzzy PID has satisfactory performance for longitudinal tracking accuracy.Moreover, the evaluation criteria shown previously in the simulation test using MPC-fuzzy PID controller change stably remain within a reasonable range. It can be inferred that the trajectory tracking controller proposed in this paper has more satisfactory performance than the LQR-dual PID controller for trajectory tracking.


### 6.2.2. Simulation Analysis of Lane-Changing Scenario

Settings of the lane-changing scenario: According to equation (9), the initial state of the vehicle is set to Xstart=0,0,0,Ystart=0,0,0, the end state of the vehicle is set to Xend=400,25,0,Xend=30,0,0, and the simulation duration is set to 30 s. The quintic polynomial trajectory corresponds to the lane-changing scenario where the vehicle accelerates first and then changes into a far-side high-speed lane. The simulation process is shown in Figures 21 and 22. The simulation results are shown in Table 13.Figure 21
Simulation process of trajectory tracking under the lane-changing scenario: (a) position tracking process; (b) speed tracking process; (c) speed error; (d) lateral position error; (e) longitudinal position error; (f) heading angle error; (g) steering angle.
(a)(b)(c)(d)(e)(f)(g)Figure 22
Trajectory tracking errors under the lane-changing scenario: (a) lateral tracking errors; (b) longitudinal tracking errors.
(a)(b)Table 13
Simulation results of the trajectory tracking under the lane-changing scenario.
ParametersLQR-dual PIDMPC-fuzzy PIDedmax (cm)2.790.16edavg (cm)1.030.0645eθmax (rad)0.00210.0023eθavg (rad)0.000720.00079evmax (m/s)0.08720.0352evavg (m/s)0.01390.0022esmax (cm)7.741.9esavg (cm)3.890.62As can be seen in Figures21 and 22 and Table 13, in the simulation using the MPC-fuzzy PID controller, the lateral position error does not exceed 0.2 cm, and the heading angle error remains within an acceptable range of 0.0023 rad under the lane-changing scenario. The lateral position error’s maximum and average are, respectively, reduced by 2.63 cm and 0.9655 cm compared with the control group. The data shown previously demonstrate that the lateral controller based on MPC has satisfactory performance for lateral tracking accuracy. The longitudinal position error does not exceed 1.9 cm, and the longitudinal speed error is less than 0.04 m/s. The longitudinal position error’s maximum and average are reduced by 5.84 cm and 3.27 cm, respectively, compared with the control group. The data shown previously demonstrate that the longitudinal controller based on fuzzy PID has satisfactory performance for longitudinal tracking accuracy.Moreover, the evaluation criteria shown previously in the simulation test using MPC-fuzzy PID controller change stably remain within a reasonable range. It can be inferred that the proposed controller has more effective performance than LQR-dual PID controller for trajectory tracking.According to the above analysis, it can be inferred that the proposed MPC-fuzzy PID controller achieves a more satisfactory trajectory tracking effect with the global architecture as proposed in Section6.


## 6.2.1. Simulation Analysis of Parking Scenario

Settings of the parking scenario: Based on equation (9), the initial vehicle state is set to Xstart=0,0,0,Ystart=0,0,0, the end state of the vehicle is set to Xend=150,0,0,Yend=12,0,0, and the simulation duration is set to 30 s. The quintic polynomial trajectory corresponds to the side parking scenario where the vehicle first accelerates and then decelerates to stop. The simulation process and relative results are shown in Figures 19 and 20 and Table 12.Figure 19
Simulation process of trajectory tracking under parking scenario. (a) position tracking process; (b) speed tracking process; (c) speed error; (d) lateral position error; (e) longitudinal position error; (f) heading angle error; (g) steering angle.
(a)(b)(c)(d)(e)(f)(g)Figure 20
Trajectory tracking errors under parking scenario: (a) lateral tracking errors of trajectory tracking tests; (b) longitudinal tracking errors of trajectory tracking tests.
(a)(b)Table 12
Simulation results of the trajectory tracking under parking scenario.
ParametersLQR-dual PIDMPC-fuzzy PIDedmax (cm)2.960.41edavg (cm)1.420.18eθmax (rad)0.00810.0081eθavg (rad)0.00420.0041evmax (m/s)0.07310.0349evavg (m/s)0.01110.0026esmax (cm)11.231.39esavg (cm)2.60.35As can be seen in Figures19 and 20 and Table 12, in the simulation using MPC-fuzzy PID controller, the lateral position error does not exceed 0.5 cm, and the heading error is less than 0.009 rad under parking scenario. The maximum and average lateral position errors are reduced by 2.55 cm and 1.24 cm, respectively, compared with the simulation using the LQR-dual PID controller. The data shown previously demonstrate that the lateral controller based on MPC has satisfactory performance for lateral tracking accuracy. The maximum longitudinal position error is 1.39 cm, the average longitudinal position error is 0.35 cm, the maximum longitudinal speed error is 0.0349 m/s, and the average longitudinal speed error is 0.0026 m/s. The maximum and average longitudinal position error is reduced by 9.84 cm and 2.25 cm, respectively, compared with the simulation using the LQR-dual PID controller. The data shown previously demonstrate that the longitudinal controller based on fuzzy PID has satisfactory performance for longitudinal tracking accuracy.Moreover, the evaluation criteria shown previously in the simulation test using MPC-fuzzy PID controller change stably remain within a reasonable range. It can be inferred that the trajectory tracking controller proposed in this paper has more satisfactory performance than the LQR-dual PID controller for trajectory tracking.


## 6.2.2. Simulation Analysis of Lane-Changing Scenario

Settings of the lane-changing scenario: According to equation (9), the initial state of the vehicle is set to Xstart=0,0,0,Ystart=0,0,0, the end state of the vehicle is set to Xend=400,25,0,Xend=30,0,0, and the simulation duration is set to 30 s. The quintic polynomial trajectory corresponds to the lane-changing scenario where the vehicle accelerates first and then changes into a far-side high-speed lane. The simulation process is shown in Figures 21 and 22. The simulation results are shown in Table 13.Figure 21
Simulation process of trajectory tracking under the lane-changing scenario: (a) position tracking process; (b) speed tracking process; (c) speed error; (d) lateral position error; (e) longitudinal position error; (f) heading angle error; (g) steering angle.
(a)(b)(c)(d)(e)(f)(g)Figure 22
Trajectory tracking errors under the lane-changing scenario: (a) lateral tracking errors; (b) longitudinal tracking errors.
(a)(b)Table 13
Simulation results of the trajectory tracking under the lane-changing scenario.
ParametersLQR-dual PIDMPC-fuzzy PIDedmax (cm)2.790.16edavg (cm)1.030.0645eθmax (rad)0.00210.0023eθavg (rad)0.000720.00079evmax (m/s)0.08720.0352evavg (m/s)0.01390.0022esmax (cm)7.741.9esavg (cm)3.890.62As can be seen in Figures21 and 22 and Table 13, in the simulation using the MPC-fuzzy PID controller, the lateral position error does not exceed 0.2 cm, and the heading angle error remains within an acceptable range of 0.0023 rad under the lane-changing scenario. The lateral position error’s maximum and average are, respectively, reduced by 2.63 cm and 0.9655 cm compared with the control group. The data shown previously demonstrate that the lateral controller based on MPC has satisfactory performance for lateral tracking accuracy. The longitudinal position error does not exceed 1.9 cm, and the longitudinal speed error is less than 0.04 m/s. The longitudinal position error’s maximum and average are reduced by 5.84 cm and 3.27 cm, respectively, compared with the control group. The data shown previously demonstrate that the longitudinal controller based on fuzzy PID has satisfactory performance for longitudinal tracking accuracy.Moreover, the evaluation criteria shown previously in the simulation test using MPC-fuzzy PID controller change stably remain within a reasonable range. It can be inferred that the proposed controller has more effective performance than LQR-dual PID controller for trajectory tracking.According to the above analysis, it can be inferred that the proposed MPC-fuzzy PID controller achieves a more satisfactory trajectory tracking effect with the global architecture as proposed in Section6.


## 7. Conclusions

The proposed combined lateral and longitudinal trajectory tracking controller based on MPC and fuzzy PID algorithms proposed realizes the following advantages:(1)
The trajectory tracking controller combines the advantages of MPC and fuzzy PID algorithm and can realize accurate tracking control under general scenarios with different speeds of quintic polynomial trajectory.(2)
The rule-based horizon parameters selection and the throttle/brake calibration table are proposed to ensure more precise trajectory tracking control.(3)
The proposed strategy decouples the lateral and longitudinal tracking problems and makes it possible to design each controller separately. By combining the lateral controller with the longitudinal controller, the controllers work in coordination and promote each other to achieve a satisfactory control effect.According to the results of the simulation tests, we can know that the proposed controller reaches a satisfactory control effect and ensures the accuracy and stability of the trajectory tracking process. The results shown in the article are promising, and the control strategy is waiting to be applied on a testbench. Due to complex vehicle work conditions, the proposed controller can be further developed by taking into account the useability of different work conditions such as path curvatures.



---

*Source: 2464254-2023-02-03.xml*