Time-Optimal Trajectory Planning and Tracking for Autonomous Vehicles

Abstract

This article presents a hierarchical control framework for autonomous vehicle trajectory planning and tracking, addressing the challenge of accurately following high-speed, at-limit maneuvers. The proposed time-optimal trajectory planning and tracking (TOTPT) framework utilizes a hierarchical control structure, with an offline trajectory optimization (TRO) module and an online nonlinear model predictive control (NMPC) module. The TRO layer generates minimum-lap-time trajectories using a direct collocation method, which optimizes the vehicle’s path, velocity, and control inputs to achieve the fastest possible lap time, while respecting the vehicle dynamics and track constraints. The NMPC layer is responsible for precisely tracking the reference trajectories generated by the TRO in real time. The NMPC also incorporates a preview algorithm that utilizes the predicted future travel distance to estimate the optimal reference speed and curvature for the next time step, thereby improving the overall tracking performance. Simulation results on the Catalunya circuit demonstrated the framework’s capability to accurately follow the time-optimal raceline at an average speed of 116 km/h, with a maximum lateral error of $0.32$ m. The NMPC module uses an acados solver with a real-time iteration (RTI) scheme, to achieve a millisecond-level computation time, making it possible to implement it in real time in autonomous vehicles.

1. Introduction

Recent autonomous driving research has aimed to achieve Level 5 self-driving capabilities [1,2] for production vehicles, which requires path tracking controllers to precisely coordinate the throttle, brakes, and steering to control vehicles along predefined paths and velocity profiles. This task becomes particularly challenging when vehicle dynamics approach handling limits during high-speed maneuvers, such as lane changes or obstacle avoidance [3]. Consequently, these challenges have propelled autonomous racing to the forefront as a popular research topic [4], as it involves controlling high-performance vehicles under highly nonlinear dynamics with short response times. If vehicles can be reliably controlled within or even beyond their handling limits in such extreme racing or drifting scenarios [5], this could enhance their maneuverability when encountering emergency situations on normal roads, thereby improving overall driving safety. The core of autonomous racing lies in trajectory optimization, which involves planning the time-optimal racing line for a given parameterized racetrack. This process typically entails a comprehensive consideration of various vehicle performance factors and optimization objectives, including drivetrain configuration [6], tire–road friction coefficients [7], active rear-axle steering [8], gear ratios, aerodynamics, roll stiffness, suspension characteristics, and other variable parameters [9,10,11,12]. Trajectory optimization problems are typically discretized via the direct collocation method and solved using nonlinear programming (NLP) solvers. The key advantage of this approach is the capability to handle high-order nonlinear dynamic system models and complex constraints, while yielding fully converged optimal solutions.

Hierarchical control architectures have been developed to enable autonomous racing cars to accurately follow time-optimal reference trajectories planned in real time. By separating the control task into high and low levels with different prediction horizons and sampling times, the hierarchical approach increases the controller’s look-ahead capability, while maintaining a computationally manageable size for NLP problems [13]. The hierarchical control scheme in [14] consisted of a long prediction horizon NMPC with a point-mass model and a short horizon NMPC with a high-fidelity vehicle model. This structure allowed for real-time feasibility, without excessively simplifying the vehicle model. Furthermore, Srinivasan et al. [15] implemented a hierarchical control structure in a race car, comprising a 200 Hz low-level controller and a 40 Hz high-level NMPC with an optimized torque vectoring strategy. This strategy minimized the model mismatch between planning and control and verified the ability to outperform professional human drivers in racing scenarios.

This research presents TOTPT, a unified hierarchical optimization framework for controlling autonomous vehicles at their handling limits. It utilizes a TRO layer that plans the time-optimal raceline and velocity offline, and an NMPC layer that tracks the references online. The open-source code for the TOTPT framework is available at the online repository (https://github.com/zlijunting/TOTPT, accessed on 10 May 2024).

This study primarily focused on the development of the following aspects:

• A unified vehicle modeling and control framework: The framework employs a consistent four-wheel double-track vehicle model across both the offline TRO and online NMPC layers. This unified modeling approach ensures coherent predictive behavior and enables efficient integration between the planning and control modules. Furthermore, proper scaling of variables and constraints, combined with well-defined problem formulations, facilitates rapid convergence of the optimization solvers in both layers.
• Tailored tire modeling: Both the TRO and NMPC layers incorporate tire ellipse constraints on all four wheels, to enhance driving stability. The TRO layer utilizes hard tire ellipse constraints to fully utilize tire grip. In contrast, the NMPC layer imposes soft tire ellipse constraints using a pure slip tire model, which is more suitable for the multiple shooting scheme and convergence requirements of NMPC.
• An adaptive reference preview algorithm: The reference trajectory adapts to the vehicle’s speed. Specifically, it utilizes the travel distance and speed predicted from the NMPC to estimate the optimal path station and corresponding reference speed and curvature for the next time step. This approach improves the trajectory tracking performance by fully leveraging the model’s predictive capabilities.

This paper is organized as follows: Section 2 describes the kinematic modeling of the vehicle relative to the reference path and the dynamics modeling for path tracking. Section 3 details the 3-DOF double-track vehicle dynamics model, covering the load transfer, wheel torques, nonlinear tire models, and vehicle constraints. Section 4 provides an overview of the hierarchical control scheme, with the offline TRO module generating optimal trajectories and the online modules handling error evaluation, path preview, and NMPC trajectory tracking. Section 5 formulates the TRO problem for offline trajectory planning, using the direct collocation method to discretize the problem into an NLP. Section 6 presents the formulation of the NMPC problem for online trajectory tracking, including the prediction model, discretization method, cost function, and constraints. Additionally, a reference preview algorithm is proposed. Section 7 presents the time-optimal trajectory planned using TRO, along with the trajectory tracking results obtained using NMPC. The path tracking performance, tire workloads, and execution efficiency are also analyzed.

2. Track Modeling

A scheme diagram of the path tracking model is illustrated in Figure 1, where R is the distance from the vehicle CG to the instantaneous center of rotation (ICR), $\kappa =\mathrm{d}{\varphi }_{\mathrm{ref}}{\left(\mathrm{d}s\right)}^{-1}$ is the path curvature at a given path station s, ${\varphi }_{\mathrm{ref}}$ is the reference course angle, and ${\mathcal{N}}_l\in {\mathbb{R}}^{+}$ and ${\mathcal{N}}_r\in {\mathbb{R}}^{+}$ are the normal distances from the path to the left and right track boundaries. The vehicle moves in the direction defined by the vehicle’s course angle $\varphi$, with a velocity of V. The relative states between the vehicle and the path include the normal distance from the vehicle’s CG to the nearest point on the path n, the relative heading $\xi =\psi -{\varphi }_{\mathrm{ref}}$, and relative course $\chi =\varphi -{\varphi }_{\mathrm{ref}}=\xi +\beta$.

The lateral deviation and relative heading are selected as the primary path tracking states, then the path tracking dynamics are described by the following equations:

$$\begin{array}{cc}\hfill \dot{n}& =V\sin \left(\chi \right),\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill \dot{\xi }& =\gamma -\frac{\mathrm{d}{\varphi }_{\mathrm{ref}}}{\mathrm{d}s}\frac{\mathrm{d}s}{\mathrm{d}t}\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill & =\gamma -\kappa \dot{s},\hfill \end{array}$$

(3)

where $\dot{s}$ is the vehicle velocity projected onto the direction of the reference course at path

$$\begin{array}{c}\hfill \dot{s}=\frac{V\cos \left(\chi \right)}{1-\kappa n}.\end{array}$$

(4)

The ‘distance traveled’ is used as the independent variable for the TRO [9]. Subsequently, the dependent variable, i.e., time traveled for a given path grid, can be obtained using the factor $S_f$, as defined by

$$\begin{array}{c}\hfill \frac{\mathrm{d}t}{\mathrm{d}s}=\frac{1-\kappa n}{V\cos \left(\chi \right)}≕S_f.\end{array}$$

(5)

Track Constraint

The lateral deviation of the vehicle is limited, to ensure that it remains within the track boundaries,

$$\begin{array}{c}\hfill -{\mathcal{N}}_r+\left(\frac{w_t}{2}+w_s\right)\le n\le {\mathcal{N}}_l-\left(\frac{w_t}{2}+w_s\right),\end{array}$$

(6)

where $w_t$ is the vehicle track width, and $w_s=0.2\mathrm{m}$ is the preserved safety distance.

3. Vehicle Modeling

The choice of vehicle models for the path-following problem is diverse [16]. Considering vehicle control near handling limits during racing maneuvers, the selected model must capture the coupling of longitudinal and lateral motions, as well as the nonlinear behavior of tire forces. In this research, a 3-degree-of-freedom (DOF) double-track vehicle model is employed to address both the TRO and NMPC problems. Aerodynamic drag and lift forces are included, while suspension dynamics are ignored. A rear-wheel drive (RWD) sports car with dual-motor is utilized as the test vehicle in this study, and the vehicle parameters are specified in

Table A1. Parameters of the vehicle model used in the study.

	Description	Symbol	Value	Unit
Env.	gravity constant	g	$9.81$	$[\mathrm{m} {\mathrm{s}}^{-2}]$
	coefficient of road friction	${\mu }_0$	$1.0$	–
	air density	$\rho$	$1.2$	$[\mathrm{k}\mathrm{g} {\mathrm{m}}^{-3}]$
	coefficient of drag	$C_d$	$0.3$	–
	coefficient of lift	$C_l$	$-0.6$	–
Vehicle	front cross sectional area	A	$1.5$	$\left[{\mathrm{m}}^2\right]$
	vehicle mass	$m_v$	1250	$\left[\mathrm{k}\mathrm{g}\right]$
	vehicle moment of inertia	$I_z$	1050	$[\mathrm{kg} {\mathrm{m}}^2]$
	distance from CG to front axle	$l_f$	$1.4$	$\left[\mathrm{m}\right]$
	distance from CG to rear axle	$l_r$	$1.4$	$\left[\mathrm{m}\right]$
	height of CG	$h_c$	$0.35$	$\left[\mathrm{m}\right]$
	track width	$w_t$	$1.5$	$\left[\mathrm{m}\right]$
	wheel spin moment of inertia	$I_w$	$1.2$	$[\mathrm{kg} {\mathrm{m}}^2]$
	wheel effective rolling radius	$r_w$	$0.3$	$\left[\mathrm{m}\right]$
Tire	long. stiffness factor	$B_x$	18	–
	long. shape factor	$C_x$	$1.3$	–
	long. peak factor linear term	$d_{1x}$	$0.95$	–
	long. peak factor constant term	$d_{2x}$	320	–
	lateral stiffness factor	$B_y$	13	–
	lateral shape factor	$C_y$	$1.4$	–
	lateral peak factor linear term	$d_{1y}$	$0.95$	–
	lateral peak factor constant term	$d_{2y}$	320	–
	maximum longitudinal friction coeff.	${\mu }_{x,\max }$	$1.0$	–
	maximum lateral friction coeff.	${\mu }_{y,\max }$	$1.0$	–

.

3.1. Double-Track Vehicle Model

Figure 2 shows the layout of the double-track vehicle model, with the vehicle heading at yaw angle $\psi$ from the x-axis of the predefined global coordinates ${\mathcal{O}}_G$. The accelerations $a_x$ and $a_y$ are located at the vehicle body coordinate ${\mathcal{O}}_B$, and $a_t$ and $a_n$ are located at the vehicle course coordinate ${\mathcal{O}}_C$. These accelerations are expressed by the following equations:

$$\begin{array}{ccc}\hfill a_x=& \frac{1}{m_v}\left[F_{xf}\cos \left(\delta \right)-F_{yf}\sin \left(\delta \right)+F_{xr}-F_{\mathrm{drag}}\right],\hfill & \end{array}$$

(7)

$$\begin{array}{ccc}\hfill a_y=& \frac{1}{m_v}\left[F_{yf}\cos \left(\delta \right)+F_{xf}\sin \left(\delta \right)+F_{yr}\right],\hfill & \end{array}$$

(8)

$$\begin{array}{cc}\hfill a_t=& \frac{1}{m_v}\left[F_{xf}\cos (\delta -\beta )-F_{yf}\sin (\delta -\beta )+F_{xr}\cos \left(\beta \right)+F_{yr}\sin \left(\beta \right)-F_{\mathrm{drag}}\cos \left(\beta \right)\right],\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill a_n=& \frac{1}{m_v}\left[F_{xf}\sin (\delta -\beta )+F_{yf}\cos (\delta -\beta )-F_{xr}\sin \left(\beta \right)+F_{yr}\cos \left(\beta \right)+F_{\mathrm{drag}}\sin \left(\beta \right)\right],\hfill \end{array}$$

(10)

where $m_v$ is the vehicle mass, $\delta$ is the front wheel steering angle, $F_{xf/l}$ is the total longitudinal tire force at the front/rear axle, $F_{yf/l}$ is the total lateral tire force at the front/rear axle, and $F_{\mathrm{drag}}=\frac{1}{2}\rho C_{d}A{V}_x^2$ is the aerodynamic drag force, where $\rho$ is the air density, $C_d$ is the drag coefficient, A is the vehicle frontal area, and $V_x$ is the longitudinal vehicle speed.

The yaw moment about the center of gravity (CG) of the vehicle is

$$\begin{array}{cc}\hfill M_z& =l_f{F}_{yf}\cos \left(\delta \right)+l_f{F}_{xf}\sin \left(\delta \right)-l_r{F}_{yr}\hfill \\ \hfill & +\frac{w_t}{2}\left[(F_{yfl}-F_{yfr})\sin \left(\delta \right)+(F_{xfr}-F_{xfl})\cos \left(\delta \right)+(F_{xrr}-F_{xrl})\right],\hfill \end{array}$$

(11)

where $l_f$ and $l_r$ are the distances from CG to the front and rear axles.

Similarly to the vehicle models in [6,7,17,18], the total velocity V, sideslip angle $\beta$, and yaw rate $\gamma$ are selected as the state variables; then, the following equations are used to describe the planar dynamics of the vehicle:

$$\begin{array}{ccc}\hfill \dot{V}& =a_t,\hfill & \end{array}$$

(12)

$$\begin{array}{cc}\hfill \dot{\beta }& =\dot{\varphi }-\dot{\psi }=\frac{a_n}{V}-\gamma ,\hfill \end{array}$$

(13)

$$\begin{array}{ccc}\hfill \dot{\gamma }& =\frac{M_z}{I_z},\hfill & \end{array}$$

(14)

where $I_z$ is the moment of inertia.

3.2. Load Transfer

A load transfer model is introduced to evaluate the tire forces when a vehicle undergoes significant changes in both longitudinal and lateral accelerations. Referring to [19], the equations for the static load transfer are

$$\begin{array}{cc}\hfill F_{zfl}& =\frac{1}{2}{m}_v(\frac{l_r}{l}g-\frac{h_c}{l}{\overline{a}}_x)-m_v(\frac{l_r}{l}g-\frac{h_c}{l}{\overline{a}}_x)\frac{h_c}{w_t}\frac{{\overline{a}}_y}{g}-\frac{F_{\mathrm{lift}}}{4},\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill F_{zfr}& =\frac{1}{2}{m}_v(\frac{l_r}{l}g-\frac{h_c}{l}{\overline{a}}_x)+m_v(\frac{l_r}{l}g-\frac{h_c}{l}{\overline{a}}_x)\frac{h_c}{w_t}\frac{{\overline{a}}_y}{g}-\frac{F_{\mathrm{lift}}}{4},\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill F_{zrl}& =\frac{1}{2}{m}_v(\frac{l_f}{l}g+\frac{h_c}{l}{\overline{a}}_x)-m_v(\frac{l_f}{l}g+\frac{h_c}{l}{\overline{a}}_x)\frac{h_c}{w_t}\frac{{\overline{a}}_y}{g}-\frac{F_{\mathrm{lift}}}{4},\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill F_{zrr}& =\frac{1}{2}{m}_v(\frac{l_f}{l}g+\frac{h_c}{l}{\overline{a}}_x)+m_v(\frac{l_f}{l}g+\frac{h_c}{l}{\overline{a}}_x)\frac{h_c}{w_t}\frac{{\overline{a}}_y}{g}-\frac{F_{\mathrm{lift}}}{4},\hfill \end{array}$$

(18)

where l is the wheelbase, $h_c$ is the height of CG, g is the gravity constant, ${\overline{a}}_x$ and ${\overline{a}}_y$ are the decision variables provided by the NLP solver, while the aerodynamic lift force, characterized by the lift coefficient $C_l$, is given by $F_{\mathrm{lift}}=\frac{1}{2}\rho C_{l}A{V}_x^2$.

To prevent algebraic loop issues being encountered by the optimization solver during function evaluations, the algebraic variables ${\overline{a}}_x$ and ${\overline{a}}_y$ are introduced, and constraints are imposed to ensure their values are consistent with the vehicle accelerations (7) and (8),

$$\begin{array}{cc}\hfill {\overline{a}}_x-a_x& =0,\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill {\overline{a}}_y-a_y& =0.\hfill \end{array}$$

(20)

To improve the convergence of online NMPC solvers, the equality constraints on accelerations are approximated using first-order dynamics as the following equations:

$$\begin{array}{c}\hfill {\dot{\overline{a}}}_x=\frac{1}{{\tau }_{\mathrm{acc}}}\left(a_x-{\overline{a}}_x\right)\end{array}$$

(21)

$$\begin{array}{c}\hfill {\dot{\overline{a}}}_y=\frac{1}{{\tau }_{\mathrm{acc}}}\left(a_y-{\overline{a}}_y\right)\end{array}$$

(22)

where the time constant ${\tau }_{\mathrm{acc}}$ is set to half the NMPC sampling time.

3.3. Wheel Torques

The torque distribution between the front and rear axles are determined by the following equations:

$$\begin{array}{ccc}\hfill T_f& =k_t{T}_t+k_b{T}_b,\hfill & \end{array}$$

(23)

$$\begin{array}{cc}\hfill T_r& =(1-k_t)T_t+(1-k_b)T_b,\hfill \end{array}$$

(24)

where $T_t$ and $T_b$ are the total traction torque and brake torque commands. $k_b$ denotes the distribution ratio of brake torque between the front and rear axles, while $k_t$ indicates the split coefficient for traction, which is determined by the drivetrain type of the vehicle. For the RWD vehicle considered in this study, $k_t=0$ and $k_b=0.6$.

The axle torques $T_f$ and $T_r$ are then redistributed to the left and right wheels in proportion to the relative vertical tire forces, similarly to the strategy in [20]. Then, the torque commands for the four wheel are determined by the following equations:

$$\begin{array}{ccccc}\hfill T_{fl}& =\frac{T_f}{2}\left(1-\frac{\mathsf{\Delta }{F}_{zf}}{F_{zf}}\right),\hfill & & \hfill T_{fr}& =\frac{T_f}{2}\left(1+\frac{\mathsf{\Delta }{F}_{zf}}{F_{zf}}\right),\hfill \end{array}$$

(25)

$$\begin{array}{ccccc}\hfill T_{rl}& =\frac{T_r}{2}\left(1-\frac{\mathsf{\Delta }{F}_{zr}}{F_{zr}}\right),\hfill & & \hfill T_{rr}& =\frac{T_r}{2}\left(1+\frac{\mathsf{\Delta }{F}_{zr}}{F_{zr}}\right),\hfill \end{array}$$

(26)

where the relative vertical loads are calculated as

$$\begin{array}{cc}\hfill \mathsf{\Delta }{F}_{zf}& =\frac{1}{2}(F_{zfr}-F_{zfl})=m_v(\frac{l_r}{l}g-\frac{h_c}{l}{\overline{a}}_x)\frac{h_c}{w_t}\frac{{\overline{a}}_y}{g},\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill \mathsf{\Delta }{F}_{zr}& =\frac{1}{2}(F_{zrr}-F_{zrl})=m_v(\frac{l_f}{l}g+\frac{h_c}{l}{\overline{a}}_x)\frac{h_c}{w_t}\frac{{\overline{a}}_y}{g}.\hfill \end{array}$$

(28)

3.4. Tire Modeling

According to the moment and force balance equations, the dynamics of wheel angular velocity are described by the following equation:

$${\dot{\omega }}_i=\frac{1}{I_w}\left(T_i-r_w{F}_{xi}\right),i\in \left\{fl,fr,rl,rr\right\},$$

(29)

where $T_i$ is the torque applied on the wheel, $I_w$ is the spin moment of inertia of the wheel, and $r_w$ is the effective rolling radius.

The longitudinal velocities of the tire trajectories are given by

$$\begin{array}{cc}\hfill V_{xfl}& =(V_x-\frac{w_t}{2}\gamma )\cos \left(\delta \right)+(V_y+l_f\gamma )\sin \left(\delta \right),\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill V_{xfr}& =(V_x+\frac{w_t}{2}\gamma )\cos \left(\delta \right)+(V_y+l_f\gamma )\sin \left(\delta \right),\hfill \end{array}$$

(31)

$$\begin{array}{ccc}\hfill V_{xrl}& =V_x-\frac{w_r}{2}\gamma ,\hfill & \end{array}$$

(32)

$$\begin{array}{ccc}\hfill V_{xrr}& =V_x+\frac{w_r}{2}\gamma ,\hfill & \end{array}$$

(33)

and the lateral velocities of the tire trajectories are given by

$$\begin{array}{cc}\hfill V_{yfl}& =(V_y+l_f\gamma )\cos \left(\delta \right)-(V_x-\frac{w_t}{2}\gamma )\sin \left(\delta \right),\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill V_{yfr}& =(V_y+l_f\gamma )\cos \left(\delta \right)-(V_x+\frac{w_t}{2}\gamma )\sin \left(\delta \right),\hfill \end{array}$$

(35)

$$\begin{array}{ccc}\hfill V_{yrl}& =V_y-l_r\gamma ,\hfill & \end{array}$$

(36)

$$\begin{array}{ccc}\hfill V_{yrr}& =V_y-l_r\gamma .\hfill & \end{array}$$

(37)

Then, the longitudinal slip $\lambda$ and sideslip angle $\alpha$ of the tire at each corner $i\in \left\{fl,fr,rl,rr\right\}$ are defined by

$$\begin{array}{cc}\hfill {\lambda }_i& =\frac{r_w{\omega }_i-V_{xi}}{V_{xi}},\hfill \end{array}$$

(38)

$$\begin{array}{cc}\hfill {\alpha }_i& ={\tan }^{-1}\left(\frac{V_{yi}}{V_{xi}}\right).\hfill \end{array}$$

(39)

3.4.1. Simplified MF Tire Model

The simplified magic formula (simplified MF) [21] (ch. 9.2.2, [22]) tire model is used to capture the nonlinear characteristics of tire forces. This model is easy to implement and has good continuity, providing advantages for optimization algorithms that require model gradients. The simplified MF for the longitudinal tire forces under pure longitudinal slip conditions is shown in the left plot of Figure 3. It is the function of the longitudinal tire slip $\lambda$, vertical tire force $F_z$, and road friction coefficient $\mu$, as shown in the following equation:

$$F_{x0}\left(\lambda ,F_z,\mu \right)=\frac{\mu }{{\mu }_0}{D}_x\sin \left[C_x{\tan }^{-1}\left(B_x\lambda \right)\right],$$

(40)

where ${\mu }_0$ is the road friction coefficient during tire testing, and $B_x$, $C_x$, and $D_x$ are fitting coefficients. Since the peak factor $D_x$ varies linearly with the vertical tire force, it can be modeled as a first-order function of $F_z$ with a linear term $d_1$ and a constant term $d_2$,

$$D_x=d_{1x}{F}_z+d_{2x}.$$

(41)

The simplified MF for the lateral tire forces under pure sideslip conditions is shown in the right plot of Figure 3, and it is expressed by the following equation:

$$F_{y0}\left(\alpha ,F_z,\mu \right)=-\frac{\mu }{{\mu }_0}{D}_y\sin \left[C_y{\tan }^{-1}\left(B_y\alpha \right)\right],$$

(42)

where

$$D_y=d_{1y}{F}_z+d_{2y}.$$

(43)

3.4.2. Combined Slip Tire Model for TRO

In highly dynamic scenarios, a combination of braking and cornering maneuvers is commonly encountered, leading to coupled lateral and longitudinal tire slips and forces. Consequently, the coupled slip effect is incorporated into the TRO, to provide an accurate reference trajectory. The combined slip model from (ch. 4.2.2, [22]) is employed to evaluate the tire forces under combined slip conditions, where the theoretical slip quantities at four corners are defined as

$${\sigma }_i=\sqrt{{\lambda }_i^2+\tan {\left({\alpha }_i\right)}^2},i\in \left\{fl,fr,rl,rr\right\}.$$

(44)

And the combined slip longitudinal and lateral tire forces of are defined by

$$\begin{array}{c}\hfill F_{xi}=\frac{{\lambda }_i}{{\sigma }_i}{F}_{x0}\left({\sigma }_i,F_{zi},\mu \right),F_{yi}=\frac{\tan \left({\alpha }_i\right)}{{\sigma }_i}{F}_{y0}\left({\sigma }_i,F_{zi},\mu \right),i\in \left\{fl,fr,rl,rr\right\}.\end{array}$$

(45)

3.4.3. Pure Sideslip Tire Model for NMPC

Considering the convergence and real-time requirements of the online trajectory tracking task, wheel dynamics are not considered in the NMPC formulation. Instead, the combined tire force is characterized by tire ellipse constraints. Consequently, the longitudinal and lateral tire forces are described by the following equations:

$$\begin{array}{c}\hfill F_{xi}=\frac{T_i}{r_w},F_{yi}=F_{y0}\left({\alpha }_i,F_{zi},\mu \right),i\in \left\{fl,fr,rl,rr\right\}.\end{array}$$

(46)

3.5. Vehicle Constraints

Quadratic nonlinear constraints are imposed to ensure that the combined tire forces remain within the tire friction ellipses, thereby preventing a loss of traction.

$${\left(\frac{F_{xi}}{{\mu }_{x,\max }{F}_{zi}}\right)}^2+{\left(\frac{F_{yi}}{{\mu }_{y,\max }{F}_{zi}}\right)}^2\le 1,i\in \left\{fl,fr,rl,rr\right\},$$

(47)

where ${\mu }_{x,\max }$ and ${\mu }_{y,\max }$ are the maximum longitudinal and lateral tire friction coefficients.

To achieve the optimal actuation efficiency, it is not desirable to apply both motor and brake torque to the wheel at the same time. Thus, an equality constraint is imposed to ensure that the total traction and braking commands are orthogonal:

$$T_t{T}_b=0.$$

(48)

Considering rear dual electric motors, two equality equations are introduced to limit the motor powers separately,

$$\begin{array}{cc}\hfill T_{rl}{\omega }_{rl}& \le P_{rl,\max },\hfill \\ \hfill T_{rr}{\omega }_{rr}& \le P_{rr,\max },\hfill \end{array}$$

(49)

where $P_{·,\max }$ is the maximum motor power for the respective motor, $\omega$ is the wheel angular velocity, and subscripts $rl$ and $rr$ denote the rear-left and rear-right wheels, respectively.

4. Control Architecture

The overall control architecture is shown in Figure 4, with modules for offline and online execution. The offline TRO module generates the time-optimal trajectory based on the racetrack data. The optimal curvature, velocity, and raceline trajectories are then provided to the online modules. During the online path tracking stage, the tracking error evaluation module calculates the lateral deviation and heading error using the algorithm proposed in [23]. The preview module queries the reference curvature and speed trajectories based on the preview path station. The NMPC module solves the optimal vehicle inputs for the online trajectory tracking. Subsequently, the low-level controller distributes the desired torque commands as rear driving torques and four-wheel braking torques.

5. Trajectory Optimization

Trajectory optimization aims to determine the reference path with the shortest travel time for a given racetrack, along with the corresponding vehicle states and input trajectories, while satisfying specified constraints. Prior to optimization, the centerline of the racetrack needs to be smoothed and discretized into uniform grid distances. Subsequently, the direct orthogonal collocation method is employed to discretize the system dynamics and cost functions in the spatial domain, thereby transforming the optimal control problem (OCP) into a standard NLP formulation.

5.1. System Dynamics in the Spatial Domain

The TRO problem is formulated in the spatial domain, using the distance along the track as the independent variable instead of time. This allows the dynamics to be expressed as a function of the path position, rather than time. The state variables of the TRO are concatenated from the vehicle states, wheel velocities, and path tracking states,

$$\mathbf{x}={\left[\begin{array}{ccccccccc}V& \beta & \gamma & {\omega }_{fl}& {\omega }_{fr}& {\omega }_{rl}& {\omega }_{rr}& n& \xi \end{array}\right]}^{\top }.$$

(50)

The control variables are the total traction/braking torque commands and front wheel steering angle,

$$\mathbf{u}={\left[\begin{array}{ccc}{T}_t& T_b& \delta \end{array}\right]}^{\top }.$$

(51)

The auxiliary variables are the algebraic longitudinal and lateral accelerations,

$$\mathbf{z}={\left[\begin{array}{cc}{\overline{a}}_x& {\overline{a}}_y\end{array}\right]}^{\top }.$$

(52)

Then, the system dynamics are the function of state, control, auxiliary and parameter variables, as shown in the following equations:

$$\begin{array}{cc}\hfill \dot{\mathbf{x}}& ={\left[\begin{array}{cccccccccccccc}\dot{V}& & \dot{\beta }& & \dot{\gamma }& & {\dot{\omega }}_{fl}& {\dot{\omega }}_{fr}& & {\dot{\omega }}_{rl}& {\dot{\omega }}_{rr}& & \dot{n}& & \dot{\xi }\end{array}\right]}^{\top }\hfill \\ & ={\left[\begin{array}{ccccccccc}\left(12\right)& \left(13\right)& \left(14\right)& \left(29\right)& \left(29\right)& \left(29\right)& \left(29\right)& \left(1\right)& \left(3\right)\end{array}\right]}^{\top }.\hfill \end{array}$$

(53)

The system dynamics in the spatial domain ${\mathbf{x}}^{\prime }$ are transformed from the time domain using the variable $S_f$,

$$\begin{array}{c}\hfill {\mathbf{x}}^{\prime }=\frac{\mathrm{d}\mathbf{x}}{\mathrm{d}s}=\frac{\mathrm{d}t}{\mathrm{d}s}\frac{\mathrm{d}\mathbf{x}}{\mathrm{d}t}≕S_f\dot{\mathbf{x}}{\mathbf{f}}_s\left(\mathbf{x},\mathbf{u},\mathbf{z},\kappa \right).\end{array}$$

(54)

The magnitudes of states, controls, and auxiliary variables are bounded due to the physical limits of the vehicle and actuators,

$$\begin{array}{cc}\hfill {\mathbf{x}}_{\min }& \le \mathbf{x}\le {\mathbf{x}}_{\max },\hfill \end{array}$$

(55)

$$\begin{array}{cc}\hfill {\mathbf{u}}_{\min }& \le \mathbf{u}\le {\mathbf{u}}_{\max },\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill {\mathbf{z}}_{\min }& \le \mathbf{z}\le {\mathbf{z}}_{\max },\hfill \end{array}$$

(57)

where the subscripts min and max denote the lower and upper bounds of the variables.

The rates of the control variables are limited according to the dynamic characteristics of the actuators,

$$\begin{array}{cc}\hfill & {\dot{\mathbf{u}}}_{\min }\le \dot{\mathbf{u}}\le {\dot{\mathbf{u}}}_{\max }.\hfill \end{array}$$

(58)

5.2. Direct Collocation

As Figure 5 shows, the distance along the track centerline $[s_0,s_{\mathrm{f}}]$ is divided into N intervals by the step length $\mathrm{d}{s}_k$, with each interval spanning $[s_k,s_{k+1}]$, where k ranges from 0 to $N-1$.

The state trajectory of the interval is approximated using the Legendre polynomial as the following equation:

$$x_k\left(\tau \right)=\sum _{i=0}^q{P}_i\left(\tau \right)x_{k,i},i=0,\dots ,q.$$

(59)

where $P_i\left(\tau \right)$ is the basis of the Legendre polynomial over the unit interval $\tau =[1,0]$, computed using Gauss–Legendre collocation points $\{{\tau }_0,{\tau }_1,\dots ,{\tau }_q\}$ (ch. 10.3, [24]) with order q, as defined by

$$P_i\left(\tau \right)=\prod _{\begin{array}{c}j=0,\\ j\ne i\end{array}}^q\frac{\tau -{\tau }_j}{{\tau }_i-{\tau }_j},i=0,\dots ,q.$$

(60)

To facilitate the representation of state trajectories within each interval for subsequent formulations, the state variables at the knot and collocation points are aggregated into a matrix denoted as ${\mathbf{X}}_k$,

$${\mathbf{X}}_k=\left[\begin{array}{cccc}{\mathbf{x}}_{k,0}& {\mathbf{x}}_{k,1}& \cdots & {\mathbf{x}}_{k,q}\end{array}\right]\in {\mathbb{R}}^{n_x\times \left(q+1\right)},k=0,\dots ,N-1.$$

(61)

The direct collocation method is utilized to discretize the TRO problem, following the approaches outlined in [11,25,26]. This transcription method involves imposing additional constraints to enforce the polynomials approximate to the system dynamics.

For each interval, the derivatives of Legendre polynomials at the collocation points must match the system dynamics, as in the following collocation equation:

$${\mathbf{X}}_k\mathbf{C}-\mathrm{d}{s}_k\left[\begin{array}{ccc}{\mathbf{x}}_{k,1}^{\prime }& \cdots & {\mathbf{x}}_{k,q}^{\prime }\end{array}\right]=\mathbf{0},k=0,\dots ,N-1,$$

(62)

where ${\mathbf{x}}_{k,j}^{\prime }={\mathbf{f}}_s({\mathbf{x}}_{k,j},{\mathbf{u}}_k,{\mathbf{z}}_k,{\kappa }_k)$ are the system dynamics in the spatial domain at the collocation points, and the control ${\mathbf{u}}_k$ is piece-wise constant for $[s_k,s_{k+1})$.

The state trajectory at the end of the interval must be consistent with the beginning of the next interval, as seen in the following continuity equation:

$${\mathbf{X}}_k\mathbf{D}-{\mathbf{x}}_{k+1}=\mathbf{0},k=0,\dots ,N-1.$$

(63)

The matrix $\mathbf{C}$ is defined using the derivatives of the Legendre polynomials evaluated at collocation points, and the matrix $\mathbf{D}$ is defined using the Legendre polynomials evaluated at the end points of the interval,

$$\mathbf{C}=\left[\begin{array}{ccc}{\dot{P}}_0\left({\tau }_1\right)& \cdots & {\dot{P}}_0\left({\tau }_q\right)\\ ⋮& \ddots & ⋮\\ {\dot{P}}_q\left({\tau }_1\right)& \cdots & {\dot{P}}_q\left({\tau }_q\right)\end{array}\right]\in {\mathbb{R}}^{\left(q+1\right)\times q},\mathbf{D}=\left[\begin{array}{c}{P}_0\left(1\right)\\ ⋮\\ P_q\left(1\right)\end{array}\right]\in {\mathbb{R}}^{q+1}.$$

(64)

Additional constraints set $\mathbf{h}$ include the tire ellipse, actuator orthogonality, motor power, and state/control limits, which are imposed at the knot points only,

$$\begin{array}{c}\hfill {\mathbf{h}}_{\min }\le \mathbf{h}({\mathbf{x}}_k,{\mathbf{u}}_k,{\mathbf{z}}_k,{\kappa }_k)\le {\mathbf{h}}_{\max },k=0,\dots ,N,\end{array}$$

(65)

where ${\mathbf{h}}_{\min }$ and ${\mathbf{h}}_{\max }$ are the lower and upper bounds of constraint.

The variation in control is mapped onto the time domain in a manner that aligns with the specified bounds within the time domain.

$$\begin{array}{c}\hfill \dot{\mathbf{u}}\approx \frac{\mathrm{d}\mathbf{u}}{\mathrm{d}s}\frac{\mathrm{d}s}{\mathrm{d}t}={\mathbf{u}}^{\prime }\dot{s}\to {\dot{\mathbf{u}}}_{\min }\le {\mathbf{u}}^{\prime }\dot{s}\le {\dot{\mathbf{u}}}_{\max }.\end{array}$$

(66)

5.3. Cost Function

The cost function is an integral part of trajectory optimization, representing the objective to be minimized. To minimize the lap time, the time cost within each discretized path interval must be incorporated, as in the following equation:

$$\mathrm{d}{t}_k=\mathrm{d}{s}_k\left[\begin{array}{ccc}{S}_f\left({\mathbf{x}}_{k,1},{\kappa }_k\right)& \cdots & S_f\left({\mathbf{x}}_{k,q},{\kappa }_k\right)\end{array}\right]\mathbf{B},k=0,\dots ,N-1,$$

(67)

where the matrix $\mathbf{B}$ aggregates the contributions of the time cost at the collocation points of intervals,

$$\mathbf{B}={\left[\begin{array}{ccc}{\int }_0^1{P}_1\left(\tau \right)\mathrm{d}\tau & \cdots & {\int }_0^1{P}_q\left(\tau \right)\mathrm{d}\tau \end{array}\right]}^{\top }\in {\mathbb{R}}^q.$$

(68)

To obtain the shortest laptime with smooth vehicle inputs, the cost function sums the time cost and the quadratic cost of variations in control and auxiliary variables over all intervals,

$${\mathcal{J}}_{\mathrm{TRO}}=\sum _{k=0}^{N-1}\left(\mathrm{d}{t}_k+{{\mathbf{u}}_k^{\prime }}^{\top }\mathbf{R}{\mathbf{u}}_k^{\prime }+{{\mathbf{z}}_k^{\prime }}^{\top }\mathbf{W}{\mathbf{z}}_k^{\prime }\right),$$

(69)

where $\mathbf{R}$ and $\mathbf{W}$ are the weighting matrices, and if it is observed that the resulting optimal input commands exhibit oscillatory behavior, then the corresponding weights in the matrices need to be incremented. The variations in variables ${\mathbf{u}}_k^{\prime }$ and ${\mathbf{z}}_k^{\prime }$ are calculated using the following difference equations:

$$\begin{array}{c}\hfill {\mathbf{u}}_k^{\prime }=\frac{{\mathbf{u}}_{k+1}-{\mathbf{u}}_k}{\mathrm{d}{s}_k},{\mathbf{z}}_k^{\prime }=\frac{{\mathbf{z}}_{k+1}-{\mathbf{z}}_k}{\mathrm{d}{s}_k},k=0,\dots ,N-1.\end{array}$$

(70)

5.4. NLP Solver

To improve the convergence of the NLP, it is necessary to properly scale the decision variables of the solver [27]. The state and control variables are scaled by the scaling factors, which are determined by their maximum expected values. This normalization process ensures that all the variables lie within a common range, typically between $-1$ and 1. The detailed workflow and implementation of the scaling procedure can be found in [28].

The overall TRO problem is summarized in

Table 1. TRO formulation.

	Description	Symbol/ Equation	Scale	Lower	Upper	Units
Minimize	Cost	${\mathcal{J}}_{\mathbf{TRO}}$ (69)
By varying
States	Velocity	V	100	0	$250/3.6$	m s−1
	Sideslip angle	$\beta$	1	$-\pi /4$	$\pi /4$	rad
	Yaw rate	$\gamma$	1	$-\pi /2$	$\pi /2$	rad s−1
	Wheel velocities	${\omega }_i$	$100/r_w$	0	$250/3.6/r_w$	rad s−1
	Lateral displacement	n	5	$-{\mathcal{N}}_r+\left(w_t/2+w_s\right)$	${\mathcal{N}}_l-\left(w_t/2+w_s\right)$	m
	Relative heading	$\xi$	1	$-\pi /4$	$\pi /4$	rad
Controls	Traction torque	$T_t$	$2\times {10}^3$	0	$4\times {10}^3$	N m
	Braking torque	$T_b$	$4\times {10}^3$	$-8\times {10}^3$	0	N m
	Wheel steering angle	$\delta$	$\pi /8$	$-\pi /8$	$\pi /8$	rad
Aux. variables	Longitudinal acceleration	${\overline{a}}_x$	g	$-3g$	$3g$	m s−2
	Lateral acceleration	${\overline{a}}_y$	g	$-3g$	$3g$	m s−2
Parameter	Path curvature	$\kappa$	–	–	–	m−1
Subject to
Constraints	Dynamics as collocation	(62), (63)	–	–	–	–
	Equality of accelerations	(19),(20)	g	$-{10}^{-3}$	${10}^{-3}$	–
	Tire ellipse	(47)	1	0	1	–
	Actuator orthogonality	(48)	1	$-{10}^{-3}$	${10}^{-3}$	–
	Motor power limitation	(49)	$2\times {10}^5/r_w$	0	150	kW
Bounds on	Rate of traction torque	${\dot{T}}_t$	$2\times {10}^3$	0	$3\times {10}^3$	N m s−1
	Rate of braking torque	${\dot{T}}_b$	$4\times {10}^3$	$-6\times {10}^3$	0	N m s−1
	Rate of wheel steering angle	$\dot{\delta }$	$\pi /8$	$-\pi /8$	$\pi /8$	rad s−1

and includes decision variables, constraints, lower and upper bounds, and scaling factors. The symbolic framework CasADi [29] is used to formulate the TRO problem and transform it into a NLP. The large-scale nonlinear optimization solver Ipopt [30] is used to solve the NLP.

6. Nonlinear Model Predictive Control

The NMPC serves as the online trajectory tracking controller of the hierarchical control architecture. Its primary objective is to solve the optimal driving inputs, to control the vehicle in following the time-optimal reference trajectories generated by the TRO layer in real time.

6.1. Prediction Model

The NMPC needs to predict the future vehicle states, inputs, path station, and tracking errors; thus, the state variables of prediction model are selected as

$$\mathbf{x}={\left[\begin{array}{ccccccccccc}V& \beta & \gamma & {\overline{a}}_x& {\overline{a}}_y& s& n& \xi & T_t& T_b& \delta \end{array}\right]}^{\top }.$$

(71)

The control variables are the rates of driving inputs:

$$\mathbf{u}=\left[\begin{array}{ccc}{\dot{T}}_{t,\mathrm{cmd}}& {\dot{T}}_{b,\mathrm{cmd}}& {\dot{\delta }}_{\mathrm{cmd}}\end{array}\right].$$

(72)

The system dynamics of the prediction model are formulated as:

$$\begin{array}{cc}\hfill \dot{\mathbf{x}}& ={\left[\begin{array}{ccccccccccccccccccccccc}& \dot{V}& & \dot{\beta }& & \dot{\gamma }& & {\dot{\overline{a}}}_x& & {\dot{\overline{a}}}_y& & \dot{s}& & \dot{n}& & \dot{\xi }& & {\dot{T}}_t& & {\dot{T}}_b& & & \dot{\delta }\end{array}\right]}^{\top }\hfill \\ & ={\left[\begin{array}{ccccccccccc}\left(12\right)& \left(13\right)& \left(14\right)& \left(21\right)& \left(22\right)& \left(4\right)& \left(1\right)& \left(3\right)& {\dot{T}}_{t,\mathrm{cmd}}& {\dot{T}}_{t,\mathrm{cmd}}& {\dot{T}}_{t,\mathrm{cmd}}\end{array}\right]}^{\top }\hfill \\ & ≕{\mathbf{f}}_p\left(\mathbf{x},\mathbf{u},\kappa \right).\hfill \end{array}$$

(73)

The first to fifth equations in (73) predict the vehicle states and tire vertical loads, the sixth to eighth equations predict the path tracking errors and driving distance. In the last three equations, we assign the rate of system inputs as the extended states, similarly to [5,15,31]. Using the variation in driving inputs as the control variable allows us to conveniently penalize it when formulating optimization problems at a higher interface level, thereby aligning with the cost function of TRO (69).

The magnitudes of states, controls, and rates of the control variables are bounded, due to the physical limits of the vehicle and actuators,

$$\begin{array}{cc}\hfill {\mathbf{x}}_{\min }& \le \mathbf{x}\le {\mathbf{x}}_{\max },\hfill \end{array}$$

(74)

$$\begin{array}{cc}\hfill {\mathbf{u}}_{\min }& \le \mathbf{u}\le {\mathbf{u}}_{\max },\hfill \end{array}$$

(75)

$$\begin{array}{cc}\hfill {\dot{\mathbf{u}}}_{\min }& \le \dot{\mathbf{u}}\le {\dot{\mathbf{u}}}_{\max }.\hfill \end{array}$$

(76)

6.2. Discretization

The implicit Runge–Kutta method (IRK) of second order is used to discretize the prediction model over the horizon $k=0,\dots ,N_p-1$,

$$\begin{array}{cc}\hfill {\mathbf{x}}_0& =\widehat{\mathbf{x}}\left(t\right),\hfill \end{array}$$

(77)

$$\begin{array}{cc}\hfill {\mathbf{x}}_{k+1}& ={\mathbf{x}}_k+\frac{t_s}{2}\left[{\mathbf{f}}_p\left({\mathbf{x}}_k,{\mathbf{u}}_k,{\kappa }_k\right)+{\mathbf{f}}_p\left({\mathbf{x}}_{k+1},{\mathbf{u}}_{k+1},{\kappa }_{k+1}\right)\right],\hfill \end{array}$$

(78)

where $\widehat{\mathbf{x}}\left(t\right)$ is the measured or estimated stated at the current time, and $t_s=0.05\mathrm{s}$ is the sample time of NMPC or the time interval for solving each optimization problem. The prediction horizon is set as $N_p=30$, resulting in the prediction time $t_p=N_p{t}_s=1.5\mathrm{s}$.

6.3. Preview Path Station

In this section, an algorithm is proposed for generating the optimal reference trajectory for NMPC based on the preview path station. The preview path stations are obtained by moving the previous optimal path station by one step and adding a correction term to predict the distance to move for an additional step. Specifically, the preview path station ${\widehat{s}}_k$ at each stage k is set equal to the previous optimal path station:

$$\begin{array}{c}\hfill {\widehat{s}}_k=s_{k+1}^{\ast },k=0,\dots ,N_p-1,\end{array}$$

(79)

This ensures that the NMPC tracks the most recent optimal path station.

For the final preview path station ${\widehat{s}}_{N_p}$, a correction term is added to extrapolate the distance the vehicle is expected to travel in the final prediction step:

$$\begin{array}{c}\hfill {\widehat{s}}_{N_p}=s_{k+1}^{\ast }+t_s{V}_{N_p}^{\ast },\end{array}$$

(80)

where $V_{N_p}^{\ast }$ is the optimal velocity at the end of the prediction horizon, obtained from the previous NMPC solution. By interpolating the reference trajectory planned by TRO using the preview path stations that adapted to the vehicle’s current velocity, accurate preview references can be obtained for the NMPC to track.

6.4. Reference Output

The output of NMPC is $\mathbf{y}={\left[V,\beta ,n,\chi \right]}^{\top }$, and the reference output at each stage is given by

$$\begin{array}{c}\hfill {\mathbf{y}}_{k,\mathrm{ref}}={\left[\begin{array}{cccc}{V}_{\mathrm{ref}}\left({\widehat{s}}_k\right)& {\tan }^{-1}\left(\delta \frac{l_r}{l}\right)& 0& 0\end{array}\right]}^{\top },\end{array}$$

(81)

meaning that the vehicle is expected to follow the reference velocity with zero path tracking errors, and the vehicle slip angle follows the kinematic reference value to maintain lateral stability.

6.5. Cost Function

Model–plant mismatches or external disturbances can lead to constraint violations that render the original optimization problem infeasible. To address this issue, a slack variable ${\mathbf{s}}_h$ is introduced to handle constraint violations, while still maintaining a feasible solution,

$$\begin{array}{c}\hfill {\mathbf{h}}_{\min }-{\mathbf{s}}_h\le \mathbf{h}\le {\mathbf{h}}_{\max }+{\mathbf{s}}_h.\end{array}$$

(82)

where $\mathbf{h}$ includes the actuator orthogonality, four tire ellipse constraints, and power limits.

To balance the objectives of accurately tracking the desired trajectory, minimizing control efforts, and permitting constraint relaxation, the cost function to be minimized over the prediction horizon $N_p$ is formulated by combining tracking errors, control efforts, and penalties on the slack variables:

$${\mathcal{J}}_{\mathrm{NMPC}}=\sum _{k=1}^{N_p}\frac{1}{2}{{\mathbf{S}}_{\mathbf{y}}^{-1}\left({\mathbf{y}}_k-{\mathbf{y}}_{k,\mathrm{ref}}\right)}_{\mathbf{Q}}^2+\sum _{k=0}^{N_p-1}\frac{1}{2}{{\mathbf{S}}_{\mathbf{u}}^{-1}{\mathbf{u}}_k}_{\mathbf{R}}^2+\frac{1}{2}{\mathbf{s}}_h^{\top }\mathbf{Z}{\mathbf{s}}_h,$$

(83)

where ${\mathbf{S}}_{\mathbf{y}}$ and ${\mathbf{S}}_{\mathbf{u}}$ are the scaling matrices, utilizing the maximum allowable deviation from the nominal value as the scaling factors. The weighting matrices $\mathbf{Q}$, $\mathbf{R}$, and $\mathbf{Z}$ penalize tracking errors, control efforts, and the slack variable, respectively. The elements within the $\mathbf{Q}$ matrix are adjusted to prioritize varying objectives, such as improving velocity tracking or decreasing lateral deviation. Similarly, the $\mathbf{R}$ matrix must be fine-tuned to prevent oscillatory behavior in the driving inputs.

The overall NMPC path tracking problem is formulated in

Table 2. NMPC formulation.

	Description	Symbol/ Equation	Scale	Lower	Upper	Units
Minimize	Cost	${\mathcal{J}}_{\mathbf{NMPC}}$ (83)
By varing
States	Velocity	V	100	0	$250/3.6$	m s−1
	Sideslip angle	$\beta$	1	$-\pi /4$	$\pi /4$	$\mathrm{rad}$
	Yaw rate	$\gamma$	1	$-\pi /2$	$\pi /2$	rad s−1
	Longitudinal acceleration	${\overline{a}}_x$	g	$-3g$	$3g$	m s−2
	Lateral acceleration	${\overline{a}}_y$	g	$-3g$	$3g$	m s−2
	Path station	s	1	0	Inf	$\mathrm{m}$
	Lateral displacement	n	5	$-{\mathcal{N}}_r+w_t/2$	${\mathcal{N}}_l-w_t/2$	$\mathrm{m}$
	Relative heading	$\xi$	1	$-\pi /4$	$\pi /4$	$\mathrm{rad}$
	Traction torque	$T_t$	$2\times {10}^3$	0	$4\times {10}^3$	N m
	Braking torque	$T_b$	$4\times {10}^3$	$-8\times {10}^3$	0	N m
	Wheel steering angle	$\delta$	$\pi /8$	$-\pi /8$	$\pi /8$	$\mathrm{rad}$
Controls	Rate of traction torque	${\dot{T}}_{t,\mathrm{cmd}}$	$2\times {10}^3$	0	$3\times {10}^3$	N m s−1
	Rate of braking torque	${\dot{T}}_{b,\mathrm{cmd}}$	$4\times {10}^3$	$-6\times {10}^3$	0	N m s−1
	Rate of wheel steering angle	${\dot{\delta }}_{\mathrm{cmd}}$	$\pi /8$	$-\pi /8$	$\pi /8$	rad s−1
Parameter	Path curvature	$\kappa$	–	–	–	m−1
$\mathbf{Subject}\mathbf{to}$
Constraints	Dynamics as IRK	(78)	–	–	–	–
	Initial condition	(77)	–	–	–	–
	Tire ellipse	(47)	1	0	1	–
	Actuator orthogonality	(48)	$0.02$	$-{10}^{-3}$	${10}^{-3}$	–

. The fast and embedded solvers acados [32] was utilized as the NMPC solver, which features the implicit Runge–Kutta (IRK) for numerical simulation and the multiple shooting approach to discretize the OCP. Real-time iteration was also implemented, to perform one sequential quadratic programming (SQP) iteration at each sampling step, to further reduce the NMPC computation time. The OCP formulation, integrators, transcription methods, weights, parameters, and QP solvers were stated at the high-level interface of Matlab. In addition, code generation was used to automatically generate a fast NMPC C code that was deployed in Simulink via the S-function. The solver parameters are specified in

Table A2. Parameters of the acados solver used in the NMPC implementation.

Option	Value
sim_method	irk
sim_method_num_stages	2
sim_method_num_steps	1
nlp_solver_max_iter	30
nlp_solver_tol_stat	${10}^{-4}$
nlp_solver_tol_eq	${10}^{-4}$
nlp_solver_tol_ineq	${10}^{-4}$
nlp_solver_tol_comp	${10}^{-4}$
qp_solver	full_condensing_hpipm
nlp_solver_exact_hessian	true
Parameter
${\mathbf{S}}_{\mathbf{y}}$	$\mathrm{diag}\left(1,0.05,0.1,0.05\right)$
${\mathbf{S}}_{\mathbf{u}}$	$\mathrm{diag}\left(2\times {10}^3,4\times {10}^3,\frac{\pi }{8}\right)$
$\mathbf{Q}$	$\mathrm{diag}\left(1,1,1,1\right)$
$\mathbf{R}$	$\mathrm{diag}\left(1,1,10\right)$
$\mathbf{Z}$	$\mathrm{diag}\left(10,10,10,10,10\right)$

.

7. Numerical Results

7.1. Simulation Setup

The simulations were performed on a desktop computer with an Intel^® i5-12500 processor. The online modules of Figure 4 were implemented and run in Simulink. The simulation was demonstrated on a lap of the Catalunya Circuit, with the raw data of track being extracted from the racetrack-database (https://github.com/TUMFTM/racetrack-database, accessed on 5 October 2024) and the time-optimal trajectories generated by the proposed TRO algorithm. The total length of the raceline $s_{\mathrm{f}}=4584\mathrm{m}$ was divided into uniform discrete intervals of $\mathrm{d}s=3\mathrm{m}$ for TRO planning and $\mathrm{d}s=1\mathrm{m}$ for NMPC tracking. To examine the convergence of NMPC at low speeds, where the numerical sensitivity to small longitudinal velocity and its associated gradients can be issues, the initial speed of the vehicle model in TRO, NMPC, and Simulink were set to $1{\mathrm{m} \mathrm{s}}^{-1}$.

7.2. Path Tracking Results

Figure 6 presents the path tracking response in global coordinates. The raceline displays the position of the vehicle’s CG with the velocity contour. It can be observed that the vehicle decelerated appropriately before reaching the apex, accelerated during the exit phase, and maintained high speeds on the straight sections of the track. The average velocity of the vehicle was $116.5\mathrm{k}\mathrm{m} {\mathrm{h}}^{-1}$. The lap time achieved by the NMPC was $141.45\mathrm{s}$, which was $1.37\mathrm{s}$ more than the optimal lap time solved by the TRO, representing a relative error of $0.98\%$.

Figure 7 presents the path tracking errors in Cartesian coordinates. The NMPC considered the future vehicle states, path tracking states, and preview references to coordinate the vehicle lateral and longitudinal motions, allowing the vehicle to follow the reference path with minimal tracking errors. Under the imposed racetrack constraints, the vehicle remained within the track boundaries during the race, as is evident from the blue line representing the lateral deviation not reaching the black line boundaries. The RMS and maximum values of the absolute lateral deviation were $0.11\mathrm{m}$ and $0.32\mathrm{m}$, respectively. The RMS and maximum values of the absolute course error were $0.33\mathrm{deg}$ and $1.28\mathrm{deg}$, respectively.

7.3. Vehicle States

Figure 8 demonstrates the performance of the NMPC controller in accurately tracking the highly dynamic trajectories planned by the TRO layer. The NMPC controlled the vehicle to approach the at-limit speed profile, while utilizing the yaw rate to follow an aggressive path curvature, and keeping the sideslip angle within a stable range.

7.4. Driving Inputs

Figure 9 shows the optimal vehicle accelerations, driving inputs, and wheel torque commands. The first plot shows that the vehicle acceleration solved by the NMPC controller was consistent with the planned acceleration from the TRO, exhibiting a similar trend in the torque commands. The steering and total torque commands were smooth, without any oscillation, which made them easier for the actuators to implement, and the total traction and braking commands satisfied the orthogonal constraint, enabling the actuators to operate with optimal efficiency. The traction and braking torques were appropriately distributed to four wheels based on the vertical load of each wheel.

7.5. Tire Workloads

Figure 10 shows the tire grip margin using the tire workload plots, where the tire workloads in longitudinal and lateral directions are defined by

$${\widehat{\mu }}_{xi}=\frac{F_{xi}}{F_{zi}},{\widehat{\mu }}_{yi}=\frac{F_{yi}}{F_{zi}},i\in \left\{fl,fr,rl,rr\right\}.$$

(84)

Under the tire ellipse constraint, the majority of the tire friction data points for all four wheels lay within the friction circle. The data points exceeding the boundary constituted $16\%$ and $13\%$ of the total data points for the left-rear wheel and right-rear wheel, respectively. The maximum values of the tire workloads did not exceed $\mu =1.1$, which falls within an acceptable tolerance range.

8. Execution Performance

Figure 11 records information about the acados solver. We configured the solver with both the SQP and SQP-RTI schemes and performed racing simulations, while keeping the other parameters unchanged. Both schemes achieved the same lap time and consistent cost function residuals. However, there was a significant difference in the computation time: the average computation time per sampling instance was 0.01 s for SQP and 0.005 s for RTI. It is noteworthy that the number of iterations using the SQP scheme alone did not exceed five, indicating that the problem exhibited good convergence and was well-defined. This provided a suitable environment for the RTI scheme, which performed only one iteration per sampling time. Furthermore, the cost function residuals being close to unity suggests that all the decision variables of the solver were appropriately scaled.

9. Conclusions

The proposed TOTPT framework utilizes a two-layered approach, with an offline TRO module and an online NMPC module. The TRO layer generates minimum-lap-time trajectories using a direct collocation method, which optimizes the vehicle’s path, velocity, and control inputs to achieve the fastest possible lap time, while respecting vehicle dynamics and track constraints. This computationally expensive offline optimization leverages a consistent double-track vehicle model to ensure coherence between the planning and tracking stages. The NMPC layer, operating in real time, is responsible for precisely tracking the reference trajectories generated by the TRO. By incorporating predicted path stations and adapting the reference trajectory based on the vehicle’s current speed and projected travel distance, the NMPC controller is able to maintain accurate path following, even under highly dynamic conditions. Notably, the NMPC formulation achieved millisecond-level execution times through the use of the RTI scheme. The authors validated the TOTPT framework through simulation on the Catalunya circuit, where the vehicle was able to track the optimal racing line with high speed and low tracking errors. The results highlight the framework’s ability to push the vehicle to its limits, which could translate to enhanced maneuverability and safety for autonomous vehicles in emergency scenarios on public roads.

Future Work

Although the proposed control scheme demonstrated precise tracking results in the simulation, certain aspects require further improvement for practical application in future work. Firstly, while the simulation results demonstrated the framework’s performance in controlling a planar vehicle model on a two-dimensional track, it would be valuable to evaluate its robustness and adaptability on tracks with different features (e.g., track curvature, elevation changes, surface conditions). Secondly, considering real-time path planning, we could either look for solvers capable of solving NLP in real-time or simplify the TRO problem by employing a point-mass vehicle model with g-g-v diagram constraints. Finally, conducting experiments in real environments is crucial. A viable approach would be to implement the algorithm in the low-cost autonomous vehicle system of the F1TENTH platform [33], conducting path-following experiments to evaluate the real-time performance and robustness of the control algorithms under practical conditions.