Learning Based MPC for Autonomous Driving Using a Low Dimensional Residual Model

This paper chooses a single-track dynamics model with nonlinear tire forces as the nominal model to extract the essential vehicle dynamics, as formulated below:

Here, $F_{f,x}$ and $F_{r,x}$ denote the longitudinal force on the front and rear wheels, respectively, while $F_{f,y}$ and $F_{r,y}$ denote the lateral force on the front and rear wheels. $F_{d}$ represents the air drag. $m$ is the vehicle mass and $I_{z}$ is the yaw moment of inertia. $l_{f}$ and $l_{r}$ denote the distances between the center of gravity (CoG) and the front/rear axle, respectively. In the equations, $F_{f,y}$ and $F_{r,y}$ are calculated using a simplified magic formula [29], based on the values of the front and rear wheel side slip angles $\alpha_{f}$ and $\alpha_{r}$:

The parameters $B_{f/r},C_{f/r},D_{f/r}$ are tire parameters used in the Magic Formula. The front and rear wheel side slip angles are calculated based on kinematic relationships:

The longitudinal tire forces are calculated based on the command torque:

where the torque distribution coefficient for the front axle is $\kappa$ and $r$ represents the rolling radius of the tire. $C_{f,r}$ and $C_{r,r}$ denote the rolling resistance for the front and rear tires. The air drag $F_{d}$ is modeled as:

where $C_{w}$ denotes the coefficient of air drag. When applied to MPC, the nominal model in (2) is linearized and discretized at each time step along the prediction horizon with associated operation points, resulting in the discrete nominal model $\mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k})$ as in (1).

While the previously presented nominal model captures the essential vehicle dynamics, discrepancies remain between the model and actual vehicle dynamics. In the following, the sources of discrepancies in the nominal model are analyzed to inform the design of the residual model. Based on the structure of the nominal model, the dynamics of the first three states, $X$, $Y$, and $\varphi$, are fully governed by kinematic relationships, while the last two states, $T$ and $\delta$, represent actuator outputs. The dynamics of these states are assumed to be sufficiently accurate, with model errors only affecting the velocity states $v_{x}$, $v_{y}$, and $\omega$. Consequently, the matrix $\mathbf{B}_{d}$ is defined as $\mathbf{B}_{d}=\begin{bmatrix}\mathbf{0}_{3\times 3}&\mathbf{I}_{3\times 3}&\mathbf{0}_{3\times 2}\end{bmatrix}^{T}$. The dynamics model corresponding to the velocity states can be expressed in matrix form as follows:

In nominal vehicle model, parameters $m$, $l_{f}$, $l_{r}$, $I_{z}$ and $C_{w}$ are considered invariable elements as they can be precisely measured through experimental calibration and exhibit negligible variation during operation. Conversely, longitudinal and lateral tire forces $[F_{f,x},F_{f,y},F_{r,x},F_{r,y}]^{T}$ are considered the variable elements. The tire forces may deviate from the model during operation due to factors such as road conditions, tire pressure, temperature and tire wear. Additionally, the coupling between longitudinal and lateral tire forces further impact model accuracy. These deviations in tire forces constitute the primary source of discrepancies in the nominal model. Consequently, the model error corresponding to the velocity states $[\Delta\dot{v}_{x},\Delta\dot{v}_{y},\Delta\dot{\omega}]^{T}$ can be expressed as follows:

where $[\Delta F_{f,x},\Delta F_{f,y},\Delta F_{r,x},\Delta F_{r,y}]^{T}$ represent the dieviation of tire forces. Considering that the front wheel steering angle $\delta$ is generally small, and the deviations in tire forces $\Delta F_{x/y,f/r}$ are also small relative to the values of tire model $F_{x/y,f/r}$, the impact of higher-order terms $\Delta F_{f/r,x/y}\sin\delta$ can be neglected, and $\cos\delta\approx 1$. Consequently, the expression for model error can be simplified as follows:

To incrementally refine the accuracy of the nominal model and adapt to vehicle dynamics variations, GPs are employed in the formulation of residual model, leveraging their flexibility and probabilistic nature to learn deviations caused by the variable elements. The training set required for GPs is constructed from real-time collected data. Training data $\mathbf{y}_{j}$ is derived by calculating the deviation between the sensor measurements and the prediction provided by the nominal model:

where $\mathbf{w}_{j}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{w})$ is gaussian noise with variance $\mathbf{\Sigma}_{w}$= diag$([\sigma_{1}^{2},\sigma_{2}^{2},\sigma_{3}^{2}])$. $\mathbf{B}_{d}^{\dagger}$ is the Moore-Penrose pseudo-inverse of $\mathbf{B}_{d}$. To enhance the coverage of feature space by the training set, a data selection procedure based on the independence metric proposed in [26] is applied to continuously maintain a training set of size $m$:

Furthermore, each dimension of the residual model output is assumed to be uncorrelated with the others. Under this assumption, GPs can provide posterior predictions of the mean $\mu_{a}(\mathbf{z}_{k})$ and variance $\Sigma_{a}(\mathbf{z}_{k})$ of the nominal model error in dimension $a$ at test point $\mathbf{z}_{*}$:

The matrix $\mathbf{K}$ denotes the covariance evaluated at all pairs of points in training set and $\begin{bmatrix}\mathbf{K}\end{bmatrix}_{ij}=k(\mathbf{z}_{i},\mathbf{z}_{j})$. The vector $\mathbf{k}_{*}=\begin{bmatrix}k(\mathbf{z}_{*},\mathbf{z}_{1}),\cdots,k(\mathbf{z}_{*},\mathbf{z}_{m})\end{bmatrix}^{T}$. The Squared Exponential (SE) kernel function is employed to define the GP kernel $k(\mathbf{z}_{i},\mathbf{z}_{j})$:

where parameter $\mathbf{M}$ defining the length-scale matrix and parameter $\sigma_{f}^{2}$ defining the squared signal variance.

The final multivariate GPs approximation of the unknown residual $\mathbf{g}(\mathbf{z}_{*})$ is obtained by aggregating the outputs for each dimension:

where $\boldsymbol{\mu}=\begin{bmatrix}\mu_{v_{x}},\mu_{v_{y}},\mu_{\omega}\end{bmatrix}^{T}$ and $\mathbf{\Sigma}=$diag$([\Sigma_{v_{x}},\Sigma_{v_{y}},\Sigma_{\omega}]^{T})$.

The features for GP are critical. In previous researches, the features for residual model corresponding to the single-track dynamic model are typically selected as the dynamic part of the vehicle states $\begin{bmatrix}v_{x},v_{y},\omega,\delta,T\end{bmatrix}$, spanning a 5-dimensional feature space. This necessitates a vast training set to cover the entire feature space, leading to unacceptable computational burdens. Additionally, it is infeasible to control the vehicle to traverse every region within this 5-dimensional space. As a result, previous researches have achieved satisfactory performance only under specific, fixed operating conditions. Accordingly, this paper extract physical variables most correlated with nominal model errors as features, aiming to reduce the dimensionality of the feature space.

As shown in (9), the primary source of nominal model error arises from discrepancies between actual tire forces and the tire model. The lateral tire forces in nominal model are functions of the tire side slip angles; therefore, deviations in lateral tire forces, $\Delta F_{f/r,y}$, are primarily functions of the tire side slip angle $\alpha_{f/r}$. Similarly, deviations in longitudinal tire force, $\Delta F_{f/r,x}$, are primarily due to differences between the tire force predicted based on the commanded torque and actual tire force, making $\Delta F_{f/r,x}$ primarily a function of the command torque $T$. When accounting for coupling effects between lateral and longitudinal tire forces, tire force deviations $\Delta F_{f/r,x,y}$ depend on both the command torque $T$ and the tire slip angle $\alpha_{f/r}$. Consequently, the residual of the vehicle dynamics model can be formulated as a function of the front wheel side slip angle $\alpha_{f}$, rear wheel side slip angle $\alpha_{r}$, and command torque $T$. Thus, these three variables are selected as the features for the residual model:

Based on the preceding analysis, the dimensionality of the features for the residual model is reduced from 5 to 3. This reduction in dimensionality significantly decreases the size of training set required to cover the entire feature space.

In (15), the 3-dimensional features space is defined. To explicitly define the valid region in the feature space and reduce the region that the training set needs to cover, three physical constraints related to the features are designed. These constraints eliminate invalid regions, and is subsequently utilized in the MPC controller. Data violating these constraints are considered invalid and excluded.

The first constraint concerns tire force limits. To ensure vehicle safety during operation, the total tire forces should not exceed the friction limits to prevent instability and hazardous conditions. Therefore, the combined longitudinal and lateral forces of the front and rear tires are constrained within the tire-road contact ellipse:

with $p_{long}$ and $p_{ellipse}$ defining the ellipse shape. The second constraint concerns the tire slip angle limits. By restricting the maximum allowable slip angles for tires, this constraint seeks to prevent the tires from reaching the lateral force saturation zone:

The third constraint is derived from the vehicle handling diagram [27], as shown in Fig. 3. The curves represent the relationship between the effective axle characteristics $F_{f/r,y}/F_{f/r,z}$ and the tire slip angles under steady-state steering. Analysis of the handling curves indicates that, beyond a certain threshold, further increases in the difference $\alpha_{f}-\alpha_{r}$ cease to enhance the lateral acceleration of vehicle. Consequently, the difference $\alpha_{f}-\alpha_{r}$ should be constrained within a reasonable range:

Building on the three aforementioned constraints, a finite valid region within the feature space is established. This region encompasses the majority of the dynamic states encountered during operation, explicitly define the scope within feature space that the training set must cover. The constructed valid region is depicted in the Fig. 4.

In MPCC, the trajectory planning and tracking tasks are formulated as a unified optimization problem that determines the control inputs required for the race car to follow the racing track. The track is parameterized by $\theta\in[0,\theta_{\text{max}}]$, where $\theta$ designates a parameter that specifies the corresponding centerline position and orientation, denoted as $[X_{c}(\theta),Y_{c}(\theta),\Phi_{c}(\theta)]$. The state variables in (2) are augmented with $\theta$ and $v_{s}$, which approximate the vehicle progression along the track and its velocity relative to the centerline. These parameters adhere to the update equation $\theta_{k+1}=\theta_{k}+v_{s}\Delta t$. The positional deviations between the vehicle position and the corresponding reference point on the track centerline are decomposed into the lag error $e_{l}$ and contour error $e_{c}$, defined as follows:

The formulation of MPCC is given by:

The cost function in (20) is composed of three primary components. The first term, $J(\mathbf{x}_{k},\theta_{k},v_{k})=q_{l}e_{l}(\mathbf{x}_{k},\theta_{k})^{2}+q_{c}e_{c}(\mathbf{x}_{k},\theta_{k})^{2}$, penalizes the contouring error. The regularization term $U(\mathbf{x}_{k},\mathbf{u}_{k})=\|\mathbf{x}_{k}\|_{\mathbf{R}_{\mathbf{x}}}^{2}+\|\mathbf{u}_{k}\|_{\mathbf{R}_{\mathbf{u}}}^{2}$ imposes a penalty on the control inputs and their changing rates, ensuring that the control actions are smooth and preventing abrupt changes in trajectory. The third term, $q_{v}v_{k}$, is designed to maximize the vehicle progression along the track within the prediction horizon. Here, $q_{l}$, $q_{c}$, and $q_{v}$ are constant weights, and $\mathbf{R}_{\mathbf{x}}$ and $\mathbf{R}_{\mathbf{u}}$ are weighting matrices.

The constraints outlined in (20) impose multiple limitations. Beyond ensuring consistency with the vehicle model in (1), the track constraints confine the vehicle position within the track radius $R$, while the state and control constraints ensure that vehicle states and control inputs adhere to the physical limitations. The constraints in Section II-D restrict the vehicle state within the valid region, preventing unsafe behaviors. To guarantee the feasibility of the optimization problem, all constraints are modeled as soft constraints.

The controller is deployed on a Nuvo-9160 industrial computer with i9-13900E CPU, operates with a sampling time of $\Delta t=50\,\text{ms}$ and a prediction horizon of $N=80$. Optimization is performed using the HPIPM library [30], with a maximum of 40 iterations to constrain the computation time. Control signals and state information of vehicle are transmitted via TCP/UDP communication protocols.

The proposed controller is first validated within Carsim simulation environment. The simulation vehicle is chosen as B-Class Hatchback 4-wheel and the handling course race line, with a total length of 2327.98m, is selected as the racing track. The track has a width of 10m and the friction coefficient is 1.0. The invariable elements of the nominal vehicle model are provided by CarSim. In the experiment, the initial speed is 10 m/s, with a maximum speed limit of 30 m/s. The vehicle completes the first lap with nominal MPC controller as a baseline. The collected data is filtered using the data selection mechanism in [26] to construct a training set of size 100, serving as the initial training set for residual model. A total of 5 laps are conducted using the proposed learning based MPC controller, with data collected after each lap used for offline updates of training set for the subsequent laps. The vehicle trajectories from the first and final laps are shown in Fig. 5.

To quantify the performance of proposed controller, the key metrics are summarized in Table I. The proposed residual model consistently improves lap times and average speed. The initial lap time of 119.63s, obtained using the nominal controller, is reduced to 104.85s after five laps of data updates, representing an overall improvement of 12.35%. The sustained increase in maximum lateral acceleration during racing demonstrates the enhanced utilization of tire dynamics, which is also illustrated in Fig. 6(6(a)). Fig. 6(6(b)) compares the vehicle speeds at each track position between the first and final laps, showing significant speed improvements at every location with the learning based MPC controller.

The predictive performance of the model is also evaluated. At each time step, the deviation between the actual system states and the model predictions is quantified using $\|\mathbf{e}\|_{\text{nom}}=\|\mathbf{x}_{k+1}-\mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k})\|$ and $\|\mathbf{e}\|_{\text{GP}}=\|\mathbf{x}_{k+1}-\mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k})-\mathbf{B}_{d}\mathbf{g}(\mathbf{z}_{k})\|$. Table II outlines the mean and standard deviation of prediction errors $e_{v_{y}},$ and $e_{\omega}$ during racing, affirming that the proposed residual model substantially mitigates the prediction discrepancies of the nominal model. Fig. 7 provides a graphical comparison of the actual error $e_{v_{y}}$ and $e_{\omega}$, and the GP predictions during the 6th lap, highlighting the superior predictive capability of the residual model.

The proposed controller is further validated on a real vehicle testing platform as shown in Fig 11. The vehicle states are measured using the onboard RTK. The racing track including multiple straights and curves is 1087m long, 10m wide, with a concrete surface. The invariable elements of the nominal model are derived from comprehensive calibration experiments. To ensure safety, speed is limited to 10m/s, and the maximum tire forces are restricted to 50% of the road adhesion limit. Both the nominal and the learning based MPC controller are each employed to complete one lap of racing. The training set of GP is constructed by filtering data in lap 1, consisting of 100 points.

The key performance metrics from both laps are summarized in Table III, indicating a 5s improvement in lap time achieved by the proposed controller. Fig. 8 provides a comparison of the longitudinal velocity at each point on the track and the trajectories from 2 laps. Fig. 9 compares the residual model prediction $e_{v_{y}}$ and $e_{\omega}$ of the nominal model errors and the actual deviations during lap 2, demonstrating the capability of residual model to accurately capture the discrepancies in nominal model. Table IV summarizes the mean and standard deviation of nominal model prediction errors $e_{v_{y}},$ and $e_{\omega}$ before and after the inclusion of the residual model, highlighting the improvement in alignment with the true vehicle dynamics. Figure 10 illustrates the distribution of the training dataset and lap 2 data within the residual model feature space, confirming that the training set only needs to cover a constrained valid region to deliver robust performance across varied operating conditions. These results validate that the proposed controller significantly enhances both model accuracy and control performance.