Optimizing Nonlinear Lateral Control for an Autonomous Vehicle

Abstract

The lack of general algorithms for the control of nonlinear systems is a generalized problem, especially when attempting to stabilize systems such as ground vehicles, which have uncertainties and are usually linearized under the assumption of small angles. To solve this problem, in this work, the implementation of a suboptimal discrete control is developed to stabilize an autonomous automobile. We assume the system is affine for the optimization procedure of finite horizon that allows us to find a solution while avoiding solving the Ricatti-type equation, commonly encountered in this kind of algorithm. This procedure is applied to the dynamical model of the lateral displacement and orientation errors of the vehicle that was discretized through the method of Euler. These nonlinear models discretized to compute a bounded control. The control is tested in different simulated scenarios to show the efficiency of the system for solving typical tasks for the path planning of an autonomous vehicle.

1. Introduction

Driving a vehicle is a very complex task, that requires the full concentration of the driver. To enable a computer system to replace the human driver effectively, it must be equipped with multiple sensors and control algorithms for various aspects of a land vehicle’s architecture. Many systems have been developed with different approaches to solve a variety of circumstances, from urban driving to highways, where the number of other vehicles, the presence of objects, and the velocity of the car are incredibly diverse. These studies concern autonomous vehicles, which are able to travel long distances without any human assistance beyond the supervision of the correct function of the developed systems. Nevertheless, different problems can be solved and improved, and current research is devoted to enhancing the response of the vehicle. This is carried out by using new control algorithms applied to nonlinear models or new approaches that enhance the measurements and reduce uncertainties. Some of the results obtained are described in the next paragraphs. Their proposed solutions often included a combination of different types of controllers, observers, estimators, adaptive control, or fuzzy control.

Jin et al. [1] created an observer for the side-slip angle with road friction adaptation. Their proposition also relies on using discrete time control. In each iteration for the state and parameter estimation, the optimization problem is solved. The integration of a continuous sensor is fused with the discrete time control.

Xiong et al. [2] designed another slip angle estimator that uses high-fidelity Inertial Measurement Unit (IMU) information, and the attitude estimator created a method such that the accumulated error is eliminated. The attitude estimator information is gathered by two estimators based on the vehicle dynamic model. This algorithm includes a delay estimation and prediction to improve the performance, given that it takes time to detect abnormal estimations created by a critical steering in the vehicle.

Shao et al. [3] reduce the risk of rollover in an efficient and robust manner, using a slip angle observer with sliding mode in combination with a switching term that depends on a linear observer. They included an adaptive sliding mode control in which, in order to have compensated for the system uncertainties, the gain is adaptively tuned. This is used in the steer-by-wire system that must be equipped to decrease lateral acceleration in the vehicle. The controller for the rollover must be properly adjusted with the other control system to prevent potential incidents such as collisions.

Wang et al. [4] used two models combined, the vehicle model and the vision model. This was made to increase the robustness of the estimation of the body slip-angle. They included an inter-sample compensation and multi-rate Kalman filter to improve the accuracy of the estimator strategy that constrains the error in the heading and the lateral position. This inter-sample period is created for the difference in the sampling time of a regular camera and the period of the motor of the vehicle, the latter being faster than the former. Their controller for reference tracking considers two degrees of liberty.

Zhang [5] considered a robot arm to steer the hand wheel with a lateral tracking control strategy that was proposed using a cost function. He obtained a strategy that constrains the error in the heading and the lateral position. His strategy also includes active acceleration and braking. The robustness in performance was achieved through the design made by linear matrix inequalities while a scheduling gain approach is used for the robustness in the control system to the time-changing variables.

Tagne et al. [6], for lateral control, designed a higher-order sliding mode control. The steering angle is used as the control input and the error in the lateral displacement is the output. Their work uses the super-twisting algorithm to ensure the controller works in high velocities, since it reduces the lateral displacement. Using high-order sliding mode and the super-twisting algorithm allows for robust control against nonlinearities and parameter uncertainties, while also reducing chattering.

Fekih et al. [7] developed a fault-tolerant control based on Linear-Quadratic Regulator (LQR) control with a feed-forward gain for path tracking. In the case of parameter variations or uncertainties, the proposal includes a weight adjustment algorithm. They also incorporated an observer to detect and identify sensor faults through sensor fusion, along with a fault-tolerant controller to ensure stability.

Wang et al. [8] proposed the union of two methods, the first one is a tire force distribution rule, and the second one is a path-tracking controller with a time-varying Model Predictive Control (MPC). In this case, the strong nonlinearities and coupling of the dynamics of the tire are described with a UNiTire model that is combined with slip conditions; however, the model for the controller was linearized using Taylor expansion.

Moreno-Gonzalez et al. [9] used the paradigm known as Model- Free control for a control that is decoupled in its architecture. The Model-Free framework reduces the dynamics of a complex system with strong nonlinearities to a simple model with online updating, ensuring stability in different velocities and dynamic constraints. The method was evaluated based on trajectory tracking, stability, safety, and passenger comfort.

Barari et al. [10] proposed a two-layer hierarchical control strategy for coordinated control. The upper layer uses a self-tunable super-twisting sliding mode control to handle parametrical uncertainties, while the lower layer employs Model Predictive Control (MPC) for control action in active front steering and direct yaw moment control. The design takes into account parametric uncertainties in mass, cornering stiffness of tires, and moment of inertia.

The hierarchical architecture used by G. Chen et al. [11] to achieve better performance and energy savings includes, among other parts, a sliding mode control for the yaw moment and the total force, an energy-efficiency optimization to reduce losing power, and a blended strategy for controlling the brake. The energy optimization in the motor also produces an energy recovery using a motor efficiency map and, to reduce losing power as a consequence of the tire sideslip, the lateral force is decreased. The steering angle is designated on the basis of the Ackerman steering theorem [12]. This proposition uses a linear model for the accelerations and a nonlinear equation for the control of the steering angle. In the area of fuzzy systems, Nguyen et al. [13] used the fuzzy static output developed with Fuzzy Lyapunov Function and the non-parallel distributed compensation to counteract the uncertain behaviors from dynamics of the vehicle and specifically the forces acting in the tires. This approach involves reformulating the control problem as an optimization problem using convexification techniques. The control constraints are expressed as linear matrix inequalities, which can be efficiently solved using semidefinite programming techniques.

Taghavifar et al. [14], proposed a fuzzy control that was implemented in the form of a fuzzy neural network to enhance the path-tracking performance. The control is adaptive and based on the design of exponential and sliding modes in the approach of fuzzy neural networks. The chattering is eliminated using a convergence control law adjusted with the base on the variable exponential sliding manifold. A hierarchical controller is included in the design for the stability of the closed-loop system, the controller was obtained using a Lyapunov approach.

However, all the approaches mentioned above were not designed using the nonlinear model of the system. The vehicle model is often linearized through the consideration of small angles. Nevertheless, the model of the vehicle has strong nonlinearities, as can be seen in the model of the bicycle described by Rajamani [12].

The present paper aims to design a control algorithm that effectively incorporates the nonlinearities inherent in the vehicle model, thereby enhancing the performance of the closed-loop system. The proposed approach introduces a suboptimal control strategy for the vehicle, leveraging information from lateral position and orientation errors. Its primary contribution involves the development of a nonlinear discrete suboptimal control, utilizing the nonlinear dynamical model of the vehicle. It is crucial to highlight that synthesizing an optimal controller for nonlinear systems, be they discrete or continuous, necessitates solving the Bellman equation, which is generally unknown. However, the proposed controller design adeptly circumvents the need to solve the Bellman equation within the suboptimal nonlinear discrete algorithm. Instead, a minimization procedure is employed to ensure the achievement of local minimum, ensuring efficient control performance. To evaluate the effectiveness of the proposed controller, extensive numerical simulations are conducted. The chosen tests include a lane-keeping assessment, where the vehicle is required to stay centered within a lane or realign itself with a new center after changing lanes. This capability enables the vehicle to avert collisions with surrounding vehicles, objects, and structures, enhancing safety and efficiency. Additionally, a velocity profile tracking test is performed, a key component of path planning for guiding a vehicle from point A to point B along a specific route. These simulations demonstrate the robustness and practical applicability of the proposed control strategy in critical real-world scenarios.

2. Materials and Methods

The methods used to obtain the results exposed in the next section can be divided into two categories. The first category is the mathematical development of the equations that describe the control with the model of the vehicle in discrete time, and the second is the description of how to make the mathematical simulation in Matlab Simulink 2022a.

The mathematical development of the control starts from the definition of controllability for a pair of points in a discrete-time system and proceeds to describe how this applies in the case of affine systems when trying to find an optimal control that minimizes certain performance indexes. Then, the suboptimal is adapted to the model of the vehicle, which has to be discretized for this purpose. The description of the settings for the mathematical simulations includes the parameters used for the model and the necessary parameters for the control. It also included the proof of the properties of the control parameters, which are defined to need these properties.

2.1. Suboptimal Control

2.1.1. Optimization Procedure of Finite Horizon

We remind the reader that the controllability of discrete systems has the following basic definition.

Definition 1. 

The pair $\left(x_o,x_1\right)$ is said to be controllable if an admissible control $u\left(k\right)$ exits and, when applied to the system defined by $x(k+1)=f\left(x\left(k\right),u\left(k\right)\right)$ takes the system from $x_0$ to $x_1$ in N finite number of steps. Where $x\left(k\right),f\left(.,.\right)\in R^n,$ $u\left(k\right)\in R^m$.

To be admissible, a control must be bounded. According to the definition, one of the problems for nonlinear discrete systems is finding the N step in which the state finally converges to $x_1$. In the present paper, we consider that the value of the step will be changed in the case that the system state is not equal to the final state $x_1$; in other words, the number of steps is fixed.

2.1.2. Affine Systems Suboptimal Discrete Nonlinear Control

The discrete affine system is described below

$$\overline{x}(k+1)=f_0\left(\overline{x}\left(k\right)\right)+f_1\left(\overline{x}\left(k\right)\right)u\left(k\right),$$

(1)

where $k=0,1,\dots ,N,$ $\overline{x}\left(k\right),f_0\left(\overline{x}\left(k\right)\right)\in R^n,$ $f_1\left(\overline{x}\left(k\right)\right)\in R^{n\times m}$ and $u\left(k\right)\in R^m.$ The sampling time for this discrete system is defined as $T_s$. The problem considered here is to find the control input $u\left(k\right)$ that minimizes the following quadratic performance index when applied to the system:

$$J=\frac{1}{2}{\overline{x}}^T\left(N\right)S\overline{x}\left(N\right)+\frac{1}{2}\sum _{k=0}^{N-1}\left\{{\overline{x}}^T\left(k\right)Q\overline{x}\left(k\right)+u^T\left(k\right)Ru\left(k\right)\right\},$$

(2)

where $t_f=T_{s}N$ is the finite time horizon, N some integer representing the step, matrices $Q,S\ge 0$ and $R>0$ have the appropriate dimensions, and they are used as weighting matrices for the relative importance of the values of $\overline{x}\left(k\right)$ and $u\left(k\right)$. The higher the values of the matrices elements, the more significant the corresponding $\overline{x}\left(k\right)$ and $u\left(k\right)$ are. Suppose that the Definition 1 is satisfied by a pair of points $\left(x_0,x_{t_f}\right)$ of the nonlinear discrete system (1). The main ideas of the dynamic programming approach exposed below, was proposed originally in [15]. This approach was applied to the Unmanned Aerial Vehicle (UAV) in [16], to an Autonomous Soaring UAV in [17] and to an hybrid exoskeleton in [18].

Theorem 1. 

Consider the quadratic performance index (2) and the system in Equation (1). The suboptimal sequence $\tilde{u}\left(N-k\right)$, which makes the performance index reach a local minimum, is

$$\tilde{u}\left(N-k\right)=-{\left[f_1^T\left(\overline{x}\left(N-k\right)\right)Q{f}_1\left(\overline{x}\left(N-k\right)\right)+R\right]}^{-1}{f}_1^T\left(\overline{x}\left(N-k\right)\right)Q{f}_0\left(\overline{x}\left(N-k\right)\right),$$

(3)

for all $k=2N,\dots ,N,$ where matrices Q and R are symmetric positive semidefinite.

Proof. 

First, consider the following definition:

$$J_{N,N}^{*}=\frac{1}{2}{\overline{x}}^T\left(N\right)S\overline{x}\left(N\right)$$

(4)

The term is independent of the control law $u\left(N\right)$ at the discrete time N. Then, it receives the name of the optimal value of J at the discrete time N. The definition of the following step is

$$\begin{array}{cc}\hfill J_{N-1,N}^{*}& =\underset{u\left(N-1\right)}{min}\left\{\frac{1}{2}{\overline{x}}^T\left(N\right)S\overline{x}\left(N\right)+\frac{1}{2}{\overline{x}}^T\left(N-1\right)Q\overline{x}(N-1)+\frac{1}{2}{u}^T(N-1)Ru(N-1)\right\}\hfill \\ \hfill & =\underset{u\left(N-1\right)}{min}\left\{J_{N,N}^{*}+\frac{1}{2}{\overline{x}}^T\left(N-1\right)Q\overline{x}(N-1)+\frac{1}{2}{u}^T(N-1)Ru(N-1)\right\},\hfill \end{array}$$

(5)

Through the state space equation given by (1) the computed value of the state $\overline{x}\left(N\right)$ is

$$\begin{array}{cc}\hfill J_{N-1,N}^{*}\left(\overline{x}\left(N-1\right),u\left(N-1\right)\right)=& \underset{u\left(N-1\right)}{min}\left\{\frac{1}{2}{\left[f_0\left(\overline{x}\left(N-1\right)\right)+f_1\left(\overline{x}\left(N-1\right)\right)u\left(N-1\right)\right]}^{T}S·\right.\hfill \\ \hfill & ·\left[f_0\left(\overline{x}\left(N-1\right)\right)+f_1\left(\overline{x}\left(N-1\right)\right)u\left(N-1\right)\right]+\hfill \\ \hfill & +\left.\frac{1}{2}{\overline{x}}^T\left(N-1\right)Q\overline{x}(N-1)+\frac{1}{2}{u}^T(N-1)Ru(N-1)\right\}\hfill \end{array}$$

(6)

The minimum value of $J_{N-1,N}$ will be found as the next step, consequently

$$u^{*}\left(N-1\right)=-{\left[f_1^T\left(\overline{x}\left(N-1\right)\right)S{f}_1\left(\overline{x}\left(N-1\right)\right)+R\right]}^{-1}{f}_1^T\left(\overline{x}\left(N-1\right)\right)S{f}_0\left(\overline{x}\left(N-1\right)\right),$$

(7)

The term ${\left[f_1^T\left(\overline{x}\left(N-1\right)\right)S{f}_1\left(\overline{x}\left(N-1\right)\right)+R\right]}^{-1}$ exists as long as the matrix $R>0$. The control algorithm given by (7), as can be seen, turns out to be the optimal control, since

$$\frac{{\partial }^2{J}_{N-1,N}\left(\overline{x}\left(N-1\right),u\left(N-1\right)\right)}{{\partial }^{2}u\left(N-1\right)}=R>0$$

(8)

The existence of the minimum is verified due to the strong convexity of the right-hand side of (6) to $u\left(N-1\right)$. For the step $N-2$, in light of the fact that the value of $u^{*}\left(N-1\right)$ has been the optimal value for the step $N-1$, and in accordance to the optimality principle of Bellman, this control formulates the optimal value for $\overline{x}\left(N-1\right)$. Subsequently, for this step $N-2$

$$\begin{array}{cc}\hfill {\tilde{J}}_{N-2,N}\left(\overline{x}\left(N-2\right)\right.,\left.u\left(N-1\right),u\left(N-2\right)\right)& =\underset{u\left(N-1\right),u\left(N-2\right)}{min}\left\{\frac{1}{2}\overline{x}{\left(N\right)}^{T}S\overline{x}\left(N\right)\right.+\hfill \\ \hfill & +\frac{1}{2}{\overline{x}}^T\left(N-1\right)Q\overline{x}(N-1)+\frac{1}{2}{\overline{x}}^T\left(N-2\right)Q\overline{x}(N-2)\hfill \\ \hfill & +\left.\frac{1}{2}{u}^T(N-1)Ru(N-1)+\frac{1}{2}{u}^T(N-2)Ru(N-2)\right\}\hfill \end{array}$$

(9)

We observe a relation, where the terms requiring $\overline{x}\left(N-m\right)$ always rely on the control $u\left(N-m-1\right)$. According to this relation, $\overline{x}\left(N\right)$ relies on $u\left(N-1\right)$, $\overline{x}\left(N-1\right)$ relies on $u\left(N-2\right)$, and so on. However, the term $u\left(N-1\right)$ found before is the optimal control and $\overline{x}\left(N-1\right)$ is derived from the state Equation (1). Subsequently,

$$\begin{array}{cc}\hfill {\tilde{J}}_{N-2,N}\left(\overline{x}\left(N-2\right),u\left(N-2\right)\right)& =\underset{u\left(N-2\right)}{min}\left\{\frac{1}{2}\overline{x}{\left(N\right)}^{T}S\overline{x}\left(N\right)\right.+\frac{1}{2}\left[f_0\left(\overline{x}\left(N-2\right)\right)\right.+\hfill \\ \hfill & +{\left.f_1\left(\overline{x}\left(N-2\right)\right)u\left(N-2\right)\right]}^{T}Q\left[f_0\left(\overline{x}\left(N-2\right)\right)\right.+\hfill \\ \hfill & \left.+f_1\left(\overline{x}\left(N-2\right)\right)u\left(N-2\right)\right]+\frac{1}{2}{\overline{x}}^T\left(N-2\right)Q\overline{x}(N-2)+\hfill \\ \hfill & +\left.\frac{1}{2}{u}^T(N-1)Ru(N-1)+\frac{1}{2}{u}^T(N-2)Ru(N-2)\right\}\hfill \end{array}$$

(10)

Now, to obtain the suboptimal control $u\left(N-2\right)$, we make use of this last equation in this step, avoiding the very complex problem of having a Riccati-type equation in the discrete domain. However, note that (10), with regard to $u\left(N-2\right)$, is strongly convex, guaranteeing that a minimum exists. Nevertheless, instead of the optimal value of $u\left(N-2\right)$, we have an approximation. The reason for this is that, in order to find the true optimal control signal $u\left(N-2\right)$, the Riccati-type equation has to be solved first, applying the solution to the nonlinear discrete model, which is hard to find. We used the above procedure to find the suboptimal control for $N-2$.

$$\tilde{u}\left(N-2\right)=-{\left[f_1^T\left(\overline{x}\left(N-2\right)\right)Q{f}_1\left(\overline{x}\left(N-2\right)\right)+R\right]}^{-1}{f}_1^T\left(\overline{x}\left(N-2\right)\right)Q{f}_0\left(\overline{x}\left(N-2\right)\right)$$

(11)

Considering the development described, we can determine that the general equations are

$$\tilde{u}\left(N-k\right)=-{\left[f_1^T\left(\overline{x}\left(N-k\right)\right)Q{f}_1\left(\overline{x}\left(N-k\right)\right)+R\right]}^{-1}{f}_1^T\left(\overline{x}\left(N-k\right)\right)Q{f}_0\left(\overline{x}\left(N-k\right)\right),$$

(12)

$${\tilde{J}}_{N-k,N}\left(\overline{x}\left(N-k\right),u\left(N-k\right)\right)={\tilde{J}}_{N-k+1,N}+\frac{1}{2}\left\{{\overline{x}}^T\left(N-k\right)Q\overline{x}(N-k)+\right.\left.{\tilde{u}}^T(N-k)R\tilde{u}(N-k)\right\},$$

(13)

for all $k=2,\dots ,N$. Reaching the minimal value of the performance index (2) is ensured by the suboptimal sequence obtained through these steps; see Equation (8). □

2.1.3. Suboptimal Control of the Land Vehicle

Consider the following mathematical model proposed in terms of the lateral and orientation errors with respect to the lane [12]:

$$\begin{array}{cc}\hfill & m\left({\ddot{e}}_1-V_x{\dot{\psi }}_{des}\right)=2C_{\alpha f}\left(\delta -f_1\right)-2C_{\alpha r}{f}_2\hfill \\ \hfill & I_z{\ddot{e}}_2=2C_{\alpha f}{\ell }_f{f}_1+2C_{\alpha r}{\ell }_r{f}_2,\hfill \end{array}$$

(14)

where

$$\begin{array}{c}\hfill f_1=\left(-arctan\left(\frac{{\dot{e}}_1-V_x{e}_2+{\ell }_f\left({\dot{e}}_2+{\dot{\psi }}_{des}\right)}{V_x}\right)\right)\\ \hfill f_2=\left(-arctan\left(\frac{{\dot{e}}_1-V_x{e}_2-{\ell }_r\left({\dot{e}}_2+{\dot{\psi }}_{des}\right)}{V_x}\right)\right)\end{array}$$

(15)

The model is illustrated in Figure 1. Model variables can be consulted in the

Table 1. Model variables.

Variables	Description
$e_1$	Distance between the center of gravity (c.g.) and the center of the lane
$e_2$	Orientation error between the orientation of the vehicle and the road orientation
$\delta$	Front wheel steering angle
$V_x$	Longitudinal velocity of the c.g. of the vehicle
${\dot{\psi }}_{des}$	Rate of change of the desired orientation of the vehicle
$C_{\alpha f}$	Individual cornering stiffness (front tires)
$C_{\alpha r}$	Individual cornering stiffness (rear tires)
${\ell }_f$	Distance of front steer axle from the c.g.
${\ell }_r$	Distance of rear steer axle from the c.g.

.

Now, we define the vector state as

$$\begin{array}{c}\hfill x={\left[\begin{array}{cccc}{x}_1& x_2& x_3& x_4\end{array}\right]}^T\\ \hfill x_1=e_1,x_2={\dot{e}}_1,x_3=e_2,x_4={\dot{e}}_2,u=\delta ,\end{array}$$

(16)

then

$$\stackrel{·}{x}={\tilde{f}}_0\left(x\right)+{\tilde{f}}_{1}u,$$

(17)

where

$$\begin{array}{cc}\hfill & \dot{x}=\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\\ {\dot{x}}_3\\ {\dot{x}}_4\end{array}\right]\hfill \\ \hfill & {\tilde{f}}_0\left(x\right)=\left[\begin{array}{c}{x}_2\\ -\frac{1}{m}\eta -\frac{1}{m}\mu -V_x{\dot{\psi }}_{des}\\ x_4\\ -\frac{{\ell }_f}{I_z}\eta +\frac{{\ell }_r}{I_z}\mu \end{array}\right]\hfill \\ \hfill & {\tilde{f}}_1=\left[\begin{array}{c}0\\ \frac{2C_{\alpha f}}{m}\\ 0\\ \frac{2C_{\alpha f}{\ell }_f}{I_z}\end{array}\right]\hfill \\ \hfill & \eta =2C_{\alpha f}arctan\left(\frac{x_2-V_x{x}_3+{\ell }_f(x_4+{\dot{\psi }}_{des})}{V_x}\right)\hfill \\ \hfill & \mu =2C_{\alpha r}arctan\left(\frac{x_2-V_x{x}_3-{\ell }_r(x_4+{\dot{\psi }}_{des})}{V_x}\right)\hfill \end{array}$$

(18)

According to the approximation of Euler, for $\overline{x}$

$$\stackrel{·}{\overline{x}}\approx \frac{\overline{x}\left(k+1\right)-\overline{x}\left(k\right)}{T_s},$$

(19)

as before the sampling time is $T_s$

$$\overline{x}\left(k+1\right)=\underset{f_0\left(\overline{x}\left(k\right)\right)}{\underbrace{T_s{\overline{f}}_0\left(\overline{x}\left(k\right)\right)+\overline{x}\left(k\right)}}+\underset{f_1}{\underbrace{T_s{\overline{f}}_1}}u\left(k\right),$$

(20)

where $k=0,1,2,3,\dots$, then

$$\overline{x}\left(k+1\right)=f_0\left(\overline{x}\left(k\right)\right)+f_{1}u\left(k\right),$$

(21)

where

$$\begin{array}{cc}\hfill & f_0\left(\overline{x}\left(k\right)\right)=\left[\begin{array}{c}{T}_s{x}_2\left(k\right)+x_1\left(k\right)\\ -\frac{1}{m}\eta \left(k\right)-\frac{1}{m}\mu \left(k\right)-T_s{V}_x{\dot{\psi }}_{des}+x_2\left(k\right)\\ T_s{x}_4\left(k\right)+x_3\left(k\right)\\ -\frac{{\ell }_f}{I_z}\eta \left(k\right)+\frac{{\ell }_r}{I_z}\mu \left(k\right)+x_4\left(k\right)\end{array}\right]\hfill \\ \hfill & f_1^T=\left[\begin{array}{cccc}0& \frac{2}{m}{T}_s{C}_{\alpha f}& 0& \frac{2}{I_z}{\ell }_f{T}_s{C}_{\alpha f}\end{array}\right]\hfill \\ \hfill & \eta \left(k\right)=2T_s{C}_{\alpha f}arctan\left(\frac{x_2\left(k\right)-V_x{x}_3\left(k\right)+{\ell }_f(x_4\left(k\right)+{\dot{\psi }}_{des})}{V_x}\right)\hfill \\ \hfill & \mu \left(k\right)=2T_s{C}_{\alpha r}arctan\left(\frac{x_2\left(k\right)-V_x{x}_3\left(k\right)-{\ell }_r(x_4\left(k\right)+{\dot{\psi }}_{des})}{V_x}\right)\hfill \end{array}$$

(22)

According to the control given by (7), the suboptimal control in the discrete time $N-1$, as a function of the error $\tilde{x}=x-x_{t_f}$, is

$$u^{*}\left(N-1\right)=-{\left[T^{T}ST+R\right]}^{-1}{T}^{T}S{f}_0\left(\overline{x}\left(N-1\right)\right),$$

(23)

where

$$\begin{array}{cc}\hfill & T=\left[\begin{array}{c}0\\ \frac{2}{m}{T}_s{C}_{\alpha f}\\ 0\\ \frac{2}{I_z}{\ell }_f{T}_s{C}_{\alpha f}\end{array}\right]\hfill \\ \hfill & S=\left[\begin{array}{cccc}{s}_{11}& s_{12}& 0& 0\\ s_{21}& s_{22}& 0& 0\\ 0& 0& s_{33}& s_{34}\\ 0& 0& s_{43}& s_{44}\end{array}\right]\hfill \\ \hfill & f_0\left(\overline{x}\left(N-1\right)\right)=\left[\begin{array}{c}{T}_s{\tilde{x}}_2\left(N-1\right)+{\tilde{x}}_1\left(N-1\right)\\ -\frac{1}{m}\tilde{\eta }(N-1)-\frac{1}{m}\tilde{\mu }(N-1)-{\tilde{a}}_1(N-1)\\ T_s{\tilde{x}}_4\left(N-1\right)+{\tilde{x}}_3\left(N-1\right)\\ -\frac{{\ell }_f}{I_z}\tilde{\eta }(N-1)+\frac{{\ell }_r}{I_z}\tilde{\mu }(N-1)+{\tilde{x}}_4\left(N-1\right)\end{array}\right]\hfill \\ \hfill & \tilde{\eta }(N-1)=2T_s{C}_{\alpha f}arctan\left(\frac{{\tilde{b}}_1(N-1))+{\ell }_f({\tilde{x}}_4\left(N-1\right)+{\dot{\psi }}_{des})}{V_x}\right)\hfill \\ \hfill & \tilde{\mu }(N-1)=2T_s{C}_{\alpha r}arctan\left(\frac{{\tilde{b}}_1(N-1)-{\ell }_r({\tilde{x}}_4\left(N-1\right)+{\dot{\psi }}_{des})}{V_x}\right)\hfill \\ \hfill & {\tilde{a}}_1(N-1)=T_s{V}_x{\dot{\psi }}_{des}+{\tilde{x}}_2\left(N-1\right),\hfill \\ \hfill & {\tilde{b}}_1(N-1)={\tilde{x}}_2\left(N-1\right)-V_x{\tilde{x}}_3\left(N-1\right),\hfill \end{array}$$

(24)

then

$$\begin{array}{cc}\hfill u^{*}(N-1)& =-m^2\frac{I_z^2}{R{m}^2{I}_z^2+4s_{44}{m}^2{\ell }_f^2{T}_s^2{C}_{\alpha f}^2+4s_{22}{I}_z^2{T}_s^2{C}_{\alpha f}^2}\left\{\frac{2}{m}{s}_{21}{T}_s{C}_{\alpha f}\left[{\tilde{x}}_1(N-1)\right.\right.+\hfill \\ \hfill & +\left.T_s{\tilde{x}}_2\left(N-1\right)\right]+\frac{2}{m}{s}_{22}{T}_s{C}_{\alpha f}[{\tilde{x}}_2\left(N-1\right)-T_s{V}_x{\dot{\psi }}_{des}-\frac{1}{m}\tilde{\eta }(N-1)-\hfill \\ \hfill & \left.-\frac{1}{m}\tilde{\mu }(N-1)\right]+\frac{2}{I_z}{\ell }_f{s}_{43}{T}_s{C}_{\alpha f}\left[{\tilde{x}}_3\left(N-1\right)+{\tilde{x}}_4{T}_s\left(N-1\right)\right]-\hfill \\ \hfill & -\frac{2}{I_z}{\ell }_f{s}_{44}{T}_s{C}_{\alpha f}\left[-\frac{1}{I_z}{\ell }_r\tilde{\mu }(N-1)\left.+\frac{1}{I_z}{\ell }_f\tilde{\eta }(N-1)-{\tilde{x}}_4\left(N-1\right)\right]\right\},\hfill \end{array}$$

(25)

for $N-k,$ where $k=2,3,\dots ,N$, we have that

$$\tilde{u}\left(N-k\right)=-{\left[T^{T}QT+R\right]}^{-1}{T}^{T}Q{f}_0\left(\overline{x}\left(N-k\right)\right),$$

(26)

where

$$\begin{array}{cc}\hfill & Q=\left[\begin{array}{cccc}{q}_{11}& q_{12}& 0& 0\\ q_{21}& q_{22}& 0& 0\\ 0& 0& q_{33}& q_{34}\\ 0& 0& q_{43}& q_{44}\end{array}\right]\hfill \\ \hfill & f_0\left(\overline{x}\left(N-k\right)\right)=\left[\begin{array}{c}{T}_s{\tilde{x}}_2\left(N-k\right)+{\tilde{x}}_1\left(N-k\right)\\ -\frac{1}{m}\tilde{\eta }\left(N-k\right)-\frac{1}{m}\tilde{\mu }(N-k)-{\tilde{a}}_1(N-k)\\ T_s{\tilde{x}}_4\left(N-k\right)+{\tilde{x}}_3\left(N-k\right)\\ -\frac{{\ell }_f}{I_z}\tilde{\eta }\left(N-k\right)+\frac{{\ell }_r}{I_z}\tilde{\mu }(N-k)+{\tilde{x}}_4\left(N-k\right)\end{array}\right]\hfill \\ \hfill & \tilde{\eta }\left(N-k\right)=2T_s{C}_{\alpha f}arctan\left(\frac{{\tilde{b}}_1(N-k)+{\ell }_f({\tilde{x}}_4\left(N-k\right)+{\dot{\psi }}_{des})}{V_x}\right)\hfill \\ \hfill & \tilde{\mu }(N-k)=2T_s{C}_{\alpha r}arctan\left(\frac{{\tilde{b}}_1(N-k)-{\ell }_r({\tilde{x}}_4\left(N-k\right)+{\dot{\psi }}_{des})}{V_x}\right)\hfill \\ \hfill & {\tilde{a}}_1(N-k)=T_s{V}_x{\tilde{x}}_4\left(N-k\right)+{\tilde{x}}_2\left(N-k\right)\hfill \\ \hfill & {\tilde{b}}_1(N-k)={\tilde{x}}_2\left(N-k\right)-V_x{\tilde{x}}_3\left(N-k\right),\hfill \end{array}$$

(27)

then

$$\begin{array}{cc}\hfill u^{*}\left(N-k\right)& =-m^2\frac{I_z^2}{R{m}^2{I}_z^2+4q_{44}{m}^2{\ell }_f^2{T}_s^2{C}_{\alpha f}^2+4q_{22}{I}_z^2{T}_s^2{C}_{\alpha f}^2}\left\{\frac{2}{m}{q}_{21}{T}_s{C}_{\alpha f}\left[{\tilde{x}}_1\left(N-k\right)\right.\right.+\hfill \\ \hfill & +\left.{\tilde{x}}_2{T}_s\left(N-k\right)\right]+\frac{2}{m}{q}_{22}{T}_s{C}_{\alpha f}\left[{\tilde{x}}_2\left(N-k\right)-T_s{V}_x{\dot{\psi }}_{des}-\frac{1}{m}\tilde{\eta }(N-k)+\right.\hfill \\ \hfill & \left.+\frac{1}{m}\tilde{\mu }(N-k)\right]+\frac{2}{I_z}{\ell }_f{q}_{43}{T}_s{C}_{\alpha f}\left[{\tilde{x}}_3\left(N-k\right)+{\tilde{x}}_4{T}_s\left(N-k\right)\right]-\hfill \\ \hfill & -\frac{2}{I_z}{\ell }_f{q}_{44}{T}_s{C}_{\alpha f}\left[\frac{1}{I_z}{\ell }_r\tilde{\eta }(N-k)\left.+\frac{1}{I_z}{\ell }_f\tilde{\mu }(N-k)-{\tilde{x}}_4\left(N-k\right)\right]\right\},\hfill \end{array}$$

(28)

for $k=2,3,$…$N.$ Notice that S and Q have to be positive semidefinite matrices (and symmetric), so $eig\left\{S\right\}$ and $eig\left\{Q\right\}$ are positive and real, and $q_{12}=q_{21},$ $q_{34}=q_{43},$ $s_{12}=s_{21},$ $s_{34}=s_{43}.$

2.2. Numerical Simulation

2.2.1. Configuration of Numerical Simulations

Two scenarios of simulation were used to illustrate the response of the proposed suboptimal control. Both are implemented in Matlab Simulink with a velocity of $V_x=30$ m/s. The first scenario is a lane-keeping task, starting with a lateral error of $1$m and the model is expected to reach $e_1=0$m with the action of the suboptimal control. The listed parameters in

Table 2. Parameters required for the simulation ¹.

Parameter	Value	Unit
m	1573	Kg
$I_z$	2873	Kgm${}^2$
${\ell }_f$	1.1	m
${\ell }_r$	1.58	m
$C_{\alpha f}$	80,000	N/rad
$C_{\alpha r}$	80,000	N/rad

¹ These parameters represent a passenger sedan [12].

were used for the lateral model in the simulation.

Furthermore, the Q matrix must be selected to be positive-definite, and the same holds for the R matrix. These matrices have been selected by trial and error and are given:

$$Q=\left[\begin{array}{cccc}2.5& 0.8& 0& 0\\ 0.8& 0.3& 0& 0\\ 0& 0& 5.25& 0.2\\ 0& 0& 0.2& 0.3\end{array}\right],R=1$$

(29)

To recreate the conditions of a lane-keeping task in the environment of Matlab Simulink, we used the block model shown in Figure 2. In the model, the principal blocks are the Lateral Model, Suboptimal Control, and ${\psi }_{des}$. The Lateral Model block contains the equations of the lateral dynamics of the vehicle in discrete time, Equations (14) and (15). The Suboptimal control block includes the proposed suboptimal control Equation (28). And the ${\psi }_{des}$ block was added to have the possibility of changing the desired orientation in accordance to the kind of road that will be simulated, but in this case, since we have a straight road, the value returned from this block is zero. The parameters from Table 1 are added in the Lateral Model Bloc and in the Suboptimal control block because these parameters were used in the equations mentioned before. The parameter $t_s$, also known as the sampling time, is set to be equal to 0.01 s. The simulation is set to use fixed time steps, to introduce $t_s$ as a constant and avoid the necessity of creating a function to introduce the sampling time to the model. The block with the mathematical expression of $\frac{1}{z}$ is the representation of the time delay that allows us to move from $x(n+1)$ to $x\left(n\right)$. The rest of the blocks are there to show the results of the different variables ($e_1$, ${\dot{e}}_1$, $e_2$, ${\dot{e}}_2$, and $u\left(N\right)$).

For the second test implemented, which is the tracking of a velocity profile, we added another two blocks, the first one is to create the velocity profile designed for this test. This profile starts at $v\left(0\right)=0$ m/s and increments smoothly until reaching the velocity of $v=2$ m/s when the time stamp is equal to 20 s, remains in the velocity of $v=2$ m/s for 60 s, and then returns to $v=0$ m/s during the next 20 s. The other block is for integrating the signal to be used in the discrete model. The Block Model for the second test is shown in Figure 3. This test uses the same parameters as before including, the fixed time step for the solver.

2.2.2. Properties of Matrices Q and R

The necessity of having positive definite matrices Q and R was established in the development of the equations for the suboptimal control. This section presents the proof that the selected Q and R are positive definite.

2.2.3. Symmetry of Matrices Q and R

For a matrix to be positive definite, first it has to be symmetric.

Definition 2. 

A matrix B is symmetric if it is equal to its transpose [19].

$$B=B^T$$

(30)

Since Matrix R is of size $1\times 1$, then R is equal to its transpose $R=R^T$. For matrix Q,

$$Q=\left[\begin{array}{cccc}2.5& 0.8& 0& 0\\ 0.8& 0.3& 0& 0\\ 0& 0& 5.25& 0.2\\ 0& 0& 0.2& 0.3\end{array}\right],$$

(31)

the values $q_{12}=q_{21}=0.8$, $q_{34}=q_{43}=0.2$. Then,

$$Q^T=\left[\begin{array}{cccc}2.5& 0.8& 0& 0\\ 0.8& 0.3& 0& 0\\ 0& 0& 5.25& 0.2\\ 0& 0& 0.2& 0.3\end{array}\right],$$

(32)

and $Q=Q^T$.

2.2.4. Positive Definiteness of Matrices Q and R

To demonstrate this property, the following definition can be used.

Definition 3. 

If all the principal minors of a real and symmetric matrix B are positive, then B is a positive-definite matrix (Criterion of Sylvester) [20].

The leading principal minor is the determinant of a principal submatrix, which is formed with the first n rows and columns of B, ($|B_{n\times n}|$) [20]. There is only one principal minor for R, and it is $|R_{1,1}|=1$. Therefore, the matrix R is positive definite.

For matrix Q, there are four principal submatrices:

$$Q_{1,1}=2.5,Q_{2,2}=\left[\begin{array}{cc}2.5& 0.8\\ 0.8& 0.3\end{array}\right],Q_{3,3}=\left[\begin{array}{ccc}2.5& 0.8& 0\\ 0.8& 0.3& 0\\ 0& 0& 5.25\end{array}\right],Q_{4,4}=\left[\begin{array}{cccc}2.5& 0.8& 0& 0\\ 0.8& 0.3& 0& 0\\ 0& 0& 5.25& 0.2\\ 0& 0& 0.2& 0.3\end{array}\right],$$

(33)

The leading principal minors of matrix Q are

$$|Q_{1,1}|=2.5,|Q_{2,2}|=0.11,|Q_{3,3}|=0.58,|Q_{4,4}|=0.17,$$

(34)

Being all positive, therefore, the matrix Q is positive definite.

3. Results

The lane-keeping test illustrates the capacity of the control to stabilize the system. It should be noticed that the error of 1 m in the lateral displacement is corrected in a short period of time, less than five seconds, and, after that, the system remains at the final state for the remainder of the time without any further lateral displacement, as can be seen in Figure 4. The suboptimal control signal keeps a small value (see Figure 5). For $e_2$, it is expected to start at zero and allow the vehicle to change its orientation to correct the position and come back to zero after that. This movement is aligned with the behavior described in the variable $e_1$; in other words, the change in the signal ends in less than 5 s, as can be seen in Figure 6.

For the tracking of a velocity profile test, the predefined profile is shown in Figure 7, as described in the configuration section for the simulation has a maximum of 2 m/s.

Adding the desired velocity profile to the suboptimal control gives the results presented in Figure 8 and Figure 9. Figure 8 shows the comparison between the velocity profile and the lateral velocity in the model. As observed, the control follows the velocity profile in the variable of ${\dot{e}}_1$, and the energy of the input control signal remains at a value less than $2\times {10}^{-3}$, see Figure 9. In this test, $e_1$ tracks the behavior of an integration of the velocity profile, Figure 10. During the first 20 s, where the velocity is increasing, the lateral displacement shows a curve. For the next 70 s, where the velocity is a constant value different from zero, the lateral displacement grows with the corresponding slope. During the time that the velocity is decreasing, the lateral displacement performs a curve in the opposite direction, and finally, when the velocity reaches zero again, the displacement becomes constant.

4. Discussion

The proposed suboptimal control solves the stabilization problem for the vehicle model without resorting to the small angle approximation and avoids solving the Bellman equation. The small angle approximation, while allowing the use of the linear model of the system, assumes that the argument of the arctan functions in the model is small.

Since the argument of the arctan functions depends on the velocity of the angle and the velocity of the lateral displacement, even with small angles in the entrance, we can have the case that the condition is not fulfilled, meaning that the argument is outside the linear behavior range. The obtained results indicate that the goal of having a control that does not rely on such assumption has been achieved.

Also, the simulation results showed that the proposed control system accomplished two important requirements. The first one is that the control easily stabilizes the whole system, and this is essential to make different control tasks with the vehicle. The second is that it keeps a moderate control signal, and this obeys the formulation as a suboptimal control; furthermore, the simulation demonstrates that the control signal generated is small in the task that implies a gradual change in the variables to track speed profiles. The simulations settings were standard for path planning of a vehicle. The results demonstrated that the suboptimal control successfully accomplishes these tasks and can be used to control the input angle $\delta$ to obtain a desired trajectory in y.

Summarizing, the main contribution of the present paper is a suboptimal control strategy for the vehicle based on the lateral position and the orientation errors with the following characteristics:

• The proposed control algorithm uses the nonlinear dynamical model of the vehicle for the controller synthesis while the proposed controllers published in the literature are based on the linearized model of the vehicle.
• The synthesis of the suboptimal control avoids the solution of the Bellman equation.