RTI-NMPC for Control of Autonomous Vehicles Using Implicit Discretization Methods

Matheus Wagner Julio E. Normey-Rico Dept. de Automação e Sistemas ( DAS), Univ. Fed. de Santa Catarina, Florianópolis-SC, Brazil. (email: [email protected]) Dept. de Automação e Sistemas ( DAS), Univ. Fed. de Santa Catarina, Florianópolis-SC, Brazil. (email: [email protected])

Abstract:

Recent efforts in the development of autonomous driving technology have induced great advancements in perception, planning and control systems. Model predictive control is one of the most popular advanced control methods, but its application to nonlinear systems still depends on the development of computationally efficient methods. This work presents a nonlinear model predictive control formulation based on real-time iteration using an implicit discretization of the system’s dynamics, with the objective of achieving greater prediction accuracy and lower computational cost when dealing with stiff dynamical systems, as is the case for vehicle dynamics. The proposed method is described and later evaluated on a simulation scenario considering modeling errors and external disturbances. The presented results demonstrate the effectiveness of the method when it comes to tracking a given trajectory and its low computational burden, measured in terms of execution time.

keywords:

Nonlinear predictive control, Motion Control Systems, Guidance navigation and control, Autonomous Vehicles, Trajectory Tracking and Path Following

1 Introduction

In recent times major research efforts, from both academia and the private sector, have been put into the development of autonomous driving technology, as it has the potential to improve the overall safety, efficiency and accessibility of transportation systems (Yu et al., 2021).

Autonomous driving systems require the interaction between several technologies. These technologies include perception, motion planning and control systems, the latter being responsible for realizing the planned trajectory by estimating the vehicle’s state and using it to define which actuation commands should be employed in order to ensure that the trajectory tracking error remains as small as possible (Alcalá et al., 2019).

A wide variety of control strategies for autonomous vehicles (AVs) have already been proposed in literature (Amer et al., 2017). Model predictive control (MPC) is a widely used advanced control technique regarded as advantageous when it comes to control of AVs, as it allows for the direct treatment of actuator and state constraints as well as the incorporation of information regarding the future reference trajectory during the determination of the control signal, at the cost of high computational complexity (Yu et al., 2021).

The non-linearity of vehicle dynamics naturally leads to nonlinear optimization problems (NLP) when formulating a MPC strategy for AV control. The direct handling of such NLPs may be unfeasible due to its computational burden in conflict with real-time requirements (Falcone et al., 2007). Several strategies to overcome the limitations imposed by the NLPs in nonlinear model predictive control (NMPC) have already been studied and usually rely on replacing the original NLP by a sequence of simpler optimization problems (OPs)(Cannon, 2004), with differences mostly due to the model employed for predictions and to the consideration of coupled or uncoupled longitudinal and lateral dynamics for vehicle control (Amer et al., 2017).

The authors of (Alcalá et al., 2019) employ a hierarchical control strategy for AV control, using a Linear Parameter Varying (LPV) MPC formulation, with the reference trajectory as scheduling variables. They consider the kinematic bicycle model for predictions and use it to formulate the LPV-MPC that acts as a high-level controller. A LPV-LQR controller, designed using the dynamic bicycle model (DBM) for the vehicle, is used as a low-level controller. The low-level controller is responsible for ensuring that the linear acceleration and steering angle commands issued by the high level controller are tracked properly. In a similar fashion, in (Pang et al., 2022) a MPC strategy that relies on the linearization of the kinematic bicycle model around a reference trajectory is proposed and evaluated using a reduced scale AV.

The RTI scheme has already been used in the development of several MPC methods for AVs control. The work presented in (López García, 2021) describes the implementation of a NMPC strategy based on RTI that uses a simplified version of the DBM for predictions. The authors of (Vaskov et al., 2023) present a stochastic NMPC strategy for AVs based on RTI aiming at handling and adapting to the uncertainties in tire-road interactions, also using the DBM for predictions. A NMPC scheme for AVs based on the RTI scheme was proposed and experimentally evaluated, using a reduced scale vehicle, in (Quirynen et al., 2020) demonstrating the effectiveness of the method in a real situation.

The MPC strategies based on the linearization of the system’s dynamics along a reference trajectory are justified by its low computational burden, at the cost of lower prediction accuracy and control quality. On the other hand, as shown in (Gros et al., 2020), RTI-based NMPC is expected to perform significantly better than methods based on linearization along a reference trajectory, at a very similar computation cost.

In this context, the objective of this work is to describe the formulation of a NMPC strategy based on the Real-Time Iteration (RTI) scheme, originally proposed in (Diehl, 2001). The present formulation differs from the original one as it employs an implicit discretization of the system’s dynamics with the objective of achieving greater prediction accuracy at a low computation cost for stiff dynamical systems, such as autonomous vehicles. The method proposed in this work differs from those found in literature by the fact that it derives the sequence of QPs solved in the RTI scheme using an implicit discretization of the system’s dynamics. For stiff dynamical systems the employment of implicit discretization schemes prior to numerical integration allows for the usage of larger time steps and for greater accuracy when compared to the use of explicit discretization schemes. In practice greater time steps may render more computationally tractable NMPC controllers with a larger prediction time horizon.

The rest article is organized as follows: Section 2 describes the formulation for the NMPC strategy considered in this study. Section 3 provides a background on vehicle dynamics and presents a detailed model used for simulation and a simplified model used for the prediction step in NPMC. Section 4 describes the case study simulation scenario considered for the evaluation of the NMPC technique and discusses the results. Finally, the conclusion of the paper is presented in section 5.

2 Implicit Real-Time Iteration Nonlinear Model Predictive Control

This section presents the derivation of the sequence of OPs to be solved to realize a RTI-NMPC strategy using the implicit Euler method for the discretization of the differential equations describing the system’s dynamics.

It is assumed that the system’s dynamics can be described by a set of first-order nonlinear differential equations in the form given by equation (1).

\dot{x}=f(x(t),u(t))

(1)

The implicit Euler discretization method is characterized by a particular approximation of the differential equation (1). This approximation is presented in (2).

x(t)-x(t-\Delta t)\approx f(x(t),u(t))\Delta t

(2)

For practical implementations of digital controllers based on NMPC, it is usually not possible to compute a control signal in a short period of time, hence it is necessary to modify equation (2) to account for the time-delay induced by the computation. In this work it is assumed that the there is a time-delay of one sampling period between the acquisition of a measurement and the update of the control signal. To account for this fact, equation (2) can be modified to produce equation (3).

x(t)-x(t-\Delta t)\approx f(x(t),u(t-\Delta t))\Delta t

(3)

To define the OP to be solved when realizing the NMPC strategy it is necessary to define the variables involved in such problem. The sequence of state predictions is described by the state prediction vector presented in equation (4). The notation $\hat{x}(t+i|t)$ stands for the predicted state at time $t+i$ using the information available at time $t$ . The same notation is used for the reference trajectory and for the control signal increment, expressed in equations (5) and (6), respectively.

\boldsymbol{\hat{x}}=\begin{bmatrix}\hat{x}(t|t)\\ ...\\ \hat{x}(t+N|t)\\ \end{bmatrix}

(4)

\boldsymbol{r}=\begin{bmatrix}r(t|t)\\ ...\\ r(t+N|t)\\ \end{bmatrix}

(5)

\Delta\boldsymbol{u}=\begin{bmatrix}u(t|t)-u(t-1|t)\\ ...\\ u(t+N|t)-u(t+N-1|t)\\ \end{bmatrix}

(6)

The discrete-time dynamics of the predictions obtained via the implicit Euler approximation can be described by equations (7) and (8).

\displaystyle\hat{x}(t+i|t)=F(\hat{x}(t+i|t),\hat{x}(t+i-1|t),u(t+i-1|t))

(7)

\displaystyle\hat{x}(t+i|t)=\hat{x}(t+i-1|t)+f(\hat{x}(t+i|t),u(t+i-1|t))\Delta t

(8)

A NLP associated with the NMPC formulation can then be defined as presented in equation (9), in which $G(.)$ defines a set of nonlinear constraints on states and inputs, $x_{m}$ represents the latest measurement of the system’s state and $u_{l}$ represents the latest control signal applied to the system. The matrix $C$ captures the relation between the internal states of the system and its output variables. Finally, $Q$ and $R$ are the weighting matrices of the cost function. A solution to this OP renders a sequence of state predictions associated with the optimal inputs along the prediction horizon.

As described in (Diehl, 2001), the RTI scheme consists of decomposing the NLP given by equation (9) into a sequence of QPs with linear constraints. At each sampling instant one QP from the sequence is then solved and the first sample of the resulting control signal sequence is applied to the system. Details regarding the convergence properties RTI can be found in (Diehl, 2001).

$\displaystyle\min_{\boldsymbol{\hat{x}},\Delta\boldsymbol{u}}$	$\displaystyle[\boldsymbol{r}-C\boldsymbol{\hat{x}}]^{T}Q[\boldsymbol{r}-C\boldsymbol{\hat{x}}]+\Delta\boldsymbol{u}^{T}R\Delta\boldsymbol{u}$	(9)
s.t.	$\displaystyle\hat{x}(t\|t)=x_{m}$
	$\displaystyle u(t-1\|t)=u_{l}$
	$\displaystyle u(t-N\|t)=u(t-N-1\|t)$
	$\displaystyle\hat{x}(t+i+1\|t)=$
	$\displaystyle F(\hat{x}(t+i\|t),\hat{x}(t+i-1\|t),u(t+i-1\|t))$
	$\displaystyle G(\hat{x}(t+i\|t),u(t+i\|t))\leq 0$

The formulation of the sequence of quadratic problems starts by defining an initial guess for the solution of the OP. The guess for the state prediction sequence is denoted by $\boldsymbol{x}_{g}$ and the guess for the control inputs sequence is denoted by $\boldsymbol{u}_{g}$ . A linear approximation for the sequence of predictions can then be derived using equation (11), in which $\delta\hat{x}(t+i|t)=\hat{x}(t+i|t)-x_{g}(t+i|t)$ and $\delta u(t+i|t)=u(t+i|t)-u_{g}(t+i|t)$ represents the deviations of the state prediction and of the predicted control input from their corresponding guesses. To simplify the notation, $F_{g}(.)$ is defined as in equation (10) and the matrices $A_{0}(.)$ , $A_{1}(.)$ and $B(.)$ are defined by equations (12), (13) and (14), respectively.

F_{g}(t+i|t)=F(x_{g}(t+i|t),x_{g}(t+i-1|t),u_{g}(t+i-1|t))

(10)

$\displaystyle\hat{x}(t+i\|t)$	$\displaystyle=F_{g}(t+i\|t)+A_{0}(t+i\|t)\delta\hat{x}(t+i-1\|t)$	(11)
	$\displaystyle+A_{1}(t+i\|t)\delta\hat{x}(t+i\|t)$
	$\displaystyle+B(t+i\|t)\delta u(t+i-1\|t)$

	$\displaystyle A_{0}(t+i\|t)$	$\displaystyle=\quad$		(12)
	$\displaystyle\frac{\partial F}{\partial\hat{x}(t+i-1\|t)}$	$\displaystyle(x_{g}(t+i\|t),x_{g}(t+i-1\|t),u_{g}(t+i-1\|t))$		(12)

	$\displaystyle A_{1}(t+i\|t)$	$\displaystyle=\quad$		(13)
	$\displaystyle\frac{\partial F}{\partial\hat{x}(t+i\|t)}$	$\displaystyle(x_{g}(t+i\|t),x_{g}(t+i-1\|t),u_{g}(t+i-1\|t))$		(13)

	$\displaystyle B(t+i\|t)$	$\displaystyle=\quad$		(14)
	$\displaystyle\frac{\partial F}{\partial u(t+i\|t)}$	$\displaystyle(x_{g}(t+i\|t),x_{g}(t+i-1\|t),u_{g}(t+i-1\|t))$		(14)

By subtracting both sides of equation (11) by $x_{g}(t+1|t)$ , it is possible to define the discrete-time dynamics of the state predictions deviations from the guess as in equation (15).

A similar procedure is adopted to provide a linear approximation for the nonlinear constraints defined by the function $G(.)$ . The result of such procedure is presented in (16), in which $G_{g}(t+i|t)$ is defined by equation (17) and the matrices $D(.)$ and $E(.)$ are given by equations (18) and (19), respectively.

$\displaystyle\delta\hat{x}(t+i\|t)$	$\displaystyle=F_{g}(t+i\|t)-x_{g}(t+i\|t)$	(15)
	$\displaystyle+A_{0}(t+i\|t)\delta\hat{x}(t+i-1\|t)$
	$\displaystyle+A_{1}(t+i\|t)\delta\hat{x}(t+i\|t)$
	$\displaystyle+B(t+i\|t)\delta u(t+i-1\|t)$

	$\displaystyle G(\hat{x}(t+i\|t),u(t+i\|t))$	$\displaystyle\approx G_{g}(t+i\|t)+D(t+i\|t)\delta\hat{x}(t+i\|t)$		(16)
		$\displaystyle+E(t+i\|t)\delta u(t+i-1\|t)$		(16)

G_{g}(t+i|t)=G(x_{g}(t+i|t),u_{g}(t+i-1|t))

(17)

D(t+i|t)=\frac{\partial G}{\partial\hat{x}}(x_{g}(t+i|t),u_{g}(t+i-1|t))

(18)

E(t+i|t)=\frac{\partial G}{\partial u}(x_{g}(t+i|t),u_{g}(t+i-1|t))

(19)

By replacing $\boldsymbol{\hat{x}}$ and $\boldsymbol{u}$ by $\boldsymbol{\hat{x}_{g}}+\delta\boldsymbol{\hat{x}}$ and $\boldsymbol{u_{g}}+\delta\boldsymbol{u}$ , respectively, in the original NLP from equation (9), as well as replacing the constraints by their respective approximations, it is possible to define a QP with linear constraints as in equation (20). Therefore the solution of the proposed OP is a sequence of state and inputs deviations, $\delta\boldsymbol{\hat{x}}$ and $\delta\boldsymbol{u}$ , which can be combined with the initial solution guess to produce the actual states prediction and control inputs sequence.

$\displaystyle\min_{\delta\boldsymbol{\hat{x}},\delta\Delta\boldsymbol{u}}$	$\displaystyle[\boldsymbol{r}-C(\boldsymbol{x_{g}}+\delta\boldsymbol{\hat{x}})]^{T}Q[\boldsymbol{r}-C(\boldsymbol{x_{g}}+\delta\boldsymbol{\hat{x}})]$	(20)
	$\displaystyle+(\Delta\boldsymbol{u_{g}}+\delta\Delta\boldsymbol{u_{g}})^{T}R(\Delta\boldsymbol{u_{g}}+\delta\Delta\boldsymbol{u_{g}})$
s.t.	$\displaystyle x_{g}(t\|t)+\delta\hat{x}(t\|t)=x_{m}$
	$\displaystyle u_{g}(t\|t)+\delta u_{g}(t\|t)=u_{l}$
	$\displaystyle u_{g}(t-N\|t)+\delta u(t-N\|t)=u_{g}(t-N-1\|t)$
	$\displaystyle+\delta u(t-N\|t)$
	$\displaystyle\delta\hat{x}(t+i\|t)=F_{g}(t+i\|t)-x_{g}(t+i\|t)$
	$\displaystyle+A_{0}(t+i\|t)\delta\hat{x}(t+i-1\|t)$
	$\displaystyle+A_{1}(t+i\|t)\delta\hat{x}(t+i\|t)+B(t+i\|t)\delta u(t+i-1\|t)$
	$\displaystyle G_{g}(t+i\|t)+D(t+i\|t)\delta\hat{x}(t+i\|t)$
	$\displaystyle+E(t+i\|t)\delta u(t+i-1\|t)\leq 0$

Equation (20), then, describes the final QP that is solved at each sampling instant to produce control signals.

In the next few sections this method is going to be employed to realize a NMPC control strategy for AVs.

3 Vehicle Dynamics Models

A defining characteristic of control strategies based on MPC is the use of a model of the system to be controlled to obtain predictions regarding its future states. In this section two vehicle dynamics models are going to be presented. The first is a four-wheel model with nonlinear tire force characteristics, which will be used to represent the plant. The second is a simplified model based on the DBM with linear tire force characteristics, which will be used to obtain the predictions for the MPC. Both models are based on the material presented in (Rajamani, 2011).

3.1 Simulation Model

The four-wheel vehicle dynamics model presented in (Rajamani, 2011) can be described by the differential equations provided in equation (21), in which $p_{x}$ and $p_{y}$ represent the vehicle’s position in the inertial reference frame, $\psi$ represents the orientation of the vehicle with respect to the inertial reference frame, $v_{x}$ and $v_{y}$ are the vehicle’s longitudinal and lateral velocities with respect to the body reference frame, $\omega$ is the angular velocity of the body reference frame with respect to the inertial reference frame and $\delta_{l}$ and $\delta_{r}$ are the left and right wheel steering angles. Also, $I$ stands for the moment of inertia of the vehicle about the $z$ axis, $m$ stands for the vehicle’s mass, $b_{lon}$ and $b_{lat}$ stands for the drag coefficient in the longitudinal and lateral directions, respectively. The quantity $v_{w}$ represents the wind absolute velocity and $\alpha_{w}$ is the orientation of the wind velocity with respect to the inertial reference frame.

$\displaystyle\dot{p}_{x}$	$\displaystyle=v_{x}cos(\psi)-v_{y}sin(\psi)$	(21)
$\displaystyle\dot{p}_{y}$	$\displaystyle=v_{x}sin(\psi)-v_{y}cos(\psi)$
$\displaystyle\dot{\psi}$	$\displaystyle=\omega$
$\displaystyle\dot{v}_{x}$	$\displaystyle=\omega v_{y}+\frac{1}{m}[F_{x,fl}cos(\delta_{l})+F_{xfr}cos(\delta_{r})$
	$\displaystyle-F_{y,fl}sin(\delta_{l})-F_{y,fr}sin(\delta_{r})+F_{x,rl}+F_{x,rr}$
	$\displaystyle-b_{lon}(v_{x}+v_{w}cos(\alpha_{w}-\psi))^{2}]$
$\displaystyle\dot{v}_{y}$	$\displaystyle=-\omega v_{x}+\frac{1}{m}[F_{x,fl}sin(\delta_{l})+F_{xfr}sin(\delta_{r})$
	$\displaystyle+F_{y,fl}cos(\delta_{l})+F_{y,fr}cos(\delta_{r})+F_{y,rl}+F_{y,rr}$
	$\displaystyle-b_{lat}(v_{y}+v_{w}sin(\alpha_{w}-\psi))^{2}]$
$\displaystyle\dot{\omega}$	$\displaystyle=\frac{1}{I}[(F_{x,fl}sin(\delta_{l})+F_{xfr}sin(\delta_{r})$
	$\displaystyle+F_{y,fl}cos(\delta_{l})+F_{y,fr}cos(\delta_{r}))l_{f}-(F_{y,rl}+F_{y,rr})l_{r}$
	$\displaystyle+(F_{x,fr}cos(\delta_{r})-F_{xfl}cos(\delta_{l})+F_{y,fl}sin(\delta_{l})$
	$\displaystyle-F_{y,fr}sin(\delta_{r})-F_{x,rl}+F_{x,rr})t_{w}$

The dynamics of the vehicle is mostly defined by the tire forces, represented as $F_{y,.}$ for lateral forces and $F_{x,.}$ for longitudinal forces. The $fl$ , $fr$ , $rl$ and $rr$ subscripts stand for front-left, front-right, rear-left and rear-right tires, respectively. The tire forces are computed based on the slip angle, defined in equation (22), and the slip ratio, defined in equation (23). The model relating those quantities to their respective tire forces is the modified Dugoff model presented in (Bian et al., 2014) and omitted in this work for brevity.

	$\displaystyle\alpha_{f}$	$\displaystyle=\delta-tan^{-1}(\frac{v_{y}+l_{f}\omega}{v_{x}})$		(22)
	$\displaystyle\alpha_{r}$	$\displaystyle=-tan^{-1}(\frac{v_{y}-l_{r}\omega}{v_{x}})$		(22)

\sigma=\frac{rw-v_{x}}{v_{x}}

(23)

It is necessary, also, to account for the wheel dynamics of the vehicle, which are given by equation (24), in which $J$ is the wheel inertia, $r$ is the wheel’s effective radius and $u_{\tau}$ is the torque applied to the wheels, one of the control inputs for the system.

$\displaystyle\dot{w}_{fr}$	$\displaystyle=\frac{1}{J}(-rF_{x,fr})$	(24)
$\displaystyle\dot{w}_{fl}$	$\displaystyle=\frac{1}{J}(-rF_{x,fl})$
$\displaystyle\dot{w}_{rr}$	$\displaystyle=\frac{1}{J}(u_{\tau}-rF_{x,rr})$
$\displaystyle\dot{w}_{rl}$	$\displaystyle=\frac{1}{J}(u_{\tau}-rF_{x,rl})$

The steering angles of the right and left front wheel are given by the Ackerman geometry and a control input to the system is the rate of change of the reference average steering angle $\delta_{c}$ , whose dynamics, along with the expressions for the Ackerman geometry, are given by equation (25).

$\displaystyle\dot{\delta}_{c}$	$\displaystyle=u_{\delta}$	(25)
$\displaystyle R$	$\displaystyle=\frac{(l_{f}+l_{r})}{tan(\delta_{c})}$
$\displaystyle\delta_{r}$	$\displaystyle=tan^{-1}(\frac{L}{R+t_{w}})$
$\displaystyle\delta_{l}$	$\displaystyle=tan^{-1}(\frac{L}{R-t_{w}})$

The use of this detailed model directly in an MPC formulation would render a great computational cost due to its high dimensionality and complexity. On the other hand, as analysed in (Zhou et al., 2023) a simplified model based on the DBM can provide an accurate approximation of the vehicle dynamics model while providing a smaller computational burden when solving the OPs for the MPC.

3.2 Prediction Model

In the DBM, as described in (Rajamani, 2011), the dynamics of a four-wheel vehicle is approximated through an equivalent two-wheel vehicle, in which the steering angle of the front wheel is given by the average between the steering angles of both front wheels from the four-wheel vehicle. This dynamic model can be expressed by equation (26), the wheels dynamics can be expressed by equation (27) and the definitions of the slip angle and the slip ratio remain the same as in the four wheel model.

Another approximation used to simplify the model is that of linear tire characteristics, which is usually sufficiently accurate for small slip angles and slip ratios. The longitudinal and lateral tire forces can then be computed based on equations (28) and (29), respectively, in which $C_{s}$ stands for the tire longitudinal stiffness coefficient and $C_{a}$ is the tire lateral stiffness coefficient.

When considering actual tire characteristics, the forces saturate as the slip angle and slip ratios increase. To capture this phenomena in the proposed MPC, a constraint on the longitudinal and lateral forces, given by the friction circle, is imposed on the predictions, as presented in (30), in which $\mu$ is the road friction coefficient and $g$ is the gravity acceleration.

$\displaystyle\dot{p}_{x}$	$\displaystyle=v_{x}cos(\psi)-v_{y}sin(\psi)$	(26)
$\displaystyle\dot{p}_{y}$	$\displaystyle=v_{x}sin(\psi)-v_{y}cos(\psi)$
$\displaystyle\dot{\psi}$	$\displaystyle=\omega$
$\displaystyle\dot{v}_{x}$	$\displaystyle=\omega v_{y}+\frac{1}{m}[2F_{x,f}cos(\delta_{c})-2F_{y,f}sin(\delta_{c})+2F_{x,r}$
	$\displaystyle-b_{lon}(v_{x}+v_{w}cos(\alpha_{w}-\psi))^{2}]$
$\displaystyle\dot{v}_{y}$	$\displaystyle=-\omega v_{x}+\frac{1}{m}[2F_{x,f}sin(\delta_{c})+2F_{y,f}cos(\delta_{c})+2F_{y,r}$
	$\displaystyle-b_{lat}(v_{y}+v_{w}sin(\alpha_{w}-\psi))^{2}]$
$\displaystyle\dot{\omega}$	$\displaystyle=\frac{1}{I}[(2F_{x,f}sin(\delta_{c})+2F_{y,f}cos(\delta_{c}))l_{f}-2F_{y,r}l_{r}$

	$\displaystyle\dot{w}_{f}$	$\displaystyle=\frac{1}{J}(-rF_{x,f})$		(27)
	$\displaystyle\dot{w}_{r}$	$\displaystyle=\frac{1}{J}(u_{\tau}-rF_{x,r})$		(27)

F_{lon}=C_{s}\sigma

(28)

F_{lat}=C_{a}\alpha

(29)

F_{lat}^{2}+F_{lon}^{2}\leq(\mu mg)^{2}

(30)

Due to its sufficient accuracy when it comes to modeling the actual vehicle dynamics (Zhou et al., 2023), in this work this simplified DBM is considered for the computations of predictions in the proposed NMPC.

4 Case Study

To evaluate the proposed approach for AV control using the RTI-NMPC the closed-loop system is evaluated in a simulation scenario. For comparison, a closed-loop system using an NMPC controller based on the direct solution of the NLP from equation (9) is also evaluated on the same simulation scenario. The models used for simulation and for the MPC predictions are those described in sections 3.1 and 3.2, respectively.

The results were acquired using a computer with an apple M2 max processor and 32GB of RAM and the solver used to realize the MPC was Ipopt.

Table 1: System Parameters.

Parameter	Value
$m$	$200$ [ $kg$ ]
$I$	$150$ [ $kg.m^{2}$ ]
$l_{r}$	$1.0$ [ $m$ ]
$t_{w}$	$0.6$ [ $m$ ]
$l_{f}$	$0.8$ [ $m$ ]
$r$	$0.6$ [ $m$ ]
$J$	$0.2$ [ $kg.m^{2}$ ]
$b_{lon}$	$0.01$ [ $Ns^{2}/m^{2}$ ]
$b_{lat}$	$0.05$ [ $Ns^{2}/m^{2}$ ]
$C_{s}$	$600000[N]$
$C_{a}$	$250000$ [ $N/rad$ ]
$\mu$	$0.9$

4.1 Scenario Description

The simulated scenario consists in the vehicle following a reference trajectory while subject to measurement noise and to wind disturbances modeled as integrated Gaussian noise, as such disturbances are related, in general, to adverse conditions for driving (Hou et al., 2019). The vehicle parameters used for simulation are presented in table 1.

The wind disturbance is modeled as integrated Gaussian noise with a mean of $2m/s$ and a standard deviation of $1.5m/s$ . The resulting wind disturbance waveform in presented in figure 2(a). The measurement noise is modeled as zero-mean Gaussian noise and the standard deviation associated with each state variable is presented in table 2. To reduce the impacts of measurement noise in the controller performance, a first-order Butterworth with cutoff frequency at $3.5Hz$ was designed, discretized using the Tustin method and applied to all measurement signals.

The system’s dynamics are discretized using a $0.04s$ time step, equal to the sampling period. The prediction horizon considered is $15$ samples. The cost function is normalized by the minimum and maximum values expected for control action and reference trajectory values. Besides the constraint given by equation (30), additional constraints are imposed to the maximum and minimum values of wheel torque, steering angle rate and steering angle. The minimum and maximum wheel torque, steering angle rate and steering values are $-300Nm$ and $300Nm$ , $-1.5rad/s$ and $1.5rad/s$ , $-45^{\circ}$ and $45^{\circ}$ respectively. A relative weight, in the cost function, of $5$ is given to both the wheel torque control signal increment and steering angle rate control signal increment.

Table 2: Measurement Noise Parameters.

State Variable	Noise Distribution Standard Deviation
$p_{x}$	$0.05$ [ $m$ ]
$p_{y}$	$0.05$ [ $m$ ]
$\psi$	$0.01$ [ $rad$ ]
$v_{x}$	$0.05$ [ $m/s$ ]
$v_{y}$	$0.05$ [ $m/s$ ]
$\omega$	$0.01$ [ $rad/s$ ]
$\omega_{fr}$	$0.01$ [ $rad/s$ ]
$\omega_{fl}$	$0.01$ [ $rad/s$ ]
$\omega_{rr}$	$0.01$ [ $rad/s$ ]
$\omega_{rl}$	$0.01$ [ $rad/s$ ]
$\delta_{c}$	$0.01$ [ $rad$ ]

4.2 Results

This section presents the results of the RTI-NMPC controller evaluation in the proposed scenario.

Figures 1(a) and 1(b) show the reference and simulated trajectories during the closed loop simulations using the RTI and direct NLP based controllers, demonstrating that both controllers are able to successfuly track the proposed trajectory.

Refer to caption — (a) Simulated trajectories during the first $2s$ of the simulation

Figures 2(b) and 2(c) present the wheel torque and steering angle control signals from both controllers. In both figures it is possible to visualize the high-frequency oscillations as a response to measurement noise, showing that the two controllers are equally sensitive to noise and produce very similar control signals. The tracking error for each controller is presented in figure 2(d), which reinforces the effectiveness of both controllers when it comes to tracking a desired trajectory and also indicates that the controller based on the direct solution of the NLP does not lead, necessarily, to superior performance. To properly capture the transient behavior of the closed-loop system, the initial tracking error, given by the difference between the initial state of the system and the first point of the reference trajectory, was set to $1m$ .

The fact that the control signals, shown in figures 2(b) and 2(c), as well as the tracking error, shown in figure 2(d), for both controllers do not present significant differences presents itself as an evidence that the solutions for the NMPC problem obtained using the RTI scheme successfully approximate the solutions obtained via nonlinear optimization methods.

The results presented in figures 3(a) and 3(b) stand for the execution times observed for the controller based on the direct NLP solution and for that based on the RTI scheme, respectively. It is clear that the practical realization of the controller based on the direct NLP solution in unfeasible, as all execution time observations for this controller are greater than the controller’s sampling period, at an average of approximately $106ms$ ,. On the other hand, the execution times observed for the controller that uses the RTI scheme are mostly below $40ms$ , the controller’s sampling period, and have an average of approximately $15ms$ , rendering the controller realizable. Although the variability in execution times is inevitable due to the iterative nature of optimization algorithms, in a practical scenario, for the few cases in which solver cannot find a solution in time, it is still possible to use sub-optimal solution to obtain the control signal.

5 Conclusion

In this work a strategy for trajectory tracking control of AVs based on the RTI scheme for NMPC considering an implicit discretization method was presented and a derivation of the quadratic sub-problems that composes the RTI scheme using the implicit Euler discretization method, specially useful when dealing with stiff dynamical systems, was provided. The ability of the proposed scheme to track a given trajectory was verified in simulation considering a detailed four-wheel vehicle dynamics model considering the effects of wind disturbances and measurement noise.

The effectiveness of the approach was evaluated based on its ability to track the reference trajectory as well as on its computational performance. The proposed strategy was also compared with another NMPC controller based on the direct solution of a nonlinear optimization problem. The presented results indicate that the proposed RTI-MPC strategy for trajectory tracking control of AVs is indeed effective. The trajectory tracking performance was satisfactory despite the presence of disturbances and modeling errors. The execution time observed when using the proposed were, on average, 7 times smaller than those observed when using a controller that solves the NMPC problem via direct nonlinear optimization, evidencing the computational efficiency associated with this strategy for nonlinear model predictive control.

{ack}

The present work was realized with the support from CAPES, a Brazilian Government entity for human resources development. Julio E. Normey-Rico thanks the CNPq project 304032/2019-0.

References

Alcalá et al. (2019) Alcalá, E., Puig, V., and Quevedo, J. (2019). Lpv-mpc control for autonomous vehicles. IFAC-PapersOnLine, 52(28).
Amer et al. (2017) Amer, N.H., Zamzuri, H., Hudha, K., and Kadir, Z.A. (2017). Modelling and control strategies in path tracking control for autonomous ground vehicles: a review of state of the art and challenges. Journal of intelligent & robotic systems, 86.
Bian et al. (2014) Bian, M., Chen, L., Luo, Y., and Li, K. (2014). A dynamic model for tire/road friction estimation under combined longitudinal/lateral slip situation. Technical report.
Cannon (2004) Cannon, M. (2004). Efficient nonlinear model predictive control algorithms. Annual Reviews in Control, 28(2).
Diehl (2001) Diehl, M. (2001). Real-time optimization for large scale nonlinear processes. Ph.D. thesis.
Falcone et al. (2007) Falcone, P., Borrelli, F., Asgari, J., Tseng, H.E., and Hrovat, D. (2007). Predictive active steering control for autonomous vehicle systems. IEEE Transactions on Control Systems Technology, 15(3).
Gros et al. (2020) Gros, S., Zanon, M., Quirynen, R., Bemporad, A., and Diehl, M. (2020). From linear to nonlinear mpc: bridging the gap via the real-time iteration. International Journal of Control, 93(1).
Hou et al. (2019) Hou, G., Chen, S., and Chen, F. (2019). Framework of simulation-based vehicle safety performance assessment of highway system under hazardous driving conditions. Transportation Research Part C: Emerging Technologies, 105.
López García (2021) López García, M. (2021). Predictive Control of an Autonomous Vehicle using the RTI method. Master’s thesis, Universitat Politècnica de Catalunya.
Pang et al. (2022) Pang, H., Liu, N., Hu, C., and Xu, Z. (2022). A practical trajectory tracking control of autonomous vehicles using linear time-varying mpc method. Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering, 236(4).
Quirynen et al. (2020) Quirynen, R., Berntorp, K., Kambam, K., and Di Cairano, S. (2020). Integrated obstacle detection and avoidance in motion planning and predictive control of autonomous vehicles. In 2020 American control conference (ACC), 1203–1208. IEEE.
Rajamani (2011) Rajamani, R. (2011). Vehicle dynamics and control. Springer Science & Business Media.
Vaskov et al. (2023) Vaskov, S., Quirynen, R., Menner, M., and Berntorp, K. (2023). Friction-adaptive stochastic nonlinear model predictive control for autonomous vehicles. arXiv preprint arXiv:2305.03798.
Yu et al. (2021) Yu, S., Hirche, M., Huang, Y., Chen, H., and Allgöwer, F. (2021). Model predictive control for autonomous ground vehicles: a review. Autonomous Intelligent Systems, 1.
Zhou et al. (2023) Zhou, S., Tian, Y., Walker, P., and Zhang, N. (2023). Impact of the tyre dynamics on autonomous vehicle path following control with front wheel steering and differential motor torque. IET Intelligent Transport Systems.

$\displaystyle\min_{\boldsymbol{\hat{x}},\Delta\boldsymbol{u}}$	$\displaystyle[\boldsymbol{r}-C\boldsymbol{\hat{x}}]^{T}Q[\boldsymbol{r}-C\boldsymbol{\hat{x}}]+\Delta\boldsymbol{u}^{T}R\Delta\boldsymbol{u}$	(9)
s.t.	$\displaystyle\hat{x}(t\|t)=x_{m}$
	$\displaystyle u(t-1\|t)=u_{l}$
	$\displaystyle u(t-N\|t)=u(t-N-1\|t)$
	$\displaystyle\hat{x}(t+i+1\|t)=$
	$\displaystyle F(\hat{x}(t+i\|t),\hat{x}(t+i-1\|t),u(t+i-1\|t))$
	$\displaystyle G(\hat{x}(t+i\|t),u(t+i\|t))\leq 0$

$\displaystyle\hat{x}(t+i\|t)$	$\displaystyle=F_{g}(t+i\|t)+A_{0}(t+i\|t)\delta\hat{x}(t+i-1\|t)$	(11)
	$\displaystyle+A_{1}(t+i\|t)\delta\hat{x}(t+i\|t)$
	$\displaystyle+B(t+i\|t)\delta u(t+i-1\|t)$

$\displaystyle\delta\hat{x}(t+i\|t)$	$\displaystyle=F_{g}(t+i\|t)-x_{g}(t+i\|t)$	(15)
	$\displaystyle+A_{0}(t+i\|t)\delta\hat{x}(t+i-1\|t)$
	$\displaystyle+A_{1}(t+i\|t)\delta\hat{x}(t+i\|t)$
	$\displaystyle+B(t+i\|t)\delta u(t+i-1\|t)$

	$\displaystyle G(\hat{x}(t+i\|t),u(t+i\|t))$	$\displaystyle\approx G_{g}(t+i\|t)+D(t+i\|t)\delta\hat{x}(t+i\|t)$		(16)
		$\displaystyle+E(t+i\|t)\delta u(t+i-1\|t)$		(16)

$\displaystyle\min_{\delta\boldsymbol{\hat{x}},\delta\Delta\boldsymbol{u}}$	$\displaystyle[\boldsymbol{r}-C(\boldsymbol{x_{g}}+\delta\boldsymbol{\hat{x}})]^{T}Q[\boldsymbol{r}-C(\boldsymbol{x_{g}}+\delta\boldsymbol{\hat{x}})]$	(20)
	$\displaystyle+(\Delta\boldsymbol{u_{g}}+\delta\Delta\boldsymbol{u_{g}})^{T}R(\Delta\boldsymbol{u_{g}}+\delta\Delta\boldsymbol{u_{g}})$
s.t.	$\displaystyle x_{g}(t\|t)+\delta\hat{x}(t\|t)=x_{m}$
	$\displaystyle u_{g}(t\|t)+\delta u_{g}(t\|t)=u_{l}$
	$\displaystyle u_{g}(t-N\|t)+\delta u(t-N\|t)=u_{g}(t-N-1\|t)$
	$\displaystyle+\delta u(t-N\|t)$
	$\displaystyle\delta\hat{x}(t+i\|t)=F_{g}(t+i\|t)-x_{g}(t+i\|t)$
	$\displaystyle+A_{0}(t+i\|t)\delta\hat{x}(t+i-1\|t)$
	$\displaystyle+A_{1}(t+i\|t)\delta\hat{x}(t+i\|t)+B(t+i\|t)\delta u(t+i-1\|t)$
	$\displaystyle G_{g}(t+i\|t)+D(t+i\|t)\delta\hat{x}(t+i\|t)$
	$\displaystyle+E(t+i\|t)\delta u(t+i-1\|t)\leq 0$