**1. Introduction**

In recent years, research on the automotive field has focused on making cars more and more able to make decisions autonomously due to the growth of electric and electronic technologies on modern road cars and to the possibility of providing increased safety and improved performance.

In the racing world, where the scenario is static and known, the perception system has a simple implementation, and good results have already been achieved, as shown, for example, in [1].

The main challenge is to improve performance through the use of models that represent the dynamics of the vehicle as faithfully as possible while ensuring an acceptable computational effort for real-time implementation.

The aim of this paper was to develop a trajectory planner able to update the trajectory online with the car progress on the track, trying to replicate the capabilities of a human

**Citation:** Montani, M.; Ronchi, L.; Capitani, R.; Annicchiarico, C. A Hierarchical Autonomous Driver for a Racing Car: Real-Time Planning and Tracking of the Trajectory. *Energies* **2021**, *14*, 6008. https://doi.org/10.3390/en14196008

Academic Editors: Guzek Marek, Rafał Jurecki and Wojciech Wach

Received: 8 July 2021 Accepted: 17 September 2021 Published: 22 September 2021

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

driver. Then, the trajectory planning was embedded with a path tracking consisting of a Linear Quadratic Regulator (LQR) that provides the car's input signals: steering wheel angle, accelerator and brake pedals signals.

In this way, even if in a racing world scenario, real-time trajectory optimization can ensure a possible future implementation in the urban area where obstacles and dynamic scenarios could be faced.

To ensure a trajectory tracking able to reduce the lap time, the state-of-the-art offers different approaches both from the architecture and methodology point of view. From the architecture point of view, the research is divided between the ones that used a real-time single algorithm to plan and track the trajectory [2,3], others that preferred to separate the planner from the tracker by having the first offline and the second work in real-time [4], and others that divided trajectory planning and tracking but implemented them both in realtime [5–7]. The first architecture type, as said in [8], gives the advantage of incorporating tire force constraints in a straight-forward way. However, the integration of trajectory planning and tracking approaches can work for autonomous RC-cars but is difficult to scale to complex scenarios due to the non-convex nature of the constraints imposed by multiple vehicles. In addition, it implies a more complex tuning. Instead, there are many studies about the offline trajectory planner with good performances in terms of lap time reduction and car set-up, allowing to implement a vehicle model with more Degrees of Freedom (DoF) [9], adding also a friction coefficient mapping [4] and involving different methodologies. Some authors preferred to develop a geometrical solution by minimizing the curvature [8] or generating a racing line using professional driving techniques [10]. However, these techniques do not provide information about vehicle dynamics, despite the fact that the trajectory of least curvature ensures lap similar to the trajectory optimization techniques [8]. Therefore, other researchers preferred to face the problem as a kinodynamic problem by finding the optimal trajectory, minimizing the lap time and considering both the kinematic constraints of the track and the dynamic constraints of the vehicle [4,11]. Facing the problem from the kinodynamic point of view considers that the non-convexity of the trajectory does not allow a global solution [8,10] but a local one [12–14] using Sequential Convex Programming (SCP) methodology [7].

The study presented in this paper has involved the development of a trajectory planner that every 0.1 s provides the speed and trajectory that the car follows thanks to an implemented path tracking that controls the car motion every 0.001 s. The realtime implementation of both trajectory planning and path tracking is advantageous in terms of performance and safety. In fact, during the race, a professional driver is able to feel the car conditions and choose the trajectory that ensures the best performance. In the same way, a trajectory planner must provide the fastest trajectory for the dynamic and scenario conditions in which the vehicle is located. In [7], it was decided to use a point mass vehicle model and implement a Model Predictive Control (MPC) with the SCP approach. To simplify the algorithm without incurring in infeasibility and further reduce the computational time, it was chosen to solve the Linear Programming (LP) problem instead of the Quadratic Programming (QP) problem and to reduce the losses due to linearization, it was chosen to represent the car's GG-V with conical constraint equations. Furthermore, the weights and variables of optimization change with the track sequences, allowing the designer to change the constraints with changes in scenario and vehicle dynamics.

In Section 2, the architecture developed is shown. Then, in Section 3, the trajectory planner model and methodologies are explained with the hypothesis and the reference system used, followed by Section 4 where the path tracking developed is shown. In the last section, the tests performed will provide the results obtained by the algorithm in terms of the car's trajectory and lap time, comparing them with those obtained by the CarRealTime (CRT) MaxPerformance event, and by a professional driver in a real-time car simulator.

#### **2. Trajectory Tracking Architecture**

In Figure 1, the architecture developed is shown. The idea is to divide the trajectory planning from the path tracking instead of implemented a single MPC with a complex and detailed vehicle and tire model by ensuring a real-time implementation of the algorithm that the current technologies do not allow otherwise. Thus, an LP optimizer is implemented as trajectory planning, giving the reference position and speed to minimize the lap time every 0.1 s, and an optimum controller is implemented to allow the car to follow reference state values by defining the input steering wheel angle, throttle pedal and braking pedal every 0.001 s. These sample time values were necessary because an increase compared to the time of 0.1 s in providing the position and speeds that the vehicle must have is excessive compared to the speed of the vehicle resulting in a delay with respect to its position; from the tests undertaken, an increase in the time of 0.001 s of the controller was found to worsen the performance of the control.

**Figure 1.** System architecture.

The trajectory planning and path tracking use the states of the car in the race track as feed-back. These states are estimated by a Localization Layer composed of an Unscented Kalman Filter (UKF) that uses the sensors measurements shown in Table 1.


All the layers shown in Figure 1 were implemented in Matlab-Simulink software and then compiled in C to be used in the real-time car simulator of Vi-Grade. The different control algorithms were integrated into the same Simulink model considering the rate transition between subsystem with different sample times. Since the difference in time step implies that the optimization layer provides the same value for 100 steps of the controller, an integration algorithm between the path planning and trajectory controller is developed. This algorithm ensures that the positions and velocities provided by the optimizer to the controller increase with the constant acceleration optimized, and after 100 steps, the vehicle achieves the position and velocity values expected by the optimizer. In the event that the vehicle fails to be in the expected condition, the change of values given by the change of optimizer steps needs to be compatible with the vehicle's possible acceleration limits based on the state the vehicle is in. The logic is shown in Equation (1):


$$X = \begin{array}{c} X\_0 - \Delta t\_c V\_x & \text{if } abs(V\_{x0} - V\_x) > 0.5, \quad \Delta t\_o \neq 0 \\ X\_0 - \Delta t\_c V\_x & \text{if } abs(V\_{x0} - V\_x) \le 0.5, \quad \Delta t\_o = 0 \end{array}$$

$$Y = \begin{array}{c} Y\_0 - \Delta t\_c V\_y & \text{if } abs(V\_{y0} - V\_y) > 0.5, \quad \Delta t\_o \neq 0 \\ Y\_0 - \Delta t\_c V\_y & \text{if } abs(V\_{y0} - V\_y) \le 0.5, \quad \Delta t\_o = 0 \end{array} \tag{1}$$

where *Vx*, *Vy*, *X*,*Y* are the speeds and positions that the car has to follow in the current time; *Vx*0, *Vy*0, *X*0,*Y*<sup>0</sup> are the speeds and positions that the car had at the previous time step of the controller; Δ*tc*, Δ*to* are, respectively, the controller sample time of 0.001 s, and the difference between the actual and previous time of the optimizer in the sample time of the controller; 0.1 is a tuning value that reduced the speed step between the actual and the future value if it is higher than 0.5 m/s.

#### **3. Trajectory Planning**

To set-up the MPC optimization problems, the Gurobi solver is used, a commercial solver with parallel algorithms for large-scale linear programs, quadratic programs, and mixed integer programs. From the Gurobi optimization reference manual [15], the problem was implemented in the following form:

$$\underset{z}{\text{minimize}}\quad -\boldsymbol{t}\_{j(\boldsymbol{\rho}^{(\text{Hn})})}^{T}\boldsymbol{\mathfrak{p}}^{(\text{Hn})} + \sum\_{i=1}^{\text{Hn}} \left(\mathcal{W}\_{(j(\boldsymbol{\rho}^{(i)}))}\boldsymbol{\eta}^{(i)} + \mathcal{R}\_{\chi\boldsymbol{F}(j(\boldsymbol{\rho}^{(i)}))}\boldsymbol{\delta}^{(i)} + \cdots \right) \tag{2}$$

$$\dots + \mathcal{R}\_{\chi\mathbb{R}(j(\boldsymbol{\rho}^{(i)}))}\boldsymbol{\lambda}^{(i)} + \mathcal{R}\_{\gamma(j(\boldsymbol{\rho}^{(i)}))}\boldsymbol{\rho}^{(i)}$$

$$\mathbf{x}^{(i)} = A\mathbf{x}^{(i-1)} + B\mathbf{u}^{(i-1)} \quad \forall i = 1, \cdots, \prime, Hn \tag{3}$$

$$N\_{j(\boldsymbol{\rho}^{(i)})} \boldsymbol{\mathfrak{p}}^{(i)} - \boldsymbol{\eta}^{(i)} = \text{diag}(N\_{j(\boldsymbol{\rho}^{(i)})}^T) T\_{(j(\boldsymbol{\rho}^{(i)})} \quad \forall i = 1, \cdots, H \text{n} \tag{4a}$$

$$A\_{\text{acc}}(\mathfrak{v}^{(i)}, \mathfrak{o}^{(k)}) \cdot \mathfrak{u}^{(i)} \le b\_{\text{acc}}(\mathfrak{v}^{(i)}) c\_{(j(\rho^{(i)})}^2 \quad \forall i = 1, \dots, Hn \tag{4b}$$

$$\forall k = 1, \dots, tn$$

$$\begin{bmatrix} R\ddot{r}\boldsymbol{\sigma}\_{(i-1,i)} \end{bmatrix} \begin{bmatrix} \boldsymbol{\mu}^{(i-1)} \\ \boldsymbol{\mu}^{(i)} \end{bmatrix} - \begin{bmatrix} \boldsymbol{\delta}^{(i)} \\ \boldsymbol{\lambda}^{(i)} \\ \boldsymbol{\rho}^{(i)} \\ \boldsymbol{\rho}^{(i)} \end{bmatrix} = \boldsymbol{0} \quad \forall i = 1, \cdots, Hn \tag{4c}$$

$$p\_x^{(i)} - \upsilon\_{t\max} \Delta t \le p\_x^{(i)} \le p\_x^{(i)} + \upsilon\_{t\max} \Delta t \quad \forall i = 1, \dots, Hn \tag{5a}$$

$$
\vec{p}\_y^{(i)} - \upsilon\_{t\max} \Delta t \le p\_y^{(i)} \le \vec{p}\_y^{(i)} + \upsilon\_{t\max} \Delta t \quad \forall i = 1, \dots, Hn \tag{5b}
$$

$$-\upsilon\_{\mathfrak{t}\max} \le \upsilon\_{\mathfrak{x}}^{(i)} \le \upsilon\_{\mathfrak{t}\max} \quad \forall i = 1, \dots, Hn \tag{5c}$$

$$-\upsilon\_{t\max} \le \upsilon\_y^{(i)} \le \upsilon\_{t\max} \quad \forall i = 1, \cdots, Hn \tag{5d}$$

$$-A\_{\text{Max}}(\vec{v}\_t) \le a\_x^{(i)} \le A\_{\text{Max}}(\vec{v}\_t) \quad \forall i = 1, \dots, Hn \tag{5e}$$

$$-A\_{\text{Max}}(\vec{v}\_t) \le a\_y^{(i)} \le A\_{\text{Max}}(\vec{v}\_t) \quad \forall i = 1, \dots, Hn \tag{5f}$$

$$
\eta^{(i)}, \delta^{(i)}, \lambda^{(i)}, \rho^{(i)} \ge 0 \quad \forall i = 1, \cdots, Hn \tag{5g}
$$

where the Objective Function (2), the Vehicle Model (3), the constraints used to represent the vehicle dynamic and track limitation (4), and the Bound Constraints (5) are shown and will be explained in the following subsections.

The problem is faced as an LP optimization, and its solutions are found by iterating the optimization every time step: the positions, *p*˜(*i*), velocities, *v*˜(*i*), and accelerations, *u*˜(*i*), are the state values optimized at the previous step; so the current iteration defines the optimum state, *x* = [*px*, *py*, *vx*, *vy*], and input, *u* = [*ax*, *ay*] for all the prediction horizon lengths. The

variables weights of the Objective Function, *W*(*j*(*p*˜(*i*))) − *RχF*(*j*(*p*˜(*i*))) − *RχR*(*j*(*p*˜(*i*))) − *Rγ*(*j*(*p*˜(*i*))), are defined by a tuning process, and the prediction horizon was chosen to be composed of 120 time steps to ensure that the optimizer should be able to see the following curve when the longest straight begins, as tests have shown that the performance was satisfactory.

#### *3.1. Models*

To ensure a computational cost of 0.1 s, the vehicle was modeled as a point mass model, with the following assumption:


Therefore, referring to the coordinate system shown in Figure 2: *x*,*y* are the global coordinate system when stationary with respect to the racetrack; *χ*,*γ* are the vehicle coordinate system; and *ψ* is the vehicle yaw angle that indicates the tangent to the trajectory. The dynamic model was implemented in the discrete state space Equation (6) and detailed by Equation (7).

$$\mathbf{x}^{(i)} = A\mathbf{x}^{(i-1)} + B\boldsymbol{u}^{(i-1)} \tag{6}$$

$$
\begin{bmatrix} p\_x^{(i)} \\ p\_y^{(i)} \\ v\_x^{(i)} \\ v\_y^{(i)} \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} p\_x^{(i-1)} \\ p\_y^{(i-1)} \\ v\_x^{(i-1)} \\ v\_y^{(i-1)} \end{bmatrix} + \begin{bmatrix} \Delta t^2 & 0 \\ 0 & \frac{\Delta t^2}{2} \\ 0 & \frac{\Delta t}{2} \\ \Delta t & 0 \\ 0 & \Delta t \end{bmatrix} \begin{bmatrix} a\_x^{(i-1)} \\ a\_y^{(i-1)} \\ a\_y^{(i-1)} \end{bmatrix} \tag{7}
$$

where, in this case, *<sup>i</sup>* indicates the time-step index; *<sup>x</sup>*(*i*) <sup>∈</sup> <sup>R</sup><sup>4</sup> is the state vector composed by the vehicle position and velocity vector in the global coordinate system, *p* = [*px*, *py*] and *<sup>v</sup>* = [*vx*, *vy*]; *<sup>u</sup>*(*i*) <sup>∈</sup> <sup>R</sup><sup>2</sup> is the input vector composed by the vehicle acceleration vector in global coordinate system, [*ax*, *ay*]; *<sup>A</sup>* <sup>∈</sup> <sup>R</sup>4*x*<sup>4</sup> is the system matrix and *<sup>B</sup>* <sup>∈</sup> <sup>R</sup>4*x*<sup>2</sup> is the input matrix made explicit in Equation (7). From these equations, it is possible to deduce that to have a linear system, the time-step, Δ*t*, must chose constant. Thus, the prediction horizon is discretized with constant time intervals, and the discrete model, shown in Equation (3), is then iterated forward in time from a time-step to the subsequent one.

**Figure 2.** Global and vehicle coordinate systems.

### *3.2. Objective Function*

The Objective Function proposed in this work is purely linear, as shown in Equation (2). This is a benefit because it reduces the optimization time and allows one to not worry about defining a positive objective matrix to ensure a feasible solution. It is composed of two strategies:


The first strategy aims to minimize the lap time, and the second one allows softening the hard limit of the constraints.

#### 3.2.1. Minimizing Time Strategy

Since a constant discretization time Δ*t* of the prediction horizon is chosen to make Equation (7) linear, maximizing the distance traveled in the direction of the tangent vector of the mid-line could be a strategy to cover the largest number of racetrack sectors. This strategy was expressed within the objective function as follows:

$$\underset{z}{\text{minimize}}\quad \left[ \begin{array}{c} -t\_{j(\vec{p}^{(Hn)})}^T \\ p\_y^{(Hn)} \end{array} \right]\_{\vec{p}} = -t\_{j(\vec{p}^{(Hn)})}^T p^{(Hn)} \tag{8}$$

where it maximized the longitudinal and lateral distance between the current, *<sup>p</sup>*(*Hn*) *<sup>x</sup>*,*<sup>y</sup>* , and previous, *<sup>p</sup>*˜(*Hn*), position points projected in the tangent direction, *tj*(*p*˜(*Hn*)), for all the points of the prediction horizon *Hn*.

#### 3.2.2. Slack Variables

To avoid the infeasible problem, the slack variable strategy is used [16]. This methodology ensures avoiding that the constraints equations are not strictly equal to a constant limit but to an enlarged or reduced one by adding to the constraint a positive variable, weighted in the objective function. In this way, the designer can strengthen or relax the constraints by increasing or decreasing the associated weights.

$$\underset{\boldsymbol{z}}{\text{minimize}} \quad \sum\_{i=1}^{Hn} \left( \mathcal{W}\_{\left(j\left(\boldsymbol{\rho}^{(i)}\right)\right)} \boldsymbol{\eta}^{(i)} + \mathcal{R}\_{\chi \boldsymbol{F}\left(j\left(\boldsymbol{\rho}^{(i)}\right)\right)} \boldsymbol{\delta}^{(i)} + \cdots$$

$$\cdots + \mathcal{R}\_{\chi \boldsymbol{R}\left(j\left(\boldsymbol{\rho}^{(i)}\right)\right)} \boldsymbol{\lambda}^{(i)} + \mathcal{R}\_{\gamma\left(j\left(\boldsymbol{\rho}^{(i)}\right)\right)} \boldsymbol{\rho}^{(i)}\right) \tag{9}$$

In Equation (9), *<sup>η</sup>*(*i*), *<sup>δ</sup>*(*i*), *<sup>λ</sup>*(*i*) and *<sup>ρ</sup>*(*i*) are the slack variables and *<sup>W</sup>*(*j*(*p*˜(*i*))), *<sup>R</sup>χF*(*j*(*p*˜(*i*))), *RχR*(*j*(*p*˜(*i*))), *Rγ*(*j*(*p*˜(*i*))) are the weights, respectively. In this work, by associating the weights to the nodes of the mid-line, (*j*(*p*˜(*i*))), it is possible to impose to the solver not only the ability to vary the constraints but also to decide whether one point of the racetrack is more important than another in terms of limit variations available, as a human driver can do.

#### *3.3. Linear and Quadratic Constraints*

To ensure optimized solutions compatible with vehicle dynamics and track limits, the vehicle accelerations and positions are limited by constraint Equations (4), which can be divided into three categories:


#### 3.3.1. Racetrack Limitations

In order to constrain the optimizer to provide output car positions within the path area, the optimization was solved by using the SCP approach as [17] suggested. Thus,

the constraint is not the entire racetrack area, which involves a non-convex problem, but smaller racetrack sections where the problem is convex and so solvable. Therefore, it was decided to sample the middle line with points 1 m apart, *TMid*(*j*), and to make these correspond to those intercepted on the inner and outer edge by the normal vector to the middle line and named, respectively, with *TIN*(*j*) and *TOUT*(*j*) in Figure 3. Then, it was possible to constrain the output states of the solver, *p*(*i*), within the path area, using the following equations:

$$\begin{aligned} \left(\boldsymbol{\mathfrak{p}}^{(i)} - \boldsymbol{T}\_{\operatorname{Out}(j(\boldsymbol{\mathfrak{p}}^{(i)}))}\right)^{T} \left(-\boldsymbol{n}\_{j(\boldsymbol{\mathfrak{p}}^{(i)})}\right) - \boldsymbol{\eta}^{(i)} &= \boldsymbol{0} \quad \forall i \in \{1, \cdots, \cdots, H\boldsymbol{n}\} \\ \left(\boldsymbol{\mathfrak{p}}^{(i)} - \boldsymbol{T}\_{\operatorname{In}(j(\boldsymbol{\mathfrak{p}}^{(i)}))}\right)^{T} \left(\boldsymbol{n}\_{j(\boldsymbol{\mathfrak{p}}^{(i)})}\right) - \boldsymbol{\eta}^{(i)} &= \boldsymbol{0} \quad \forall i \in \{1, \cdots, \cdots, H\boldsymbol{n}\} \\ \boldsymbol{\eta}^{(i)} &\geq \boldsymbol{0} \quad \forall i \in \{1, \cdots, \cdots, H\boldsymbol{n}\} \end{aligned} \tag{10}$$

where *p*(*i*) is the position state vector found by the optimization in the current optimization step; *TOut*(*j*(*p*˜(*i*)) and *TIn*(*j*(*p*˜(*i*)) are the limits given by the outer and inner edge referred to the solution found in the previous iteration step, *<sup>p</sup>*˜(*i*); *nj*(*p*˜(*i*)) is the normal direction to the mid-line, which allows the distance between the two points to be projected in the normal direction; and *η*(*i*) is the slack variable, which, being constrained in the Bound Constraints Equation (5g) as positive, forces the position of the vehicle to be less than zero making the racetrack constraints more or less rigid depending on the weight assigned to it in the Objective Function (2). Therefore, it is possible to vary the weight of the slack variable in a differentiated way according to the position of the car in the track, being able, for example, to soften it in the presence of a curb by allowing the car to go out of the limits of the track where possible to improve performance.

**Figure 3.** Representation of the racetrack linear approximation.

### 3.3.2. GG Limitations

In order to represent the exchange of forces occurring at the point of contact between wheel and road, it was decided to not implement a tire model but to bound the car's acceleration inside the GG diagram, measured at the center of gravity under steady-state conditions, such as [7,18,19].

The papers [20–22] show that the acceleration constraints area has the shape of two semi-ellipses: one stands for the forward acceleration, shown by the blue line in Figure 4a, and the other stands for backward acceleration, shown by the red line in Figure 4a.

**Figure 4.** Single GG diagram of the vehicle and its change with the vehicle speed. (**a**) Approximation method for the g-g diagram: two half ellipses for tire limits and a line for engine limit. (**b**) Autonomous driving car GG-V diagram, evaluated through simulation tests.

Empirically defining the maximum acceleration values, *AγMax*(*vt*), *AχbackwardMax*(*vt*) and *A<sup>χ</sup> f orwardMax*(*vt*), the semi-axes of the ellipses are defined and so, the tire, aerodynamic and vehicle system limits are taken into account. Therefore, if the blue and red lines in Figure 4a show the limits just mentioned, the horizontal orange line shows the maximum performance that the car's engine can achieve in forward acceleration. In addition, these limits change with the vehicle speed because of the drag and engine influence [23,24]; thus, in Figure 4b, different GG diagrams were implemented in the function of the car speed obtaining the car GG-V diagram.

However, to better represent the vehicle dynamic and improve the performance, the idea was to implement the GG-V as quadratic constrains and not linearize its curves as is usually done, while maintaining the sample time under the specification of 0.1 s. However, due to the ability of the LP to handle only one convex quadratic constraint for each variable, a mixed approach was implemented: a conical constraint to reproduce the bigger, lower region associated with the vehicle braking capabilities exactly, and linear constraints to approximate the upper region, which is associated with the vehicle forward acceleration capabilities, being tested empirically, resulting in this region being well-described by the three orange dotted lines shown in Figure 4a. Therefore, the lower GG constraints are implemented as a negative semi-ellipse with the semi-axes values equal to the car's maximum deceleration. This inequality constraint is shown in the vehicle coordinate system by Equation (11) and in the global coordinate system by Equation (12).

$$(A\_{\chi\text{backward}})^2 + (\frac{A\_{\gamma}}{A\_{\gamma\text{Max}}(v\_t)})^2 - (\frac{A\_{\gamma}}{A\_{\gamma\text{Max}}(v\_t)})^2 - c^2 \le 0 \tag{11}$$

$$\begin{split} &a\_x^2(\frac{\upsilon\_x^2}{\upsilon\_t^2}A\_{\gamma \text{Max}}^2 + \frac{\upsilon\_y^2}{\upsilon\_t^2}A\_{\chi \text{backward} \text{Max}}^2) + a\_y^2(\frac{\upsilon\_y^2}{\upsilon\_t^2}A\_{\gamma \text{Max}}^2 + \frac{\upsilon\_x^2}{\upsilon\_t^2}A\_{\chi \text{backward} \text{Max}}^2) + \\ &+ a\_x a\_y(\frac{\upsilon\_x \upsilon\_y}{\upsilon\_t^2}(A\_{\gamma \text{Max}}^2 - A\_{\chi \text{backward} \text{Max}}^2)) - A\_{\chi \text{backward} \text{Max}}^2 A\_{\gamma \text{Max}}^2 c^2 \le 0 \end{split} \tag{12}$$

Instead, the upper GG constraints are represented as a set of lines tangent at the positive semi-ellipse. These lines are calculated by finding the tangents at the ellipse in points described by different angles, *θ*, thanks to Equation (13) in the coordinate vehicle system and Equation (14) in the global coordinates system.

$$(A\_{\chi\_{forwardMax}}^{A\_{\chi\_{forwardMax}}}) \cos \theta + (\frac{A\_{\gamma}}{A\_{\gamma Max}(v\_t)}) \sin \theta - c \le 0 \tag{13}$$

$$\begin{split} a\_x(\frac{\upsilon\_x}{\upsilon\_t}A\_{\gamma \text{Max}}\cos\theta - \frac{\upsilon\_y}{\upsilon\_t}A\_{\chi \text{forwardMax}}\sin\theta) + a\_y(\frac{\upsilon\_y}{\upsilon\_t}A\_{\gamma \text{Max}}\cos\theta + \\ + \frac{\upsilon\_x}{\upsilon\_t}A\_{\chi \text{forwardMax}}\sin\theta) - A\_{\chi \text{forwardMax}}A\_{\gamma \text{Max}}c \le 0 \end{split} \tag{14}$$

In all the equations shown, the ratio between the longitudinal speed component, *vx*, and speed vector, *vt* = *v*2 *<sup>x</sup>* + *v*<sup>2</sup> *<sup>y</sup>*, is the cosine of the yaw angle *ψ*; and the ratio between the lateral speed component, *vy*, and speed vector, *vt* = *v*2 *<sup>x</sup>* + *v*<sup>2</sup> *<sup>y</sup>*, is the sine of the yaw angle *ψ*. Both of these ratios are used to rotate the input vector *u* = [*ax*, *ay*] in the global coordinate system and group the constraints in Equation (4b).

The variable *c* is a scalar coefficient that can expand or narrow the boundary of the GG diagram and change the tire performances representing degraded contact conditions due, for example, to rain or tire wear. *c* could be scaled in a global manner for all of the track or could be varied online in local sections being related to the position of the vehicle on the racetrack.

#### 3.3.3. Jerk Limitations

Since the vehicle is modeled as a point mass, sudden variations in acceleration, *u* = [*ax*, *ay*], can lead to the generation of an angular trajectory that does not represent a real trajectory of a racing car, which actually tends to be smooth. Usually, to avoid this problem, the quadratic variation of the accelerations is implemented as a term of the Objective Function to minimize it. In this study, to maintain the Objective Function as linear, the Jerk Limitations are implemented as constraint equations that require the jerks to be equal to positive slack variables. Therefore, the slack variables are inserted in the cost function as a linear term and a certain weight is assigned to them according to the point on the track where the car is located. In this way, the optimizer is able to determine the accelerations variation necessary to increase performance and maintain a smooth trajectory, and the designer can choose which direction of acceleration to favor in relation to where the car is located on the track, e.g., in a straight line the lateral acceleration can be forced to vary more slowly, favoring a faster variation of the longitudinal accelerations.

In Equation (4c), the strategy is shown, where  *δ*(*i*), *λ*(*i*), *ρ*(*i*), *ρ*(*i*) , are the slack variables used to soften the zero hard limit given to the input vector variation,  *u*(*i*−1), *u*(*i*) , rotated in the global coordinate system by *Rrot* ˜ (*i*−1,*i*) . In addition, it was chosen to implement different slack variables for different acceleration vectors diversifying the jerk of the various dynamic system: *δ* for forward longitudinal acceleration, *λ* for backward longitudinal acceleration, and *ρ* for lateral acceleration.

#### *3.4. Bound Constraints*

As usual, the optimization variables must be limited to effectively guide the solver in the optimization phases. This allows a faster search for the correct solution and reduces the possibility of facing the infeasible problem that results in a reduction in the optimization time. For this reason, the following trust regions were added to the solver: Equation (5a,b), which limit the positions; Equation (5c,d), which limit the speeds; Equation (5e,f), which limit the accelerations; and Equation (5g), which imposes a slack variable that is greater than zero.

#### **4. Path Tracking**

Regarding the path tracking, an optimum control was chosen to be implemented by decoupling the longitudinal and lateral vehicle dynamics and ensuring a low computational cost. The developed LQR has a sample time of 0.001 s to define the car input with a certain continuity as a driver will do. In this way, the path planning is embedded with a path tracking to compose the trajectory tracking with a dynamic vehicle model and a reduced time step.

#### *4.1. Vehicle Model*

To ensure a linear model to the LQR, the longitudinal and lateral vehicle dynamic are decoupled and modeled separately, obtaining a model where the longitudinal part defines the inputs pedals throttle and brake percentage, and the lateral part defines the steering wheel angle signal input.

#### 4.1.1. Longitudinal Model

About the longitudinal model, it is supposed to have the single track vehicle model, shown in Figure 5, in which the longitudinal acceleration of the vehicle is set equal to the sum of the longitudinal tire forces:

$$a\_{\mathbf{x}} = \sum\_{i=1}^{4} F\_{\mathbf{x}i} \tag{15}$$

**Figure 5.** Single track vehicle model.

Considering the total longitudinal force that the car has to have as a percentage of the maximum tracking force that the engine and braking system can express, the longitudinal vehicle model could be implemented as:

$$
\vec{x} = \frac{CF\_{\text{max}}}{m} \tag{16}
$$

where *x*¨ is the vehicle longitudinal acceleration; *C* is the throttle or brake pedal percentage in the range [0–100](%); *Fmax* is the maximum force that the engine and brake system can give; and *m* is the vehicle mass.
