MPC-Based Dynamic Velocity Adaptation in Nonlinear Vehicle Systems: A Real-World Case Study

Abstract

Technological advancements have positively impacted the automotive industry, leading to the development of autonomous cars, which aim to minimize human intervention during driving, and thus reduce the likelihood of human error and accidents. These cars are distinguished by their advanced driving systems and environmental benefits due to their integration of cutting-edge autonomous technology and electric powertrains. This combination of safety, efficiency, and sustainability positions autonomous vehicles as a transformational solution for modern transportation challenges. Optimizing vehicle speed is essential in the development of these vehicles, particularly in minimizing energy consumption. Thus, in this paper, a method to generate the maximum velocity profile of a vehicle on a real road, extracted using online mapping platforms while ensuring compliance with maximum legal speed limits, is proposed. A nonlinear model, closely aligned with real-world conditions, captures and describes vehicle dynamics. Further, a nonlinear model predictive control strategy is proposed for optimizing the vehicle’s performance and safety in dynamic driving conditions, yielding satisfactory results that demonstrate the effectiveness of the method.

1. Introduction

The research into the development of autonomous vehicles has attracted significant attention from both the automotive industry and academia over the past decade. This focus has led to numerous theoretical advancements, which have subsequently been applied in practice, driving technological progress and increasing the production of vehicles that align with the envisioned ideal. An autonomous car is generally defined as a vehicle capable of self-driving, requiring little to no human intervention [1]. These contain integrated complex systems, modern technologies for detection, information, communication, and advanced automatic control methods [2]. The primary components constituting the driving process are perception, trajectory planning, and motion control. The vehicle motion process involves the generation of a trajectory that connects the initial and final points of the vehicle’s path, provides information about the velocity profile, and ensures compliance with imposed constraints, such as kinematic limits, dynamic feasibility, and safety requirements.

The motion control component represents a process capable of making decisions based on the vehicle’s current position, velocity, and orientation. It employs a logical algorithm to analyze these parameters and subsequently sends specific commands to the integrated subsystems [3]. The primary objectives of motion control are longitudinal and lateral control, which manage the vehicle’s motion along these respective axes. The longitudinal control refers to the applied traction force to achieve a specific acceleration for maintaining the desired speed. In contrast, the lateral control guides the vehicle along a prescribed path by adjusting the steering angle.

Existing research often focuses on trajectory tracking [4], path planning methods [5], or velocity profile generation [6] individually. However, the integration of these aspects is relatively rare. In instances where they are combined [7,8], the input trajectory is typically not derived from real-world scenarios. Moreover, in many research papers, race track scenarios are utilized due to their advantage of providing a safe environment for testing [9].

The utilization of nonlinear models that integrate both lateral and longitudinal dynamics, along with orientation, has proven to be a realistic and feasible representation for vehicle motion. This approach enables the implementation of trajectory tracking and velocity profile algorithms. The challenges associated with using these types of models are primarily due to the nonlinearity of vehicle dynamics [10] and the time-varying states and inputs [11]. To effectively manage the nonlinearities and constraints, model predictive control [12] is widely used in various applications [13,14]. Concisely, regarding the use of the nonlinear predictive model in vehicle control, many solutions have been proposed in several studies [15,16,17]. Considering the mentioned methodologies, the main contributions of this paper compared to our previous work [18,19,20] and the existing literature are as follows:

• A nonlinear model is used to describe the vehicle dynamics, providing a comprehensive and complex representation of its motion.
• The applied control algorithm is represented by a nonlinear model predictive control strategy, an advanced technique that enables control of the system during evasive maneuvers and effectively handles the nonlinearities of the model.
• An improved algorithm for velocity profile generation, which includes a deceleration component, is introduced.
• The results were obtained in simulation with a real trajectory extracted from an online mapping platform and the maximum legal velocity limits imposed for each road sector.

The defining and distinctive aspects of this study are characterized by the equal integration of the trajectory tracking algorithm and the generation of a maximum velocity profile. The complexity of this methodology arises from the utilization of a nonlinear model to characterize the vehicle dynamics, coupled with the implementation of nonlinear model predictive control (MPC). This implementation utilizes a road extracted from online mapping platforms as a reference, adhering to legal speed limits for specific road sections. This approach enables us to highlight not only how velocity is adjusted in more challenging turns, but also the time required to change velocity thresholds. It is important to note that, from a physical perspective, an instantaneous transition from one speed to another is not feasible.

This work is structured as follows: Section 2 reviews the literature on vehicle dynamics models and control methods, focusing on both individual dynamics (lateral and longitudinal) and combined models. Section 3 presents the model proposed in this study. Section 4 describes the algorithm for generating the speed profile, and Section 5 outlines the control strategy applied to the entire process. Section 6 details the steps used for extracting the road and generating trajectory points. Finally, the results are illustrated and analyzed in Section 7, and the conclusions based on this study are presented in Section 9.

2. Related Work

The goal of this study is to propose the methodology for implementing a velocity adaptation algorithm using a nonlinear model for vehicle dynamics and a model predictive control method to optimize the vehicle’s performance and safety in dynamic driving conditions. Nevertheless, parts of the whole problem have already been addressed in the specialized literature. Thus, in this section, research related to vehicle models and applied control techniques is briefly presented.

The primary objective is to justify the selection of the nonlinear model for vehicle dynamics and the nonlinear model predictive control (NMPC) technique, based on comprehensive literature reviews.

2.1. Vehicle Dynamics Modeling

Vehicle dynamics researchers have tried to provide a complete analysis and understanding of vehicle dynamic behavior based on different driving situations: daily driving, emergency situations, or car racing [21].

The most common approximation of a real system is achieved using mathematical models, developed through careful observation of its behavior and the impact that an estimated model can have on the real process (see Figure 1). In the literature on vehicle dynamics modeling, several approaches are identified: using models that reflect only a single dynamic, either longitudinal [22,23] or lateral [10,24]; combining two models to result in decoupled modeling [18,19]; or using models that describe a dynamic as comprehensively as possible [10,25,26,27]. In [23], a comprehensive vehicle longitudinal model is proposed that includes vehicle dynamics (influenced by tire forces, aerodynamic drag, rolling resistance, and gravity) and power train dynamics (including the engine, transmission, and wheel dynamics) to realistically simulate wheel behavior during braking and throttling. A four-wheel dynamic model is used, which is able to represent behavior that is similar to a real vehicle; it achieved good performance for high velocity values. Rajamani very explicitly described the bicycle model for the lateral dynamics in [10], which assumes the vehicle’s front and rear axles can be represented as single points. The key elements include the lateral forces acting on the tires, the yaw moment, and the resulting vehicle behavior, e.g., side-slip and yaw rate. One disadvantage of this model is the sensitivity in sudden driving conditions, such as sharp turns or high-speed maneuvers.

Models are divided into two categories: linear and nonlinear. Many studies have demonstrated the efficiency of linear models [28,29], which are frequently used due to their reduced complexity and exclusion of variable parameters. However, nonlinear models offer a closer representation of reality when it comes to vehicle dynamics [30].

2.2. Control Techniques

The design of control strategies that can handle system dynamics, especially during evasive maneuvers at high speed or emergency driving, has proven to be very efficient in the development and implementation of automated technologies. In terms of challenging driving situations, the main components that stand out are as follows: risk monitoring, driver monitoring, decision making, and path planning and control [31]. For such a controller, many works have proposed control strategies such as PID control [32], linear quadratic regulator (LQR) [33], or sliding mode control (SMC) [34]. The limitations of these methods are that they are not inherently optimal and are not well-suited for handling constrained optimization problems [31].

A suitable algorithm for vehicle control is MPC, which is an optimal control technique designed to predict future states of the model and can additionally handle nonlinearities. In [12], Camacho offers a detailed explanation and an ample study about the benefits and implementation of model predictive control. He demonstrates that MPC should not be seen only as a technique for managing linear and relatively slow processes.

The recent advancements have shown promising results in these areas, with emerging applications in nonlinear and hybrid processes. The dominance of linear models in MPC applications is due to their ease of identification from process data and effectiveness near specific operating points; this simplicity has significantly contributed to their commercial success.

The term nonlinear MPC (NMPC) refers to predictive controllers using nonlinear dynamic models, which introduce additional complexity. NMPC becomes essential in scenarios where process nonlinearities are significant, and market demands necessitate frequent operational changes.

The main advantages of MPC are its ability to manage a large number of variables and constraints [35], as well as its enhanced tracking performance at medium to high lateral and longitudinal accelerations [36]. Papers [37,38] provide relevant work in the field of integrating MPC into applications like controlling active front steering or differential brake for vehicle lateral stability. Control is essential for achieving automatic driving, with longitudinal control handling speed tracking and lateral control ensuring accurate steering [39].

2.2.1. Lateral Control

The goal of lateral control is to follow a trajectory, minimizing lateral and angular deviations to zero. It manages the vehicle’s yaw motion and the steering angle of the front wheels [39]. One of the most common studies on lateral control utilized a bicycle model [13], where a nonlinear MPC approach is proposed to solve the double-lane change maneuver. The controller proved to be efficient, but only for low velocity values. The works in [40,41] present results with a more accurate prediction of system dynamics, but with an increased computational load. In general, lateral control encompasses the concept of trajectory tracking, where the trajectory can either be a predefined path or computed online, this approach is also utilized in obstacle avoidance applications [42]. A comparative study of four path tracking controllers is presented in [42]. The analyzed methods include three classical algorithms—Pure Pursuit, Stanley controller, and Sliding Mode Control—along with a new kinematic lateral speed control. The fourth controller aims to enhance passenger comfort by smoothing lateral movements. The results analyze three criteria: precision at low speeds, stability, and passenger comfort. In terms of precision at low speeds, the proposed lateral velocity kinematic controller was the most accurate, with similar performance from the sliding kinematic controller. The kinematic controllers and Pure Pursuit provided the best comfort and safety, with smooth and precise movements. Regarding stability, all controllers except Stanley demonstrated good stability at all tested speeds.

2.2.2. Longitudinal Control

The controller for longitudinal dynamics is typically used in systems that manage the vehicle’s longitudinal motion. The study of longitudinal dynamics and control is found in many applications: adaptive cruise control, speed tracking, or velocity profile generation [18,19]. A solution to solve the ACC problem is proposed in [43], and represents a combination of motion planning and a hybrid MPC; the results proved to be satisfactory in terms of the tracking and computational calculation problem.

A solution using NMPC in a speed tracking application can be found in [44], where the controller uses an input integrator for improved robustness and speed, treating input derivatives as state variables. This method simplifies solving control equations, making the process faster and more efficient. A nonlinear control law for the longitudinal dynamics which also contains powertrain dynamics and gear ratio is proposed in [45]. The Lyapunov-based controller design ensures stability and convergence of the speed tracking error.

3. Vehicle Dynamics

Vehicles have multiple subsystems that interact under specific operating conditions. These interactions can significantly impact the effectiveness of decentralized control schemes that consider only local dynamics [25]. Therefore, a comprehensive vehicle model that includes relevant dynamic subsystems and their interactions is necessary.

Equations (1)–(3) present a model that integrates the body dynamics for longitudinal and lateral motion along with orientation [45]:

$$\begin{array}{cc}\hfill m\ddot{x}& =F_x-F_r,\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill m\ddot{y}& =-m\dot{x}\dot{\psi }+F_{yf}+F_{yr},\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill I\ddot{\psi }& =a{F}_{yf}-b{F}_{yr},\hfill \end{array}$$

(3)

where m denotes the vehicle’s mass, the term $\ddot{x}$ represents the vehicle’s acceleration in the longitudinal direction x, while $\dot{x}$ denotes the longitudinal velocity. Similarly, $\ddot{y}$ signifies the vehicle’s acceleration in the lateral direction y, while $\dot{y}$ represents the lateral velocity. The term $\ddot{\psi }$ represents the angular acceleration, and $\dot{\psi }$ denotes the yaw rate of the vehicle. The force $F_x$ is the longitudinal force applied to the vehicle and the term $F_r$ represents the resistant force opposing the motion, which includes aerodynamic drag and rolling resistance. The forces $F_{yf}$ and $F_{yr}$ represent the lateral forces acting on the front and rear tires, respectively. The distances a and b are the distances from the vehicle’s center of mass to the front and rear axles, respectively. This model accounts for various phenomena significantly influencing chassis dynamics, particularly in critical driving situations. It incorporates nonlinear longitudinal and lateral tire behavior, providing a robust framework for understanding and predicting the vehicle’s performance under complex conditions [25].

The vehicle coordinates in the fixed frame (X, Y, $\mathsf{\Psi }$) are calculated using the kinematic model given by [45]:

$$\begin{array}{cc}\hfill \dot{X}& =\dot{x}cos\mathsf{\Psi }-\dot{y}sin\mathsf{\Psi },\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill \dot{Y}& =\dot{x}sin\mathsf{\Psi }+\dot{y}cos\mathsf{\Psi },\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill \dot{\mathsf{\Psi }}& =\dot{\psi }.\hfill \end{array}$$

(6)

The forces used in Equations (4)–(6) are described as follows [10]:

$$\begin{array}{cc}\hfill F_r& =-0.5\rho A{C}_d{(\dot{x}-v_w)}^2,\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill F_{yf}& =2C_{\alpha f}(\delta -{\theta }_{vf}),\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill F_{yr}& =-2C_{\alpha r}{\theta }_{vr},\hfill \end{array}$$

(9)

where $\rho$ is the air density, A is the frontal area of the vehicle referring to the area of the vehicle that faces the airflow and contributes to the aerodynamic drag, $C_d$ represent the drag coefficient, and $v_w$ is the wind velocity.

The lateral forces contain the following parameters: $C_{\alpha f}$ and $C_{\alpha r}$, which represent the cornering stiffness of the front and rear tires, respectively, ${\theta }_{vf}$ and ${\theta }_{vr}$, which are the slip angles at the front and rear wheels, respectively, and $\delta$, which is the steering angle.

The following relations can be used to calculate the slip angles [10]:

$$\begin{array}{cc}\hfill {\theta }_{vf}& =arctan\left({\displaystyle \frac{v_{cf}}{v_{lf}}}\right),\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill {\theta }_{vr}& =arctan\left({\displaystyle \frac{v_{cr}}{v_{lr}}}\right).\hfill \end{array}$$

(11)

The lateral wheel velocities $v_{cf},v_{cr}$ and the longitudinal wheel velocities $v_{lf},v_{lr}$ are computed using [46]:

$$\begin{array}{cc}\hfill v_{cf}& =v_{yf}cos\left({\delta }_f\right)-v_{xf}sin\left({\delta }_f\right),\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill v_{cr}& =v_{yr}cos\left({\delta }_r\right)-v_{xr}sin\left({\delta }_r\right),\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill v_{lf}& =v_{xf}cos\left({\delta }_f\right)+v_{yf}sin\left({\delta }_f\right),\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill v_{lr}& =v_{xr}cos\left({\delta }_r\right)+v_{yr}sin\left({\delta }_r\right),\hfill \end{array}$$

(15)

where the velocity components for each axis are described by [46]:

$$\begin{array}{cc}\hfill v_{yf}& =\dot{y}+a\dot{\psi },\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill v_{yr}& =\dot{y}-b\dot{\psi },\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill v_{xf}& =\dot{x},\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill v_{xr}& =\dot{x}.\hfill \end{array}$$

(19)

Assumption 1.

Steering is considered only for the front axle, with the steering angles of the front wheels assumed to be equal, i.e., ${\delta }_f=\delta$, ${\delta }_r=0$.

The presented complete model, given by Equations (1)–(6), represent the vehicle components in the proposed method for computing the maximum velocity profile and the trajectory tracking concept. This model serves as a mathematical representation of the dynamics of a real vehicle, capturing the essential aspects of its motion. It combines the longitudinal, lateral, and orientation dynamics, along with the forces applied and the velocities for each wheel. Note that this model is essential for advanced control strategies to optimize the vehicle’s performance and safety.

4. Velocity Profile

The generation of a maximum velocity profile for a vehicle involves determining the maximum speed that the vehicle can achieve under specific conditions. This aspect is essential for understanding the vehicle’s performance capabilities and is influenced by various factors, including road topography.

The presented concept (see Figure 2) involves the vehicle moving along a predefined road, with known maximum velocity limits, acceleration and deceleration parameters, and trajectory given in Cartesian coordinates. The path is described by its curvature $k\left(y\right)$, which indicates how sharply a curve bends at a particular point. The objective is to determine the optimal velocity profile that enables the vehicle to safely adhere to the specified trajectory with a minimum total travel time [47].

To describe the curvature of the trajectory, the following equation is used as follows:

$$\begin{array}{c}\hfill k\left(y\right)={\displaystyle \frac{\ddot{y}\left(x\right)}{{(1+\dot{y}{\left(x\right)}^2)}^{3/2}}}.\end{array}$$

(20)

The first step in obtaining the velocity profile for a vehicle along its trajectory is to compute the maximum velocity allowed for the vehicle at each point along its path [48]:

$$\begin{array}{c}\hfill \dot{x}\left(y\right)=\sqrt{{\displaystyle \frac{\mu g}{\parallel k\left(y\right)\parallel }}}.\end{array}$$

(21)

The forward integration step provides the integration of the acceleration parameter, i.e., $a_{max}$, thereby enabling the vehicle to incrementally increase its velocity. The value obtained from Equation (22) is compared with that derived from Equation (21), and the minimum of the two values is selected [48] as follows:

$$\begin{array}{c}\hfill \dot{x}(y+\Delta y)=\sqrt{{\dot{x}}^2\left(y\right)+2a_{max}\Delta y}.\end{array}$$

(22)

The final step in the algorithm involves backward integration, during which the vehicle systematically reduces its velocity. This adjustment is made in response to the specific shape of the trajectory and is governed by the application of the deceleration parameter, i.e., $a_{min}$, ensuring that the vehicle’s speed aligns accurately with the given path [48]:

$$\begin{array}{c}\hfill \dot{x}(y-\Delta y)=\sqrt{{\dot{x}}^2\left(y\right)-2a_{min}\Delta y}.\end{array}$$

(23)

In the final step of obtaining the velocity profile, the value calculated using Equation (23) is compared with the value from Equation (22). The minimum of these two values is selected to ensure the most appropriate velocity adjustment according to the system’s constraints and requirements.

To provide a comprehensive overview of the proposed algorithm, a logical diagram is illustrated in Figure 3. In this diagram, $v1$ represents the velocity computed using (21), $v2$ is derived from (22), and $v3$ is calculated using (23). This visual representation aids in understanding the sequential application of these equations in the algorithm’s process flow.

Furthermore, based on a fundamental assumption, it is considered that the velocity values are always greater than or equal to zero [47], thereby prohibiting any movement in the reverse direction. This constraint ensures that the velocity parameters adhere strictly to forward motion scenarios within the algorithm’s operational framework.

5. Nonlinear MPC Design

The foundation of model predictive control (MPC) lies in its ability to use a prediction model of the dynamic system. This model implies the computation of the future states of the system over a predetermined finite time horizon. By iteratively solving an optimization problem at each time step, MPC determines the optimal control actions while respecting system constraints. The optimization problem incorporates the system’s dynamics, predicted future states, control inputs, and constraints on states and inputs, ensuring that the control actions are both feasible and optimal over the specified prediction horizon.

The dynamic system can be described as a state-space model [45]:

$$\begin{array}{c}\hfill \chi (k+1)=\chi \left(k\right)+T_{s}f(\chi \left(k\right),u\left(k\right)),\end{array}$$

(24)

where $f(\chi ,u)$ is the state-space representation of the vehicle model, u is the control input, $T_s$ is the sample time, and $\chi$ is the state-space vector.

The state-space vector and the control input are defined as follows:

$$\begin{array}{cc}\hfill \chi & ={[\dot{x},\dot{y},\psi ,\dot{\psi },X,Y]}^T,\hfill \end{array}$$

(25)

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

(26)

The cost function to be minimized is formulated as follows:

$$\begin{array}{c}\hfill J_c=\sum _{i=0}^{N_p-1}{H}_r+H_u,\end{array}$$

(27)

with the following components:

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}\hfill & H_r={[h_r(k+i)-h(k+i)]}^T{Q}_1[(h_r(k+i)-h(k+i)],\hfill \\ \hfill & H_u=\Delta u{(k+i)}^T{Q}_2\Delta u(k+i),\hfill \end{array}\right.\end{array}$$

(28)

where h and $h_r$ represent the predicted and the reference outputs, $Q_1$ is the weighting matrix associated with the tracking error, and $Q_2$ is the weighting matrix associated with the control input. These parameters determine the performance of the MPC feedback control [45]. The goal of this cost function is to minimize the sum of the squared differences between the predicted outputs and the reference outputs over the prediction horizon $N_p$, as well as to minimize the control input effort.

The position of the vehicle is given by the global coordinates $X,Y$ and the heading angle $\mathsf{\Psi }$. These parameters, together with the longitudinal velocity $\dot{x}$, define the reference signal: $h_r={[{\dot{x}}_r,{\dot{y}}_r,{\dot{\psi }}_r,X_r,Y_r]}^T$.

Thus, the NMPC problem is formulated as follows [45]:

$$\begin{array}{cc}\hfill argminJ\left(\chi \right(k),U),& \end{array}$$

(29)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.\chi (k+1)=& f_d(\chi \left(k\right),u\left(k\right)),\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill h\left(k\right)=& {\left[\begin{array}{c}\dot{x},\dot{y},\psi ,\dot{\psi },X,Y\end{array}\right]}^T,\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill u_{min}\le & u\left(k\right)\le u_{max},\hfill \end{array}$$

(32)

where $f_d(\chi \left(k\right),u\left(k\right))$ is the discrete state-space representation of $f(\chi ,u)$, U is the optimization vector, and $u_{min}$ and $u_{max}$ are the lower and upper limits of the control input u, respectively.

The state variable $\dot{x}$ must range from zero to the maximum velocity determined by the velocity profile algorithm. To ensure this condition is met, the following constraints are imposed:

$$\begin{array}{cc}\hfill 0\le & \dot{x}\left(k\right)\le {\dot{x}}_r\left(k\right).\hfill \end{array}$$

(33)

Following the implementation of the nonlinear model predictive control algorithm, we address the trajectory tracking problem by minimizing the difference between the reference trajectory and the obtained trajectory. Simultaneously, one can employ the velocity profile algorithm to ensure the vehicle moves safely along the path. This dual approach enables precise trajectory tracking and enhances the safety and stability of the vehicle’s motion.

The diagram shown in Figure 4 illustrates the advanced control framework for vehicle dynamics described in this paper, utilizing model predictive control. The process initiates with the collection of road data, which are used to generate future reference positions for the vehicle over the prediction horizon, represented as $X_r$ and $Y_r$ for each current time step k. Additionally, a velocity profile is created to outline the desired speeds over the prediction horizon. With these reference trajectories and velocities established, the MPC controller calculates the optimal control actions. The embedded prediction model within the MPC uses the vehicle’s current state $h\left(k\right)$ to forecast the future states over the prediction horizon. Concurrently, the controller evaluates a cost function that quantifies how closely the predicted states match the reference trajectories and velocities. This cost function also accounts for the required control efforts by penalizing deviations from the reference trajectories. Additionally, it considers the values of reference inputs related to lateral velocity ${\dot{y}}_r$ and yaw rate ${\dot{\psi }}_r$, assuming them to be zero to ensure smooth and efficient driving behavior. The motivation for including these terms is to maintain stability and improve the overall driving experience. Constraints are enforced to ensure that the predicted velocity stays within safe and feasible limits. The MPC algorithm then optimizes the control inputs, specifically the steering angle $\delta \left(k\right)$ and the longitudinal force $F_x\left(k\right)$. These optimized control inputs are applied to the vehicle, which updates its state accordingly. The updated vehicle state is then fed back into the system, facilitating continuous real-time adjustments.

This feedback loop ensures that the vehicle consistently follows the reference trajectory while adhering to dynamic constraints and minimizing control efforts. By continuously predicting future states, evaluating costs, and optimizing control actions in real time, the MPC framework effectively governs the vehicle’s path, thereby enhancing both safety and performance on the road.

6. Road Extraction

The road used to obtain the simulation results is extracted from online mapping platforms. The conventional method for defining a roadway in applications that utilize the Global Positioning System (GPS) is through the use of geographical coordinates, specifically latitude and longitude.

The dynamic model for the vehicle, along with the algorithms for control and velocity profiling, utilize Cartesian coordinates. Consequently, it is necessary to convert the latitude and longitude coordinates to Cartesian coordinates, as described in (34) and (35):

$$\begin{array}{cc}\hfill x& =R\lambda ,\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill y& =R\mathsf{\Phi }\left(\varphi \right),\hfill \end{array}$$

(35)

where R is the radius of the Earth in meters, $\lambda$ defines the selected value for longitude, and $\varphi$ represents the selected value for latitude. The component $\mathsf{\Phi }\left(\varphi \right)$ defines the functions which are used to convert the latitude $\varphi$ to a corresponding value for the lateral coordinate y in the Cartesian system:

$$\begin{array}{c}\hfill \mathsf{\Phi }\left(\varphi \right)=ln[tan({\displaystyle \frac{\varphi }{2}}+{\displaystyle \frac{\pi }{4}})].\end{array}$$

(36)

This transformation is commonly used in cartography and map projections to handle the distortion that occurs at higher latitudes.

In Figure 5a, the selected route (70,173 Stuttgart to 72,250 Freudenstadt, Germany) from the online mapping platform is depicted using geographical coordinates. The starting point is indicated at (48.7773° N, 9.1803° E) and the ending point is at (48.4640° N, 8.4119° E). Additionally, the corresponding Cartesian coordinates (see Figure 5b) resulting from the conversion algorithm confirm the desired approach, which can be effectively utilized in the proposed implementation.

The road in Figure 5a is divided into two segments: $s1$ and $s2$. This segmentation will result in shorter trajectories, simplifying the analysis of the obtained trajectory data. To ensure a realistic implementation, it is necessary to determine the maximum legal speed for each road segment within the entire obtained trajectory. This will allow the results to be compared accurately with real-world data and the information from various mapping platforms.

Moreover, the first road segment, $s1$, features a path where the maximum velocity limits are lower compared to the second segment, $s2$, which predominantly has higher speed limits (see Figure 6 as an illustration).

In Figure 6, the trajectory is presented, color-coded according to the maximum velocity limits for each segment. The trajectory has been translated to zero to facilitate clarity and enhance computational efficiency.

The motivation for selecting this road is supported by the numerous velocity segments and the curves that constitute the road. These aspects increase the difficulty of following the road and will demonstrate the efficiency of the proposed approach for the velocity profile and trajectory tracking algorithm.

The selected trajectory is composed of a series of points with a distance of approximately one meter between each of them. For this reason, it cannot be passed through each point precisely due to the vehicle’s higher velocity. To navigate the entire route at the maximum allowed velocity, the following logic, as described in Algorithm 1, is implemented.

Algorithm 1 Trajectory Following and Velocity Profile Algorithm

1: Initialize $X\_ref$, $Y\_ref$, $v_x$, $v_{max}$, k, with the first $N_p$ elements of their respective arrays 2: while the end point of the trajectory is not met do 3:     Update $v\_ref$ using the velocity profile algorithm, current references and velocity constraints 4:     Compute the predicted states using the NMPC algorithm 5:     for each predicted point do 6:         Find the closest point in the current trajectory 7:         Update reference values for $X\_ref$, $Y\_ref$, $v_x$, $v_{max}$, and k using the closest point 8:     end for 9: end while

The reference states are initialized with the first $N_p$ elements, where $N_p$ represents the prediction horizon. The reference velocity is computed using the algorithm specified by Equations (20)–(23). Subsequently, applying the control method to the dynamic system allows for the prediction of future states of the process, including the future position described by coordinates ($x,y$). Given the predicted future position, determined based on the vehicle’s velocity, the estimated point on the trajectory that the vehicle will achieve can be computed. Finally, the future reference is updated according to this prediction, and the entire process is iteratively repeated until the vehicle reaches the end point of the trajectory.

7. Illustrative Results

This section presents the results obtained by applying the proposed control method to the dynamic system. Additionally, it includes the implementation details and performance outcomes of the velocity profile algorithm. Note that a preliminary processing of the trajectory data was performed to enhance its suitability for further analysis.

For the NMPC simulation, MATLAB software was utilized, and the optimization problem was solved using the fmincon solver due to its capability to handle nonlinear objective functions and constraints. It offers flexible constraint management, accommodating both equality and inequality constraints, which are essential for maintaining system safety and performance limits. The solver employs gradient-based optimization methods, enhancing efficiency, particularly when gradients are provided or computed.

Figure 7 illustrates the structured implementation within the proposed application, providing a clearer understanding of the connectivity between the algorithms discussed in the previous sections. The description of each step of the implementation is provided as follows:

• Perception: This provides information describing the trajectory coordinates ($x,y$) and maximum allowed velocity values for each road segment.
• Reference generation: Based on the inputs, the reference velocity using Equations (20)–(23) is estimated. Using the computed velocity, the position where the vehicle arrives can be subsequently estimated, as described in Section 6.
• NMPC: In this module, the reference for the vehicle states is received. By applying the nonlinear model predictive control (NMPC) algorithm, the control signals, including traction force and steering angle, are computed.
• Vehicle model: Iteratively, at each time step, the future states of the dynamic process are estimated and computed based on the results obtained from the controller.

The vehicle model used in this study is described by Equations (1)–(19), where the vehicle state variables are shown in

Table 1. Vehicle State Variables.

Symbol	Name	Unit
x	Longitudinal position	m
y	Lateral position	m
$\psi$	Yaw angle	rad
$\dot{x}$	Longitudinal velocity	m/s
$\dot{y}$	Lateral velocity	m/s
$\dot{\psi }$	Yaw rate	rad/s
$\ddot{x}$	Longitudinal acceleration	m/s2
$\ddot{y}$	Lateral acceleration	m/s2
$\ddot{\psi }$	Angular acceleration	rad/s2
$\delta$	Steering angle	rad
$F_x$	Traction force	N
$\theta$	Slip angle	rad

and the constant parameters are shown in

Table 2. Vehicle Constant Parameters.

Symbol	Name	Value
m	Vehicle mass	1094 kg
$C_{\alpha f}$	Stiffness coefficient of the front tire	63,291 N/rad
$C_{\alpha r}$	Stiffness coefficient of rear tire	50,041 N/rad
a	Distance from CoG to front axle	1.108 m
b	Distance from CoG to rear axle	1.392 m
I	Vehicle moment of inertia	1608 kgm2
g	Gravitational acceleration	9.81 m/s2
$\rho$	Air density	1.2024 kg/m3
A	Vehicle frontal area	1.5 m2
$C_d$	Aerodynamic drag coefficient	0.5
$v_w$	Wind velocity	2 m/s
$a_{min}$	Deceleration	2 m/s2
$a_{max}$	Acceleration	6 m/s2

.

To accord the controller, we use the sample time $T_s=0.1$ seconds and the following tuning parameters:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{N}_p=5,\hfill \\ Q_1=diag\{5000,100,25,100,150\},\hfill \\ Q_2=diag\{1^{-5},100\}.\hfill \end{array}\right.\end{array}$$

The selection of NMPC parameters is essential for achieving optimal performance. For the proposed application, parameters are chosen based on a combination of theoretical guidelines and empirical tuning.

The velocity constraints for each time step are defined such that the velocity $\dot{x}$ must lie within the range $[0,{\dot{x}}_r]$, where ${\dot{x}}_r$ represents the reference velocity, as is expressed in (33). In this manner, we ensure that the velocity does not exceed the reference value calculated by the planner. In our case, the maximum allowable speed is 100 km/h.

7.1. First Road Segment—s1

For the first proposed scenario represented by the road segment $s1$, Figure 8 illustrates the results obtained with the proposed control method used for both the lateral and longitudinal dynamics.

The longitudinal dynamic is represented by the longitudinal velocity, which is obtained using the velocity profile algorithm described by Equations (20)–(23).

As illustrated, the key points of the proposed algorithm are achieved; the maximum velocity computed does not exceed the legally imposed speed limit. Moreover, the transition between a lower limit to a higher one is not instantaneous, but instead follows a ramp response, which is physically accurate and represents a natural behavior for velocity. Additionally, when a curved trajectory is followed, the algorithm decreases the velocity to ensure safe movement along the road.

7.2. Second Road Segment—s2

In the second proposed scenario illustrated in Figure 9, the difference is characterized by the higher maximum velocity limits imposed. This aspect increases the complexity and difficulty of the velocity adaptation algorithm, especially during tight maneuvers where the vehicle needs to decrease its velocity to ensure good trajectory following.

The top graph in Figure 9 shows the lateral position, in which one can observe the close alignment between the reference and the controlled trajectory that demonstrates the effectiveness of the controller in maintaining the correct position with minimal deviation. The velocity is illustrated in the bottom graph, and the obtained results indicate the effectiveness of the velocity profile algorithm. Similarly to the first scenario, the maximum legal velocity is respected, and the speed is correctly adapted to account for road curvature, ensuring accurate trajectory following. The controller effectively reduces the velocity where necessary to maintain safe and smooth navigation along the trajectory.

8. Performance Analysis and Discussion

For a comprehensive analysis, in addition to the graphical representation, performance values for both road segments, $s1$ and $s2$, were computed. This numerical analysis will allow us to better observe the influence of higher velocity values on the performance metrics.

8.1. Performance Metrics

The performance metrics represent an efficient method to evaluate obtained results, each offering unique insights into model accuracy and error characteristics. These metrics provide an interpretation of the average prediction error and a comprehensive measure of overall error magnitude, facilitating optimization.

8.1.1. Mean Squared Error (MSE)

The mean squared error (MSE) is calculated as the average of the squares of the differences between the predicted and actual values [49]:

$$\mathrm{MSE}={\displaystyle \frac{1}{n}}\sum _{i=1}^n{({\dot{x}}_i-{\widehat{\dot{x}}}_i)}^2.$$

(37)

8.1.2. Root Mean Squared Error (RMSE)

The root mean squared error (RMSE) provides an error measure in the same unit as the target variable [49]:

$$\mathrm{RMSE}=\sqrt{{\displaystyle \frac{1}{n}}\sum _{i=1}^n{({\dot{x}}_i-{\widehat{\dot{x}}}_i)}^2}.$$

(38)

8.1.3. Mean Absolute Error (MAE)

The mean absolute error (MAE) is calculated as the average of the absolute differences between the reference and controlled velocities [50]:

$$\mathrm{MAE}={\displaystyle \frac{1}{n}}\sum _{i=1}^n|{\dot{x}}_i-{\widehat{\dot{x}}}_i|.$$

(39)

8.1.4. Correlation Coefficient

The Correlation Coefficient (often denoted as r) measures the linear relationship between the reference and controlled velocities [49]:

$$r={\displaystyle \frac{{\sum }_{i=1}^n({\dot{x}}_i-\overline{y})({\widehat{\dot{x}}}_i-\overline{\widehat{\dot{x}}})}{\sqrt{{\sum }_{i=1}^n{({\dot{x}}_i-\overline{\dot{x}})}^2{\sum }_{i=1}^n{({\widehat{\dot{x}}}_i-\overline{\widehat{\dot{x}}})}^2}}},$$

(40)

where ${\dot{x}}_i$ is the reference velocity, ${\widehat{\dot{x}}}_i$ is the controlled velocity, $\overline{\dot{x}}$ is the mean of the reference velocities, $\overline{\widehat{\dot{x}}}$ is the mean of the controlled velocities, and n is the number of observations.

8.1.5. Distribution and Cumulative Distribution Function

To interpret the obtained results, the usage of the distribution and cumulative distribution function (CDF) analysis provides a deep understanding of the system’s dynamics, enabling the design of effective and resilient control solutions. These also represent essential tools in the following:

• Understanding and characterizing system behavior;
• Evaluating and improving control performance;
• Supporting predictive control strategies.

8.2. Numerical Results for the Longitudinal Dynamics

The distribution and CDF analysis for the first road segment, $s1$, are illustrated in Figure 10. In the case of the distribution results, one can observe that the medians and interquartile ranges (IQRs) of the reference and controlled velocities are very similar, indicating that the controlled velocities closely follow the reference velocities in terms of central tendency and variability. The cumulative distribution function (CDF) results indicate that the distribution of the controlled velocities closely matches that of the reference velocities across all three categories of values: low, mid, and high. There are some very minor deviations, but these have a minimal impact on the overall system performance.

For the second proposed scenario, $s2$, the results obtained regarding the distribution show many outliers below the lower whisker, as can be seen in Figure 11. This behavior is expected, considering the difficulty the vehicle faces when it needs to decrease its velocity rapidly from a high value to a low one in order to stay on the road during tight curves. Here, the influence of high velocity values on the vehicle can be better observed, demonstrating how they increase the difficulty in maintaining control and trajectory following. However, the similar overall ranges admit that the controlled velocity follows the reference one. In the case of the CDF curves for both datasets, they are very close, indicating a matched distribution.

In

Table 3. Velocity Performance Metrics.

Metric	s1	s2
Mean Squared Error (MSE)	0.14981	0.32611
Root Mean Squared Error (RMSE)	0.38705	0.57106
Mean Absolute Error (MAE)	0.18927	0.32099
Correlation Coefficient	0.99801	0.99634

, the numerical results of the error-based performance metrics and the correlation coefficient calculated using Equations (37)–(40) are presented. The low values for the performance metrics indicate good control performance, and the value obtained for the correlation coefficient, which is almost equal to 1, represents an almost perfect linear relationship for both proposed scenarios.

8.3. Numerical Results for the Lateral Dynamics

Regarding the analysis of the performance metrics for the trajectory in the first case, i.e., $s1$, the results of the distribution and the cumulative distribution function are illustrated in Figure 12. Analyzing the figures on the left and the right, one can observe that the controlled trajectory follows the reference trajectory, highlighting the similarities between the median and the spread. Additionally, it shows an alignment of the distributions.

For the second case, i.e., $s2$, the results illustrated in Figure 13 confirm the efficiency of the controller once again, with the controlled trajectory closely following the reference trajectory.

Table 4. Trajectory Performance Metrics.

Metric	s1	s2
Mean Squared Error (MSE)	0	0
Root Mean Squared Error (RMSE)	0.0011	0.0020
Mean Absolute Error (MAE)	0.0006	0.0010
Correlation Coefficient	1	1

shows the values of the performance metrics for the two cases. The small values of the errors demonstrate the controller’s good accuracy with a minimum magnitude of the errors, and the correlation coefficient, in both cases, indicates a perfect linear relationship between the two trajectories.

Considering all the results presented for the proposed scenarios, both graphical and numerical, we can conclude that the controlled velocities closely follow the reference velocities. This indicates good control performance, allowing the vehicle to move smoothly and safely along the given trajectory.

8.4. Discussion

The proposed application, as demonstrated by the results, achieves satisfactory performance in guiding a vehicle along a specified route. It ensures safety while allowing the vehicle to reach its maximum speed, all within the legal limits imposed.

The main contribution of this paper is the development of a framework that uses a nonlinear model to describe vehicle dynamics. This type of model brings the framework closer to the real behavior specific to a vehicle. To handle the nonlinearities, an NMPC (nonlinear model predictive control) controller is used, with constraints imposed on the velocity. Furthermore, this paper introduces a new perspective by using a real road as a reference trajectory, obtained from online mapping platforms. Most existing studies typically address trajectory issues through simpler scenarios, such as lane changes [51] or trajectories generated using methods like quartic function [52] or spline interpolation [53].

A realistic approach to road dynamics can be found in studies examining the behavior of racing cars on renowned circuits such as Silverstone [54] or Thunderhill Raceway [55]. The findings from these studies validate the effectiveness of various control methods; however, they primarily focus on vehicles operating in high-speed, aggressive driving conditions. In the method proposed in this paper, we analyze a standard route that reflects typical driving scenarios. Furthermore, we adopt a more realistic perspective on vehicle speed. To achieve this, we impose legal speed limits based on the selected routes, ensuring that the vehicle adheres to these constraints. Additionally, the integrated velocity algorithm effectively captures a range of driving behaviors, allowing for speed adaptations that correspond to the road’s geometric characteristics. Due to the variability in speed values, it was essential to implement a strategy for generating trajectory points. This ensures that the proposed algorithm considers the vehicle’s velocity when estimating its future position. Based on this estimation, the algorithm selects a position point that is closer to the predicted trajectory.

The obtained research results can be applied to various automated vehicle applications. For instance, they can be used to estimate a more realistic driving time for a vehicle on a specific road. This estimation has advantages over existing online applications because it considers the time required for a vehicle to reach a new maximum velocity when transitioning from a road segment with a lower speed limit to one with a higher limit. Additionally, it accounts for situations where the trajectory includes tight curves that necessitate deceleration below the maximum legal speed, which also increases travel time. We propose future work to cover and develop more applications starting with the proposed framework.

Another key point to address is the portability of the proposed automated control system to different vehicles. The control algorithm can be tuned for different vehicle dynamics by adjusting the parameters within the NMPC. However, successful implementation on different vehicles would require thorough validation and calibration for each specific vehicle type to ensure optimal performance and safety.

Future research will focus on developing a more sophisticated vehicle model and integrating communication protocols to enable vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) interactions. This will facilitate the creation of vehicle platoons that share environmental information, such as sensor data and road surface characteristics, leading to more realistic simulations. Additionally, we aim to implement an adaptive nonlinear model predictive control (NMPC) system capable of adjusting parameters in real time based on the vehicle’s current state and environment. Furthermore, the trajectory planning component will be enhanced to effectively manage unexpected obstacles and changes on the road.

9. Conclusions

In this paper, a framework is proposed that consists of a nonlinear model for vehicle dynamics controlled using a NMPC algorithm. This framework enables the implementation of a method for the maximum velocity profile and a trajectory tracking method. The vehicle dynamics are described by a nonlinear model that captures the longitudinal and lateral dynamics, as well as the orientation. The motivation for choosing this model lies in its ability to realistically simulate real-world behavior due to its complexity. It is important to note that the chosen trajectory plays a significant role in this context. To apply and test the algorithm, a road was extracted using online mapping platforms, and the maximum allowed velocities for each sector were identified. This approach enabled us to observe velocity behavior and highlight variations as the vehicle transitions between road segments with different speed limits. For the control part, a nonlinear model predictive control algorithm was implemented, an advanced method capable of handling nonlinearities effectively. Overall, the key points of this paper were achieved as follows: the results indicate good controller performance and the efficiency of the vehicle profile algorithm as the vehicle successfully manages to follow the imposed trajectory.