Learning-Based Model Predictive Control for Autonomous Racing

Abstract

In this paper, we present the adaptation of the terminal component learning-based model predictive control (TC-LMPC) architecture for autonomous racing to the Formula Student Driverless (FSD) context. We test the TC-LMPC architecture, a reference-free controller that is able to learn from previous iterations by building an appropriate terminal safe set and terminal cost from collected trajectories and input sequences, in a vehicle simulator dedicated to the FSD competition. One major problem in autonomous racing is the difficulty in obtaining accurate highly nonlinear vehicle models that cover the entire performance envelope. This is more severe as the controller pushes for incrementally more aggressive behavior. To address this problem, we use offline and online measurements and machine learning (ML) techniques for the online adaptation of the vehicle model. We test two sparse Gaussian process regression (GPR) approximations for model learning. The novelty in the model learning segment is the use of a selection method for the initial training dataset that maximizes the information gain criterion. The TC-LMPC with model learning achieves a 5.9 s reduction (3%) in the total 10-lap FSD race time.

1. Introduction

Autonomous driving (AD) is an active field of research for academia and the industry. Autonomous driving cars are of growing importance nowadays, and this is expected to increase in the future. Most stakeholders aim at improving road safety [1], reducing traffic congestion and emissions [2], and deploying universal mobility. The applications span from personal vehicles, robotaxis, and delivery vehicles to long-haul trucks, shuttles, and mining vehicles [3,4,5,6].

This paper focuses on autonomous electric racing, a subfield of autonomous driving that aims to contribute to the broader problem by introducing innovations in autonomous technology through sport [7]. This type of synergy is well established between Formula One and Formula E and the automotive industry. In particular, we aim to contribute to the tasks of trajectory planning and control.

Paden et al. [8] surveyed planning and control algorithms for AD in the urban setting. Several controllers that resort to a kinematic bicycle model have been designed, e.g., the Stanley controller [9]. To handle more demanding driving manoeuvres, more complex controllers must be designed. Advances in computing hardware and mathematical programming have made model predictive control (MPC) feasible for real-time use in AD [10].

We target the 10-lap trackdrive event of the Formula Student Driverless (FSD) autonomous racing competition. Formula Student (www.formulastudent.de/about/concept/ (accessed on 20 April 2023)) is a student engineering competition where teams design and compete with a formula race car. The autonomous racing competition environment is controlled in the sense that no other agents, such as other vehicles or pedestrians, are near the track. The track is composed of blue and yellow cones on the left and right borders, respectively. The perception and decision modules in this setting are simpler than in urban AD. In contrast, the motion planning and control problem can be greater as teams aim to drive at the limits of handling [11,12].

Obtaining a sufficiently accurate vehicle model in these conditions without rendering the MPC computationally intractable is a challenging task. Moreover, the recursive feasibility of optimization-based problems is still a challenging issue with respect to their real-time implementation due to the limited computing capacity of processors [13,14]. To overcome this issue, machine learning (ML) techniques have been used to improve the formulation of the MPC using collected data. A recent survey of learning-based model predictive control (LMPC) applications [15] divides the field into the three following categories: (i) system dynamics learning, using ML techniques as a data-based adaptation of the prediction model or uncertainty description (also referred to as model learning); (ii controller design learning, which targets an MPC controller’s parameterization, e.g., the cost function or terminal components; (iii) MPC for safe learning, which concerns approaches where MPC is used as a safety filter for learning-based controllers.

A cautious LMPC that combines a nominal model with Gaussian process regression (GPR) techniques to model the unknown dynamics was presented in [16]. It has been applied to trajectory tracking with a robotic arm [17]. Alternatively, Bayesian linear regression (BLR) has been used to model the unknown dynamics [18]. The authors argue that this simple model is more accurate in estimating the mean behavior and model uncertainty than GPR and generalizes to novel operating conditions with little or no tuning. Further, they propose a framework that combines BLR model learning with cost learning [19]. However, the algorithm was tested on a slow-moving robot in off-road terrain. Therefore, the main challenge is related to changing dynamics throughout different parts of the terrain.

An LMPC for autonomous racing was applied to the AMZ Driverless FSD prototype [20]. The proposed formulation considers a nominal vehicle model where GPR models residual model uncertainty. The approach is based on model predictive contouring control (MPCC) [21] and cautious MPC [16]. However, the focus is placed only on learning a more accurate vehicle model. We augment this approach by also using the safe set.

The main contribution of this work is the adaptation of Rosolia et al.’s [22] terminal component LMPC (TC-LMPC) architecture to the full-sized autonomous electric racing FSD competition, shown in Figure 1. The same authors present an adaptation to the AD problem [23]; however, it was tested in a miniaturized 1/10 race car. The increased velocities of the full-sized application require a higher control frequency; for this reason, we implement a computationally efficient solution resorting to C++ and FORCESPRO [24,25]. Further, Rosolia et al.’s [23] approach considers tracks with segment-wise constant curvature, while the formulation here presented considers more complex tracks, such as those that we encounter in FSD competitions. Finally, with respect to vehicle modeling, in [23], a linear model is learned using a local linear regressor, while we use the nonlinear version of the bicycle model. Without these adaptations, the completion of a single lap in the full-sized racing application would not be possible. Moreover, we extend the TC-LMPC approach with GPR for model learning [26] by actively choosing the initial training set [27]. We call the combination of the TC-LMPC architecture with ML for model learning TC-LMPC_ML. We show that TC-LMPC reduces the best lap time by 10% for the 10-lap FSD race. In the work of Kabzan et al. [20], a 10% lap time reduction was also achieved. However, when we activate the model learning component, we show that a 10% lap time reduction can be sustained throughout the event but with a total race time reduction of 3% when compared to the TC-LMPC without the model learning part.

This paper is organized as follows. In Section 2, we provide the theoretical background of the methods used in the LMPC architecture, including the original TC-LMPC formulation. In Section 3, we describe the TC-LMPC architecture for autonomous racing, which improves performance by learning the terminal components (safe set and terminal cost), as well as our adaptation to the full-sized autonomous electric racing FSD competition—controller design learning. Section 4 explains how the TC-LMPC_ML architecture uses GPR for system dynamics learning. Finally, we introduce the implementation details and show the simulation results for model learning and for both architectures in Section 5. Section 6 provides a summary of the main contributions and suggestions for future research directions.

2. Learning-Based Model Predictive Control

We use bold lowercase letters for vectors $\mathit{x}\in {\mathbb{R}}^n$ and bold capitalized letters for matrices $\mathit{X}\in {\mathbb{R}}^{n\times m}$, while scalars are non-bold.

2.1. Model Predictive Control

The idea of receding horizon control (RHC) is that an infinite-horizon sub-optimal controller can be designed by repeatedly solving finite-time constrained optimal control (FTCOC) problems in a receding horizon fashion [28]. At each sampling time, starting at the current state, an open-loop optimal control problem is solved over a finite horizon. The computed optimal input signal is applied to the process only during the following sampling interval $[t,t+1]$. At the next time step $t+1$, a new optimal control problem based on new measurements of the state is solved over a shifted horizon.

MPC is an RHC problem where the FTCOC problem with a prediction horizon of N is computed by solving online the following optimization problem:

$$\begin{array}{cc}{J}_{t\to t+N}^j\left(x_t^j\right)=\hfill & \\ \underset{u_{t|t},\cdots ,u_{t+N-1|t}}{min}[\sum _{k=t}^{t+N-1}q(x_{k|t},u_{k|t})+p\left(x_{t+N|t}\right)]\hfill & \end{array}$$

(1)

$$\begin{array}{ccc}s.t.\hfill & & \\ & x_{k+1|t}=A{x}_{k|t}+B{u}_{k|t}\hfill & \forall k\in [t,\cdots ,t+N-1]\hfill \end{array}$$

(2)

$$\begin{array}{cccc}& & x_{k|t}\in \mathcal{X},u_{k|t}\in \mathcal{U}\hfill & \forall k\in [t,\cdots ,t+N-1]\hfill \end{array}$$

(3)

$$\begin{array}{ccccccccccccccccc}{x}_{t+N|t}\in {\mathcal{X}}_f\hfill & & & & & & & & & & & & & & & & \end{array}$$

(4)

$$\begin{array}{ccccccccccccccccc}{x}_{t|t}=x\left(t\right)\hfill & & & & & & & & & & & & & & & & \end{array}$$

(5)

where x is the state and u the control input. The subscript $k|t$ represents a given quantity in the prediction horizon with respect to time t. Equation (5) imposes the current system state to be the initial condition of the generic FTCOC problem. Equation (2) represents the discrete-time linear time-invariant system dynamics. State and input constraints are given by (3). The terminal constraint is given by (4), which forces the terminal state $x_{t+N|t}$ into some set ${\mathcal{X}}_f$. The stage $q(·,·)$ and terminal cost $p\left(x_{t+N|t}\right)$ are any arbitrary, continuous, strictly positive functions.

2.2. TC-LMPC—Terminal Component Learning

This work is based on the TC-LMPC architecture first proposed in [22]. This is a reference-free iterative control strategy able to learn from previous iterations. At each iteration, the initial condition, the constraints, and the objective function do not change. The authors show how to design a terminal safe set—$\mathcal{SS}$—and a terminal cost function—the Q-function—such that the following theoretical guarantees hold:

• Nonincreasing cost at each iteration;
• Recursive feasibility, i.e., state and input constraints are satisfied at iteration j if they were satisfied before;
• Closed-loop equilibrium is asymptotically stable.

Considering (1), the terminal cost is given by the Q-function: $p\left(x_{t+N|t}\right)=Q^{j-1}\left(x_{t+N|t}\right)$. Meanwhile, the terminal constraint corresponds to the terminal safe set $\mathcal{SS}$: ${\mathcal{X}}_f={\mathcal{SS}}^{j-1}$.

At the jth iteration, the inputs applied to the system and the corresponding state evolution are collected in the vectors given by (6) and (7), respectively.

$$\begin{array}{c}\hfill {\mathit{u}}^j=\left[u_0^j,u_1^j,\cdots ,u_t^j,\cdots \right]\end{array}$$

(6)

$$\begin{array}{c}\hfill {\mathit{x}}^j=\left[x_0^j,x_1^j,\cdots ,x_t^j,\cdots \right]\end{array}$$

(7)

The safe set ${\mathcal{SS}}^j$, given by (8), is the collection of all state trajectories at iteration i for $i\in M^j$—the set of indexes k corresponding to the iterations that successfully steer the system to the final point $x_F$.

$${\mathcal{SS}}^j=\left\{\begin{array}{c}{\displaystyle \bigcup _{i\in M^j}}{\displaystyle \bigcup _{t=0}^{\infty }}{x}_t^i\end{array}\right\}$$

(8)

The $Q^j$ function, defined in (9), assigns to every point in the safe set the minimum cost-to-go along the trajectories therein.

$$\forall x\in {\mathcal{SS}}^j,Q^j\left(x\right)=J_{t^{\ast }\to \infty }^{i^{\ast }}\left(x\right)=\sum _{k=t^{\ast }}^{\infty }q(x_k^{i^{\ast }},u_k^{i^{\ast }})$$

(9)

where $i^{\ast }$ corresponds to the iteration that minimizes such a cost starting at the particular state x, and $t^{\ast }$ is the respective time of this state in this iteration.

The safe set works as a safe region given the shorter horizon N. For example, in autonomous racing, it can account for the shape of the track beyond the horizon. In this way, the controller is informed by past experimental data regarding how fast he can travel at a particular part of the track without having to compute the global optimal racing line.

2.3. Gaussian Processes Regression

Gaussian processes [29] are a non-parametric, probabilistic machine learning approach to learning in kernel machines. They provide a fully probabilistic predictive distribution, including estimates of the uncertainty of the predictions. Consider an unknown latent function $\mathit{g}:{\mathbb{R}}^{n_z}\to {\mathbb{R}}^{n_g}$ that is identified from a collection of inputs ${\mathit{z}}_k\in {\mathbb{R}}^{n_z}$ and corresponding outputs ${\mathit{y}}_k\in {\mathbb{R}}^{n_g}$.

$${\mathit{y}}_k=\mathit{g}\left({\mathit{z}}_k\right)+{\mathit{w}}_k$$

(10)

where ${\mathit{w}}_k\sim \mathcal{N}(\mathbf{0},{\Sigma }^{\mathit{w}})$ is independent and identically distributed Gaussian noise with diagonal variance ${\Sigma }^{\mathit{w}}=\mathrm{diag}\left([{\sigma }_1^2,\cdots ,{\sigma }_{n_g}^2]\right)$. The set of n input and output data pairs form a dictionary $\mathcal{D}$:

$$\mathcal{D}=\left\{\&\mathit{Y}=[{\mathit{y}}_1^T;\cdots ;{\mathit{y}}_n^T]\in {\mathbb{R}}^{n\times n_g},\mathit{Z}=[{\mathit{z}}_1^T;\cdots ;{\mathit{z}}_n^T]\in {\mathbb{R}}^{n\times n_z}\right\}$$

(11)

Assuming a Gaussian prior on $\mathit{g}$ in each output dimension $d\in \{1,\cdots ,n_g\}$, such that they can be treated independently, the posterior distribution in dimension d at an evaluation point $\mathit{z}$ has a mean and variance given by Equations (12) and (13), respectively. Further, in this situation, one refers to $\mathit{Y}$ as $\mathit{y}$. In other words, there is a collection of $n_g$n-dimensional vectors ${\mathit{y}}^d$.

$$\begin{array}{cc}\hfill {\mu }^d\left(\mathit{z}\right)& ={\mathit{k}}_{\mathit{z}\mathit{Z}}^d{\left({\mathit{K}}_{\mathit{Z}\mathit{Z}}^d+\mathit{I}{\sigma }_d^2\right)}^{-1}{\mathit{y}}^d\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill {\Sigma }^d\left(\mathit{z}\right)& =k_{\mathit{z}\mathit{z}}^d-{\mathit{k}}_{\mathit{z}\mathit{Z}}^d{\left({\mathit{K}}_{\mathit{Z}\mathit{Z}}^d+\mathit{I}{\sigma }_d^2\right)}^{-1}{\mathit{k}}_{\mathit{Z}\mathit{z}}^d\hfill \end{array}$$

(13)

where ${\mathit{K}}_{\mathit{Z}\mathit{Z}}^d$ is the Gramian matrix, i.e., ${\left[{\mathit{K}}_{\mathit{Z}\mathit{Z}}^d\right]}_{ij}=k^d({\mathit{z}}_i,{\mathit{z}}_j)$, ${\left[{\mathit{k}}_{\mathit{Z}\mathit{z}}^d\right]}_j=k^d({\mathit{z}}_j,\mathit{z})$, ${\mathit{k}}_{\mathit{Z}\mathit{z}}^d={\left({\mathit{k}}_{\mathit{Z}\mathit{z}}^d\right)}^{\mathrm{T}}$, and $k_{\mathit{z}\mathit{z}}^d=k^d(\mathit{z},\mathit{z})$ corresponds to the kernel function used. The specification of the prior is important because it fixes the properties of the covariance functions considered for inference—in particular, the type of kernel function $k^d(\mathit{z},\overline{\mathit{z}})$ used and its hyperparameters.

The multivariate Gaussian process approximation is given by $\mathit{g}\left(\mathit{z}\right)\sim \mathcal{N}\left({\mathit{\mu }}^g\left(\mathit{z}\right),{\Sigma }^g\left(\mathit{z}\right)\right)$, where ${\mathit{\mu }}^g\left(\mathit{z}\right)=[{\mu }^1\left(\mathit{z}\right);\cdots ;{\mu }^{n_g}\left(\mathit{z}\right)]$ and ${\Sigma }^g\left(\mathit{z}\right)=[{\Sigma }^1\left(\mathit{z}\right);\cdots ;{\Sigma }^{n_g}\left(\mathit{z}\right)]$.

2.4. Sparse Approximations for Gaussian Process Regression

The computational complexity of GPR strongly depends on the number of data points n. In particular, a computational cost of $\mathcal{O}\left(n^3\right)$ is incurred whenever a new training point is added to the dictionary $\mathcal{D}$. This is due to the need to invert $\left({\mathit{K}}_{\mathit{Z}\mathit{Z}}^d+\mathit{I}{\sigma }_d^2\right)$ in Equations (12) and (13), which is a $n\times n$ matrix. Moreover, the evaluation of the mean and variance has a complexity cost of $\mathcal{O}\left(n\right)$ and $\mathcal{O}\left(n^2\right)$, respectively.

Several sparse approximation techniques have been proposed to allow the application of GPR to large problems in machine learning [30]. An additional set of $m<n$ latent variables $\overline{\mathit{g}}=[{\overline{g}}_1,\cdots ,{\overline{g}}_m]$, which are called inducing variables or support points, are used to approximate (12) and (13). These are values of the Gaussian process evaluated at the inducing inputs ${\mathit{Z}}_{ind}=[{\overline{\mathit{z}}}_0^T;\cdots ;{\overline{\mathit{z}}}_m^T]$. The latent variables are represented as $\overline{\mathit{g}}$ rather than $\overline{\mathit{y}}$ as they are not real observations, thus not including the noise variance.

The simplest sparse approximation method is the Subset of Data (SoD) approximation, i.e., it solves (12) and (13) by substituting $\mathit{Z}$ by ${\mathit{Z}}_{ind}$. It is often used as a baseline for sparse approximations. The computational complexity is reduced to $\mathcal{O}\left(m^3\right)$ for training and $\mathcal{O}\left(m\right)$ and $\mathcal{O}\left(m^2\right)$ for the mean and variance, respectively.

The Fully Independent Training Conditional approximation [31] assumes that all the training data points are independent. The computational complexity is $\mathcal{O}\left(n{m}^2\right)$ initially and $\mathcal{O}\left(m\right)$ and $\mathcal{O}\left(m^2\right)$ per test case for the predictive mean and variance, respectively. FITC can be viewed as a standard GP with a particular non-stationary covariance function parameterized by the pseudo-inputs. The mean and variance are given by

$$\begin{array}{cc}\hfill {\mu }^d\left(\mathit{z}\right)& ={\mathit{k}}_{\mathit{z}{\mathit{Z}}_{ind}}^d\Theta {\mathit{K}}_{{\mathit{Z}}_{ind}\mathit{Z}}{\Lambda }^{-1}{y}^d={\mathit{k}}_{\mathit{z}{\mathit{Z}}_{ind}}^d{\mathit{i}}^d\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill {\Sigma }^d\left(\mathit{z}\right)& ={\mathit{k}}_{\mathit{z}{\mathit{Z}}_{ind}}^d{\mathit{K}}_{{\mathit{Z}}_{ind}{\mathit{Z}}_{ind}}^{-1}{\mathit{K}}_{{\mathit{Z}}_{ind}\mathit{z}}-{\mathit{K}}_{\mathit{z}{\mathit{Z}}_{ind}}\Theta {\mathit{K}}_{{\mathit{Z}}_{ind}\mathit{z}}\hfill \end{array}$$

(15)

where $\Theta ={\left({\mathit{K}}_{{\mathit{Z}}_{ind}{\mathit{Z}}_{ind}}+{\mathit{K}}_{{\mathit{Z}}_{ind}\mathit{Z}}{\Lambda }^{-1}{\mathit{K}}_{\mathit{Z}{\mathit{Z}}_{ind}}\right)}^{-1}$, ${\mathit{i}}^d$ is the information vector taken for each d sparse model, and $\Lambda =\mathrm{diag}\left({\mathit{K}}_{\mathit{Z}\mathit{Z}}-{\mathit{K}}_{\mathit{Z}{\mathit{Z}}_{ind}}{\mathit{K}}_{{\mathit{Z}}_{ind}{\mathit{Z}}_{ind}}^{-1}{\mathit{K}}_{{\mathit{Z}}_{ind}\mathit{Z}}\right)$.

3. Performance-Driven Controller Learning

3.1. Learning Terminal Components for Autonomous Racing

Learning terminal components is considered a sub-field of controller design learning for LMPC [15]. Rosolia and Borrelli first introduced in [23] the adaptation of the core TC-LMPC architecture to the autonomous racing problem. This is formulated as a minimum time problem, where an iteration j corresponds to a lap. Therefore, the stage cost is given as follows:

$$q(x_k,u_k)=\left\{\begin{array}{c}1\mathrm{if}{x}_k\notin \mathcal{L}\\ 0\mathrm{if}{x}_k\in \mathcal{L}\end{array}\right.$$

(16)

where $\mathcal{L}$ is the set of points beyond the finish line. A slower trajectory contains more points until the finish line, thus having a greater cost associated.

The vehicle dynamics considers the states and input vector quantities in (17) and (18), respectively.

$$\begin{array}{cc}\hfill & \mathit{x}=[s,e_y,e_{\psi },v_x,v_y,r]\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill & \mathit{u}=[a,\delta ]\hfill \end{array}$$

(18)

where $\mathit{x}$ collects states that describe the vehicle movement and $\mathit{u}$ is the vector of control inputs. s is the distance along the track centerline measured from the start line; $e_y$ and $e_{\psi }$ are the lateral distance and heading angle errors between the vehicle pose and the centerline, respectively. These quantities are given in the curvilinear abscissa reference frame $\xi$-$\eta$—see Figure 2—also known as the Frenet reference frame. A given track is defined by the curvature $k\left(s\right)$ and maximum admissible lateral error $e_y^{max}\left(s\right)$ along the track centerline. $v_x$ and $v_y$ are the longitudinal and lateral vehicle velocities, respectively, while r is the vehicle yaw rate. The inputs are the longitudinal acceleration a and the steering angle $\delta$.

Rosolia and Borrelli [32] further extended this architecture by proposing a local TC-LMPC that significantly reduced the computational burden by using a subset of the stored data. In particular, the local convex safe set ${\mathcal{CS}}_l^j$ is built around the candidate terminal state $c_t$ using the $N_p^{SS}$-nearest neighbours from each of the previous $N_l^{SS}$ laps. Notice that $N_l^{SS}=j-l$. These points are collected in the matrix ${\mathit{D}}_l^j$, defined in (19), which is updated at each time step. The candidate terminal state $c_t$ is the estimated value for ${\mathit{x}}_{t+N|t}$, calculated at time $t-1$. The approximation of the cost-to-go is computed using the costs associated with the selected states in ${\mathit{D}}_l^j$.

$${\mathit{D}}_l^j=[{\mathit{x}}_{t_1}^l,\cdots ,{\mathit{x}}_{t_{N_p^{SS}}}^l,\cdots ,{\mathit{x}}_{t_1}^j,\cdots ,{\mathit{x}}_{t_{N_p^{SS}}}^j]$$

(19)

3.2. TC-LMPC for Formula Student Driverless

The original TC-LMPC architecture represents the vehicle pose in the local coordinate frame, (17). However, the discretization method used, which assumes a constant track curvature $\kappa$ within a sampling period, fails to properly describe tracks with rapidly changing curvatures. This issue was not taken into consideration in [26,32].

Therefore, we propose to change the vehicle model pose states to the global coordinate frame X–Y. In this case, s and $e_y$ are still calculated because these quantities provide immediate information regarding the track, but they are not used to characterize the vehicle pose dynamics. The longitudinal control input is $P\in [-1,1]$, which represents a pedal setpoint. This corresponds to the normalized acceleration and brake pedal travel, i.e., the way in which the actual prototype is controlled both in simulation and reality. Thus, for our application, Equations (17) and (18), respectively, become

$$\begin{array}{cc}\hfill & \mathit{x}=[x,y,\psi ,v_x,v_y,r]\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill & \mathit{u}=[P,\delta ]\hfill \end{array}$$

(21)

The adopted cost function is composed of five main parts and is given as follows:

$$\begin{array}{cc}\hfill J_{t\to t+N}^{LMPC,j}=\underset{u_{t|t},...,t_{t+N-1|t}}{min}[\sum _{k=t}^{t+N-1}& \left({\left({\mathit{x}}_{k+1|t}-{\mathit{x}}_{k|t}\right)}^T{\mathit{Q}}_{deriv}\left({\mathit{x}}_{k+1|t}-{\mathit{x}}_{k|t}\right)\right)+\hfill \\ \hfill \sum _{k=t}^{t+N-2}& \left({\left({\mathit{u}}_{k+1|t}-{\mathit{u}}_{k|t}\right)}^T{\mathit{R}}_{deriv}({\mathit{u}}_{k+1|t}-{\mathit{u}}_{k|t})\right))+\hfill \\ \hfill \sum _{k=t+1}^{t+N}& \left(Q_{v_y}·v_{y_{k|t}}^2\right)+\sum _{k=t}^{t+N}\left(Q_{lag}·e_{l_{k|t}}^2\right)+\hfill \\ \hfill \sum _{k=t+1}^{t+N}& \left(Q_{lane}^{lin}·{ϵ}_{lan{e}_{k|t}}+Q_{lane}^{quad}·{ϵ}_{lan{e}_{k|t}}^2\right)+\hfill \\ \hfill \sum _{k=t+1}^{t+N}& \left(Q_{v_{ub}}^{lin}·{ϵ}_{v_{k|t}}+Q_{v_{ub}}^{quad}·{ϵ}_{v_{k|t}}^2\right)+\hfill \\ \hfill \sum _{k=t+1}^{t+N}& \left(Q_{el}^{lin}·{ϵ}_{e{l}_{k|t}}+Q_{el}^{quad}·{ϵ}_{e{l}_{k|t}}^2\right)+\hfill \\ \hfill \sum _{i=1}^{N_l^{SS}\times N_p^{SS}}& \left(Q_{termcost}\left({\alpha }_i\times {\mathit{Q}}_i^j\left(c_t\right)\right)\right)+\hfill \\ \hfill \sum _{i=1}^{N_l^{SS}\times N_p^{SS}}& \left({\mathit{Q}}_{slack}{\left({\mathit{x}}_{t+N|t}-{\alpha }_i{\mathit{D}}_i^j\left(c_t\right)\right)}^2\right)]\hfill \end{array}$$

(22)

First, the derivative terms apply a penalty on the squared change of a given quantity between consecutive steps along the prediction horizon, both for dynamic states and inputs. This enables us to obtain smooth trajectories. Second, a quadratic cost is applied on $v_y$ to act as a regularization cost, which forces the vehicle into its stable domain and helps convergence. Third, a quadratic cost is applied to the lag error, which measures the accuracy of the global to local coordinate transformation [21].

The fourth part concerns the soft constraints on the states. Bounds on the states are not implemented as hard constraints since one cannot exclude that the real system moves outside the constraint range due to, for instance, model mismatch, which would render the problem infeasible [28]. Thus, the bound on a given state $x\le x_{max}$ is approximated by $x\le x_{max}+ϵ$, where $ϵ\ge 0$, and a term $l\left(ϵ\right)$ is added to the cost functional. It can be shown that $l\left(ϵ\right)=uϵ+v{ϵ}^2$ with a sufficiently high u and $v>0$ ensures that no constraint violation occurs, provided that there exists a feasible input. The bounded states are $v_x$ and $e_y$, where $e_y$ needs to be bounded to stay within the track width. Additionally, a velocity ellipse is implemented to ensure that the vehicle remains within its physical limits.

Finally, the last part of the cost functional in (22) contains two penalty terms on the terminal components of the MPC. A linear cost is applied to the product of ${\alpha }_i$—the coefficients of the local safe set ${\mathit{D}}_l^j$ convex hull—and ${\mathit{Q}}^j$—the cost-to-go of each point in the safe set, (9). Moreover, a quadratic penalty is applied to the error between the terminal state and the safe set points ${\mathit{D}}_l^j$, i.e., ${\mathit{x}}_{t+N|t}-{\alpha }_i{\mathit{D}}_i^j\left(c_t\right)$, for each point i in the safe set. While this slack term ensures that the terminal state lies within the convex hull, the terminal cost favors points in the safe set that result in faster laps.

Bounds on both inputs’ maximum level and rate are applied. These bounds may be more restrictive than the actual physical limits imposed by the robotic platform.

The control actuation ${\mathit{u}}_t^j$ applied at the current sampling time corresponds to the previous sampling time solution shifted by one step, (23). This delay aims to achieve a constant control rate given the solver’s processing time—which is significant in real time and may be inconsistent across the experiment.

$${\mathit{u}}_{t|t}={\mathit{u}}_t^j={\mathit{u}}_{t|t-1}^{\ast ,j}$$

(23)

3.3. Formula Student Driverless Vehicle Model

The system dynamics in (2) results from the discretization of (24), which is the sum of an a priori physics-based model $f_t$ and a term to model the unknown dynamics $g_t$, given as follows:

$${\dot{\mathit{x}}}_t=h_t({\mathit{x}}_t,{\mathit{u}}_t)=f_t({\mathit{x}}_t,{\mathit{u}}_t)+g_t({\mathit{x}}_t,{\mathit{u}}_t)$$

(24)

The pose dynamics are given by the following equations:

$$\begin{array}{cc}\hfill \dot{x}& =v_{x}cos\left(\psi \right)-v_{y}sin\left(\psi \right)\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill \dot{y}& =v_{x}sin\left(\psi \right)+v_{y}cos\left(\psi \right)\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill \dot{\psi }& =r\hfill \end{array}$$

(27)

The dynamic part of the vehicle model is modeled by a dynamic bicycle model—Figure 3. This model is frequently used in automotive control algorithms [33] and is given as follows:

$$\left[\begin{array}{c}{\dot{v}}_x\\ {\dot{v}}_y\\ \dot{r}\end{array}\right]=\left[\begin{array}{c}{\displaystyle \frac{1}{m}}(F_x-F_{F,y}sin\delta +m{v}_{y}r)\\ {\displaystyle \frac{1}{m}}(F_{R,y}+F_{F,y}cos\delta -m{v}_{x}r)\\ {\displaystyle \frac{1}{I_z}}(F_{F,y}{l}_{F}cos\delta -F_{R,y}{l}_R)\end{array}\right]$$

(28)

where m is the vehicle mass and $I_z$ is the rotational inertia about the vertical axis z. The front and rear axles are identified by the subscripts $a\in \{F,R\}$, respectively. $l_a$ is the distance between the vehicle center of gravity and the corresponding axle. The lateral force $F_{a,y}$ is given as

$$F_{a,y}=-2D_{a}sin\left(C_{a}arctan\left(B_a{\alpha }_a\right)\right)$$

(29)

where the tire coefficients $B_a$, $C_a$, and $D_a$ are experimentally identified and the slip angle ${\alpha }_a$—the angle between the tire centerline and its velocity vector—is computed as follows:

$$\begin{array}{cc}\hfill {\alpha }_F& =arctan\left({\displaystyle \frac{v_y+l_{F}r}{v_x}}\right)-\delta \hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill {\alpha }_R& =arctan\left({\displaystyle \frac{v_y-l_{R}r}{v_x}}\right)\hfill \end{array}$$

(31)

The longitudinal force $F_x$ is given as follows:

$$F_x=2{\displaystyle \frac{T_{max}\times \varphi \times P}{r_{wheel}}}-C_{roll}\times m\times g+{\displaystyle \frac{1}{2}}\rho \times C_d\times A_f\times v_x^2$$

(32)

where $T_{max}$ is the maximum available torque at each of the two rear-axle in-wheel motors. $\varphi$ is the transmission gear ratio and $C_{roll}$ is the roll resistance factor. Concerning the aerodynamic drag force, $\rho$ is the air density, $A_f$ is the vehicle frontal area used as a reference for the force calculation, and $C_d$ is the drag coefficient.

4. System Dynamics Learning

Gaussian process regression is used to predict the error between the vehicle model—Equations  (25)–(28)—and the available measurements, i.e., estimate $g_t$ in Equation (24). We do not make use of the uncertainty prediction. Thus, Equations (13) and (15) are disregarded in our application.

We assume that the modeling error only affects the dynamic part of the first-principle model [20], i.e., the velocity states. Therefore, the training outputs—(10)—are given by the difference in the velocity components between the measurement ${\mathit{x}}_{k+1}$ and the nominal model prediction:

$${\mathit{y}}_k={\mathit{B}}_d^{†}\left({\mathit{x}}_{k+1}-f({\mathit{x}}_k,{\mathit{u}}_k)\right)$$

(33)

where ${\mathit{B}}_d^{†}$—the Moore–Penrose pseudo-inverse of ${\mathit{B}}_d=[{\mathbf{0}}_{3\times 3};{\mathit{I}}_{3\times 3}]$—is a selection matrix introduced to consider only the velocity components. Hence, GPR predicts the error in the three velocities: $d\in \{e_{v_x},e_{v_y},e_r\}$, and $n_g=3$.

The GPR training input, i.e., the feature state, is $\mathit{z}=\{v_x;r;P;\delta \}$. This is based on the assumption that the model errors are independent of the vehicle position. This disregards potential modeling shortcomings introduced by, for instance, segments of the track that have different traction conditions, e.g., due to puddles. Further, we removed $v_y$ as we identified a strong correlation between this quantity and r. This is not surprising as both quantities characterize the lateral movement of the vehicle. The removal of $v_y$ instead of r is justified by the fact that it is difficult to precisely estimate $v_y$, while r is measured directly using a gyroscope. These approximations substantially reduce the learning problem’s dimensionality.

The covariance function—$k_{\mathit{z}\mathit{z}}^d$ in (12) and (13)—used is the squared-exponential kernel with the independent measurement noise component ${\sigma }_{n,d}^2{\delta }_{\mathit{z}\overline{\mathit{z}}}$:

$$k_{SE}^d(\mathit{z},\overline{\mathit{z}})={\sigma }_{f,d}^{2}exp\left(-{\displaystyle \frac{1}{2}}{\displaystyle \frac{{(\mathit{z}-\overline{\mathit{z}})}^{\mathrm{T}}(\mathit{z}-\overline{\mathit{z}})}{l_d^2}}\right)+{\sigma }_{n,d}^2{\delta }_{\mathit{z}\overline{\mathit{z}}}$$

(34)

Two sparse approximations have been explored: (i) SoD and (ii) FITC. The first approximation method is essentially a full GP that does not use all data available. This means that the computations for the error prediction ${\mathit{g}}_t$ in (24) are those of an exact GP given by (12). Meanwhile, for the second approach, the computations are those of (14).

The SoD approximation is often seen as naive as only a fraction of the available dataset is used. Hence, in order to improve the probability of good performance, rather than selecting the m points randomly, we use a technique to select points included in the active set that maximizes the information gain criterion [27].

Finally, it should be noted that some quantities can be pre-computed that otherwise could prevent real-time feasibility. In particular, ${\left({\mathit{K}}_{\mathit{Z}\mathit{Z}}^d+\mathit{I}{\sigma }_d^2\right)}^{-1}{\mathit{y}}^d$ in (12) only needs to be recomputed whenever the training data $\mathcal{D}$ are changed—this is the training step. In the SoD offline learning case, it is only computed once before the controller is launched. Inference corresponds to the rest of the computation of (12).

In the FITC approximation, the training step corresponds to the determination of the information vector ${\mathit{i}}^d$, which only requires recomputation when either the training dataset $\mathcal{D}$ or the inducing points $Z_{ind}$ are changed. In our application, however, the inducing points are updated at each sampling time. They are equally distributed along the last sampling time’s shifted predicted trajectory. This is a sensible placement of the inducing points since the new test cases are expected to be near the preceding ones, as the trajectory does not change significantly at consecutive sampling times. This means that the online adaptation of the dictionary $\mathcal{D}$ is immediately possible.

GPR is generally susceptible to outliers, which can hinder the model error learning performance [20]. Moreover, large and sudden changes in the GPR predictions can lead to erratic driving behavior. To attenuate these effects, we only include data points in the dictionary $\mathcal{D}$ (both online and offline) whose measurements fall within predefined bounds $\pm {\mathit{y}}_{lim}$, defined from physical considerations and empirical knowledge.

The hyperparameters are tuned offline based on pre-collected data. There are two main reasons that this is not done online. First, this optimization is not real-time feasible. Second, it is assumed that the general trend of the model error remains constant throughout the vehicle’s operation.

5. Implementation and Results

5.1. Implementation

Figure 4 exhibits the autonomous racing software architecture used by the FST Lisboa team. In the Trackdrive event, the entire control module in Figure 4 (green nodes) is substituted by the TC-LMPC algorithm.

FSSIM (https://github.com/AMZ-Driverless/fssim (accessed on 20 April 2023)) is the vehicle simulator used. AMZ Driverless developed this vehicle simulator, dedicated to the FSD competition, and released it open-source to other teams. This team reported 1% lap time simulator accuracy compared with their actual FSG 2018 10-lap trackdrive run [34]. Due to real-time requirements, this simulator does not simulate raw sensor data, e.g., camera or LiDAR data. Instead, cone observations around the vehicle are simulated using a given cone sensor model. This means that, in simulation, the perception module in Figure 4 (red nodes) is not the same as in actual prototype testing.

We developed a custom ROS/C++ implementation of the TC-LMPC architecture using FORCESPRO [24,25]—a solver designed for the embedded solving of MPC—to solve the optimization problem. The computations associated with GPR model error prediction described in Section 4 resort to the C++ open-source albatross (https://swiftnav-albatross.readthedocs.io/en/latest/index.html (accessed on 20 April 2023)) library developed by Swift Navigation.

5.2. Model Learning Analysis

In this section, we investigate the performance of the sparse GPR approximations for the prediction of the model error. We use the compound average error to evaluate the overall model learning performance: $\overline{\left|\right|e_{nom}\left|\right|}$ is the average 2-norm error of the nominal model, i.e., $\overline{\left|\right|e_{nom}\left|\right|}=\left|\right|{\mathit{B}}_d^{†}\left({\mathit{x}}_{k+1}-f({\mathit{x}}_k,{\mathit{u}}_k)\right)\left|\right|$, and $\overline{\left|\right|e_{GP}\left|\right|}$ is the corresponding error of the corrected dynamics, i.e., $\overline{\left|\right|e_{GP}\left|\right|}=\left|\right|{\mathit{B}}_d^{†}\left({\mathit{x}}_{k+1}-f({\mathit{x}}_k,{\mathit{u}}_k)-\mathit{g}\left({\mathit{z}}_k\right)\right)\left|\right|$. The results shown here correspond to the average over 10 laps. To evaluate the effectiveness of the proposed solution, the results shown were collected using FST Lisboa’s—the FSD team from the University of Lisbon—autonomous vehicle parameters in the MPC dynamic vehicle model, while using the AMZ Driverless car model in the simulator.

For the SoD approximation, we use the active set selection method described in Section 4 to find the active set of different sizes $m_{SoD}$ for each model d from a dictionary of $n_{SoD}=\mathrm{43,329}$, collected over 10 different runs with changing parameters. Morever, we use the active set of the SoD approximation as the training set for the FITC approximation, i.e., $n_{FITC}=m_{SoD}$.

Table 1. Model learning performance analysis.

${\mathit{m}}_{\mathit{S}\mathit{o}\mathit{D}}$	$\overline{\left|\right|{\mathit{e}}_{\mathit{n}\mathit{o}\mathit{m}}\left|\right|}$	$\overline{\left|\right|{\mathit{e}}_{\mathit{G}\mathit{P}}\left|\right|}$	${\mathit{m}}_{\mathit{F}\mathit{I}\mathit{T}\mathit{C}}$	$\overline{\left|\right|{\mathit{e}}_{\mathit{n}\mathit{o}\mathit{m}}\left|\right|}$	$\overline{\left|\right|{\mathit{e}}_{\mathit{G}\mathit{P}}\left|\right|}$
300	0.26	0.08	10	0.26	0.17
600	0.25	0.07	20	0.26	0.15
200	0.27	0.09	10	0.27	0.12
300	0.27	0.07	20	0.27	0.13

shows the model error prediction fitness for both sparse GPR approximations. For the SoD results, on the left, the horizontal dashed line separates the results of the offline (above) and online (below) schemes. For the online FITC results, on the right, the horizontal dashed line separates the results of $n_{FITC}=300$ (above) and $n_{FITC}=400$ (below) schemes.

We conclude that there is a positive correlation between $m_{SoD}$ and the model error fitting ability. In the offline fashion, the 2-norm average error reduction, i.e., reduction from $\overline{\left|\right|e_{nom}\left|\right|}$ to $\overline{\left|\right|e_{GP}\left|\right|}$, is 63.9% and 70.7% with $m_{SoD}=300$ and $m_{SoD}=600$, respectively. The computation cost evolves linearly with m but, within this range, takes acceptable values given the current node rate of 20 Hz. Arguably, we could further increase $m_{SoD}$ but likely with negligible model learning improvements.

Instead, we should aim to adapt the training data online as this would enable adaptation to changing conditions or even simply the collection of data from dynamic maneuvers not included in the original dictionary. With a dictionary size of $m_{SoD}\le 300$ data points, the model error reduction is 65.4% and 75.7% for $m_{SoD}=200$ and $m_{SoD}=300$, respectively. In particular, it enables more aggressive maneuvers toward the end of the event while keeping the corrected dynamics error relatively low.

In the online case, the model with $m_{SoD}=300$ performs significantly better than the model with $m_{SoD}=200$. While the average node processing time is well within the limits, it too often breaks the real-time requirement.

As explained in Section 4, the FITC sparse approximation is a natural candidate to enable online learning. The best-performing model with $m_{FITC}=10$ yields model learning performance comparable to that of the SoD approximation. However, the corrected dynamics average error is 0.12, slightly above the SoD benchmark value of 0.09.

5.3. Simulation Results—Model Mismatch Influence

In

Table 2. FSG lap times and model error.

	TC-LMPC		TC-LMPCML
Lap	Time [s]	$\overline{\left|\right|{\mathit{e}}_{\mathit{n}\mathit{o}\mathit{m}}\left|\right|}$	Time [s]	$\overline{\left|\right|{\mathit{e}}_{\mathit{n}\mathit{o}\mathit{m}}\left|\right|}$	$\overline{\left|\right|{\mathit{e}}_{\mathit{G}\mathit{P}}\left|\right|}$
1	19.36	0.22	18.92	0.21	0.06
2	19.33	0.21	18.75	0.21	0.07
3	19.34	0.21	18.73	0.21	0.07
4	19.33	0.22	18.71	0.21	0.07
5	17.78	0.29	17.17	0.29	0.08
6	17.45	0.30	16.85	0.31	0.11
7	17.44	0.31	16.77	0.32	0.13
8	17.51	0.29	16.84	0.32	0.11
9	17.43	0.30	16.80	0.31	0.12
10	17.43	0.30	16.96	0.32	0.13

, we show the lap times along the 10-lap trackdrive event and the average model error for the FSG track. The initial safe set was collected using a pure pursuit controller [35] following the track centerline. It is composed of four laps with lap times of around 28.80 s. If an initial safe set containing faster trajectories is collected, the initial laps would be faster but would eventually converge to approximately the same lap time. The controllers herein have a prediction horizon of $N=20$, which corresponds to a look-ahead time of 1 s, unless stated otherwise. The TC-LMPC results prove the iterative improvement nature of the architecture. The first lap is immediately 33% faster compared to the path-following controller. Equivalently, the last lap is 39% faster. Furthermore, the last lap corresponds to a 10% improvement compared to the first TC-LMPC lap.

Figure 5 shows the 10-lap FSG trackdrive trajectories using the TC-LMPC strategy. The finish line is at the origin and the vehicle runs clockwise. It can be seen that TC-LMPC exploits the track layout to improve the vehicle’s performance. Nevertheless, the third column of Table 2 shows a severe model mismatch, which causes the vehicle to violate the track constraint. See, for instance, the exit of the hairpin, where the trajectory is on top of the track boundary. This indicates that a cone was hit, since the trajectory corresponds to the center of mass. Furthermore, the approach to the slalom segment is not considered optimal by empirical vehicle dynamics standards. The vehicle brakes too late, which leads to a slower slalom with greater steering actuation required.

We now analyze the performance of the controller when the GPR model learning scheme is deployed. Table 2—TC-LMPC_ML—shows that the last lap is 41% and 10% faster when compared to the pre-collected path-following lap and the first lap, respectively. Furthermore, the total event time, 176.5 s, is 5.9 s faster than when using TC-LMPC, a 3% improvement. These results correspond to the online SoD model with $m_{SoD}=200$. Figure 6 shows the corresponding 10-lap FSG trackdrive trajectories. It is clear that the reduced model mismatch prevented the vehicle from violating the track constraint. However, it seems that there is still room for improvement during the slalom segment.

5.4. Simulation Results—Prediction Horizon Influence

We subsequently tested TC-LMPC_ML with increasing prediction horizons. In

Table 3. FSG lap times and model error [$N=30$].

	Default Parameters			Aggressive Parameters
Lap	Time [s]	$\overline{\left|\right|{\mathit{e}}_{\mathit{n}\mathit{o}\mathit{m}}\left|\right|}$	$\overline{\left|\right|{\mathit{e}}_{\mathit{G}\mathit{P}}\left|\right|}$	Time [s]	$\overline{\left|\right|{\mathit{e}}_{\mathit{n}\mathit{o}\mathit{m}}\left|\right|}$	$\overline{\left|\right|{\mathit{e}}_{\mathit{G}\mathit{P}}\left|\right|}$
1	17.16	0.27	0.07	16.37	0.39	0.14
2	16.92	0.26	0.07	16.18	0.37	0.15
3	16.91	0.26	0.07	16.17	0.37	0.15
4	16.88	0.26	0.07	16.13	0.37	0.15
5	16.69	0.28	0.08	16.17	0.36	0.15
6	16.66	0.28	0.09	16.21	0.37	0.15
7	16.67	0.28	0.08	16.19	0.36	0.15
8	16.68	0.28	0.08	16.14	0.37	0.16
9	16.68	0.27	0.08	16.15	0.37	0.16
10	16.69	0.27	0.08	16.11	0.36	0.15

, we display the lap times and modeling errors for two sets of controller gains with $N=30$ or a look-ahead time of 1.5 s. The results on the left correspond to the parameters used thus far in this section. For the controller on the right, we reduced some derivative costs and the regularization cost on $v_y$ and increased the Q-function-associated cost to promote greater track progress at each sampling time.

With a longer horizon, both controllers achieve optimal trajectories in the slalom segment and can safely navigate the vehicle around the track such that the safe set loses its relative importance. Specifically, the controllers are able to predict consistently until the slowest point on a given corner. In this way, the information conveyed by the safe set regarding the types of maneuvers that follow is not as valuable. The safety characteristic only applies when model learning is deployed. Otherwise, the severe model mismatch hinders the performance. This is substantiated by the fact that both controllers achieve small lap times in the first few laps and quickly converge to their steady-state lap times of around 16.75 s for the default controller and 16.16 s for the aggressive controller.

Both models used the offline version of the SoD approximation with $m_{SoD}=600$. The model learning scheme is able to significantly reduce the model mismatch to enable safe aggressive racing. For the first case, with default parameters, the corrected dynamics model average error is around 0.08, which corresponds to a reduction of about 70% compared to the nominal model. Meanwhile, for the aggressive controller, the final model mismatch is on average 0.15, a reduction of 60%. The model learning scheme can achieve an acceptable value of corrected dynamics model mismatch, even in the case of offline model learning on the aggressive controller. It should be noted that such high nominal model errors were not present in the original training dataset.

6. Conclusions and Future Work

This paper presented the extension of the results of the TC-LMPC architecture to the autonomous electric racing FSD context. We have demonstrated that the TC-LMPC using a dynamic bicycle model with an appropriate terminal safe set and Q-function leads to safe iterative improvements. Despite the significant model mismatch, the best lap time is reduced by 10%, when compared to the first lap using the same controller, at the 10-lap FS event. When using GPR with the active training set selection method introduced for model learning, we are able to successfully predict the modeling error, which leads to a 5.9 s total time reduction for the 10-lap FSD event.

The main limitation of the proposed solution, inherent in the use of model-based controllers, is model mismatch. This can impact the feasibility of the solution, and the theoretical guarantees of the TC-LMPC listed above might not hold. The other major limitation is that this architecture requires pre-collected data, which requires a suboptimal but safe controller. Alternatively, replication of this work may be possible by resorting to manual data collection, which should be available in most practical applications.

These simulation results are promising for an experimental implementation on the full-sized FST10d electric racing prototype. An important focus of the research on learning model-based controllers has been on reducing model mismatch. An implementation of an online dictionary management procedure [20] is under study. Further, research work is underway to test the Automatic Relevance Determination GPR kernel and other ML techniques, such as the use of Bayesian linear regression or neural networks. The minimum-time stage cost for the TC-LMPC Q-function could be augmented such that points that yield long-term benefits are more favored. We are also studying the use of the state uncertainty measurement, which is inherent with the application of Gaussian processes, in a robust learning-based model predictive control framework with constraint tightening. Finally, the automatic adjustment of the MPC parameters using a reward function exploited by a reinforcement learning algorithm is also a future line of research.