MPC-based path tracking with PID speed control for autonomous vehicles

In this paper, a new coupled lateral and longitudinal controller based on model predictive control (MPC) framework was proposed for an autonomous vehicle to track the desired trajectory and speed. Considering the constraints of control input limit and state output admissible, we used a spatial-based 8 degrees of freedom (DOF) vehicle model as the prediction model and used a high-fidelity model, i.e., a 14-DOF vehicle model as the plant model in the formulation of MPC algorithm. For the lateral control, the MPC controller generates the optimal road-wheel steering angle; for the longitudinal control, the PID controller embedded in the optimization solution generates the total driving or braking wheel torque. All these control inputs were passed to the plant simultaneously. The developed vehicle models were simulated with step steering input and compared with the simulation result of CarSim vehicle model for validation. We implemented the proposed controller for path tracking and speed control with MATLAB considering an 8-shaped curved trajectory as the reference. The simulation results showed that the path tracking and speed tracking performance were good using the combined lateral and longitudinal control strategy.


Introduction
With the advancement in computer and sensor technology, autonomous vehicles which ensure reliable and safe navigation without driver control and continuous monitoring, have received worldwide attention and rapid development during last decades, not only in the research field, but also in the industrial, academic and military fields. Several competitions such as the DARPA (Defense Advanced Research Projects Agency) Challenges in the USA; the Korean Autonomous Vehicle Competitions (AVC) in Korea; the Intelligent Vehicle Future Challenge of China and many other contests have been held to advance the development of autonomous vehicles [1,2].
Driving control including lateral control and longitudinal control is one of the core issues in the research of autonomous vehicles. The lateral control aims to track the desired trajectory and heading angle, while the longitudinal control aims to track the desired speed. To this end, various controllers have been developed via classical control theory, modern control theory and robust control theory, etc. such as PID control [3,4], optimal control [5,6], robust backstepping and sliding mode control [7,8], etc. However, these control methods did not consider the actuator saturation and physical limit. Model predictive control, which combines prediction model, receding horizon optimization and feedback correction, has advantages to handle these issues due to its consideration of input constraints and state admissible [9]. Recent research shows that MPC algorithm is useful to control the dynamics of multiple vehicles considering safety constraints and the stability of these algorithms is also well studied [10]. An MPC-based path tracking controller considering the handling stability and IWMSME 2020 IOP Conf. Series: Materials Science and Engineering 892 (2020) 012034 IOP Publishing doi: 10.1088/1757-899X/892/1/012034 2 environmental constraints was proposed in [2] to address the complicated nonlinear constraints of sideslip and rollover in motion planning and path following for high-speed autonomous vehicles; A path following control scheme considering yaw and lateral stabilization for obstacle avoidance was proposed in [11] using combined steering and braking, in which a full tenth-order vehicle model and a simplified bicycle model were used respectively for nonlinear model predictive control (NMPC) formulation; An NMPC controller was introduced in [12] for path tracking with consideration of input limit and state output constraints by controlling the wheel steering, driving and braking and the NMPC algorithm was transformed to a linear model predictive control (LMPC) based on online linearization in order to reduce the computational burden. However, in the above studies, the lateral and longitudinal control were studied in a decoupled way. It is assumed that the longitudinal velocity was constant in the path tracking controller design. On the other hand, the coupling with the lateral dynamics was not taken into account when dealing with the longitudinal control [13]. The automotive vehicle can be treated as nonlinear system with varied parameters and strong couplings between the lateral and longitudinal dynamics. Actually, the vehicle speed is usually varied along the path according to the road information. For example, when the vehicle is entering a curve with small radius, it needs to reduce the speed to avoid large lateral acceleration to ensure safety; When the vehicle drives out of a curve and runs on a straight road, it needs to increase the speed to go through the road as fast as possible. Hence, a time varied speed should be considered in path tracking and a combination of the lateral and longitudinal control is quite necessary to improve the tracking performance due to the difficulty of handling the complicated traffic environment with separate controller [14].
We aim at designing a coupled lateral and longitudinal controller, it is therefore essential for us to identify the different coupling effects. The lateral and longitudinal dynamics coupling effects fall into three levels: kinetic coupling, tire force coupling and weight shift coupling. An example of kinetic coupling effect is that the lateral cornering force of front wheel has a component in the longitudinal direction; Tire force coupling effect can be described that given a coefficient of friction, the magnitude of the resultant of lateral and longitudinal forces on each tire is limited by a function of the direction of the resultant; Weight shift coupling effect can be shown that longitudinal acceleration affects the lateral dynamics by redistributing the tire normal forces, while lateral acceleration changes the weight distribution between the left and right tires [15,16]. In the literature, some approaches have been proposed to address the problem of controlling the lateral and longitudinal vehicle dynamics in a coupled way: for instance, a combined lateral and longitudinal controller based on sliding mode control theory was proposed in [17] to deal with these coupling effects; A flatness-based nonlinear controller was introduced in [18] for path tracking using combined lateral and longitudinal vehicle control; An integrated controller based on backstepping approach was presented in [19] for lane change and collision avoidance. In [20], a coupled longitudinal and lateral controller based on nonlinear backstepping theory and adaptive sliding mode control technique was proposed for automated driving control in emergency obstacle avoidance. In [21], an integrated control method including a spatial-based predictive control and geometric corridor planning was proposed for adaptive cruise control coupled with obstacle avoidance. Another integrated control scheme of adaptive cruise control with auto-steering was presented in [22] to design safe interaction between lateral and longitudinal controllers based on a proper logic-based control strategy. In [23], two coupled controllers were presented to tackle the challenge of lateral and longitudinal coupling effects: one was developed using Lyapunov control theory while the other one was based on Immersion and Invariance with sliding mode control. The model used in the controller design was four-wheel vehicle model using multi-body formalism based on Euler-Lagrange algorithm.
In this paper, in order to realize the coupled longitudinal and lateral control, we propose a novel MPC-based path tracking strategy with PID speed control for autonomous vehicles considering the constraints of input limit and output state admissible. The varied longitudinal velocity will be considered in the formulation of model predictive control algorithm. For the lateral control, we use MPC approach to generate the optimal road-wheel steering angle; for the longitudinal control, we use IWMSME 2020 IOP Conf. Series: Materials Science and Engineering 892 (2020) 012034 IOP Publishing doi:10.1088/1757-899X/892/1/012034 3 PID controller embedded in the solution to generate the total acceleration or braking wheel torque. All the control inputs are passed to the plant simultaneously to track the reference path and desired speed. We use a double-track spatial-based 8-DOF vehicle model considering roll dynamics as the prediction model and a higher fidelity model, i.e., a 14-DOF vehicle model as the plant, since an accurate vehicle model is essential for the development of automotive control system. Moreover, to investigate the performance of the proposed controller in curved road situation, we consider an 8-shaped curved path as the reference.
The remainder of this paper is organized as follows: In Section 2, we described the tire model, an 8-DOF vehicle model, a 14-DOF vehicle model and a spatial-dependent vehicle model; In Section 3, we specified the MPC algorithm for path tracking and the PID controller for speed tracking; In Section 4, we first compared the simulation results of step steer response between the developed vehicle models and the CarSim vehicle model for validation; Then, we implemented the proposed controller for combined path tracking and speed control considering the references path to be an 8-shaped curve trajectory; In Section 5, we summarized the paper and outlined the future work.

Models for control synthesis
Since models are central to the formulation of model predictive control algorithm, we firstly present the development of vehicle dynamics models in this section including a linear tire model, an 8-DOF vehicle model, a 14-DOF vehicle model and a spatial-dependent vehicle model.

Tire model
Except for aerodynamics forces and gravity, all of the forces which affect vehicle behavior are provided by the tires. Because tire forces produce primary external influence and they have highly nonlinear performance, it is essential to use a realistic tire model, especially when investigating large control inputs that results in response near the limits of the linear character scale of the tire. The tire lateral and longitudinal forces are assumed to depend on normal force, slip angle, surface friction, and slip ratio. However, when the slip ratio and slip angle are limited within small values, the tire model can be simplified and generate linearized lateral force and longitudinal force [11,24].
Under this assumption, the tire lateral force can be given as , , where is the tire slip angle, is tire cornering stiffness related to tire normal force and road-tire friction coefficient ; The tire longitudinal force can be given as , , , where , is the slip ratio of front tire or rear tire, is the tire longitudinal stiffness which also related to the tire normal force and road-tire friction coefficient.

8-DOF vehicle model
When studying vehicle handling performance in the situations that do not involve large longitudinal accelerations, we usually used an 8-DOF vehicle model as a simplified lower order model to consider the vehicle roll dynamics. The 8-DOF model considers longitudinal, lateral, yaw and roll dynamics for the chassis and each wheel rotational dynamics without the pitch and heave motions [25]. The 8-DOF full vehicle model is shown in Figure 1.
The equation of chassis for the 8-DOF vehicle model according to Newton's laws are given as: In these equations, the longitudinal force at the tire contact patch was denoted by , the lateral force at the tire contact patch was denoted by , and the subscript 'ij' denotes left front (lf), right front (rf), left rear (lr), and right rear (rr), respectively. The total mass of the vehicle is denoted by , the distance of vehicle center of mass (C.M.) from front axle by , the distance of the vehicle C.M. from rear axle by , the forward velocity, lateral velocity, and the vertical velocity of the vehicle C.M. by , and w, the roll angle by , the roll inertial by , the yaw inertial by , the product of roll and yaw inertial by , the front and rear roll center distance below sprung mass C.M. by and , the front/rear unsprung mass by and , the front/rear track width by and . The front and rear suspensions are represented simply by their respective equivalent roll stiffness ( / ) and roll damping coefficients ( / ). We should notice that the roll DOF equation, i.e., Eq.(4), is given by considering moments about the vehicle roll center , rather than the sprung mass C.M.

14-DOF vehicle model
Considering the suspension DOF at each corner, a 14-DOF vehicle model is capable of modeling the vehicle pitch and heave motions, besides the same benefit of an 8-DOF vehicle model. A 14-DOF vehicle model is also suitable to consider nonlinear spring and damper and vehicle behavior response to normal tire forces. All these advantages make the 14-DOF model better represent the coupling effects of vehicle lateral, longitudinal, roll and yaw motion, particularly during extreme maneuvers. Furthermore, the 14-DOF model is capable of predicting vehicle response even after wheel lift-off and it is therefore suitable to be used in rollover prediction/prevention strategies [25]. The schematic of the 14-DOF model investigating the vehicle roll behavior is presented in Figure 2. The 14-DOF model includes 6 DOF at the vehicle C.M. and 2 DOF at each wheel. The body-fixed coordinates are attached at the vehicle C.M. and aligned in coordinate frame 1, i.e., the principal directions. The forward velocity, lateral velocity, and the vertical velocity of the sprung mass are indicated by , , w, respectively. The pitch angle, yaw angle and roll angle of the sprung mass are IWMSME 2020 IOP Conf. Series: Materials Science and Engineering 892 (2020) 012034 IOP Publishing doi:10.1088/1757-899X/892/1/012034 5 denoted by , , . The roll angular velocity, pitch angular velocity and yaw angular velocity are denoted by , and . The moments transmitted to the sprung mass along the , and directions are denoted by , and and the subscript 'ij' denotes left front (lf), right front (rf), left rear (lr), and right rear (rr), respectively [25].
According to Newton's laws, the equation of motion for the 6-DOF sprung mass of the 14-DOF model can now be written as [25], where is the sprung mass and the cardan angles , , can be obtained by integrating the following equations [25], .
More details about the development on 14-DOF vehicle model is specified in [25].

Spatial-dependent vehicle model
In this study, in order to know the vehicle position explicitly at each sampling of the optimization routine and retain the capability of the solver to take into account the varied longitudinal velocity, we considered transforming a time-dependent model into a spatial-dependent model [26]. To this end, we introduce a spatial bicycle model to specify the transformation approach from time-dependent to spatial-dependent. Figure 3 illustrates the curvilinear coordinate system used in the spatial bicycle model as well as the states of the vehicle model. The coordinate defines the arc-length along the track. The states of  The following kinematic equations can be derived from Figure 3 is the projected vehicle speed along direction of the path, and are the radius of curvature and the heading angle of the reference path.
is the time derivative of . Then the vehicle's velocity along the path is written as [26] ⋅ ⋅ ⋅ .
where is the projected vehicle position along the arc length of the path. We obtain the derivative of with respect to using simple relationships in the new curvilinear coordinate system and the fact that ⋅ . Then the spatial-dependent vehicle model is formulated as [26]: , .
(18) In this paper, based on the above transformation, a spatial-dependent 8-DOF vehicle model will be employed as the prediction model in the formulation of MPC algorithm.

Coupled lateral and longitudinal controller design
Model predictive control aims to use a dynamic model to forecast system behavior, and optimize the control move at current time to bring the predicted output as close as possible to the given function. The computational burden will be increased with the increasement of system complexity. Compared with NMPC, we therefore utilize LMPC to design the coupled controller due to its advantages of simpler calculation and real-time performance.
In order to track the desired path and speed, we proposed an MPC-based path tracking with PID speed control to realize the coupled lateral and longitudinal control by a combined use of steering, accelerating and braking. In every simulation step, the longitudinal velocity in the prediction model is updated with measured value and the control input sequence is calculated by LMPC controller. Then, the driving or braking wheel torque is computed by PID controller using speed error between the plant and the reference. The optimal wheel steering angle and total wheel torque are passed to the plant simultaneously to track the desired path and speed. The schematic of the MPC-based path tracking with PID speed control is illustrated in Figure 4.

Lateral dynamics control
We use LMPC approach to implement the vehicle lateral control and a linearized 8-DOF vehicle model as the prediction model in the formulation of model predictive control algorithm.
The equations around the operating point is given as: , .
(20) Using the Taylor series expansion at the operating point and ignoring higher order terms, we can obtain [9] , , , Subtracting Eq.(20) from Eq.(21) results in , In order to apply this model to the MPC controller, we describe the equation in the form of discretized state-space representation [9]: where 0 , , 0 , 0 (m is the dimension of control input, n is the dimension of state variable, and p is the dimension of output). The predicted state output over the prediction horizon in a compact matrix form is given as ,

Cost function definition.
The ability of the cost function is to make the autonomous vehicle track the desired path rapidly and smoothly. Therefore, the system status deviation and the optimization of the control output should be combined into the controller.
Considering the soft constraints concept [27], the objective function of the path tracking controller can be given as [9]: Considering Eq.(25), the objective function can be given as [9] .
(27) To solve the following optimization problem, the objective function is converted into a standard quadratic form [9]. , 1 , , , , , where 0 0 , 2 0 , and is the tracking error in the predictive horizon .

Constraint analysis.
In a real physical system, the control input and state output are bounded with actuator saturation and physical limitations. The constrains imposed on the control sequence and the state outputs can be described as follows [9]: 1. The constraints imposed on control input are given as , 0,1, ⋯ , 1 (29) 2. The constraints imposed on control increments are given as , 0,1, ⋯ , 1 (30) 3. The constraints imposed on the output are given as , 0,1, ⋯ , 1 (31) Since the variables to be solved are control increments in the control horizon, the constraints should be written in the form of control increment or the form of control increment multiplied by the transformation matrix. Thus, the constraints listed above should be converted to obtain the transformation matrix, which is described as [9]: where 1 is the column vector of ones, is the identity matrix with a dimension of m, ⊗ is Kronecker product, and 1 is the previous control input [9]. Combining Eq.(32) through (34) can be converted into [9] , (35) where and are the lower and upper bound of the control input, respectively. Combining Eq. (25) and Eq.(31), the output constraints can be given as: , (36) After obtaining the solution of optimization for cost function, the control increments sequence in the control horizon can be given as [9] * * , * , … , * .
(37) The first element of the sequences is taken as the actual control input increment, thus the control input signal is given as:

Longitudinal dynamics control.
The wheel rotational dynamics of front right and rear right wheel in the plant are given as [28], (39) , (40) where the and is driving torque and braking torque applied to the front right wheel. is braking torque applied to the rear right wheel. For simplicity, we assume the driving torque is divided equally to front two wheels and the braking torque is divided equally to the front and rear four wheels.
The longitudinal velocity and acceleration of the plant model can be obtained by considering a simplified one-wheel vehicle model and the longitudinal motion equation for the chassis of 14-DOF vehicle model.
As it is known to us, most industrial controllers in use today are PID controllers or modified PID controllers. The usefulness of PID controls lies in their general applicability to most control systems. Herein, we implement the longitudinal controller using the PID controller.
The speed tracking error is defined as where refers to the desired speed, refers to the speed of the plant; refers to the desired longitudinal acceleration, refers to the longitudinal acceleration of the plant. is the sample time and when using the arc length dependent model, / . In the PID controller, when the speed of the plant vehicle is smaller than the desired speed, then , 0; when the speed of the plant is greater than the desired speed,

Simulation results
In this part, we firstly validated the developed vehicle models by comparing the output responses with CarSim vehicle model using step steering input. Then, we implement the proposed controller for combined lateral and longitudinal control considering the references path to be an 8-shaped curve trajectory.

Vehicle model validation
It is important to use a precise and accurate model for the development of a control system. Thus, the developed vehicle models should be validated before implementing the path tracking control and speed tracking control. CarSim is a software with plenty of high-fidelity vehicle models that has been validated with experimental results on an actual vehicle. Herein, we compare the simulation result of steering response with that of CarSim vehicle model to investigate the validation. Assuming the longitudinal velocity is constant in the simulation, we apply the same road-wheel steering angle as input signal to the 8-DOF model, the 14-DOF model and the CarSim model to implement the step steering simulation.
Step steering simulation: the amplitude of step steering angle is 0.0087(rad), i.e., 0.5(deg) and the vehicle longitudinal velocity is constant at 33.73(m/s). The output responses of the vehicle models including roll angle, yaw rate, lateral velocity and lateral acceleration are presented in Figure 5 and

Tracking reference definition
We employ an 8-shaped curved reference trajectory with time-varied speed to investigate the path tracking and speed tracking performance in the situation of curved road. The reference path is an 8shaped curved path with a straight line. The vehicle first drives on a straight line with the speed increased from 0.5 / to 10 / , and then drives along the 8-shaped curved path with a radius of 30 and a constant speed. The reference trajectory and the reference speed profile with respect to the arc length are defined in Figure 7:  (2) Compared with the reference trajectory and speed profile, the path tracking performance and the speed tracking performance of the proposed coupled controller for lateral and longitudinal control are illustrated in Figure 9: (3) The control input of front wheel steering angle and total driving/braking wheel torque are illustrated in Figure 10:  (4) Numeral tracking errors of the vehicle state variables are summarized and given in Table 1: In this section, we implement the proposed MPC-based path tracking with PID speed control to address the coupled lateral and longitudinal control problem, in which the 8-DOF vehicle model is used as the prediction model, and the high-fidelity model, i.e., the 14-DOF vehicle model is used as the plant model. In order to consider the time-varied speed in the path tracking, a PID speed controller is embedded in the model predictive control framework. Based on the simulation results, the proposed IWMSME 2020 IOP Conf. Series: Materials Science and Engineering 892 (2020) 012034 IOP Publishing doi:10.1088/1757-899X/892/1/012034 14 controller shows good tracking performance of following the desired path and the desired speed by the use of combined lateral and longitudinal control. Considering the 8-shaped trajectory with a radius of 30m, the tracking errors between the plant and the objectives for the heading angle ' ', yaw rate ' ', lateral position ' ', longitudinal position ' ' and the vehicle speed ' ' are relatively small, which are shown in Figure 8, Figure 9 and Table 1.

Conclusions and future work
In the literature of path tracking, numerous researches implemented the lateral guidance of autonomous vehicles with the assumption of constant speed, so that the lateral and longitudinal control problem can be investigated in a decoupled way. However, the automotive vehicle is nonlinear system with strong couplings between the longitudinal and lateral motion, moreover, the speed is usually varied along the trajectory. Thus, in this paper, we proposed a novel MPC-based path tracking with PID speed control to deal with the coupled lateral and longitudinal vehicle dynamics. We used an 8-DOF vehicle model as the prediction model and used a high-fidelity model, i.e., a 14-DOF vehicle model to approximate the plant. In order to know explicitly the vehicle position at each sampling instant of an optimization and maintain the freedom of the solver to time-varied speed, we transformed the time-dependent model to spatial-dependent model to formulate the LMPC algorithm. The path tracking controller generated the optimal road-wheel steering angle and the PID speed controller embedded in the solution generated the total accelerating or braking wheel torque. All these control input signals were passed to the plant model simultaneously to realize the coupled control of path tracking and speed tracking. Furthermore, we considered an 8-shaped curved path as the reference path to investigate the tracking performance of the proposed controller in curved road situation. Based on the simulation results, we report relatively small tracking errors between the plant and the reference objectives for the heading angle ' ', yaw rate ' ', lateral position ' ' and longitudinal position ' ' of vehicle C.M. The proposed coupled controller presents good tracking performance of following the reference path and speed.
In the future work, we will consider the reference path to be an arbitrary curve of continuously varying curvature and implement the combined lateral and longitudinal control using the coupled controller. Furthermore, we will consider generating a reference speed profile for optimal time travel along the predefined trajectory.