Research on Automatic Driving Trajectory Planning and Tracking Control Based on Improvement of the Artificial Potential Field Method

Abstract

With the continuous increase in motor vehicle ownership in recent times, traditional transportation has been unable to meet people’s travel needs. Research on autonomous driving technology will help solve a series of problems associated with driving, such as traffic accidents, traffic congestion, energy consumption, and environmental pollution. In this paper, an improved artificial potential field method is proposed to complete the planning of automatic driving trajectories by adding the distance adjustment factor, dynamic road repulsive field, velocity repulsive field, and acceleration repulsive field. The invasive weed algorithm is introduced to solve the defects associated with the traditional artificial potential field method. The prediction model—for which corresponding constraint variables were set and an optimal objective function was established to build up the MPC model controller to achieve the goal of trajectory tracking—was linearized and discretized from a vehicle dynamics model. Finally, co-simulation based on MATLAB and CarSim was used to verify the practicability of the model.

1. Introduction

At present, with the rapid development of computer technology and communication technology, it is no longer merely a dream that automatic driving technology will replace heavy driving tasks, which is gradually becoming a reality. The development of autonomous driving involves the gradual withdrawal of driver operations [1]. The Internet of Things and intelligent technology have been gradually applied in vehicles, enabling the creation of cars that have their own systems for thinking, and the “perception–decision–execution” actions originally completed by drivers are being gradually replaced by vehicles [2]. Autonomous driving is a highly complex product integrating many advanced technologies, such as environmental perception, decision planning, and following control [3]. Among these, trajectory planning is the basis of automatic driving, and trajectory tracking is the key technology of automatic driving. Reasonable trajectory planning and accurate trajectory tracking can ensure road safety and help to avoid traffic accidents, maintain passenger comfort and reduce traffic congestion. At present, Tesla’s autonomous driving cars utilize ADAS (Advanced Driver Assistance System), which uses a number of technologies in the field of computer vision and machine learning, such as simultaneous localization and mapping (SLAM) and recurrent neural network (RNN) [4].

In the research field of autonomous vehicle trajectory planning, the existing research results can be roughly divided into artificial potential field (APF) methods, graph search-based trajectory planning algorithms, sampling-based trajectory planning algorithms, and bionic intelligent algorithms [5]. In the traditional artificial potential field method, all information about the environment is mainly expressed as geometric dimensions, with the corresponding gravitational and repulsive potential field values formed around obstacles in building a topological structure of the actual environment of vehicle driving. Tang Zhirong et al. [6] elliptically processed the distance in the traditional repulsive potential field to improve the obstacle avoidance function and allow the planned trajectory to meet the multi-constraint conditions of vehicle driving. An Linfang et al. [7] proposed a new obstacle point construction method to solve the local minimum problem. Xiu Caijing et al. [8] improved the smoothness of planning trajectory by integrating vehicle constraints with objective function. However, this method could only solve the problem of vehicle trajectory planning in a static environment and did not consider trajectory planning in dynamic environments, so it was difficult to apply it to automatic driving. The algorithm has a simple principle and positive real-time performance, but it is easy to fall into the problem of local optimal solution and unreachable targets in application, and constraints such as vehicle dynamics are lacking. Parameter setting is shown to affect the robustness of the algorithm [9]. Taheri E et al. [10] improved the rapidly random tree (RRT) algorithm and proposed a fuzzy greedy search mechanism to improve the calculation efficiency of the algorithm by deleting redundant nodes. Yusheng Ci et al. [11] proposed a new approach to solve coordinated control and demonstrated the selectivity of coordinated control. Boroujeni Z et al. [12] improved the A* algorithm to the Fu-A* algorithm, which adapted the grid size to the speed, and the grid size changes with the speed value to improve the smoothness of the planned trajectory. Yang D C et al. [13] divided trajectory planning into multi-segment planning, sorted the multi-segment planning using an ant colony algorithm, and then carried out the trajectory planning with the improved probability map method. Lei W et al. [14] combined the principle of artificial potential field method with a genetic algorithm, used the artificial potential field method to initialize and solve the population, and added adaptive factors into the objective function to improve the iteration efficiency of the genetic algorithm. Song X et al. [15] improved the intelligent water drop algorithm via the selection strategy, which improved the heuristics of searching and enhanced the algorithm applicability. Xiaoning Wang et al. [16] proposed a method to provide correction speed guidance to reduce the negative effects of bounded rationality.Jianhua L et al. [17] integrated an ant colony algorithm and artificial potential field method, which were used for global and local planning, respectively, and the resulting improved algorithm had a higher convergence efficiency. Liu Y et al. [18] combined the artificial potential field method and moved the collusion-prone sampling points to the collusion-free region through the compound force field, thus improving the feasibility of the algorithm.

Trajectory tracking control is a key technology of autonomous vehicles that is mainly achieved through active wheel steering or differential braking [19]. Relevant scholars have studied the path tracking problem of autonomous vehicles through proportion integration differentiation (PID) control [20], linear active disturbance rejection control [21], pure track model, fuzzy adaptive control [22], model predictive control [23,24], etc., but because differential braking affects the longitudinal motion of the vehicle and given the complexity and uncertainty of the subsequent high-speed vehicle path, achieving front wheel active steering with real-time path tracking currently faces technical difficulties related to the complex working conditions involved. The use of the model predictive control method can improve control performance by optimizing the objective function and taking the vehicle kinematics and dynamic constraints into account. Zhou L H et al. [25] added a linear quadratic regulator (LQR) method to model predictive control to ensure tracking yaw stability. Lina Wu et al. [26,27] proposed a new approach for on-ramp metering and speed guidance control on urban expressways based on MPC and connected vehicles (CV). Wang H et al. [28] proposed a path tracking method for model predictive control on account of a variable predictive view and realized the adaptive optimization of predictive view. To reduce the error in track tracking, Morales S et al. [29] established omnidirectional configuration to realize track tracking and integrated a multi-level control method into a linear quadratic regulator track tracking controller. Chen, J. et al. [30] proposed the linear time varying model predictive control (LTV-MPC) method in the study of trajectory tracking control methods and analyzed the influence of selected control parameters on the algorithm processing speed from multiple directions. Lina Wu et al. [31] proposed a judgment criterion and classification method of supersaturation state relied on intersection spacing, traffic demand and intersection capacity. Pan, L.B. et al. [32] divided the problem of path tracking into two parts: vehicle steering following and speed following control. Zhang, H. et al. [33] improved the regulator by iterating the running process of the iterative linear quadratic regulator (ILQR) several times and applied the method to trajectory tracking control in the field of robots. Therefore, it can be inferred that the fundamental purpose of driverless vehicle path tracking is to eliminate the path deviation between the actual route and the expected route during the process of autonomous driving [34]. The current mainstream trajectory tracking control methods include pure tracking control [35], linear quadratic regulator controller [36], and model predictive control.

In view of this, the traditional artificial potential field method is improved in this paper by adding the distance adjustment factor, which can solve the problem of unreachable targets. The invasive weed method was introduced to improve solving of the local minimum problem. Aiming at the relative velocity and acceleration of obstacles in a dynamic environment, the dynamic road repulsive potential field based on velocity variation was established, and environment modeling was carried out to ensure the practicability and accuracy of trajectory planning in dynamic environments. This was then modeled by using the improved artificial potential field method to draw the right track, a simplified vehicle dynamic model and the tire model derived from the “magic formula” through the linearization and discretization of the model, with the linear and discrete dynamic model as the prediction model, and the corresponding constraint variables were set by constructing the optimal objective function for the model predictive control of path tracking. At last, co-simulation was carried out using MATLAB (2016b, MathWorks, Torrance, CA, USA) and CarSim (2016, MSC, Ann Arbor, MI, USA). The experimental results show the high tracking accuracy of the controller under different road adhesion coefficients and vehicle speeds, and all parameters change within a reasonable range, reflecting perfect driving stability. As a result of this research, we present an improved scientific planning algorithm for use in unmanned vehicle trajectory planning and trajectory tracking.

2. Trajectory Planning Algorithm Based on Improvement of the Artificial Potential Field Method

2.1. Traditional Artificial Potential Field Method

2.1.1. Gravitational Field

The gravitational field function of the traditional artificial potential field method is:

$$U_{att}=\frac{1}{2}{k}_{att}{(X-X_g)}^2$$

(1)

where:

• $U_{att}$ is the gravitational potential field at the target point;
• $k_{att}$ is the gain coefficient of the gravitational potential field, which is positive real number;
• $X$ is the position vector of autonomous vehicle;
• $X_g$ is the target position vector of the autonomous vehicle.

The negative gradient of the gravitational potential field is obtained as follows:

$$F_{att}=-grad(U_{att})=k(X_g-X)$$

(2)

2.1.2. Repulsive Force Field

The specific potential field function expression of the repulsive potential field is:

$$U_{rep}=\{\begin{array}{ll}\frac{1}{2}{k}_{rep}{(\frac{1}{p}-\frac{1}{p_0})}^2& ,\hspace{1em}\hspace{1em}\hspace{1em}p\le p_0\\ 0& ,\hspace{1em}\hspace{1em}\hspace{1em}p>p_0\end{array}$$

(3)

where:

• $U_{rep}$ is the repulsive force field of the obstacle vehicle;
• $k_{rep}$ is the gain coefficient of the repulsive potential field, and is a positive real number;
• $p$ is the shortest distance in space between the car and the obstacle vehicle;
• $p_0$ is the maximum range of impact that an obstacle vehicle can have.

When the vehicle is within the maximum influence range, Formula (3) will be used for calculation; if it is outside the maximum influence range, the repulsive force field will be zero, and the negative gradient of the repulsive force potential field will be obtained. The potential field diagram is illustrated in Figure 1.

$$F_{rep}=\{\begin{array}{ll}{k}_{rep}(\frac{1}{p}-\frac{1}{p_0})\frac{1}{p^2}\frac{\partial p}{\partial X},& p\le p_0\\ 0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} ,& p>p_0\end{array}$$

(4)

2.2. Improved Artificial Potential Field Method

2.2.1. Improved Repulsive Potential Field

The main function of road repulsion potential field is to ensure that autonomous vehicles can drive in the middle of the road, so the road repulsion field can be expressed by some mathematical formulas. Regarding the characteristics of a two-lane highway, the shape is very similar to a traditional abacus, while the car is similar to the beads on the abacus and can only move in the area specified by the abacus. Therefore, a rectangular coordinate system can be established in the lane to divide the road into n equal parts. When the car is driving, it is regarded as a particle. The distance between the vehicle and the road boundary on both sides and the coordinates of the vehicle itself can be obtained through calculation. The schematic diagram of road boundary repulsion is shown in Figure 2.

In order to solve the unreachable target problem of the artificial potential field method, this paper proposes an improved repulsive potential field function by adding a distance adjustment factor on the basis of the original repulsive potential field function, where the repulsive force on the vehicle at the target point can be minimized. The improved repulsive potential field function is:

$$U_{rep}=\{\begin{array}{ll}\frac{1}{2}{k}_{rep}{(\frac{1}{p}-\frac{1}{p_0})}^2[1-e^{-\frac{{(x-x_g)}^2+{(y-y_g)}^2}{R^2}}]& ,\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}p\le p_0\\ 0& ,\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}p>p_0\end{array}$$

(5)

where:

• $[x,y]$ is the real-time coordinate point of autonomous driving vehicle;
• $[x_g,y_g]$ is the coordinates of the target point;
• $R$ is the radius of the autonomous driving vehicle.

The repulsive force of the potential field can be obtained:

$$-U_{rep}^{\prime }(x)=-k_{rep}(x-x_g){(\frac{1}{p}-\frac{1}{p_0})}^2{e}^{-\frac{{(x-x_g)}^2+{(y-y_g)}^2}{R^2}}+k_{rep}(\frac{1}{p}-\frac{1}{p_0})[1-e^{-\frac{{(x-x_g)}^2+{(y-y_g)}^2}{R^2}}]\frac{x-x_0}{p^3}$$

(6)

$$-U_{rep}^{\prime }(y)=-k_{rep}(y-y_g){(\frac{1}{p}-\frac{1}{p_0})}^2{e}^{-\frac{{(x-x_g)}^2+{(y-y_g)}^2}{R^2}}+k_{rep}(\frac{1}{p}-\frac{1}{p_0})[1-e^{-\frac{{(x-x_g)}^2+{(y-y_g)}^2}{R^2}}]\frac{y-y_0}{p^3}$$

(7)

$$\left|F_{rep}\right|=\sqrt{U_{rep}^{\prime }{(x)}^2+U_{rep}^{\prime }{(y)}^2}$$

(8)

According to Formulas (6) and (8), the repulsive potential field will change only when it is close to the target point. However, the search efficiency of the algorithm is reduced in the improved method. Therefore, this paper improves the repulsion, that is, adding the distance regulatory factor on the basis of the original repulsion. The repulsion force formula is as follows:

$$\left|F_{rep}\right|=\{\begin{array}{ll}\frac{k_{rep}}{p^2}(\frac{1}{p}-\frac{1}{p_0})[1-e^{-\frac{{(x-x_g)}^2+{(y-y_g)}^2}{R^2}}]& ,\hspace{1em}\hspace{1em}p\le p_0\\ 0& ,\hspace{1em}\hspace{1em}p>p_0\end{array}$$

(9)

The formula (9) not only improves the repulsive potential field to ensure that the repulsion force of the vehicle tends to zero at the target point but also considers the body radius of the autonomous vehicle, which makes the trajectory more secure and reliable.

The improved gravitational potential field and repulsive potential field are shown in Figure 3.

After the improvement of the potential field function, the value of the potential field is 0 only when the vehicle travels to the target point, which solves the problem that the target of the traditional potential field function is unreachable, and the extreme value of the improved repulsive field is removed to avoid the situation of excessive repulsive force.

2.2.2. Establish Velocity Repulsion Field and Acceleration Repulsion Field

This paper proposes to establish the velocity repulsion potential field and acceleration repulsion potential field relying on obstacles and construct the dynamic traffic environment information in line with the actual movement, so as to ensure that the vehicle can meet the trajectory planning in both static and dynamic scenes. The expressions of velocity repulsion field and acceleration repulsion field are as follows:

$$U_{rep}(v)=\{\begin{array}{l}\frac{1}{2}{k}_v{(v-v_0)}^2,\hspace{1em}\hspace{1em}p\le p_0\\ 0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} ,\hspace{1em}\hspace{1em}p>p_0\end{array}$$

(10)

$$F_{rep}(v)=\{\begin{array}{l}{k}_v(v-v_0),\hspace{1em}\hspace{1em}p\le p_0\\ 0\hspace{1em}\hspace{1em}\hspace{1em} ,\hspace{1em}\hspace{1em}p>p_0\end{array}$$

(11)

$$a=\frac{1}{m}(F_{attc}-F_{repc})$$

(12)

where:

• $k_v$ is the gain coefficient of velocity repulsion field;
• $v$ is the controlled vehicle speed;
• $v_0$ is the velocity of the obstacle;
• $F_{attc}$ is gravitational velocity component;
• $F_{repc}$ is the repulsive velocity component;
• $m$ is the quality of the car.

The stress diagram of the vehicle is shown in Figure 4.

It can be seen from Figure 4 that under the joint action of all resultant force potential fields, the force expression equation of the vehicle is:

$$U_{total}={\displaystyle \sum _{i=1}^n{U}_{rep}}+U_{att}+U_{road}+U_{rep}(v)$$

(13)

$$F_{total}={\displaystyle \sum _{i=1}^n{F}_{rep}}+F_{att}+F_{road}+F_{rep}(v)$$

(14)

2.3. Trajectory Planning Analysis of Invasive Weed Algorithm

In this paper, the invasive weed algorithm is proposed to solve the local optimal problem. The invasive weed algorithm has the advantages of strong guidance and fast convergence. The fusion of the improved artificial potential field algorithm with the invasive weed algorithm can not only effectively improve the search efficiency of the algorithm but also solve the problem of the autonomous vehicle falling into the local optimal solution.

Before the fusion of both algorithms, the following need to be clear:

• It is necessary to determine whether the car has reached the target point;
• It is necessary to determine whether the vehicle has fallen into the local optimal solution trap at the current moment;
• Repulsion force is redistributed by selecting the optimal subdestination;
• It is necessary to determine whether the local optimal solution trap has been avoided at the current moment.

In order to detect whether the autonomous driving vehicle has fallen into the local optimal solution, the following principle is set in this paper, that is, when the vehicle moves within ten consecutive steps in a two-dimensional plane, as well as the change values of abscissa and ordinate are less than three times the step length, the vehicle is judged to have fallen into the local optimal trap. According to the criterion that the farther away from the local minimum point and the closer to the destination within a unit step length, the optimal subdestination for escape is selected. Its principle can be expressed by Formula (15):

$$\mathsf{\Omega }=(X_1,X_2,X_3,X_4)$$

(15)

where:

• $X_1$ is the destination;
• $X_2$ is the optimal sub-destination;
• $X_3$ is the local optimal solution point;
• $X_4$ is the initial position after escaping the local optimal solution.

The covariance value of relative position coordinates is minimized as the iterative principle, and when the value of $\mathsf{\Omega }$ is the smallest, the corresponding $X_2$ is the solution. The trajectory planning steps after the integration of the improved of artificial potential field method and the invasive weed algorithm are as follows:

Step 1. Initialize the parameter values of the system;

Step 2. Vehicle trajectory planning is carried out according to the improved artificial potential field method;

Step 3. Determine whether the vehicle has reached the target point. If so, it ends. Go to step 8;

Step 4. Judge whether the vehicle falls into the local optimal solution. If so, proceed to the next step. If not, go to Step 2;

Step 5. The invasive weed method is called upon to generate the optimal subdestination;

Step 6. The repulsion field is redistributed for trajectory planning;

Step 7. Determine whether to escape the trap of local optimal solution. If so, go to Step 2; if not, go to Step 5;

Step 8. Trajectory planning is complete and the algorithm ends.

The algorithm flow chart is shown in Figure 5.

2.4. Simulation Experiment of Improved Artificial Potential Field Method

In this paper, the trajectory planning simulation of the autonomous vehicle is carried out before and after the improvement of the artificial potential field method. First, a two-lane road is chosen as the test road. The total length is set as 100 m, the width of each lane is 3.5 m, the initial position of the autonomous vehicle is built for (0, −1.75), the target position is set up as (100, 2), and the speed is set to 10 m/s. In addition, two static obstacle vehicles and three dynamic obstacle vehicles are set in the simulation test to verify the accuracy of the improved artificial potential field method in static and dynamic environments. The static obstacle vehicle initial position is (10, 1.75) and (30, 1.5), the initial position of the dynamic obstacle vehicle is (45, 1.75), (60, −1.75), and (70, 1.75) with corresponding speeds of 3, 6, and 2 m/s, respectively. The simulation results are as follows:

In Figure 6a, the autonomous vehicle starts from the initial position at a constant speed and smoothly surpasses static obstacle vehicle 1 under the action of the compound potential field, thus completing obstacle avoidance. Moreover, under the action of the static obstacle vehicle 2, the direction is gradually converted in preparation for the next lane change. In Figure 6b, the autonomous vehicle alters lanes in advance under the combined force and successfully avoids the obstacle vehicle 2 to overtake from the same lane. The simulation results adequately demonstrate the ability of the autonomous vehicle to avoid obstacles and change lanes to overtake vehicles in a static environment, which reflects the accuracy of the algorithm.

When the vehicle has traveled a certain distance, as seen in Figure 6c, the autonomous vehicle system detects the location information of dynamic obstacle vehicles in the road. Then, lane changing and overtaking behavior are again completed under the combined action of the velocity repulsive force field and road repulsive boundary field. Lane changing differs from avoiding a static obstacle in that the algorithm calculation takes the velocity repulsive and acceleration repulsive fields into account. When the dynamic obstacle vehicle completes lane changing to overtake the dynamic obstacle vehicle, the planned trajectory in improved artificial potential field method is relatively smoother and more adequately satisfies the realistic driving requirements than that of the traditional method. In Figure 6d, driving of the autonomous vehicle is maintained to complete overtaking of the dynamic obstacle vehicle 2 until it reaches the target point and smoothly completes trajectory planning.

As shown in Figure 7, since the trajectory planned by the artificial potential field method before improvement does not have the effect of dynamic road repulsion field, trajectory deviation will occur at the initial position, which does not satisfy the actual driving requirements. In addition, when approaching the static obstacle vehicle 2, trajectory planning will fail or trajectory “regress” will occur due to the local optimal solution, and trajectory oscillation and swing will occur when approaching the target point. However, the unsatisfactory phenomenon of unreachable targets does not appear when using the improved artificial potential field method. Moreover, when the planned trajectory above falls into a local optimal solution, it will be replanned by invoking the invasive weed algorithm. As a result, the trajectory in the driving process is smooth and meets the driving requirements.

In summary, the improved artificial potential field method can not only solve the problems of target unreachable and local optimal solution but also facilitate conformation of the planned trajectory to the actual situation, which has a smoother and shorter length trajectory than that of the method prior to improvement. Hence, the improved artificial potential field method is recommended in this paper to obtain excellent practicability and calculation accuracy.

3. Vehicle Dynamics Model

3.1. Vehicle 3-DOF Dynamics Model

The principle of the vehicle dynamics model is to analyze the relationship between mechanical properties and relative motion of each object in the environment, so as to ensure stable control of the vehicle during trajectory tracking. In order to reduce the complex solving process of the algorithm and improve the efficiency of trajectory tracking, the following assumptions are made in the actual modeling process:

• The driving condition of the environmental road surface is superior, the vehicle only undergoes planar two-dimensional movement parallel to the road surface;
• The vehicle is rigid, and there is no consideration of the impact of the vehicle suspension system;
• The vehicle turns with the front wheel, and the angle changes of the left and the right wheels remain the same;
• The transverse and longitudinal coupling relationship of automobile tires is not considered;
• The influence of aerodynamics is ignored;
• The situation of vehicle load transfer is not considered;
• Derived from the simplified model, the monorail model of the vehicle is established, and the stress analysis is shown in Figure 8.

where:

• $oxyz$ is the vehicle’s own coordinate system;
• $OXY$ is the ground coordinate system;
• $Z$-axial upward is the positive direction, and the judgment rule is the right-hand rule.

The equation can be obtained from the force analysis of the vehicle:

$$\begin{array}{l}m\stackrel{·}{x}=m\stackrel{·}{y}\stackrel{·}{\dot{\phi }}+2F_{xf}+2F_{xr}\\ m\stackrel{·}{y}=-m\stackrel{·}{x}\stackrel{·}{\phi }+2F_{yf}+2F_{yr}\\ I_z\stackrel{··}{\phi }=2a{F}_{yf}-2b{F}_{yr}\end{array}$$

(16)

where:

$$\begin{array}{l}{F}_{xf,r}=F_{lf,r}\cos \delta -F_{cf,r}\sin {\delta }_{f,r}\\ F_{yf,r}=F_{lf,r}\sin \delta +F_{cf,r}\cos {\delta }_{f,r}\end{array}$$

(17)

where:

• $m$ is the vehicle quality;
• $a$ is the distance from the vehicle center of mass to the front axle;
• $b$ is the distance from the vehicle center of mass to the rear axle;
• $I_z$ is the moment of inertia of the z-axis;
• $\stackrel{·}{\phi }$ is the yaw rate;
• $F_{xf,r}$ is the X-axis component force of front and rear tires;
• $F_{yf,r}$ is the y-axis component force of front and rear tires;
• $F_{lf,r}$ is the longitudinal force of front and rear tires;
• $F_{cf,r}$ is the lateral force of front and rear tires;
• ${\delta }_{f,r}$ is the front and rear wheel angle.

The longitudinal force and lateral force exerted by the tire of a controlled vehicle are expressed as a complex function composed of multiple parameters:

$$\begin{array}{l}{F}_l=f_l(\alpha ,s,\mu ,F_z)\\ F_c=f_c(\alpha ,s,\mu ,F_z)\end{array}$$

(18)

where:

• $F_l$ is the positive force on the tire;
• $F_c$ is the lateral force on the tire.

Due to the nonlinearity of the vehicle dynamics model based on three degrees of freedom, this model cannot be directly applied for trajectory tracking. Hence, we express the nonlinear system as a state space quantity:

$$\begin{array}{l}{\stackrel{·}{\xi }}_{dyn}=f_{dyn}({\xi }_{dyn},u_{dyn})\\ \begin{array}{c}\end{array}{\eta }_{dyn}=h_{dyn}({\xi }_{dyn})\\ {\xi }_{dyn}={[\stackrel{·}{y},\stackrel{·}{x},\phi ,\stackrel{·}{\phi },Y,X]}^T\\ \begin{array}{cc}& \end{array}{u}_{dyn}={\delta }_f\\ \begin{array}{c}\begin{array}{c}\begin{array}{c}\end{array}\end{array}\end{array}\begin{array}{c}\end{array}{\eta }_{dyn}={[\phi ,Y]}^T\end{array}$$

(19)

where:

• ${\xi }_{dyn}$ is the state quantity of nonlinear system;
• $u_{dyn}$ is the control quantity of nonlinear system;
• ${\eta }_{dyn}$ is the output of the nonlinear system.

3.2. Tire Model

Considering that the stability of vehicle driving is closely related to the longitudinal force and lateral force on the tire, this paper selects the “magic formula” tire model, which can accurately describe the mechanical characteristics of the tire, so it is widely used in the field of vehicle dynamics [37]. The principle is to use trigonometric function combination to fit the curve of the actual data of the tire, and the mechanical relationship of the tire can be completely expressed using fewer formulas.

The general expression of the model is:

$$Y(x)=D\sin \{C\arctan [Bx-E(Bx-\arctan (Bx))]\}$$

(20)

where:

• $Y$ is the output variable;
• $x$ is the input variable;
• $B$ is the stiffness factor;
• $C$ is curve shape factor;
• $D$ is the curve peak factor;
• $E$ is the curve curvature factor.

The longitudinal force of the tire is calculated as follows:

$$\begin{array}{l}{F}_l=D\sin \{C\arctan [Bx-E(Bx-\arctan (Bx))]\}+S_v\\ \begin{array}{cccc}\begin{array}{ccc}& & \end{array}& & & \end{array}\begin{array}{c}\end{array}x=s+S_h\\ \begin{array}{cccc}& & & \end{array}B=\frac{(B_3{F}_z^2+B_4{F}_z)e^{-B_5{F}_z}}{C\times D}\\ \begin{array}{cccc}\begin{array}{cccc}& & & \end{array}& & & \end{array}C=B_0\\ \begin{array}{cccc}\begin{array}{cc}\begin{array}{cc}& \end{array}& \end{array}& & & \end{array}D=B_1{F}_z^2+B_2{F}_z\\ \begin{array}{cccc}\begin{array}{cc}& \end{array}& & & \end{array}E=B_6{F}_z^2+B_7{F}_z+B_8\\ \begin{array}{cccc}\begin{array}{ccc}& & \end{array}& & & \end{array}{S}_h=B_9{F}_z+B_{10}\end{array}$$

(21)

where:

• $S_h$ is the drift in the horizontal direction;
• $S_v$ is drift in the vertical direction.

The relationship between tire longitudinal force and sideslip angle can be obtained according to Formula (21), as shown in Figure 9.

The calculation formula of tire lateral force is:

$$\begin{array}{l}\begin{array}{c}\end{array}{F}_c=D\sin \{C\arctan [Bx-E(Bx-\arctan (Bx))]\}+S_v\\ \begin{array}{cccc}\begin{array}{ccc}& & \end{array}& & & \end{array}\begin{array}{c}\end{array}x=\alpha +S_h\\ \begin{array}{cccc}& & & \end{array}B=\frac{A_3\sin (2\arctan \frac{F_z}{A_4})(1-A_5\left|\gamma \right|)}{C\times D}\\ \begin{array}{cccc}\begin{array}{cccc}& & & \end{array}& & & \end{array}C=A_0\\ \begin{array}{cccc}\begin{array}{cc}\begin{array}{cc}& \end{array}& \end{array}& & & \end{array}D=A_1{F}_z^2+A_2{F}_z\\ \begin{array}{cccc}\begin{array}{cc}& \end{array}& & & \end{array}E=A_6{F}_z+A_7\\ \begin{array}{cccc}\begin{array}{ccc}& & \end{array}& & & \end{array}{S}_h=B_9{F}_z+B_{10}\\ \begin{array}{cccc}\begin{array}{cc}& \end{array}& & & \end{array}{S}_v=A_{11}{F}_z\gamma +A_{12}{F}_z+A_{13}\end{array}$$

(22)

where:

• $\alpha$ is the tire sideslip angle;
• $\gamma$ is the camber angle of the tire.

The relationship between the tire lateral force and sideslip angle is shown in Figure 10, and the values of various parameters are shown in

Table 1. Parameter values of tire model.

Parameter	Value	Parameter	Value
A1	−34	B0	2.37272
A2	1250	B1	−9.46
A3	3036	B2	1490
A4	128	B3	130
A5	0.00501	B4	276
A6	−0.02103	B5	0.0886
A7	0.77394	B6	0.00402
A8	0.002289	B7	0.0615
A9	0.013442	B8	1.2
A10	0.0037	B9	0.0299
A11	19.1656	B10	−0.176
A1	1.21356
A1	6.2606
A0	1.65

.

3.3. Vehicle Model Simplification

The calculation for the nonlinear vehicle dynamics model is too complicated. To reduce the difficulty of data processing, the hypothesis of small angle is proposed, that is, every angle in the model becomes an ideal situation.

$$\sin \theta \approx 0,\begin{array}{cc}& \end{array}\cos \theta \approx 1,\begin{array}{cc}& \end{array}\tan \theta \approx 0$$

(23)

The generation $\theta$ here refers to all angles involved in the model, including wheel angle, wheel sideslip angle and centroid sideslip angle, etc. Relying on the assumption of small angle, the tire sideslip angle can be calculated as:

$$\begin{array}{l}{\alpha }_f=\frac{\stackrel{·}{y}+a\stackrel{·}{\phi }}{\stackrel{·}{x}}-{\delta }_f\\ {\alpha }_r=\frac{\stackrel{·}{y}-b\stackrel{·}{\phi }}{\stackrel{·}{x}}\end{array}$$

(24)

Through force analysis of the tire, the lateral force of the tire can be obtained as follows:

$$\begin{array}{l}{F}_{cf}=C_{cf}({\delta }_f-\frac{\stackrel{·}{y}+a\stackrel{·}{\phi }}{\stackrel{·}{x}})\\ \begin{array}{c}\end{array}{F}_{cr}=C_{cr}\frac{\stackrel{·}{y}-b\stackrel{·}{\phi }}{\stackrel{·}{x}}\end{array}$$

(25)

The longitudinal force of the wheel is expressed as:

$$F_{lf}=C_{lf}{S}_f\begin{array}{cc}& \end{array}{F}_{cr}=C_{lr}{S}_r$$

(26)

The simplified nonlinear dynamics model of the vehicle can be expressed as follows:

$$\begin{array}{l}m\stackrel{·}{x}=m\stackrel{·}{y}\stackrel{·}{\dot{\phi }}+2[C_{lf}{S}_f+C_{cf}({\delta }_f-\frac{\stackrel{·}{y}+a\stackrel{·}{\phi }}{\stackrel{·}{x}}){\delta }_f+C_{lr}{S}_r]\\ m\stackrel{·}{y}=-m\stackrel{·}{x}\stackrel{·}{\phi }+2[C_{cf}({\delta }_f-\frac{\stackrel{·}{y}+a\stackrel{·}{\phi }}{\stackrel{·}{x}})-C_{cr}\frac{\stackrel{·}{y}-b\stackrel{·}{\phi }}{\stackrel{·}{x}}]\\ \begin{array}{cc}& \end{array}{I}_z\stackrel{··}{\phi }=2[a{C}_{cf}({\delta }_f-\frac{\stackrel{·}{y}+a\stackrel{·}{\phi }}{\stackrel{·}{x}})+C_{cr}\frac{\stackrel{·}{y}-b\stackrel{·}{\phi }}{\stackrel{·}{x}}]\\ \begin{array}{cccc}\begin{array}{cc}& \end{array}& & & \end{array}\stackrel{·}{X}=\stackrel{·}{x}\cos \phi -\stackrel{·}{y}\sin \phi \\ \begin{array}{cccc}\begin{array}{cc}{}_{}^{}& \end{array}& & & \end{array}\stackrel{·}{Y}=\stackrel{·}{x}\sin \phi +\stackrel{·}{y}\cos \phi \\ \begin{array}{ccc}\begin{array}{ccc}& & \end{array}& & \end{array}{\xi }_{}={[\stackrel{·}{y},\stackrel{·}{x},\phi ,\stackrel{·}{\phi },Y,X]}^T\\ \begin{array}{cc}& \end{array}\begin{array}{cccc}\begin{array}{ccc}& & \end{array}& & & \end{array}{u}_{}={\delta }_f\end{array}$$

(27)

where:

• $\xi$ is the system state quantity;
• $u$ is the system control quantity.

4. Trajectory Tracking Control Based on Model Predictive Control Algorithm

4.1. Model Predictive Control

4.1.1. Linear Time-Varying Prediction Model

The main principle of linear time-varying prediction model is model prediction, rolling optimization solution and error feedback correction for automatic driving trajectory tracking through linearized model. The following three aspects of the model will be introduced in succession:

• Prediction model

The theory of matrix is introduced for more concise expression. The simplified form is as follows:

$$Y(t)={\mathsf{\Psi }}_t\xi (t|t)+{\mathsf{\Theta }}_t\Delta U(t)$$

(28)

$$Y(t)\left[\begin{array}{l}\begin{array}{c}\end{array}\eta (t+1|t)\\ \begin{array}{c}\end{array}\eta (t+2|t)\\ \begin{array}{cc}& \end{array}\cdots \\ \eta (t+N_c|t)\\ \begin{array}{cc}& \end{array}\cdots \\ \eta (t+N_p|t)\end{array}\right]\begin{array}{c}\begin{array}{ccc}& {\mathsf{\Psi }}_t=\left[\begin{array}{l}{\tilde{C}}_t\widetilde{A_t}\\ {\tilde{C}}_t{\tilde{A}}_t^2\\ \begin{array}{c}\end{array}\cdots \\ {\tilde{C}}_t{\tilde{A}}_t^2\\ \begin{array}{c}\end{array}\cdots \\ {\tilde{C}}_t{\tilde{A}}_t^2\end{array}\right]& \begin{array}{cc}& \Delta U(t)=\left[\begin{array}{l}\begin{array}{c}\begin{array}{c}\begin{array}{c}\end{array}\end{array}\end{array}\begin{array}{c}\end{array}\begin{array}{c}\end{array}\Delta u(t)\\ \begin{array}{c}\end{array}\Delta u(t+1|t)\\ \begin{array}{cc}& \end{array}\dots \\ \Delta u(t+N_c|t)\end{array}\right]\end{array}\end{array}\end{array}$$

(29)

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

(30)

where:

• $\xi (t|t)$ is the current state quantity of the system;
• $\Delta U(t)$ is the increment for system control.

The state quantity and output quantity of the system can be obtained from Formulas (29)~(31), and the state quantity and control increment can be obtained through calculation.

2.

Rolling optimization

In the actual process of tracking and controlling the trajectory, the control increment of the system will constantly change and is an unknown variable. Therefore, it is necessary to design the optimized objective function of the system, which should adapt to the actual system and accurately express the solution process. After optimizing the objective function, the optimal control sequence of the system in a specific time domain is finally obtained. The expression of the objective function is as follows:

$$J(k)={\displaystyle \sum _{j=1}^N{\tilde{\chi }}^T}(k+j|k)Q\tilde{\chi }(k+j)+{\tilde{u}}^T(k+j-1)R\tilde{u}(k+j-1)$$

(31)

where:

• $\tilde{\chi }$ is the system state quantity;
• $\tilde{u}$ is the system control quantity;
• $\tilde{\chi }(k+j|k)$ is the predicted value of the state quantity at the next time by the system at time K.

The form constructed by Formula (31) is relatively simple and easy to implement, but when the control quantity of the system changes suddenly, the system will have the disadvantage of not accurately restricting the control increment because the elasticity of the objective function is not sufficient. Therefore, this paper proposes to transform the objective function into a quadratic programming problem. Convert Formula (31) into quadratic function as follows:

$$J={\displaystyle \sum _{i=1}^{N_p}{\Vert \eta (t+i|t)-{\eta }_{ref}(t+i|t)\Vert }_Q^2}+{\displaystyle \sum _{i=1}^{N_c-1}{\Vert \Delta u(t+i|t)\Vert }_R^2}$$

(32)

where:

• $Q,R$ is the weight coefficient matrix.

In order to bring the system closer to the actual operation effect and meet the dynamic constraints of autonomous vehicles, some constraints need to be added:

$$\begin{array}{l}\begin{array}{c}\end{array}{u}_{\min }(t+k)<u(t+k)<u_{\max }(t+k)\\ \Delta u_{\min }(t+k)<\Delta u(t+k)<\Delta u_{\max }(t+k)\\ \begin{array}{c}\end{array}{y}_{\min }(t+k)<y(t+k)<y_{\max }(t+k)\\ \end{array}$$

(33)

where:

• $u(t+k)$ is the control quantity constraint of the system;
• $\Delta u(t+k)$ is the control increment constraint of the system;
• $y(t+k)$ is the output constraint of the system.

The optimal control sequence can be obtained by solving the constraints of the system according to Formula (33). However, in practical engineering, because the environmental information is constantly changing, the model is not guaranteed to obtain an optimal solution at all times. Therefore, a relaxation factor needs to be added to the objective function. The added function model is as follows:

$$J={\displaystyle \sum _{i=1}^{N_p}{\Vert \eta (t+i|t)-{\eta }_{ref}(t+i|t)\Vert }_Q^2}+{\displaystyle \sum _{i=1}^{N_c-1}{\Vert \Delta u(t+i|t)\Vert }_R^2}+\rho \epsilon$$

(34)

where:

• $\rho$ is the weight coefficient;
• $\epsilon$ is the relaxation factor.

Formula (28) is substituted into Formula (34) for simplification to obtain:

$$\begin{array}{l}\begin{array}{ccc}& & \end{array}E(t)={\mathsf{\Psi }}_t\tilde{\xi }(t|t)-Y_{ref}(t)\\ Y_{ref}={[{\eta }_{ref}(t+1|t),\cdots ,{\eta }_{ref}(t+N_p|t)]}^T\end{array}$$

(35)

Matrix transformation of Formula (35) can be obtained as follows:

$$J={[\Delta U{(t)}^T,\epsilon ]}^T{H}_t[\Delta U{(t)}^T,\epsilon ]+G_t[\Delta U{(t)}^T,\epsilon ]+P_t$$

(36)

$$\begin{array}{l}{H}_t=\left[\begin{array}{cc}{\mathsf{\Theta }}_t^T{Q}_e{\mathsf{\Theta }}_t+R_e& 0\\ 0& \rho \end{array}\right]\\ \begin{array}{c}\end{array}{G}_t=[\begin{array}{cc}2E{(t)}^T{Q}_e{\mathsf{\Theta }}_t& 0\end{array}]\\ \begin{array}{cc}& \end{array}{P}_t=E{(t)}^T{Q}_{e}E(t)\end{array}$$

(37)

The quadratic programming problem is obtained from Formula (28), and its expression is:

$$\begin{array}{l}\underset{\Delta U(t)}{\min }{[\Delta U{(t)}^T,\epsilon ]}^T{H}_t[\Delta U{(t)}^T,\epsilon ]+G_t{[\Delta U{(t)}^T,\epsilon ]}^T\\ \begin{array}{c}\begin{array}{cc}& \end{array}\end{array}{U}_{\min }<U(t-1)+{\displaystyle \sum _{i=1}^k\Delta U(i)}<U_{\max }(t+k)\\ \begin{array}{ccc}& & \begin{array}{cc}& \end{array}\end{array}\Delta U_{\min }<\Delta U(k)<\Delta U_{\max }\\ \begin{array}{c}\begin{array}{cc}& \end{array}\end{array}{y}_{\min }-\epsilon <{\mathsf{\Psi }}_t\xi (t|t)+{\mathsf{\Theta }}_t\Delta U(t)<y_{\max }+\epsilon \end{array}$$

(38)

where:

• $\epsilon$ is a positive real number.

3.

Feedback correction

Formula (38) can be solved as follows:

$$\begin{array}{l}\Delta U_t^{\ast }=[\Delta u_t^{\ast },\Delta u_{t+1}^{\ast },\cdots ,\Delta u_{t+N_c-1}^{\ast }]\\ \begin{array}{cc}& \end{array}u(t)=u(t-1)+\Delta u_t^{\ast }\end{array}$$

(39)

4.1.2. System Linearization

In order to improve the real-time performance and applicability of the algorithm in this paper, approximate linearization method is used to optimize the solution. Its principle is as follows:

First, the basic form of the general discrete model is clarified:

$$\begin{array}{l}\xi (k+1)=f(\xi (t),u(t))\\ \begin{array}{ccc}& & \end{array}\xi (t)\in \chi \\ \begin{array}{ccc}& & \end{array}u(t)]\in \mathsf{\Gamma }\end{array}$$

(40)

where:

• $\chi$ is the state constraint quantity;
• $\mathsf{\Gamma }$ is the control constraint quantity.

The relationship between the control quantity and state quantity of the system at a specific time can be expressed as:

$$\begin{array}{l}{\xi }_0(k+1)=f({\xi }_0(k),u_0(k))\\ \begin{array}{ccc}& & \end{array}{\xi }_0(t)=0\end{array}$$

(41)

Taylor expansion of Formula (41) can be used to obtain the first-order formula:

$$\xi (k+1)=f({\xi }_0(k),u_0(k))+\frac{\partial f}{\partial \xi }|{}_{{\xi }_0(k),u_0(k)}(\xi (k)-{\xi }_0(k))+\frac{\partial f}{\partial u}|{}_{{\xi }_0(k),u_0(k)}(u(k)-u_0(k))$$

(42)

Formulas (41) and (42) can be simultaneously obtained as follows:

$$\xi (k+1)={\xi }_0(k+1)+\frac{\partial f}{\partial \xi }|{}_{{\xi }_0(k),u_0(k)}(\xi (k)-{\xi }_0(k))+\frac{\partial f}{\partial u}|{}_{{\xi }_0(k),u_0(k)}(u(k)-u_0(k))$$

(43)

Hypothesis:

$$\begin{array}{l}{A}_{k,0}=\frac{\partial f}{\partial \xi }|{}_{{\xi }_0(k),u_0(k)},\begin{array}{ccc}& & B_{k,0}=\frac{\partial f}{\partial \xi }|{}_{{\xi }_0(k),u_0(k)}\end{array}\\ \begin{array}{cc}& \end{array}\begin{array}{c}\end{array}\begin{array}{c}\end{array}\delta \xi (k+1)=\xi (k+1)-{\xi }_0(k+1)\\ \begin{array}{cccc}& & & \end{array}\delta \xi (k)=\xi (k)-{\xi }_0(k)\\ \begin{array}{cccc}& & & \end{array}\delta u(k)=u(k)-u_0(k)\\ \begin{array}{ccc}& & \end{array}{A}_{k,0}\in R^{m\times n},\begin{array}{ccc}& & B_{k,0}\in R^{n\times m}\end{array}\end{array}$$

(44)

Then, Formula (43) can be simplified as:

$$\delta \xi (k+1)=A_{k,0}\delta \xi (k)+B_{k,0}\delta u(k)$$

(45)

and further simplified:

$$\begin{array}{l}{A}_{k,t}=A_{t,t}\\ B_{k,t}=B_{t,t}\end{array}$$

(46)

The linear time-varying system model can be obtained, which simplifies the calculation steps and improves the real-time performance of the model.

4.2. Design of Model Predictive Controller

4.2.1. Linear Error Model

It can be seen from the derivation of Formula (28):

$$\stackrel{•}{{\xi }_{dyn}}=f_{dyn}({\xi }_{dyn},u_{dyn})$$

(47)

According to Formula (47), the longitudinal speed of the autonomous vehicle has not been changed in trajectory tracking control. Only the control angle of the front wheel angle of the vehicle is controlled, and Formula (47) is linearized:

$${\stackrel{•}{\xi }}_{dyn}=A_{dyn}(t){\xi }_{dyn}(t)+B_{dyn}(t)u_{dyn}(t)$$

(48)

where:

$$A_{dyn}(t)=\frac{\partial f_{dyn}}{\partial {\xi }_{dyn}}|{}_{\begin{array}{l}\xi ={\xi }_r\\ u=u_r\end{array}}=\left[\begin{array}{cccccc}\frac{-2(C_{cf}+C_{cr})}{m{\stackrel{•}{x}}_t}& \frac{\partial f_{\stackrel{•}{y}}}{\partial \stackrel{•}{x}}& 0& -{\stackrel{•}{x}}_t+\frac{-2(b{C}_{cr}-a{C}_{cf})}{m{\stackrel{•}{x}}_t}& 0& 0\\ \stackrel{•}{\phi }-\frac{2C_{cf}{\delta }_{f,t-1}}{m{\stackrel{•}{x}}_t}& \frac{\partial f_{\stackrel{•}{x}}}{\partial \stackrel{•}{x}}& 0& {\stackrel{•}{y}}_t-\frac{2a{C}_{cf}{\delta }_{f,t-1}}{m{\stackrel{•}{x}}_t}& 0& 0\\ 0& 0& 0& 1& 0& 0\\ \frac{2(b{C}_{cr}-a{C}_{cf})}{I_z{\stackrel{•}{x}}_t}& \frac{\partial f_{\stackrel{•}{\phi }}}{\partial \stackrel{•}{x}}& 0& \frac{-2(b{C}_{cr}+a{C}_{cf})}{I_z{\stackrel{•}{x}}_t}& 0& 0\\ \cos ({\phi }_t)& \sin ({\phi }_t)& {\stackrel{•}{x}}_t\cos ({\phi }_t)-{\stackrel{•}{y}}_t\sin ({\phi }_t)& 0& 0& 0\\ -\sin ({\phi }_t)& \cos ({\phi }_t)& -{\stackrel{•}{y}}_t\cos ({\phi }_t)-{\stackrel{•}{x}}_t\sin ({\phi }_t)& 0& 0& 0\end{array}\right]$$

(49)

$$B_{dyn}(t)=\frac{\partial f_{dyn}}{\partial {\xi }_{dyn}}|{}_{\begin{array}{l}\xi ={\xi }_r\\ u=u_r\end{array}}=\left[\frac{2C_{cf}}{m},\frac{2C_{cf}({\delta }_{f,t-1}-\frac{{\stackrel{•}{y}}_t+a{\stackrel{•}{\phi }}_t}{{\stackrel{•}{x}}_t})}{m},0,\frac{2a{C}_{cf}}{I_z},0,0\right]$$

(50)

$$\begin{array}{l}\frac{\partial f_{\stackrel{•}{y}}}{\partial \stackrel{•}{x}}=\frac{2C_{cf}({\stackrel{•}{y}}_t+a{\stackrel{•}{\phi }}_t)+2C_{cr}({\stackrel{•}{y}}_t-b{\stackrel{•}{\phi }}_t)}{m{\stackrel{•}{x}}_t}-{\stackrel{•}{\phi }}_t\\ \frac{\partial f_{\stackrel{•}{x}}}{\partial \stackrel{•}{x}}=\frac{2C_{cf}{\delta }_{f,t-1}({\stackrel{•}{y}}_t+a{\stackrel{•}{\phi }}_t)}{m{\stackrel{•}{x}}_t}\\ \frac{\partial f_{\stackrel{•}{\phi }}}{\partial \stackrel{•}{x}}=\frac{2a{C}_{cf}({\stackrel{•}{y}}_t+a{\stackrel{•}{\phi }}_t)-2b{C}_{cr}({\stackrel{•}{y}}_t-b{\stackrel{•}{\phi }}_t)}{I_z{\stackrel{•}{x}}_t}\end{array}$$

(51)

Discretization of the linear equation obtained by Formula (47) can be obtained as follows:

$$\begin{array}{l}{\xi }_{dyn}(k+1)=A_{dyn}(k){\xi }_{dyn}(k)+B_{dyn}(k)u_{dyn}(k)\\ \begin{array}{cccc}& & & \end{array}{A}_{dyn}(k)=1+T{A}_{dyn}(t)\\ \begin{array}{cccc}& & & \end{array}\begin{array}{c}\end{array}{B}_{dyn}(k)=T{B}_{dyn}(t)\end{array}$$

(52)

4.2.2. Objective Function Optimization

In order to make the model more consistent with the actual situation, the constraint conditions need to be added, and the objective function can be obtained by applying Formula (34) to the vehicle dynamics model:

$$J={\displaystyle \sum _{i=1}^{N_p}{\Vert {\eta }_{dyn}(t+i|t)-{\eta }_{dyn}{}_{,ref}(t+i|t)\Vert }_Q^2}+{\displaystyle \sum _{i=1}^{N_c-1}{\Vert \Delta u_{dyn}(t+i|t)\Vert }_R^2}+\rho \epsilon$$

(53)

where:

• $\rho$ is the weight coefficient;
• $\epsilon$ is the relaxation factor;
• $Q,R$ is the weight factor.

Coupled with the relevant dynamic constraints, Formula (28) becomes the optimal solution for solving the problem trajectory tracking control:

$$\begin{array}{c}\underset{\Delta u_{dyn,\epsilon }}{\min }={\displaystyle \sum _{i=1}^{N_p}{\Vert {\eta }_{dyn}(t+i|t)-{\eta }_{dyn}{}_{,ref}(t+i|t)\Vert }_Q^2}+{\displaystyle \sum _{i=1}^{N_c-1}{\Vert \Delta u_{dyn}(t+i|t)\Vert }_R^2}+\rho \epsilon \\ \begin{array}{c}\begin{array}{cccc}& & & \end{array}\end{array}{U}_{dyn,\min }\le A\Delta U_{dyn,t}+U_{dyn,t}\le U_{dyn,\max }\\ \begin{array}{cccc}& & & \end{array}\begin{array}{c}\end{array}\Delta U_{dyn,\min }\le A\Delta U_{dyn,t}\le \Delta U_{dyn,\max }\\ \begin{array}{c}\begin{array}{cccc}\begin{array}{ccc}& & \end{array}& & & \end{array}\end{array}{y}_{hc,\min }<y_{hc}<y_{hc,\max }\\ \begin{array}{ccc}\begin{array}{ccc}& & \end{array}& & \end{array}{y}_{sc,\min }-\epsilon <y_{sc}<y_{sc,\max }\epsilon \\ \begin{array}{cccc}\begin{array}{cccc}\begin{array}{cc}& \end{array}& & & \end{array}& & & \end{array}\epsilon >0\end{array}$$

(54)

where:

• $y_{hc}$ is a hard constraint;
• $y_{sc}$ is a soft constraint;
• $y_{hc,\min }$ is the hard constraint minimum;
• $y_{hc,\max }$ is the maximum value of the hard constraint;
• $y_{sc,\min }$ is the soft constraint minimum;
• $y_{sc,\max }$ is the maximum value of the soft constraint.

According to Formula (28), the output form of future time can be obtained:

$$Y_{dyn}(t)={\mathsf{\Psi }}_{dyn}{\xi }_{dyn}(t|t)+{\mathsf{\Theta }}_{dyn}\Delta U_{dyn}(t)$$

(55)

where:

$$Y_{dyn}=\left[\begin{array}{c}{\eta }_{dyn}(t+1|t)\\ {\eta }_{dyn}(t+2|t)\\ \cdots \\ {\eta }_{dyn}(t+N_c|t)\\ \cdots \\ {\eta }_{dyn}(t+N_p|t)\end{array}\right]\begin{array}{c}\end{array}{\mathsf{\Psi }}_{dyn}=\left[\begin{array}{c}{\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}\\ {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^2\\ \cdots \\ {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_c}\\ \cdots \\ {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_p}\end{array}\right]\begin{array}{c}\end{array}\Delta U_{dyn}(t)\left[\begin{array}{c}\Delta U_{dyn}(t|t)\\ \Delta U_{dyn}(t+1|t)\\ \cdots \\ \Delta U_{dyn}(t+N_c|t)\end{array}\right]$$

(56)

$${\mathsf{\Theta }}_t=\left[\begin{array}{cccc}{\tilde{C}}_{\Delta dyn}{\tilde{B}}_{\Delta dyn}& 0& 0& 0\\ {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}{\tilde{B}}_{\Delta dyn}& {\tilde{C}}_{\Delta dyn}{\tilde{B}}_{\Delta dyn}& 0& 0\\ \cdots & \cdots & \ddots & \cdots \\ {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_c-1}{\tilde{B}}_{\Delta dyn}& {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_c-2}{\tilde{B}}_{\Delta dyn}& \cdots & {\tilde{C}}_{\Delta dyn}{\tilde{B}}_{\Delta dyn}\\ {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_c}{\tilde{B}}_{\Delta dyn}& {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_c-1}{\tilde{B}}_{\Delta dyn}& \cdots & {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}{\tilde{B}}_{\Delta dyn}\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_c-1}{\tilde{B}}_{\Delta dyn}& {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_c-2}{\tilde{B}}_{\Delta dyn}& \cdots & {\tilde{C}}_{\Delta dyn}{\tilde{A}}_{\Delta dyn}^{N_p-N_c-1}{\tilde{B}}_{\Delta dyn}\end{array}\right]$$

(57)

Finally, Formula (55) is converted into quadratic form for solution:

$$J={[\Delta U_{dyn}{(t)}^T,\epsilon ]}^T{H}_{dyn}[\Delta U{(t)}^T,\epsilon ]+G_{dyn}[\Delta U{(t)}^T,\epsilon ]+P_{dyn}$$

(58)

where:

$$\begin{array}{l}{H}_{dyn}=\left[\begin{array}{cc}{\mathsf{\Theta }}_{{}_{dyn}}^T{Q}_e{\mathsf{\Theta }}_{dyn}+R_e& 0\\ 0& \rho \end{array}\right]\\ \begin{array}{c}\end{array}{G}_{dyn}=[\begin{array}{cc}2E_{dyn}{(t)}^T{Q}_e{\mathsf{\Theta }}_{dyn}& 0\end{array}]\\ \begin{array}{cc}& \end{array}{P}_{dyn}=E_{dyn}{(t)}^T{Q}_e{E}_{dyn}(t)\\ E_{dyn}(t)={\mathsf{\Psi }}_{dyn}{\xi }_{dyn}(t|t)-Y_{dyn,ref}(t)\end{array}$$

(59)

4.2.3. Constraint Solving

• Centroid sideslip constraint

The sideslip angle will affect the driving stability of the vehicle. In order to keep the tracking track stable, the sideslip angle cannot exceed a reasonable range. Relevant literature combines the dynamic road boundary with the centroid sideslip angle, and the relationship between the centroid sideslip angle and the road boundary is deduced as follows:

$$\left|\beta \right|\le \arctan (0.02 \mu g)$$

(60)

According to Formula (60), the reasonable range of centroid side deflection angle is inferred:

$$\begin{array}{l}\left|\beta \right|\le {10}^{°}(good pavement)\\ \left|\beta \right|\le {10}^{°}(slippery pavement)\end{array}$$

(61)

2.

Vehicle attachment constraints

The constraints of vehicle attachment conditions will affect the dynamic performance of the autonomous vehicle. Therefore, this constraint needs to be analyzed. The relationship between vehicle acceleration and road surface is as follows:

$$\sqrt{a_x^2+a_y^2}\le \mu g$$

(62)

where:

• $a_x$ is longitudinal acceleration;
• $a_y$ is the lateral acceleration.

If the longitudinal speed of the vehicle is constant, it can be rewritten as:

$$\left|a_y\right|\le \mu g$$

(63)

The constraint range of acceleration is related to the road condition. Driving in a perfect road environment will result in a wider range of acceleration. However, a large variation in acceleration will greatly reduce the riding comfort of passengers because the large lateral acceleration will affect the driving stability of the vehicle, but an excessively small range will lead to no solutions being discovered when solving. Therefore, it is necessary to set the acceleration as a soft constraint and let the system adjust the constraints according to changes in the actual situation. The setting conditions are as follows:

$$a_{y,\min }-\epsilon \le a_y\le a_{y,\max }-\epsilon$$

(64)

where:

• $a_{y,\min }$ is the minimum constraint of acceleration;
• $a_{y,\max }$ is the maximum constraint of acceleration.

3.

Tire sideslip restraint

When autonomous vehicle is modeled as a single-track model, the sideslip angle is not considered as the state of the system, so it is necessary to solve the problem. The relationship between the mass sideslip angle and the state and control volume is as follows:

$$\begin{array}{l}{\alpha }_f=\frac{\stackrel{•}{y}+a\stackrel{•}{\phi }}{\stackrel{•}{x}}-{\delta }_f\\ {\alpha }_f=\frac{\stackrel{•}{y}-b\stackrel{•}{\phi }}{\stackrel{•}{x}}\end{array}$$

(65)

When the tire sideslip angle is less than 3°, the ideal situation of the small angle assumption should be considered and Formula (65) addressed through approximate linearization. The constraints of the front wheel sideslip angle are as follows:

$$-2.5°<{\alpha }_f<2.5°$$

(66)

5. Joint Simulation Experiment Based on MATLAB and CarSim

5.1. Co-Simulation Platform

At present, co-simulation platform has the popular trend to be used in the field of automatic driving. Xu, R. et al. [38]. employed OpenCDA which consists of one generalized framework and fast-prototyping tool to develop and operate CDA systems during simulation. This opened system has the advantage of being able to obtain the high precisely results under different CDA algorithms at both the traffic and individual autonomy levels. Li Yongyi. et al. [39]. improved the estimation accuracy of model parameters by generalized recursive least squares (GRLS).

The trajectory tracking controller designed based on the vehicle dynamics model is mainly used for the trajectory tracking of autonomous vehicles under medium- and highspeed traffic conditions. In order to verify the tracking accuracy and stability of the above model predictive tracking controller under different working conditions and different speed conditions, the reference trajectory planned by the improved of artificial potential field method will be used for verification.

First, the body size and parameters of the autonomous vehicle are set in CarSim. The body size and parameters are illustrated in Figure 11 and

Table 2. Vehicle parameters.

Parameter	Value	Unit
Sprung mass	1723	kg
Width for animator	1850	mm
Yaw inertia	4175	kg·m2
Axle base	2700	mm
Height of wheel center	325	mm
Height of the center of mass	460	mm

.

After the vehicle size and parameter data are established in CarSim, the simulation conditions are set, and the model is dispatched through the external interface to Simulink. At this time, the CarSim and Simulink modules constitute the co-simulation platform, and the co-simulation can only be carried out with input information. The co-simulation platform is demonstrated in Figure 12.

5.2. Controller Parameter Setting

The simulation sets the parameters of the model predictive controller itself. The parameter settings are shown in

Table 3. Controller parameter table.

Parameter	Nomenclature	Value
Np	Prediction step length	15
Nc	Control step size	8
T	Sampling period	0.02
Q	Weight matrix	$\left[\begin{array}{ccc}200& 0& 0\\ 0& 100& 0\\ 0& 0& 100\end{array}\right]$
R	Weight factor	110,000
ρ	Weight coefficient	1000

below.

5.3. Simulation Analysis of Different Pavement Adhesion Coefficients

As different road adhesion coefficients will have different effects on the tires during vehicle driving and thus affect the tracking performance of the model predictive controller, it is necessary to select different road adhesion coefficients and retrograde comparative simulation. In this paper, μ = 0.85 and μ = 0.5 are selected for simulation tests. The speed is 36 km/h. The simulation results are as follows.

As shown in Figure 13, when the speed is 36 km/h, the curve changes in the figure are basically consistent under both μ = 0.85 and μ = 0.5 conditions, indicating that different road adhesion coefficients have little influence on vehicle track tracking at low speeds. The reason is that when the speed is low, a large contact area is maintained between the vehicle tires and road surface during the process of moving, so as to reduce the influence of the road adhesion coefficient on the friction between the tire and the ground. As can be seen from Figure 13a, when the vehicle speed is 36 km/h, the vehicle can accurately track the reference trajectory planned by the improved artificial potential field method under both μ = 0.85 and μ = 0.5 conditions. From Figure 13b–d, when driving at a speed of 36 km/h, the yaw angle of the vehicle is controlled between (−6 deg, 7.2 deg) under the conditions of μ = 0.85 and μ = 0.5, and an extreme value of about 1.34 m appears in the third and sixth seconds. At this time, the vehicle is changing lanes and overtaking, which is in line with the actual situation, and the change value is within a reasonable range; the yaw velocity is controlled within (−6.5 deg/s, 5 deg/s), which is far lower than the limit value. However, the range of the sideslip angle of the vehicle’s centroid is controlled within (−0.42 deg, 0.65 deg), which is far lower than the threshold. In summary, at lower speeds, vehicle under different road adhesion coefficients can better track the reference trajectory and all parameters of a vehicle traveling in the reasonable scope. The not only ensures the trajectory tracking accuracy but also the maintenance vehicle stability, thus showing the excellent adaptability of the controller for different road adhesion coefficients.

As shown in Figure 14a, when the vehicle speed changes to 72 km/h, the controller has a positive tracking effect on the reference trajectory under two working conditions, namely μ = 0.85 and μ = 0.5. However, the vehicle yaw angle changes in different trends under conditions of different road adhesion coefficients at the same condition, as shown in Figure 14b. The yaw angle fluctuates in a small range at the first and third second, which changes in a reasonable range. As illustrated in Figure 14c, when the speed reaches 72 km/h, the variation range of vehicle yaw velocity under μ = 0.85 and μ = 0.5 conditions increases significantly, but both ranges fluctuate within (−15 deg/s, 15 deg/s) and do not exceed the limit range. As Figure 14d shows, when the vehicle speed increases to 72 km/h, the fluctuation range of the sideslip angle is slightly higher than that of 36 km/h when the road adhesion coefficient is 0.85. If the road adhesion coefficient is 0.5, the fluctuation range is (−1.3 deg, 1.4 deg). However, the range of values in both working conditions fluctuates within a reasonable range and does not exceed the limit value. In summary, at low speed, the vehicle can track the reference trajectory stably under different road adhesion coefficients. However, for a low road adhesion coefficient, the curves in the figure all fluctuate with the increase in vehicle speed. The reason is that when there is high speed, the vehicle cannot maintain a large contact area between the tire and the road surface during the moving process. Moreover, the contact area between tire and road surface will decrease as the road adhesion coefficient decreases, which will lead to a decrease in vehicle driving stability. The tracking accuracy can, however, also be maintained to ensure the stable driving of the vehicle.

5.4. Simulation Analysis of Different Vehicle Speeds

In order to verify the real-time robustness of the controller, three different speeds were selected for simulation in this paper, which are 36, 54 and 72 km/h, respectively, and the road adhesion coefficient is taken as 0.85. The simulation results are shown in Figure 15.

As illustrated in Figure 15a, when the road adhesion coefficient is 0.85, vehicles at three different speeds can accurately track the reference trajectory and quickly tend to stable driving. It can be seen from Figure 15b that the yaw angle of vehicles at different speeds maintain the same trend.

As can be seen from Figure 15c, as the speed rises from 36 to 72 km/h, the fluctuation range of vehicle yaw velocity changes from (−6.7 deg/s, 4.6 deg/s) to (−13.5 deg/s, 14.2 deg/s), showing a significant increase, but the values are within the constraint range and do not exceed the limit value. As shown in Figure 15d, when the vehicle speed increments, the vehicle centroid sideslip angle increases gradually, but the value of the vehicle centroid sideslip angle at the three speeds does not exceed the range of 4 deg.

By comparing the track tracking error under different working conditions, it can be seen that the maximum tracking error is 0.134 m, and the tracking accuracy exceeds 98%, which is higher than the 93.8% reported by Zhu Weida. The comparison diagram of tracking accuracy under different working conditions is shown in

Table 4. Comparison of tracking accuracy under different working conditions.

Speed (km/h)	Adhesion Coefficient of Pavement	Tracking Lateral Error (m)	Tracking Accuracy
36	0.85	0.095	98.6%
	0.5	0.102	98.5%
72	0.5	0.134	98.1%
	0.85	0.121	98.3%

:

In summary, the controller can still accurately track the reference trajectory when the speed changes and ensure the stability of the vehicle in the process of driving, indicating that the controller has superior adaptability and robustness and relatively high tracking accuracy.

6. Conclusions

Research on autonomous vehicles is of great significance to the construction of intelligent transportation and intelligent cities. This paper mainly studies the trajectory planning and trajectory tracking control of autonomous vehicles and can be summarized as follows:

• In the study, through increasing the target distance adjustment factor to solve the problem of inaccessible targets, the invasive weeds algorithm produces an optimal sub-destination, and the repulsive force redistribution problem is converted into a local optimal solution to establish the dynamic road repulsion field, velocity repulsion field, and acceleration repulsion field by relying on the change in speed to realize trajectory planning in a dynamic environment.
• An MPC model predictive controller was built to establish a vehicle dynamics model and tire model based on a “magic formula”, with the simplification of the vehicle dynamics model.
• A co-simulation platform was built, and two different road adhesion coefficients and three different vehicle speeds were selected for simulation. With the increase in the vehicle speed from 36 to 72 km/h, the values of the sideslip angle of the vehicle center of mass at the three speeds do not exceed the range of 4 deg. At the same time, the maximum tracking error is 0.134 m, and the tracking accuracy exceeds 98%, which indicates that the driving stability is excellent.