Efﬁcient Nonlinear Model Predictive Control of Automated Vehicles †

: In this paper, an efﬁcient model predictive control (MPC) of velocity tracking of automated vehicles is proposed, in which a reference signal is given a priori. Five degree-of-freedom vehicle dynamics with nonlinear tires is chosen as the prediction model, in which coupling characteristics of longitudinal and lateral dynamics are taken into account. In order to balance computational burden and prediction accuracy, Koopman operator theory is adopted to transform the nonlinear model into a global linear model. Then, the global linear model is used in the design of MPC to reduce online computational burden and avoid solving nonconvex/nonlinear optimization problems. Furthermore, the effectiveness of Koopman operator in vehicle dynamics control is veriﬁed using a Matlab/Simulink environment. Validation results demonstrate that dynamic mode decomposition with control (DMDc) and extended dynamic mode decomposition (EDMD) algorithms are more accurate in model validation and dynamic prediction than local linearization, and DMDc algorithm has less computational burden on solving optimization problems than the EDMD algorithm.


Introduction
In recent decades, automated vehicles have developed rapidly, which are of great significance in improving driving comfort [1] and energy efficiency [2]. The automated driving system mainly includes positioning, perception, planning and decision-making, and control modules [3][4][5][6]. The planning and decision-making module decide the reference trajectory or reference velocity based on the information obtained by the sensing module and the state of the vehicle. The control module can track the reference trajectory or velocity by regulating the accelerator, brake, and steering wheel of vehicles.
Vehicle velocity tracking [7] is an important automated driving control, which can be considered as a simplified adaptive cruise control (ACC). Velocity tracking usually adopts the strategy of hierarchical control, where an upper decision-making layer calculates the reference velocity profile based on environment information. However, the sensor noise and the interaction of surrounding vehicles have great influence on decision-making. Therefore, a new distributed mean-field-type filter is proposed in [8] to handle noises, partial-observed and high-dimensional data, which improves the accuracy of vehicle tracking. Not only is a smoothing algorithm of the measured data proposed in [9] to reduce the impact of road in [38] based on the EDMD algorithm. A linear MPC, named Koopman MPC, is proposed in [39] to accurately track the reference velocity. However, it fails while the yaw rate is changing. A three-state single-track model of vehicles with linear tires is considered in [40]. While the nonlinear characteristic of tires is obvious, i.e., in the scenario of a large steering angle or small road adhesion coefficient, good velocity tracking performance cannot be achieved by the proposed MPC scheme. A Koopman MPC is proposed in [41] for velocity tracking of vehicles, in which Koopman operators are represented by a deep neural network. The strategy has a large error in tracking the reference velocity, which might result in the deviation of the driving state of vehicles and the handling stability problems, in particular, when the curvature of the road is constantly changing.
In this paper, an automated driving model predictive control of vehicles is proposed, in which vehicle dynamics and nonlinearity of tires are approximated by an identified Koopman linear model. Furthermore, a linear MPC is designed to guarantee accurate and implementable velocity tracking. Note that, different from [42], tire nonlinearity is considered when establishing vehicle dynamics model, and the DMDc algorithm is compared with the EDMD algorithm in model accuracy and online computational burden.
The main contributions of this paper are as follows: • Automated vehicle control is reduced to a reference velocity tracking problem, in which the reference signal is provided by the module of perception and planner. In order to reflect full operating conditions, nonlinear vehicle dynamics and nonlinear tires which represent the coupling characteristics of longitudinal and lateral dynamics are considered. • Koopman operator theory is adopted to transform the nonlinear vehicle model and tire into a global linear model. Thus, the trade-off between prediction accuracy and online computational burden is achieved.
This paper is organized as follows: Vehicle dynamics is introduced in Section 2. MPC considering nonlinear vehicle dynamics and nonlinear tires is introduced in Section 3. Linear MPC based on the Koopman operator is discussed in Section 4. Section 5 presents simulation results. The paper is concluded in Section 6.

Problem Setup
Velocity tracking is an important part of classical cruise control. As shown in Figure 1, the goal of velocity tracking is to track the reference velocity profile accurately in real time by regulating the driving, braking, and steering of vehicles, when the reference vehicle velocity is determined by the planning and decision-making module. In this section, vehicle dynamics considering tire nonlinearity and the objective of velocity tracking control are introduced.

Vehicle Dynamics
The longitudinal, lateral, and yaw motion of vehicles are considered in this paper. Suppose that the vehicle is front-wheel steering and four-wheel driving, and vehicle dynamics considering the lateral and longitudinal coupling shown in Figure 2 is established. According to Newton's Second Law, the vehicle longitudinal, lateral and yaw dynamics can be expressed as: where v x , v y , and ω are the longitudinal velocity, lateral velocity, and yaw rate of the vehicle, respectively, m is the mass of vehicles, δ is the front steering angle, F l f and F lr are the longitudinal forces of front and rear wheel, F s f and F sr are the lateral forces of front and rear wheel, l f and l r are distances of wheels from the center of gravity, and I z is the moment of inertia around yaw axis.
The dynamics of wheels is shown in Figure 3, which can be described accordingly by where ω f and ω r are angular velocity of front and rear tires, R e is the wheel rolling radius, J is the moment of inertia of wheel, and T f and T r are the total torque of front and rear wheel. In this paper, the single track model of vehicle is adopted. The torque distribution mode is as follows: the front and rear axles are distributed equally in driving and braking condition. Define T as the total torque of the vehicle. Then, the torque T f and T r can be expressed as A five-degree-of-freedom (5DOF) model of vehicles is obtained by combining (1) Denote the state of the system as x = v x , v y , ω, ω f , ω r T , the control input as u = [δ, T] T , and the output as y = v x , v y , ω T . Thus, the vehicle dynamics (4) can be rewritten as where x ∈ R 5 , u ∈ R 2 , C = [I 3×3 , 0 3×2 ], I is an identity matrix, and f : R 5 × R 2 → R 3 represents the related nonlinear mapping. Furthermore, the discrete counterpart of vehicle dynamics (5) is provided that the sampling time is T s .

Nonlinear Tire Model
Tires are the key components of vehicles which carry all the loads and influence handling stability. Under different vertical forces F n , the relationship between slip ratio and longitudinal force is shown in Figure 4, and the relationship between slip angle and lateral force is shown in Figure 5. It can be seen from Figure 5 that, when the slip angle is small, the slip angle and lateral force is in an approximate linear relationship; as the slip angle increases, lateral force is saturated. That is, the small-angle approximation method [43] is no longer applicable while vehicles are fast steering at a high speed. In order to accurately reflect the nonlinear properties of tires, the longitudinal and lateral tire force is calculated by the following magic formula [44]: where B l , C l , D l , E l and B s , C s , D s , E s are magic formula parameters of the longitudinal tire force F l and lateral tire force F s , respectively, α f and α r are the front and rear tire slip angles, and k f and k r are the front and rear tire slip ratios. The tire slip angle and slip ratio can be calculated by where v w f x , v w f y , v w rx , and v w ry are the longitudinal and lateral velocity of the front and rear wheels under the tire coordinate system.
Wheel velocities v w f x , v w f y , v w rx and v w ry can be expressed as: where v f x , v f y , v rx and v ry are the longitudinal and lateral velocity of the front and rear wheels under the vehicle coordinate system, and where l f (l r ) is distance from the front (rear) axle to mass center of the vehicle.

Control Objective
The control objective of velocity tracking is to accurately track the reference velocity profile, that is, to ensure that vehicles drive at the reference velocity, i.e., where v re f and ω re f (k) are the reference longitudinal velocity, lateral velocity, and yaw rate, respectively.

NMPC of Automated Vehicles
In this section, a nonlinear model predictive controller is proposed to solve the velocity tracking problem. Suppose that the reference velocity is In order to track the time-varying reference velocity signal, the following optimization problem will be solved at each time instant.
where x p (k + i|k) and y p (k + i|k) are the predicted state and predicted output, N is the prediction horizon, r(k) = [r(k|k), r(k + 1|k), . . . , r(k + N − 1|k)] is the sequence of the reference signal, ] is the sequence of the control input, x p min / max and u min/max are constraints of the system state and control input. In order to track the reference velocity profile while guaranteeing that the control action is as small as possible, both the tracking error of the prediction state to the reference signal and the control input are included in the cost function. That is, the cost function is chosen as: where Q and R are positive semi-definite weighting matrices.
) as the optimal control sequence and the related optimal cost function. The first element of U * (k), i.e., u * (k|k) will be applied to vehicles.
The optimal control input of NMPC summarized in Algorithm 1 will be obtained through the solution of Problem 1 at each time instant, in which the optimal cost function is achieved as well.Furthermore, the proposed NMPC scheme can attenuate model-plant mismatches and external disturbances since the optimization problem is solved in a receding horizon manner. However, as the involved optimization problem is nonlinear and nonconvex, the proposed NMPC might suffer problems of heavy computational burden and local minima, which will seriously deteriorate the obtained performance.

Algorithm 1 NMPC of automated vehicles
Input: The prediction horizon N, weighting matrices Q, R, and initial value of the state x(k) Output: Optimal control input u * (k|k) 1: for k = 0, 1, . . . do 2: Obtain the current state x(k), and the reference signal r(k) 3: Solve Problem 1 to obtain U * (k) 4: Apply u * (k|k) to the vehicle 5: end for

Koopman-Based MPC of Automated Vehicles
The vehicle dynamics emerges nonlinear and strong coupling characteristics. In this section, Koopman linear models are constructed based on DMDc and EDMD algorithms, respectively. The identified Koopman linear model is used to design a model predictive controller, where the involved optimization problem is a quadratic programming problem. Note that, in order to obtain the approximated linear model, vehicle dynamics (6) is treated as a "transparent box" here.
The core idea of the Koopman operator theory is to express the evolution of nonlinear dynamical systems through an infinite-dimensional linear operator [28]. Define κ as the infinite-dimensional Koopman operator acting on the observation function ϕ. Under the action of κ, the nonlinear evolution of vehicles can be transformed into a linear evolution: The infinite-dimensional Koopman operator κ is difficult to realize in practice. When the Koopman operator is applied to a real controlled system, a finite-dimensional approximation of κ is often carried out. In this paper, the DMDc algorithm and the EDMD algorithm, two common methods to approximate the Koopman operator in finite dimensions, are applied to construct the global linear model of vehicles.

DMDc-MPC
The traditional DMD algorithm is only suitable for describing autonomous systems. Instead, the DMDc algorithm [31] extends the traditional DMD algorithm to systems under control. Construction of a Koopman linear model and a linear MPC based on the DMDc algorithm will be introduced in this subsection.
Based on the DMDc algorithm, the vehicle dynamics (6) can be approximated by a discrete linear model: wherex(k) ∈ R 5 andŷ DMD (k) ∈ R 3 are the state and output of the constructed linear model, respectively, the matrix C DMD is set to [I 3×3 , 0 3×2 ], and the matrices A DMD ∈ R 5×5 and B DMD ∈ R 5×2 are the parametric matrices that need to be identified. In order to identify A DMD and B DMD , collect state and control input data of the vehicle dynamics (6), and construct the following data matrices: where X 1 ∈ R 5×k max , X 2 ∈ R 5×k max , Y ∈ R 3×k max , U ∈ R 2×k max , and k max is the number of snapshots.
According to the Koopman linear model (17), the constructed data matrices can be expressed as: where G = A DMD B DMD is a finite-dimensional approximate matrix of the Koopman operator, and Ω = X 1 U T is a reconstructed augmented data matrix.
Perform singular value decomposition (SVD) on matrix Ω. Denote the truncated rank of SVD as p. In this paper, set p = 5. The matrix Ω can be expressed as whereŨ ∈ R (5+2)×5 is the unitary matrix, andΣ ∈ R 5×5 is a diagonal matrix.
To obtain the matrices A DMD and B DMD from the matrix G, decompose the matrixŨ as:Ũ Thus, the matrices A DMD and B DMD in the Koopman linear model can be expressed as: The finite-dimensional approximation of the Koopman operator using the DMDc algorithm is summarized in Algorithm 2.
Based on the Koopman linear model (17), a linear model predictive controller, i.e., DMDc-MPC, is designed. The optimization problem of DMDc-MPC is as follows: wherex(k + i|k) andŷ DMD (k + i|k) are the predicted state and predicted output based on the Koopman linear model (17), respectively, N is the prediction horizon,x DMD min / max , and u DMD min / max are constraints of the system state and control input, and U(k) is the sequence of the control input. The cost function can be expressed as: where Q DMD and R DMD are positive semi-definite weighting matrices.
The first element u * (k|k) of the optimal control sequence U * (k) will be applied to the vehicle to implement velocities' tracking control.
The DMDc-MPC algorithm based on Koopman operator is summarized in Algorithm 3, and the related control diagram is shown in Figure 6.

Algorithm 3 DMDc-MPC based on Koopman operator
Input: The prediction horizon N, weighting matrices Q DMD , R DMD , parameter matrices A DMD , B DMD , C DMD , and initial value of the system state x(k) Output: optimal control input u * (k|k) 1: for k = 0, 1, . . . do 2: Obtain the current state x(k), and the reference signal r(k) 3: Solve Problem 2, and obtain U * (k) 4: Apply u * (k|k) to the vehicle system 5: end for

EDMD-MPC
The EDMD algorithm [36] provides another idea for extending the traditional Koopman operator to the controlled system. The Koopman operator of the controlled system is defined as the Koopman operator of the autonomous system that evolves on a lifted state space.
Denote control to go as u(k) := u(i) ∞ i=k . Set the lifted state space as the combination of the current system state and control to go, i.e., χ(k) = [x(k), u(k)] T . The evolution of the lifted state χ(k) is where F(·) represents the nonlinear mapping from χ(k) to χ(k + 1), and S is the left shift operator for updating the control sequence, i.e., u(k + 1) = Su(k).
Define κ as the Koopman operator acting on the observation function ϕ (χ(k)), i.e., The approximation of the Koopman operator κ of the vehicle dynamics (6) is obtained by solving the least squares optimization problem: T is the vector of selected lifting functions, A EDMD ∈ R N ψ ×N ψ and B EDMD ∈ R N ψ ×2 are the coefficient matrices to be calculated, and N ψ is the number of lifting functions with N ψ 5. After solving the optimization problem (28), the discrete linear model of vehicles can be constructed as: where z(k) ∈ R N ψ andŷ EDMD (k) ∈ R 3 are the lifted state and output, respectively. The state z(k) = ψ(x(k)) := ψ 1 (x(k)), ψ 2 (x(k)), . . . , ψ N ψ (x(k)) T . The matrix C EDMD ∈ R 3×N ψ in (29) can be obtained by solving the following least squares optimization problem: A finite-dimensional approximation of the Koopman operator using the EDMD algorithm is summarized in Algorithm 4.

Ensure:
The matrices A EDMD , B EDMD , C EDMD 1: Construct the data matrices (18); 2: Lift the dimension of X 1 and X 2 with ψ to obtain X 1,li f t and X 2,li f t ; 3: Solve the optimization problem (28) and (30) to obtain A EDMD , B EDMD , C EDMD .
Based on the Koopman linear model (29), a linear model predictive controller, i.e., EDMD-MPC, is designed. The optimization problem of EDMD-MPC is as follows: ,v y (k + i|k),ω(k + i|k)] T are the predicted state and output based on the Koopman linear model (29), respectively, N is the prediction horizon, U(k) is the sequence of the control input,ŷ min / max and u EDMD min / max are constraints, and the cost function can be expressed as: where Q EDMD and R EDMD are positive semi-definite weighting matrices.
The EDMD-MPC algorithm based on Koopman operator is summarized in Algorithm 5, and the control diagram is shown in Figure 7.

Algorithm 5 EDMD-MPC based on Koopman operator
Input: The prediction horizon N, weight matrices Q EDMD , R EDMD , parameter matrices A EDMD , B EDMD , C EDMD , and the initial value of the state x(k) Output: optimal control input u * (k|k) 1: for k = 0, 1, . . . do 2: Obtain the current state x(k), and the reference signal r(k) 3: Lift the dimension of the state x(k) and obtain z(k|k) 4: Solve Problem 3, and obtain U * (k) 5: Apply u * (k|k) to the vehicle system 6: end for 1 ,

Remark 1.
Since it is difficult to directly collect data of vehicles to reflect the required dynamic characteristics due to safety consideration, the vehicle dynamics (6) is used as "data generator". Thus, the proposed scheme is a hybrid mechanistic-data driven approach which can balance the computational burden of MPC and accuracy of prediction.

Simulation Results
In order to verify the effectiveness of the proposed scheme, simulation experiments are carried out in the Matlab R2016b environment. The Koopman linear model which can approximate vehicle dynamics is identified with the DMDc/EDMD algorithm, respectively.
The effectiveness of the proposed model predictive controller with the approximated global linear model is verified under different driving scenarios.
The parameters of the vehicle model are shown in Table 1, and the parameters of the magic formula of tires are shown in Table 2.

Data Collection and Model Identification
In principle, Koopman operator theory is data-based, i.e., only input-output data are needed. However, vehicle dynamics (4) is used to 'produce' data in this paper. That is, the proposed scheme is a kind of mixture of data-mechanism. In other words, measurement noise of sensors can be avoided indeed.
Set the sampling period T s to 10 ms, and discretize (5) using the Runge-Kutta method to obtain vehicle dynamics (6).
Select 1000 trajectories with the time duration of 2 s to form a dataset. In order to obtain data that can better reflect the dynamic characteristics of vehicles, 1000 trajectories in the dataset are divided equally to form a straight driving subdataset and a curve driving subdataset, respectively. The settings of the two subdatasets are as follows: • Straight driving subdataset: The initial values of longitudinal velocity v x , lateral velocity v y , and yaw rate ω are randomly selected in [1,30]  When using the EDMD algorithm to identify the Koopman linear model, a lifting function should be selected first. The lifting functions ψ i are chosen to be itself (i.e., ψ 1 = v x , ψ 2 = v y , ψ 3 = ω, ψ 4 = ω f , ψ 5 = ω r ) and 100 Gaussian radial basis functions, so the dimension of the lifted state of the discrete linear model is 105. The expression of the Gaussian radial basis function is where θ center is the randomly selected center value, and σ is the kernel width.

Remark 2.
While constructing the dataset, an approximate expression of the initial angular velocity of the front and rear tires is adopted, i.e., v x /R e . Thus, initial values of ω f and ω r are determined by the initial value of longitudinal velocity v x and tire radius R e , that is, initial values of ω f and ω r are within [1/R e , 30/R e ]rad/s.

Model Validation
Here, simulation experiments are carried out to verify the effectiveness of the identified Koopman linear model through comparison of states of the actual vehicle system and the linear system approximated by the Koopman operator. Two scenarios are set as follows: • Scenario 1 (longitudinal motion): The initial state of the vehicle system is set to [25, 0, 0, 25/R e , 25/R e ] T , the torque T is set to 600 Nm, and the front steering angle δ is set to 0. • Scenario 2 (lateral and longitudinal coupling motion):The initial state of the vehicle system is set to [15, 1, −0.45, 15/R e , 15/R e ] T , the torque T is set to −400 Nm, and the front steering angle is δ = 0.15 cos(5t). Precision refers to how close the model's predictions are to the observed values. The more precise the model, the closer the data point to the observed value. In order to test predicted precision, the Root Mean Square Error (RMSE ) is used as an objective evaluation index, i.e., where x true (k) and x pred (k) are the actual state of vehicle system and the state of the identified Koopman linear model at time instant k, respectively. Under the two scenarios, the evolutions of local linearization and the identified Koopman linear models constructed by DMDc and EDMD algorithms are shown in Figures 8 and 9. Accordingly, the values of RMSE of deviations between the real states and the predicted states with different methods are shown in Tables 3 and 4, respectively. Table 3. RMSEs of deviations between the real states and the predicted states with different methods (Scenario 1).
Step  In Scenario 1, the front steering angle is set to 0, i.e., the coupling characteristics are weak. In this scenario, local linearization can achieve higher approximated accuracy than the Koopman linear model constructed by DMDc and EDMD algorithms.
In Scenario 2, the front steering angle is time-varying, i.e., coupling characteristics are strong. As shown in Figure 10, in Scenario 2, the slip angle of the front and rear tires of the vehicle works in its nonlinear region. When coupling characteristics and tire nonlinearity are significant, the obtained Koopman linear model can approximate the system dynamics accurately. However, local linearization is failed. Compared with local linearization, the data-driven Koopman linear model can predict the system dynamics accurately in various scenarios, especially in scenarios of strong tire nonlinearity. Compared with the DMDc algorithm, the Koopman linear model constructed by the EDMD algorithm has higher accuracy. However, the modeling process of EDMD algorithm is more complex, and the dimension of obtained Koopman linear model is much higher.
Note that, compared with [41], both nonlinear dynamics of vehicles and nonlinear characteristics of tires are considered in this paper.

Velocity Tracking
In order to verify the effectiveness of both DMDc-MPC and EDMD-MPC, simulation experiments are carried out under three different cases: • Case 1: The reference of the lateral velocity and yaw rate are set to 0, and the longitudinal velocity is time-varying. Note that the Gaussian distributed random signal is injected into the reference signal in this case.   (36) Acceleration and deceleration in the longitudinal direction are considered in Case 1. Furthermore, measurement noise, i.e., Gaussian distribution with mean 0 and variance [10 −2 , 10 −4 , 10 −4 ] T , is incorporated into the reference signals. Figures 11 and 12 show that vehicles with both DMDc-MPC and EDMD-MPC can accurately track the reference velocity, and keep the lateral velocity and yaw rate close to zero. Vehicles are accelerating at the longitudinal direction and changing lanes in Case 2, where the lateral and longitudinal motions are coupled. Since the lateral velocity is low, tire does not enter its nonlinear region, and the vehicle nonlinearity is mainly reflected in the lateral and longitudinal coupling characteristics. As shown in Figures 13 and 14, vehicles with both DMDc-MPC and EDMD-MPC can effectively track the reference velocity signal with small deviation while the reference lateral velocity and yaw rate are changing. Compared with [40,41], the controller proposed in this paper makes the deviation between vehicle state and the reference velocity smaller, and the RMSE of deviations is about 10% of [41].

Remark 4.
In cases of obvious lateral and longitudinal coupling characteristics, local linearization modeling fails to track the reference velocity signal. Furthermore, due to the complex structure of vehicle dynamics (6), updating the Jacobian matrix at each time instant will generate tremendous computational burden.
In order to verify the effectiveness of the proposed controller in scenario where the vehicle lateral and longitudinal coupling nonlinearity and tire nonlinearity are significant, the experiment in Case 3 is carried out. The designed controllers can effectively track the reference velocity signal, which is shown in Figures 15 and 16. Furthermore, the slip angles of the front and rear tires of vehicles are shown in Figures 17 and 18. When the slip angle of front and rear tires gradually reaches its peak, the nonlinear characteristic of tires emerges. The experimental results show that the proposed controller can still ensure tracking accuracy when the nonlinearity of the tire is significant.    RMSM of the proposed DMDc-MPC and EDMD-MPC in different cases is shown in Table 5. The accuracy of the Koopman linear model based on the EDMD algorithm is higher, so the tracking accuracy of EDMD-MPC is slightly higher than that of DMDc-MPC.
The simulations are carried out with Processor Intel Core TM i7-10700CPU @2.90 GHz produced by Intel Corporation, Santa Clara, CA, USA, and 16 GB RAM produced by Ramaxel Technology, Shenzhen, China. The optimization problem is solved by qpOASES [45]. The average computation time of the involved optimization problem is shown in Table 6, which is much smaller than the sampling time 10 ms. Compared with EDMD-MPC, DMDc-MPC can achieve similar performance with smaller computation time.

Conclusions and Future Research
In this paper, an efficient model predictive control for velocity tracking of automated vehicles was proposed, provided that a reference signal was given a priori. A 5-DOF nonlinear model of vehicles combined with nonlinear tires is introduced to cover full operation conditions. An approximated global linear model is obtained based on Koopman operator theory in order to reduce the online computational burden of MPC and avoid solving nonconvex/nonlinear optimization problems. The DMDc algorithm, EDMD algorithm, and the local linearization method are compared in different driving scenarios. Local linearization is only suitable for scenarios with weak coupling characteristics such as longitudinal driving scenarios. The Koopman linear model obtained by the DMDc algorithm and EDMD algorithm can guarantee the accurate prediction of system dynamics in various complex driving scenarios. The control strategy adopted in this paper can ensure the control accuracy and acceptable computational burden in complex scenarios with obvious coupling characteristics and tire nonlinearity, which is helpful to improve driving safety.
Future research will focus on adaptive cruise control and platooning of vehicles with a performance guarantee using efficient nonlinear model predictive control.

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

Conflicts of Interest:
The authors declare no conflict of interest.