LQR-MPC-Based Trajectory-Tracking Controller of Autonomous Vehicle Subject to Coupling Effects and Driving State Uncertainties

This paper presents a lateral and longitudinal coupling controller for a trajectory-tracking control system. The proposed controller can simultaneously minimize lateral tracking deviation while tracking the desired trajectory and vehicle speed. Firstly, we propose a hierarchical control structure composed of upper and lower-level controllers. In the upper-level controller, the linear quadratic regulator (LQR) controller is designed to compute the desired front wheel steering angle for minimizing the lateral tracking deviation, and the model-predictive controller is developed to compute the desired acceleration for maintaining the planed vehicle speed. The lower-level controller enables the achievement of the desired steering angle and acceleration via the corresponding component devices. Furthermore, an observer based on the Extended Kalman Filter (EKF) is proposed to update the vehicle driving states, which are sensitive to the trajectory-tracking control and difficult to measure directly using the existing vehicle sensors. Finally, the Co-simulation (CarSim-MATLAB/Simulink) results demonstrate that the proposed coupling controller is able to robustly realize the trajectory tracking control and can effectively reduce the lateral tracking error.


Introduction
Autonomous vehicle technology is undoubtedly one of the hottest topics at the moment. Vehicular autonomy can be achieved in three main steps: the vehicle's perception, its trajectory planning, and its control. The vehicle control system, that is, the trajectorytracking control system, is the most important part of autonomous driving. The main goal for a trajectory-tracking control system is to design a control system that consists of lateral and longitudinal control modules and can perform the task of trajectory tracking for an autonomous vehicle. Indeed, the trajectory-tracking process of autonomous vehicles is coordinated by the lateral and longitudinal motion systems with complex coupling relationships, which should be taken into account when designing a trajectorytracking controller [1,2]. Thus, the determination of a method to address the problem of controlling the lateral and the longitudinal vehicle dynamics in a coupled way remains a challenging task.
In the past few years, numerous control techniques have been studied to address the problem of trajectory tracking in autonomous vehicles. The existing control methods, such as sliding mode control (SMC) [3][4][5], robust control [6], model predictive control (MPC) [7][8][9][10][11][12], the linear quadratic regulator (LQR) [13][14][15][16][17][18][19][20][21], and the classic PID control [8,19], were proposed to pursue the task of lateral and longitudinal control. However, most of these studies aimed to address the lateral and longitudinal control separately. Recently, some researchers have been dedicated to addressing lateral and longitudinal vehicle dynamics control in a coupled way. Specifically, the authors of [20] proposed a coupling lateral and longitudinal control, while the literature on the lateral and longitudinal coupling control approach are not mature. Additionally, it is worth mentioning that to verify the control algorithm, most researchers usually assume that the vehicle states measured by the vehicle sensors are ideal values, which will affect the final control effect of the algorithm.
Inspired by the above discussion and considerations, this paper presents a trajectorytracking control approach under the consideration of vehicle states' uncertainty and lateral and longitudinal coupling relationships. The novelties and contributions of this paper are summarized as follows: • A trajectory-tracking controller, which considers the coupling relationships of lateral and longitudinal dynamics for autonomous vehicles, is designed, guaranteeing: (i) the vehicle enables the tracking of the desired trajectory, and (ii) the speed error and lateral position error are within a small range; • A vehicle driving state observer based on the EKF is proposed to update the vehicle driving states, such as longitudinal velocity and yaw rate, which are sensitive to the trajectory-tracking control and hard to measure directly using the existing vehicle sensors; • Tests in a co-simulation (CarSim-MATLAB/Simulink) environment is presented to validate the effectiveness of the proposed controller.
With respect to the existing literature, the main merits of the proposed trajectorytracking control approach are as follows. Compared with [16], this paper considers the coupling relationships between lateral and longitudinal dynamics and designs a longitudinal controller to track the planned vehicle speed. Compared with the path tracking approach presented in [10], the designed controller is tested in a wider speed range (30-118 km/h). Moreover, compared with the longitudinal control approach presented in [7,9], an EKF observer is established to estimate the longitudinal velocity, which is sensitive to the control process and is hard to measure directly.
The remainder of this paper is organized as follows. In Section 2, the framework of the proposed trajectory-tracking control system and the notation applied throughout this paper are described. In Section 3, a series of mathematical models are developed for the controller's design: the lateral and longitudinal coupling control system model and the vehicle state estimation system model. A coupling controller for a trajectory-tracking control system is designed in Section 4. To demonstrate the efficiency of the proposed controller, the quintic polynomial trajectory planning approach is proposed to generate the desired trajectory and the desired vehicle speed in Section 5, while the co-simulation results are presented and analyzed. Finally, Section 6 summarizes the contents of this paper and describes the future work.

Framework of Trajectory-Tracking Control System
A hierarchical control structure is proposed to construct the lateral and longitudinal coupling controller for a trajectory-tracking control system, as shown in Figure 1. The overall target of the control system is to compute the desired front wheel steering angle and the desired acceleration and implement this target via the corresponding component devices to consequently track the desired trajectory. The hierarchical control structure is composed of an upper-level controller and a lower-level controller. The upper-level controller is designed to compute the desired front wheel steering angle and the desired acceleration. The EKF observer is established to obtain the longitudinal velocity and yaw rate, which are uncertain due to the coupling relationships and are hard to measure directly using the existing vehicle sensors. The lower-level controller achieves the output of the upper-level controller via the steering and driving devices to track the desired trajectory.
For the reader's reference, Table 1 summarizes the main symbols and their corresponding descriptions that appear in this paper.  For the reader's reference, Table 1 summarizes the main symbols and their corresponding descriptions that appear in this paper.

Trajectory-Tracking Control System Modeling
Overly complex vehicle models will occupy more computing resources and cause the control problem to be harder to solve. Thus, the real-time control algorithm can not meet the needs of real vehicle applications [27,28]. Therefore, in this section, considering the influence of the lateral motion, yaw motion, and longitudinal velocity of the vehicle, a simplified vehicle dynamics model that can reflect the vehicle motion characteristics as far as possible by means of constraint simplification and approximation is described, as shown in Figure 2.
The model is established based on the following assumptions: • The vehicle runs on a flat road; that is, the vertical movement of the vehicle is not considered; • The influence of the vehicle suspension system is not considered; • The steering angle acts directly on the front wheels of the vehicle (Ackermann drive); • The tires are in good contact with the ground; • Load transfer is ignored; • Air resistance is ignored.

Trajectory-Tracking Control System Modeling
Overly complex vehicle models will occupy more computing resources and cause the control problem to be harder to solve. Thus, the real-time control algorithm can not meet the needs of real vehicle applications [27,28]. Therefore, in this section, considering the influence of the lateral motion, yaw motion, and longitudinal velocity of the vehicle, a simplified vehicle dynamics model that can reflect the vehicle motion characteristics as far as possible by means of constraint simplification and approximation is described, as shown in Figure   The model is established based on the following assumptions:


The vehicle runs on a flat road; that is, the vertical movement of the vehicle is not considered;  The influence of the vehicle suspension system is not considered;  The steering angle acts directly on the front wheels of the vehicle (Ackermann drive);  The tires are in good contact with the ground;  Load transfer is ignored;  Air resistance is ignored.
By analyzing Newton's second law and the torque balance equation, the dynamics differential equation of the vehicle can be obtained as follows: By analyzing the 3-DOF vehicle dynamics model, assuming that the front wheel steering angle is small, the lateral force of the tire is equal to the product of the cornering stiffness and the slip angle of the tire [28], and by setting =̇, the dynamics differential equation can be obtained as follows: By analyzing Newton's second law and the torque balance equation, the dynamics differential equation of the vehicle can be obtained as follows: .
By analyzing the 3-DOF vehicle dynamics model, assuming that the front wheel steering angle is small, the lateral force of the tire is equal to the product of the cornering stiffness and the slip angle of the tire [28], and by setting v y = . y, the dynamics differential equation can be obtained as follows: where m is the mass of the vehicle; l f and l r are the front and rear wheelbases of the vehicle, respectively; v x and v y represent the velocity components of the velocity V at the center of the vehicle's mass along the X and Y axis, respectively; a x is the longitudinal acceleration of the vehicle; ϕ is the yaw angle of the vehicle; . ϕ is the yaw rate of the vehicle; β is the slip angle of the center of mass, which denotes the angle of the current velocity of the center of mass with respect to the longitudinal axis of the car; δ is the front wheels' steering angle; α f and α r represent the slip angles of the front and rear tires, respectively; C a f and C ar represent the cornering stiffness of the front and rear tires, respectively; F y f and F yr represent the lateral forces of the front and rear tires, respectively; and I z is the moment of inertia of the vehicle around the Z axis.
The intelligent vehicle will have errors in the process of tracking the desired trajectory, as shown in Figure 2. Defining the distance between the vehicle's centroid and the projection point of the desired trajectory centerline as the lateral distance error e d , and the deviation between vehicle and desired trajectory direction as the heading error e ϕ , this tracking error can be measured and minimized.
The heading error is defined as ϕ + β − θ r [28]. Since the value of the slip angle β is so small, by setting e ϕ = ϕ − θ r as the approximate heading error (this approximation is explained in Section 4.2), . e ϕ can be obtained: where θ r is the heading angle at the current time for the desired trajectory. By analyzing the vehicle dynamics model and supposing that the value of ϕ − θ r is small, . e d can be obtained: where . y = Vsinβ and v x = Vcosβ. Substituting Equations (6) and (5)   .. ..
Since the curvature of the road is usually gentle, the second derivative of θ r is ignored, and so .. e ϕ can be simplified to: ..
By writing Equations (5)- (7) and (9) in matrix form, the state space equation of the lateral tracking error of the vehicle can be obtained as follows: As with many previous studies [12,28,29], the longitudinal control of the vehicle can be represented by a first-order inertial system: where K s = 1 is the gain of system, τ d is the time constant, a des is the desired acceleration, and a is the vehicle's actual acceleration. The state equation for longitudinal motion can be expressed as: .
where ξ = [V, a] T is the system state, V is the speed of the center of vehicle mass but also the control output of the system, and u L = a des is the control input. The lateral and longitudinal controllers have been modeled as shown in Equations (10)- (12). Thus, the linear time-varying model for the lateral and longitudinal coupling controllers can be established as follows: where e ϕ , V, a T is the system state, and u i,t = [δ, a des ] T is the control input.

Control Objectives
In this work, we aim to propose a controller that can track the desired trajectory and the speed by considering the coupling relationships between the vehicle's lateral and longitudinal dynamics. Therefore, aiming to ensure the accurate tracking to the timevarying planning trajectory, the speed and the lateral position of the vehicle should be controlled. The targets of the coupling controller can be considered as: where v r is the planning speed; V is the speed at the center of vehicle mass, and when the vehicle reaches the steady state, the value of V should approach v r and the error should be as small as possible; e d denotes the lateral position error. The objective of longitudinal control is to track the planning speed to improve the lateral tracking accuracy. At this point, the first objective of the coupling controller can be expressed as Equation (16).
The trajectory-tracking controller ensures that the vehicle travels along the planned trajectory. Therefore, another objective is to track the desired trajectory and minimize the lateral position error, and it can be expressed as Equation (17).
In addition, to track the planning speed while achieving ride comfort, some physical characteristics of the vehicle should also be considered. Thus, the acceleration and the acceleration increment should be constrained as: where a L,min , a L,max , ∆a L,min , and ∆a L,max are the bounds of the acceleration and the acceleration increment.

Upper Level Controller Design
Since longitudinal velocity is time-varying, Equation (10) is rewritten as a linear time-varying system as follows: The controller is used in the discrete-time model. Thus, the continuous system needs to be converted into a discrete system. Ignoring the influence of term C t . θ r , by using the midpoint Euler method and the forward Euler method on Equation (19) and simplifying it, the discretization model for the lateral tracking error can be obtained as follows: where The optimal control performance function for LQR is established as follows: where X k is the state variable of the system, u k is the control variable of the system, and Q and R are the weighting matrices of the state error and the control quantity, respectively. According to several experiments, the weighting matrices must be set to Q = diag [28,1,4,1] and R = 10. When J is minimum, the control quantity u is the desired front wheel steering angle. Substituting Equation (20) into (22) as a constraint, the Lagrangian control problem with multiplicative constraints is constructed as follows: Now, the Hamiltonian function can be constructed as follows: Substituting Equation (24) into (23) and simplification obtains: Now, the extreme value of Equation (25) can be obtained as follows: where P k+1 is the solution of Riccati equation T P k+1 A k , the control quantity of the LQR controller can be obtained as follows: where K = [k 1 , k 2 , k 3 , k 4 ] is the gain of the LQR controller. Substituting Equation (27) into (19) obtains: According to the above equation, no matter what the value of gain K is, the distance error and heading error of the intelligent vehicle cannot be guaranteed to be zero in the control process; that is, there is a steady-state error in the system. Hence, in this paper, the feedforward controller based on feedback is designed to eliminate the steady-state error.
The actual control quantity after introducing feedforward δ f is: Substitute Equation (29) into (19) so that . X = 0; that is, assume that the system reaches a steady state where the steady-state error is: By solving Equation (30) and simplifying it, the following can be obtained: The analytic Equation (31) shows that when the lateral distance error e d = 0, the feedforward control quantity δ f is: In Equation (5), the actual heading error is assumed to be e ϕ = ϕ − θ r for ease of calculation. When the vehicle reaches the steady state, it is necessary to make the heading error e θ = 0, and there is e ϕ = ϕ − θ r = −β. Thus, it is not necessary to design a feedforward controller to eliminate the steady-state error of e ϕ . Meanwhile, the authors of [30] proved that the steady-state equilibrium can still be achieved where the values of lateral error and heading angle error are nonzero.
The proposed MPC is a discrete-time strategy, and the continuous system should be discretized.
By using the forward Euler method on Equation (12) and simplifying it, the discretization model for a longitudinal control system can be obtained as follows: where where T s is the sampling period. The output equation of the longitudinal control system can be expressed as: We next track the desired speed accurately and smoothly by penalizing acceleration or excessive changes in acceleration. Formulating the control problem with a cost function: where N p is the prediction time domain; N c is the control time domain; Π p,(k+i|k) is the prediction for the control output variable; Π re f ,(k+i|k) is the reference for the control output variable; (k + i|k) represent the fact that the value at sampling time (k + i) is predicted based on the information from the sampling time k, where i = 1, 2, . . . , N p ; Q L is the weight matrix of the system output, reflecting the tracking accuracy of the control system to the reference velocity; and R L is the weight matrix of the system control increment.
Considering the comfort of the driver, the acceleration and the acceleration increment should be constrained, and the constraints can be formulated as follows: where u L,min and u L,max are the minimum and maximum acceleration, respectively, and ∆u L,min and ∆u L,max are the minimum and maximum acceleration increment, respectively. Notice that the longitudinal velocity and yaw rate are time-varying during the driving process. As presented in the introduction, the time-varying characteristic of the vehicle states is a critical issue in the trajectory-tracking controller design, so the EKF observer is designed to estimate them.
As can be seen from the geometric relationship in Figure 2, the slip angles of the front and rear tires and the slip angle of the centroid of the vehicle can be expressed as: Through calculation and simplification in combination with Equation (4), the state equation and observation equation based on a 3-DOF nonlinear vehicle model can be obtained: where ω = .
ϕ is the yaw rate of the vehicle, a x is the longitudinal acceleration of the vehicle, and a y is the lateral acceleration of the vehicle.
The state Equation (41) and the observation Equation (42) are discretized to obtain: Rewriting the discretized state equation and observation equation into the form of state space equation yields: where x(k) = [ω, β, v x ] T is the state variable, u(k) = [δ, a x ] T is the input variable, and a y is the observation variable. W k and V k are the system noise and measurement noise, respectively, and they are independent of each other and their mean value is zero, the variance of W k is Q k , and the variance of V k is R k . Then, the state equation and observation equation are linearized to obtain the Jacobian matrix: The Extended Kalman Filter consists of a prediction step and an update step.

1.
The prediction step first predicts the state variablex k|k−1 at time k by the state variablê The prediction error covariance matrix P k, k−1 is then computed: 2.
In the update step, the state variablex k|k−1 is modified by Kalman gain K l , and the state variablex k at time k is then computed: where Kalman gain K l = P k, k−1 H T HP k, k−1 H T + R k −1 .
The state error covariance matrix P k is then computed: where Q k and R k are both Gaussian white noise with zero mean and independence. For the EKF observer, the appropriate system noise covariance matrix Q k and measurement noise covariance matrix R k need to be selected, and the algorithm starts with the initial values X 0 and P 0 . In this paper, we consider that Q k and R k are constants and that the setting of these parameters follows the method proposed by Schneider and Georgakis [31]. Q k and R k depend on the system and the sensors and should be determined by the user's system and sensors. After constant debugging, we obtain two values: Q k = diag[1, 1, 1] and R k = 1. The initial value of the state error covariance matrix is set as P 0 = diag [0.001, 0.001, 0.001].

Lower Level Controller Design
In order to validate the proposed control algorithm, a lower-level controller needs to be established to convert the output of the coupling controller into the input of the actuator of the simulated vehicle. Among them, the steering system's control input is the front wheel steering angle, and the control inputs of the drive system are the throttle opening and brake master cylinder pressure. A control strategy is proposed to convert the desired acceleration from the coupling controller into the control input of the drive system. The control strategy consists of two parts: the switching logic of the brake and drive and the calculation of the actuator control input. The control schematic of the drive/brake system is defined as shown in Figure 3. the calculation of the actuator control input. The control schematic of the drive/brake system is defined as shown in Figure 3.  To prevent the vehicle from controlling the throttle pedal and the brake pedal simultaneously, it is necessary to design a switching logic for the drive and brake. In this paper, the switching of the drive and brake can be determined by comparing to zero, and if ≥ 0, the drive control is applied, otherwise the brake control is applied. The switching logic can be expressed as follows: where is the acceleration in drive mode while is the acceleration in brake mode. A PI controller is applied to convert the desired acceleration into actuator control inputs, which are the desired throttle opening and the desired brake master cylinder pressure for the simulated vehicle platform. Considering the dynamic performance of the simulated vehicle, the desired throttle opening and brake master cylinder pressure are limited to a certain range. This paper restricts it as follows: where ℎ and are desired throttle opening and brake master cylinder pressure, respectively, and ℎ _ = 1 and _ = 6 are the maximum throttle opening and brake master cylinder pressure, respectively.

Simulation Verification
To verify the effectiveness of the proposed controller, a co-simulation was performed with CarSim and MATLAB/Simulink, and the simulation results are compared with a classic MPC controller. The CarSim software delivers an accurate and efficient method for simulating the performance of a real vehicle [32]. The details of the simulation are described in the following subsections.

Trajectory Tracking Algorithm Verification
This subsection describes the approach of generating the desired trajectory for the trajectory-tracking controller, and three defferent simulation scenarios are designed to verify the effectiveness of the proposed controller. To prevent the vehicle from controlling the throttle pedal and the brake pedal simultaneously, it is necessary to design a switching logic for the drive and brake. In this paper, the switching of the drive and brake can be determined by comparing a des to zero, and if a des ≥ 0, the drive control is applied, otherwise the brake control is applied. The switching logic can be expressed as follows: where a t is the acceleration in drive mode while P b is the acceleration in brake mode. A PI controller is applied to convert the desired acceleration into actuator control inputs, which are the desired throttle opening and the desired brake master cylinder pressure for the simulated vehicle platform. Considering the dynamic performance of the simulated vehicle, the desired throttle opening and brake master cylinder pressure are limited to a certain range. This paper restricts it as follows: where Thr and Pre are desired throttle opening and brake master cylinder pressure, respectively, and Thr_sat = 1 and Pre_sat = 6 are the maximum throttle opening and brake master cylinder pressure, respectively.

Simulation Verification
To verify the effectiveness of the proposed controller, a co-simulation was performed with CarSim and MATLAB/Simulink, and the simulation results are compared with a classic MPC controller. The CarSim software delivers an accurate and efficient method for simulating the performance of a real vehicle [32]. The details of the simulation are described in the following subsections.

Trajectory Tracking Algorithm Verification
This subsection describes the approach of generating the desired trajectory for the trajectory-tracking controller, and three defferent simulation scenarios are designed to verify the effectiveness of the proposed controller.

Desired Trajectory Setting
A quintic polynomial function can plan a trajectory that has a continuous third derivative and smooth curvature according to the initial and final states of the vehicle, so it is widely used in lane change scenarios [33]. As a result, this paper employs a time-based quintic polynomial function to generate the desired trajectory, and the effectiveness of the proposed coupling controller is verified by using a lane change scenario. The lane-change trajectory is shown in Figure 4. P 0 and P f present the initial position and the final position of the planning trajectory, respectively.

Desired Trajectory Setting
A quintic polynomial function can plan a trajectory that has a continuous third derivative and smooth curvature according to the initial and final states of the vehicle, so it is widely used in lane change scenarios [33]. As a result, this paper employs a time-based quintic polynomial function to generate the desired trajectory, and the effectiveness of the proposed coupling controller is verified by using a lane change scenario. The lane-change trajectory is shown in Figure 4. 0 and present the initial position and the final position of the planning trajectory, respectively. X Y P0 (x0,y0,v0,a0) Pf(xf,yf,vf,af) θr The quintic polynomial function can be described as: where 0 , 1 , ..., 5 , 0 , 1 , …, 5 are the designed parameters, which can be determined by the initial and final states of the vehicle. The heading angle θ r and the curvature k r for planning the trajectory can be computed by: The velocity and acceleration constraints for planning the trajectory can be computed according to:

Trajectory-tracking Simulation Scenarios Setting
To verify the effectiveness of the proposed controller, a co-simulation was established in CarSim and MATLAB/Simulink environment. The block diagram of the co-simulation is shown in Figure 5. Three simulation scenarios are set (low speed, medium-high speed, and high speed) in this section. The main parameters for the co-simulation are shown in Table 2. It should be pointed out that is the longitudinal movement distance in the process of lane change, is the lane width, the tire-road friction factor is 0.85, and the road environment is applied to all the simulation scenarios. In addition, the conventional MPC-based lateral and longitudinal coupling controller is provided for comparison. The quintic polynomial function can be described as: where a 0 , a 1 ,..., a 5 , b 0 , b 1 , . . . , b 5 are the designed parameters, which can be determined by the initial and final states of the vehicle. The heading angle θ r and the curvature k r for planning the trajectory can be computed by: The velocity and acceleration constraints for planning the trajectory can be computed according to:    a p (t) = .. ..

Trajectory-Tracking Simulation Scenarios Setting
To verify the effectiveness of the proposed controller, a co-simulation was established in CarSim and MATLAB/Simulink environment. The block diagram of the co-simulation is shown in Figure 5. Three simulation scenarios are set (low speed, medium-high speed, and high speed) in this section. The main parameters for the co-simulation are shown in Table 2. It should be pointed out that D is the longitudinal movement distance in the process of lane change, W is the lane width, the tire-road friction factor f is 0.85, and the road environment is applied to all the simulation scenarios. In addition, the conventional MPC-based lateral and longitudinal coupling controller is provided for comparison.
In the first scenario, the simulation vehicle enters the lane changing trajectory with an initial velocity of 30 km/h, reaches the end of the planning trajectory with a final velocity of 40 km/h, and continues to travel at the final velocity. The other two simulation scenarios are similar to the first one; the detailed information of the three scenarios is shown in Table 3. Scenarios A, B, and C represent the first scenario, the second scenario, and the third scenario, respectively.

Simulation Results and Analysis
The simulation results of the trajectory-tracking controller are displayed in Figures 6-8. Figures 6 and 7 show the estimated results of the yaw rate and longitudinal velocity, respectively, under three scenarios. Figures 6 and 7 show that the estimated value is very close to the real value; there is a relatively large error in the middle of the lane change, but all the estimated errors are within a reasonable range, which means that the EKF can estimate the yaw rate and longitudinal velocity accurately. Figure 8 shows the trajectory tracking results of the coupling controller proposed in this paper, and compares them with the desired trajectory. The vehicle can effectively track the desired trajectory at different gear speeds, as shown in Figure 8a. Moreover, the lateral tracking error of the vehicle is always kept within the range of 0.06 m, and even within 0.03 m in two of the specific scenarios as show in Figure 8b.   In the first scenario, the simulation vehicle enters the lane changing trajectory w an initial velocity of 30 km/h, reaches the end of the planning trajectory with a final ve ity of 40 km/h, and continues to travel at the final velocity. The other two simulation narios are similar to the first one; the detailed information of the three scenarios is sho in Table 3. Scenarios A, B, and C represent the first scenario, the second scenario, and third scenario, respectively.

. Simulation Results and Analysis
The simulation results of the trajectory-tracking controller are displayed in Figu 6-8. Figures 6 and 7 show the estimated results of the yaw rate and longitudinal velo respectively, under three scenarios. Figures 6 and 7 show that the estimated value is v close to the real value; there is a relatively large error in the middle of the lane change, all the estimated errors are within a reasonable range, which means that the EKF can mate the yaw rate and longitudinal velocity accurately. Figure 8 shows the trajec tracking results of the coupling controller proposed in this paper, and compares th with the desired trajectory. The vehicle can effectively track the desired trajectory at ferent gear speeds, as shown in Figure 8a. Moreover, the lateral tracking error of the hicle is always kept within the range of 0.06 m, and even within 0.03 m in two of specific scenarios as show in Figure 8b.

Comparision of Simulation Results
To further demonstrate the advantages of the proposed trajectory-tracking controller, Figures 9 and 10 also show the simulation results of the trajectory tracking for the proposed controller and MPC controller under the same vehicle parameters and simulation scenarios. As shown in Figure 9, both controllers can complete the single lane change condition and enter the target lane within the planning time. However, compared with the MPC controller, the lane changing trajectory of the proposed controller is closer to the desired trajectory. Moreover, as shown in Figure 10, the maximum trajectory tracking error of the MPC controller exceeds 0.15 m, and this error can be reduced by choosing the

Comparision of Simulation Results
To further demonstrate the advantages of the proposed trajectory-tracking controller, Figures 9 and 10 also show the simulation results of the trajectory tracking for the proposed controller and MPC controller under the same vehicle parameters and simulation scenarios. As shown in Figure 9, both controllers can complete the single lane change condition and enter the target lane within the planning time. However, compared with the MPC controller, the lane changing trajectory of the proposed controller is closer to the desired trajectory. Moreover, as shown in Figure 10, the maximum trajectory tracking error of the MPC controller exceeds 0.15 m, and this error can be reduced by choosing the

Comparision of Simulation Results
To further demonstrate the advantages of the proposed trajectory-tracking controller, Figures 9 and 10 also show the simulation results of the trajectory tracking for the proposed controller and MPC controller under the same vehicle parameters and simulation scenarios. As shown in Figure 9, both controllers can complete the single lane change condition and enter the target lane within the planning time. However, compared with the MPC controller, the lane changing trajectory of the proposed controller is closer to the desired trajectory. Moreover, as shown in Figure 10, the maximum trajectory tracking error of the MPC controller exceeds 0.15 m, and this error can be reduced by choosing the appropriate predictive time domain and control time domain. As shown in Figure 10a, it can be kept within 0.13 m, but it is still large compared with the proposed controller. Figure 11 compares the speed-tracking errors for the two controllers. As shown in Figure 11, the performance of the speed tracking for the two approaches is similar. appropriate predictive time domain and control time domain. As shown in Figure 10a, it can be kept within 0.13 m, but it is still large compared with the proposed controller. Figure 11 compares the speed-tracking errors for the two controllers. As shown in Figure 11, the performance of the speed tracking for the two approaches is similar.
(a) (b) (c)   To compare the trajectory-tracking accuracy of the two controllers, we provide an extensive comparison of the two controllers. The errors when reaching the end of trajectory planning are shown in Table 4. The maximum and average values of the lateral position errors are described as shown in Tables 5 and 6, respectively.  To compare the trajectory-tracking accuracy of the two controllers, we provide an extensive comparison of the two controllers. The errors when reaching the end of trajectory planning are shown in Table 4. The maximum and average values of the lateral position errors are described as shown in Tables 5 and 6, respectively. To compare the trajectory-tracking accuracy of the two controllers, we provide an extensive comparison of the two controllers. The errors when reaching the end of trajectory planning are shown in Table 4. The maximum and average values of the lateral position errors are described as shown in Tables 5 and 6, respectively. Additionally, the RMSE performance index between the actual lateral position of the simulated vehicle and the desired trajectory was computed to further compare the trajectory-tracking accuracy of the two controllers. The results are shown in Table 7. The RMSE is formulized as follows: where y i is the actual position of the simulated vehicle in CarSim at time i, and Y i is the desired position at the time i.

Conclusions
This paper presented a lateral and longitudinal coupling controller for a trajectorytracking control system while an observer based on the EKF was established to obtain the longitudinal velocity and yaw rate, which are sensitive to the trajectory-tracking control and hard to measure directly using the existing vehicle sensors. A coupling controller combining LQR and MPC was proposed to track the desired trajectory, and the controller ensured that the lateral position error and the speed tracking error were kept within a small range. The performance of the proposed controller has been verified in three different scenarios by co-simulation (CarSim-Matlab/Simulink). Finally, the simulation results were compared with the MPC controller, and the results illustrated that the proposed controller was superior to the MPC controller.
With respect to our future research, the next task is to consider the influence of axle load transfer on vehicle mass, that is, to develop an observer to estimate the vehicle mass, thereby attempting to address the problem of the mass changes in the front axle and rear axle caused by acceleration, which leads to the change in cornering stiffness.

Data Availability Statement:
The data presented in this study are available on request from the corresponding author.