Predictive Path Following and Collision Avoidance of Autonomous Connected Vehicles

Abstract

This paper considers nonlinear model predictive control for simultaneous path-following and collision avoidance of connected autonomous vehicles. For each agent, a nonlinear bicycle model is used to predict a sequence of the states and then optimize them with respect to a sequence of control inputs. The objective function of the optimal control problem is to follow the planned path which is represented by a Bézier curve. In order to achieve collision avoidance among the networked vehicles, a geometric shape must be selected to represent the vehicle geometry. In this paper, an elliptic disk is selected for that as it represents the geometry of the vehicle better than the traditional circular disk. A separation condition between each pair of elliptic disks is formulated as time-varying state constraints for the optimization problem. Driving corridors are assumed to be also Bézier curves, which could be obtained from digital maps, and are reformulated to suit the controller algorithm. The algorithm is validated using MATLAB simulation with the aid of ACADO toolkit.

1. Introduction

Autonomous driving has gained a lot of attention in the last decade, from both industry and academia, due to its anticipated impacts on the enhancement of traffic efficiency. There are many types of controllers that can be used for autonomous driving. In [1], sensitivity analysis is used to design an automatic path following controller by means of an automatic modification of desired output trajectories. A novel path planning algorithm is presented in [2] for planning and execution of heavy multi-body vehicles with multi-axle steering. Sliding mode control is used in [3] for trajectory tracking and control of autonomous high-speed ground vehicles.

Nonlinear Model Predictive Control (NMPC) has proven successful results and has been employed in many recent publications [4,5] because it combines both control and collision avoidance into one algorithm which can be used for critical maneuvers. It is an optimization-based controller that has been widely used for various transportation systems such as autonomous ships [6] and autonomous vehicles [4]. It is mainly used for two famous problems: Trajectory tracking and path following. Recently, it started to get attention to be used for collision avoidance because of its ability to handle nonlinear constraints [7]. In [8], NMPC is used for the trajectory tracking of a scaled autonomous racing car with a higher level planner to account for collision avoidance by building local approximations of the controls from linearized time-varying models. A bi-level NMPC is developed in [9] for intersection scenarios to allocate and update optimal and collision-free timeslots when each vehicle is allowed to occupy the intersection, and provide optimal actuation commands given the allocated timeslots using another low level controller. Experimental validation of NMPC is given in [4] to show that online nonlinear optimization tools are efficient for real-time applications on hardware with limited processing resources, and a comparison of different embedded nonlinear optimization algorithms for autonomous vehicle control is provided. In [10], a legible model predictive control scheme is presented for overtaking of autonomous vehicles on highways in order to enhance the safety and improve the efficiency.

It has been shown recently in [11] that NMPC can meet the safety and robustness requirements of autonomous systems by demonstrating some real world scenarios of autonomous driving under consideration of unknown obstacles. The issue of critical maneuvers of autonomous vehicles is discussed in [12], where a real-time trajectory planning is achieved using model predictive control employing homotopic classes to avoid the computation complexity of sophisticated models.

The main contribution of this work is to fill the gap in NMPC algorithms for autonomous driving by employing paths and road corridors that could be obtained directly from existing digital maps.

This paper is organized as follows. Section 2 describes the vehicle model employed by NMPC, the generation of the reference trajectory, and clarifies the control objective. The controller synthesis is presented in Section 3: A brief introduction of NMPC is given first for the trajectory tracking of the vehicle, followed by how to utilize state and control constraints for comfort driving. After that, the development of the collision avoidance constraints is presented, followed by some guidelines for the implementation of the dynamic optimization problem and the software used for that. MATLAB-based simulation results are given in Section 4. Section 5 concludes this paper and provide our outlook for future work.

2. Problem Formulation

2.1. Vehicle Modeling

Nonlinear Model Predictive Control (NMPC) employs a vehicle dynamic model in order to predict the future trajectory of the vehicle over a predefined prediction horizon. This model is the key for a successful vehicle control system. In the literature of vehicle control, there are different models of vehicles varying in the complexity from kinematic to complex kinetic models [13]. While kinetic models provide accurate prediction of the vehicle motion, they might increase the complexity in the design of collision avoidance systems, and require, during the deployment, high computation time and small sampling interval. It has been shown in the literature that the kinematic model can provide reasonable accuracy by adding constraints on the rate of change of the inputs to be consistent with the low level controllers of the vehicle [14,15]. Therefore, the state space representation of the nonlinear kinematic bicycle model [16]

$$\begin{array}{cc}\hfill \dot{x}\left(t\right)& =V\left(t\right)cos\left(\delta \right(t)+\beta (t\left)\right)\hfill \\ \hfill \dot{y}\left(t\right)& =V\left(t\right)sin\left(\delta \right(t)+\beta (t\left)\right)\hfill \\ \hfill \dot{\psi }\left(t\right)& =\frac{V\left(t\right)}{l_r}sin\left(\beta \left(t\right)\right)\hfill \\ \hfill \dot{V}\left(t\right)& =a\left(t\right),\hfill \end{array}$$

(1)

is used in this paper, where $\left(x\right(t),y(t\left)\right)$ indicates the position of the center of gravity (CG) of the vehicle, $V\left(t\right)$ is the longitudinal velocity, $\psi \left(t\right)$ is the heading angle of the vehicle, $\beta \left(t\right)=arctan(\frac{l_r}{L}tan\left(\delta \left(t\right)\right))$ is the side-slip angle between the velocity vector and the longitudinal direction of the vehicle, $a\left(t\right)$ is the longitudinal acceleration input used to control the velocity, $\delta \left(t\right)$ is the steering angle, L is the wheelbase, and $l_r$ is the distance between CG and the rear wheel (see Figure 1).

In order to achieve consistency between the kinematic model and the real vehicle model, interval constraints on the control inputs and their rate of change must be imposed. These constraints play a key role in achieving comfort driving and avoids extreme driving conditions by limiting the maximum and minimum value for the longitudinal acceleration and steering angle inputs. The maximum/minimum longitudinal acceleration can be obtained experimentally and vary from one vehicle to the other based on the engine size. The steering angle can be limited in terms of a maximum safe lateral acceleration to achieve normal driving behavior and avoid extreme slipping [14]. Therefore, the acceleration and steering angle constraints

$$\begin{array}{cc}\hfill a_{min}& \le a\le a_{max},\hfill \\ \hfill {\delta }_{min}& \le \delta \le {\delta }_{max},\hfill \end{array}$$

(2)

must be considered. Constraints on time derivative of the control inputs are beneficial for avoiding jerky behavior, and are represented in the model by the augmentation of the control inputs, $(a,\delta )$, as extra states and define the rate of change as a new virtual inputs, $(a_{dot},{\delta }_{dot})$, as

$$\begin{array}{cc}\hfill \dot{a}& =a_{dot}\hfill \\ \hfill \dot{\delta }& ={\delta }_{dot}.\hfill \end{array}$$

(3)

Therefore, the rate of change constraints are formulated as

$$\begin{array}{cc}\hfill -a_{do{t}_{max}}& \le a_{dot}\le a_{do{t}_{max}}\hfill \\ \hfill -{\delta }_{do{t}_{max}}& \le {\delta }_{dot}\le {\delta }_{do{t}_{max}}.\hfill \end{array}$$

(4)

Based on that, a compact state-space model of the system is

$$\dot{\mathit{x}}\left(t\right)=\mathit{f}(\mathit{x},\mathit{u}),$$

(5)

where $\mathit{x}={\left[xy\psi Va\delta \right]}^T\in {\mathbb{R}}^n$, $n=6$ is the state vector, $\mathit{u}={\left[a_{dot}{\delta }_{dot}\right]}^T\in {\mathbb{R}}^m$, $m=2$, is the control input vector, and $\mathit{f}(·,·)$ is a continuously differentiable function.

2.2. The Path-Following Problem

In the domain of vehicle steering control, there are two common techniques: Trajectory tracking and path following. In trajectory tracking problems, time and space reference are provided to the controller and the vehicle may accelerate/decelerate in order to compensate for the tracking error. Considering our vehicle model (5), the objective of the trajectory tracking is to design a controller that steers the system states $\mathit{x}\left(t\right)$ to follow a time-dependent reference trajectory ${\mathit{x}}^{ref}\left(t\right)$ and therefore the tracking error is $\mathit{e}\left(t\right)=\mathit{x}\left(t\right)-\mathit{x}{\left(t\right)}^{ref}\left(t\right)$ [17].

The path following problem is handled differently as the controller objective is to steer the vehicle to be geometrically on a certain path while maintaining a reference speed, and without adding strict requirements when to be where on this path [18]. Consider a path curve $\mathcal{P}$

$$\mathcal{P}=\{\mathit{y}\in {\mathbb{R}}^{n_p}|\theta \in [{\theta }_0,{\theta }_1]\mapsto \mathit{y}=\mathit{p}\left(\theta \right)\},$$

(6)

which is parameterized by the function $\mathit{p}\left(\theta \right)$, where $\theta$ is the time-dependent path parameter whose time evolution is not specified. The objective of the controller is to choose the system input $\mathit{u}$ and the timing $\theta \left(t\right)$ such that the path is followed as exactly as possible.

Given the vehicle state space Model (5), which is assumed to have an output function $\mathit{h}\left(\mathit{x}\left(t\right)\right)\in {\mathbb{R}}^{n_p},n_p\le n$, and the Path (6), the path following objective could be represented as:

• Path Convergence: The system output $\mathit{y}=\mathit{h}\left(\mathit{x}\right)$ converges to the set $\mathcal{P}$, i.e.,: $\underset{t\to \infty }{lim}\Vert \mathit{h}(\mathit{x})-\mathit{p}(\theta )\Vert =0$
• Monotonous Forward Motion: The system moves along $\mathcal{P}$ in the direction of increasing values of $\theta$, i.e., $\dot{\theta }\ge 0$ and ${\theta }_0\le \theta \le {\theta }_1$.

2.3. Map Representation Using Bézier  Curves

Two of the necessary requirements of any autonomous driving controller is the ability to handle the road boundaries generated from digital maps and have a path representation that could be followed by the motion controller.

In [19], an automatic procedure for generating driving boundaries from OpenStreetMap is presented, which expands its original representation, replacing polylines by polynomial-based roads, whose sections are defined using Bézier curves. This method has been validated successfully on a peri-urban environment without any human intervention. Therefore, we assume that the road is represented by polynomials whose sections are defined using Bézier curves. The road corridor is represented by two Bézier curves, one for the left boundary and the other for the right one. The path that should be follow by the vehicle, which is assumed to be the at the center of the lane is represented also by a Bézier curve.

Bézier curves [20] have recently attracted many researchers for digital map representation for autonomous driving [19]. They are defined as

$$\mathcal{B}(\theta ,n)=\sum _{i=0}^n{b}_{i,n}\left(\theta \right){\mathit{P}}_i,0\le \theta \le 1,$$

(7)

where n is the order of the polynomial, ${\mathit{P}}_n\in {\mathbb{R}}^2$ are the control points defining the curvature of the curve (see Figure 2), and $b_{i,n}$ the Bernstein coefficients given by

$$b_{i,n}\left(\theta \right)={{}^n{}^{}C}_i{\theta }^i{(1-\theta )}^{n-i},i=0,\cdots ,n,$$

(8)

with ${{}^n{}^{}C}_i=\frac{n!}{i!(n-i)!}$.

Depending on the Bézier polynomial degree, it would be possible to define any arbitrary road as one Bézier curve. However, higher degrees of the polynomial leads to high computation time and difficulties in calculating the control points. Moreover, for the purpose of autonomous driving, defining the whole road in just one huge polynomial is not required and makes the control problem intractable. Therefore, the common approach in computer graphics is to define complex paths by concatenating quadratic and cubic curves, taking into consideration the continuity constraints [19]. The overall control problem is to utilize the kinematic bicycle Model (5) to design a controller that follows the planned reference path defined by a Bézier Curve (7) and avoid collision with nearby objects, that are assumed to be connected with our own vehicle, by doing necessary maneuvers. The controller should be able to respect the road boundaries or corridor which are also assumed to be represented by a left and right Bézier  curves.

3. Controller Synthesis

3.1. Nonlinear Predictive Path-Following Control

Nonlinear Model Predictive Control (NMPC), also known as Receding Horizon Control and Moving Horizon Optimal Control, is a famous control technique for vehicle control systems that has roots in optimal control. It has been widely adopted in industry as an effective way to deal with multi-variable constrained control problems [21]. The basic concept of MPC is to optimize the forecast of the system, generated by a nominal dynamic model, and optimize it to produce the best decision, i.e., the control signal at the current time slot, while planning the nominal future control actions. This is achieved by employing the prediction of the system model to formulate and solve a dynamic optimization problem online where tracking errors, namely the difference between the predicted output and the desired reference, is minimized over a future horizon, possibly subject to constraints on the control inputs (the manipulated variables) and the states. These constraints could be nonlinear and time-varying over the prediction horizon.

This optimization problem is solved in discrete control intervals suitable for the system dynamics. At each step, the MPC algorithm computes an open-loop sequence of control input adjustments. The first input in the optimal sequence is injected into the system, and the entire optimization is repeated for the subsequent control intervals.

Consider a nonlinear state space Model (5) with $\mathit{x}\in {\mathbb{R}}^n$ and $\mathit{u}\in {\mathbb{R}}^m$ subject to generic state and control constraints

$$\begin{array}{cc}\hfill \mathit{x}\left(t\right)& \in \mathcal{X}\subset {\mathbb{R}}^n\hfill \\ \hfill \mathit{u}\left(t\right)& \in \mathcal{U}\subset {\mathbb{R}}^m,\hfill \end{array}$$

(9)

where $\mathcal{X}$ and $\mathcal{U}$ define the admissible state and control sets, respectively. These admissible sets will be designed later to achieve comfort driving, respect physical constraints of the vehicle, and perform collision avoidance.

In order to include the Path (6) into the control problem, we treat its parameter $\theta$ as an extra virtual state whose time evolution $t\mapsto \theta \left(t\right)$ is controlled by an extra virtual input [18]. Therefore, an additional differential equation

$$\dot{z}=u_z,$$

(10)

is added to the system where $z=\theta$ is the extra state that represents the path time evolution, and $u_z$ is the virtual control input that adds an extra degree of freedom in the controller design.

The optimal control problem (OCP) of the NMPC used for path following will be

$$\begin{array}{cc}\hfill \underset{\mathit{x}\left(t\right),\mathit{u}\left(t\right),z\left(t\right),u_z\left(t\right)}{\min }J(\mathit{x},\mathit{u},z,u_z)=& {\int }_{t=t_k}^{T_p+t_k}\ell (\mathit{x}\left(t\right),\mathit{u}\left(t\right),z\left(t\right),u_z\left(t\right))dt+F\left(\mathit{x}(t_k+T_p)\right)\hfill \\ & \mathrm{subject}\mathrm{to}:\hfill \\ & \dot{\mathit{x}}\left(t\right)=f(\mathit{x},\mathit{u})\hfill \\ & \dot{z}\left(t\right)=u_z\left(t\right)\hfill \\ & \mathit{x}\left(t\right)\in \mathcal{X},\hfill \\ & \mathit{u}\left(t\right)\in \mathcal{U},\hfill \\ & z\left(t\right)\in \mathcal{Z},\hfill \\ & u_z\left(t\right)\in {\mathcal{U}}_{‡},\hfill \end{array}$$

(11)

where $\ell (·,·,·,·)$ is the stage cost given by a positive definite function, $T_p$ is the prediction horizon, $\mathcal{Z}$ is the admissible set for the path parameter, and ${\mathcal{U}}_{‡}$ is the admissible set for the path timing law. The stage cost function for the path following problem of the autonomous vehicle could be defined as

$$\ell (\mathit{x},\mathit{u},z,u_z)={\Vert \mathit{h}\left(\mathit{x}\right)-\mathit{p}\left(z\right)\Vert }_{Q_1}+{\Vert V-V^{ref}\Vert }_{Q_2}+{\Vert \mathit{u}\Vert }_{R_1}+{\Vert u_z\Vert }_{R_2}.$$

(12)

Here, $Q=\mathrm{diag}(Q1,Q2)$ is positive semi-definite matrix, $R=\mathrm{diag}(R1,R2)$ is strictly positive-definite, $\mathit{h}\left(\mathit{x}\right)$ is the output function for the vehicle state space Model (5) and is defined as: $\mathit{h}\left(\mathit{x}\right)={\left[xy\psi \right]}^T$, and 

$$\mathit{p}\left(z\right)=\left[\begin{array}{c}\mathcal{B}(z,2)\\ {\psi }^{ref}\end{array}\right]\in {\mathbb{R}}^3,$$

is the reference path which is defined using quadratic Bézier curves.

3.2. Collision Avoidance

In order to account for collision avoidance of nearby vehicles, an NMPC is employed, due to its ability to handle time-varying nonlinear constraints by adding coupling constraints to the vehicle optimization problem with its neighbors to guarantee that the solution of the OCP provides a collision-free trajectory. The vehicles are assumed to be connected and they are exchanging information about their future predictions.

Consider $N_o$ encountered vehicles indexed by a superscript $\left[i\right]$, for instance, the state vector of each neighbor is denoted by ${\mathit{x}}^{\left[i\right]}$. Each of them is represented geometrically by an elliptic disk (see Figure 3) which is parameterized by the center position $(x,y)$, orientation (heading angle) $\psi$, semi-major radius $r_1$, and semi-minor radius $r_2$. The elliptic disk is the closed type of conic section results from the intersection of a cone by a plane, and is expressed as

$$\overline{𝒜}\equiv \left\{X\right|X^T𝒜X\le 0\},$$

(13)

where $X={\left[xy1\right]}^T$ is the 3-D column vector containing the homogeneous coordinates,

$$𝒜=\left[\begin{array}{ccc}A& -B& B{y}_p-A{x}_p\\ -B& C& B{x}_p-C{y}_p\\ B{y}_p-A{x}_p& B{x}_p-C{y}_p& A{x}_p^2-2B{x}_p{y}_p+C{y}_p^2-1\end{array}\right],$$

$A=(\frac{cos{\left(\psi \right)}^2}{{r_1}^2}+\frac{sin{\left(\psi \right)}^2}{{r_2}^2})$, $B=\frac{sin\left(2\psi \right)}{2}(\frac{1}{{r_1}^2}-\frac{1}{{r_2}^2})$, and $C=(\frac{sin{\left(\psi \right)}^2}{{r_1}^2}+\frac{cos{\left(\psi \right)}^2}{{r_2}^2})$.

Based on that, the collision avoidance is formulated in terms of coupling state constraints that represent the separation condition between the moving elliptic disk of the vehicle $\overline{\mathcal{A}}$ and the neighbor vehicle ${\overline{\mathcal{A}}}^{\left[i\right]}$. In the following, we recall a proposition that formulates the collision avoidance constraints.

Proposition 1.

Consider a pair of moving elliptic disks$\overline{\{\mathcal{A}}\left(t\right),{\overline{\mathcal{B}}}^{\left[i\right]}\left(t\right)\}$in the Euclidean plane${\mathit{E}}^2$represented by the matrices $\mathcal{A}$ and${\mathcal{B}}^{\left[i\right]}\left(t\right)$, respectively. Let${\mathcal{P}}^{\left[i\right]}(\lambda ,t)=C_3^{\left[i\right]}\left(t\right){\lambda }^3+C_2^{\left[i\right]}\left(t\right){\lambda }^2+C_1^{\left[i\right]}\left(t\right)\lambda +C_0^{\left[i\right]}\left(t\right)$be their characteristic polynomial and${\mathsf{\Delta }}^{\left[i\right]}\left(t\right)$denote the discriminant of${\mathcal{P}}^{\left[i\right]}(\lambda ,t)$with respect to λ. Suppose that every pair$\{\overline{\mathcal{A}}\left(T_0\right),{\overline{\mathcal{B}}}^{\left[i\right]}\left(T_0\right)\}$is separate, then the motions of them are collision-free for all$t\in [t_k,t_k+T_p]$if

• $C_2^{\left[i\right]}\left(t\right)<0$
• ${\mathsf{\Delta }}^{\left[i\right]}\left(t\right)>0$, $\forall t\in [t_k,t_k+T_p].$

Proof. 

See [17]. ☐

3.3. Road Corridor Respect

In this subsection, we develop the sufficient constraints that could be added to the optimization Problem (11) to guarantee our vehicle respect the driving corridors, generated from digital maps, in all the maneuvering. As for the reference path, it is assumed that we get two Bézier curves from the map; one for the left boundary and the other for the right one. Explicit representations of Bézier Curves (7) are not suitable for formulating the nonlinear constraints, and therefore we want to turn (7) into an implicit curve equation, that is one where we have no dependence on the parameter $\theta$, i.e., $F(x,y)=0$. This can be done using the resultant (or the Bézout’s determinant). The advantage is that $F(x,y)>0$ holds for points on one side of the Bézier curve, and $F(x,y)<0$ for those on the other side. As a result, we get the following representation for quadratic curves [20]:

$$F(x,y)={((a_0-x)b_2-a_2(b_0-y))}^2-((a_0-x)b_1-a_1(b_0-y))(a_1{b}_2-a_2{b}_1)$$

where $x\left(\theta \right)=a_2{\theta }^2+a_1\theta +a_0$, and $y\left(\theta \right)=b_2{\theta }^2+b_1\theta +b_0$. The right and left boundary constraints are $F_R(x,y)>0$ and $F_L(x,y)<0$, respectively.

3.4. Optimization Problem Implementation

In order to utilize the aforementioned infinite dimensional OCP of the NMPC, a proper formulation of the optimization problem implementation is required. There are two possible categories when it comes to dynamic optimization problems; indirect and direct methods. Indirect methods, such as calculus of variations and dynamic programming, consider the continuous NMPC as it is and does not require discretization of the problem. They provide exact analytic solution of the optimal control problem [22], but they are not common to be used for nonlinear optimal control problems due to their inability to handle a wide class of systems because they require a guess of the optimal control structure [22].

Direct methods, such as collocation [23] and multiple shooting [24], are the most desirable technique for formulating and parameterizing NMPC optimization problem. The main idea behind them is introducing the discretization and finite parameterization into the optimal control problem formulation which results in a Nonlinear Programming Problem (NLP), and then solve it directly with numerical methods. In the multiple shooting technique, the one used in this paper, the nonlinear continuous ordinary differential equation (ODE) and the OCP are discretized at a proper sampling interval $T_s=\frac{T_p}{N},N=2,3,\cdots$ [24].

Consider a general OCP of the form

$$\begin{array}{cc}\hfill \underset{\overline{\mathit{x}}\left(·\right),\overline{\mathit{u}}\left(·\right)}{\min }=& {\int }_{t=t_k}^{T_p+t_k}\ell (\overline{\mathit{x}}\left(t\right),\overline{\mathit{u}}\left(t\right))dt\hfill \\ & \mathrm{subject}\mathrm{to}:\hfill \\ \hfill \overline{\mathit{x}}\left(t_k\right)-{\overline{\mathit{x}}}_k& =0\hfill \\ \hfill \dot{\overline{\mathit{x}}}\left(t\right)-f(\overline{\mathit{x}}\left(t\right),\overline{\mathit{u}}\left(t\right))& =0,t\in [t_k,t_k+T_p],\hfill \\ \hfill r(\overline{\mathit{x}},\overline{\mathit{u}})& =0,\hfill \\ \hfill h(\overline{\mathit{x}},\overline{\mathit{u}})& \ge 0,t\in [t_k,t_k+T_p],\hfill \end{array}$$

(14)

where $\dot{\overline{\mathit{x}}}\left(t\right)=f(\overline{\mathit{x}}\left(t\right),\overline{\mathit{u}}\left(t\right))=0$ is the ODE model of the system, $\overline{\mathit{x}}\left(t_k\right)$ is the initial condition, and $h(\overline{\mathit{x}},\overline{\mathit{u}})$ is a vector function which represents the optimization state and control constraints. The control input $\overline{\mathit{u}}\left(t\right)$ is discretized at the time instants

$$T_d=\{t_{k+1},t_{k+2},\cdots ,t_{k+N}=t_k+T_p\},$$

such that

$$\overline{\mathit{u}}\left(t\right)={\mathit{q}}_i\forall t\in [t_i,t_{i+1}].$$

The nonlinear ODE, which can not be solved analytically, must be solved numerically on each subinterval $[t_i,t_{i+1}]$ using a suitable integration technique, e.g., fourth-order Runge–Kutta, and starting with an artificial initial value ${\mathit{s}}_i$. The trajectory pieces $\overline{\mathit{x}}(t;{\mathit{s}}_i,{\mathit{q}}_i)$ are obtained by solving the initial value problem (IVP)

$$\begin{array}{cc}\hfill \dot{\overline{\mathit{x}}}(t;{\mathit{s}}_i,{\mathit{q}}_i)& =f({\overline{\mathit{x}}}_i(t;{\mathit{s}}_i,{\mathit{q}}_i),{\mathit{q}}_i),t\in [t_k,t_k+T_p],\hfill \\ \hfill \overline{\mathit{x}}(t;{\mathit{s}}_i,{\mathit{q}}_i)& ={\mathit{s}}_i,\hfill \end{array}$$

(15)

as function of $\mathit{s}$ and $\mathit{q}$, and also and compute numerically the integral

$$l_i({\mathit{s}}_i,{\mathit{q}}_i):={\int }_{t=t_i}^{t_{i+1}}\ell ({\overline{\mathit{x}}}_i(t_i;{\mathit{s}}_i,{\mathit{q}}_i),{\mathit{q}}_i)dt.$$

Based on that, the NLP formulation of Problem (14) is represented as

$$\begin{array}{cc}\hfill \underset{\mathit{s},\mathit{q}}{\min }=& \sum _{i=k}^{k+N}l({\mathit{s}}_i,{\mathit{q}}_i)\hfill \\ & \mathrm{subject}\mathrm{to}:\hfill \end{array}$$

(16a)

$$\begin{array}{cc}\hfill {\mathit{s}}_i-{\overline{\mathit{x}}}_i& =0\hfill \end{array}$$

(16b)

$$\begin{array}{cc}\hfill {\mathit{s}}_{i+1}-{\overline{\mathit{x}}}_i(t_{i+1};{\mathit{s}}_i,{\mathit{q}}_i)& =0,i=0,\cdots ,N-1,\hfill \end{array}$$

(16c)

$$\begin{array}{cc}\hfill r({\mathit{s}}_i,{\mathit{q}}_i)& =0,i=0,\cdots ,N,\hfill \end{array}$$

(16d)

$$\begin{array}{cc}\hfill h({\mathit{s}}_i,{\mathit{q}}_i)& \ge 0,i=0,\cdots ,N,\hfill \end{array}$$

(16e)

where (16b) represents the initial condition, Equation (16c) represents the continuity condition between each two consecutive subintervals, Equations (16d) and (16e) represent the discretized constraints, and ${\overline{\mathit{x}}}_i\left(t_{i+1}\right)\ne {\mathit{s}}_{i+1}$ is the solution of the IVP (15). The NLP (16) is solved using sequential quadratic programming (SQP), with the aid of the method of Lagrange multipliers where the Hessian matrix is approximated using Gauss–Newton [25]. Each quadratic problem (QP) is then solved using active-set method [26].

The open-source ACADO Code Generation toolkit [26] is used in the implementation of the NMPC algorithm. Based on a symbolic representation of optimal control problems, ACADO generates an optimized and self-contained C code for the SQP formulation with static memory for a good real-time performance. The C code can be integrated into MATLAB as MATLAB executable (mex) files.

The main steps of the implementation are briefly described as follows:

• The continuous state space model is symbolically defined using C-code or the MATLAB interface, then it is simplified employing automatic differentiation tools and using zero entries in the Jacobian matrix. The result is an efficient real time C-code for the integration of the continuous nonlinear system which will be used for the prediction.
• The optimization problem cost function and constraints are symbolically defined, parametrized by the aforementioned direct multiple shooting technique, and the resulting, large but sparse, Quadratic Problem (QP) is condensed.
• The discretized QP is then solved using qpOASES solver.

A representation of the usage of NMPC for path-following and collision avoidance is shown in Figure 4, and Algorithm 1 describes the NMPC usage as follows.

Algorithm 1 NMPC Algorithm for Path Following.

1: Set the time index $k=0$, the sampling interval $T_s$, the prediction horizon $T_p=N{T}_s$, weight matrices Q and R.

2: Measure the values of the states $\overline{\mathit{x}}\left(t_k\right)$ or estimate them.

3: Solve the optimization Problem (16) over the discrete time instants $T_d=[t_{k+1}\cdots t_{k+N}=T_p]$ and get the optimal control sequence $[\overline{\mathit{u}}\left(t_k\right)={\mathit{q}}_k\cdots \overline{\mathit{u}}(t_{k+N-1}={\mathit{q}}_{k+N-1})]$ and the corresponding predicted states $[\overline{\mathit{x}}\left(t_k\right)={\mathit{q}}_k\cdots \overline{\mathit{x}}(t_{k+N}={\mathit{q}}_{k+N})]$.

4: Apply only the first control element $\overline{\mathit{u}}\left(t_k\right)$.

5: wait for the next sample and set the time index $k=k+1$, then go to step 2.

Remark 1.

There are many state estimation techniques in the literature that could be used to get the state vector$\overline{\mathit{x}}\left(t_k\right)$. A survey for these techniques is given in [27].

4. Simulation Results

In this section, simulation results are presented to demonstrate the validity and assess the performance of the proposed NMPC scheme for path-following and collision avoidance of autonomous vehicles. Simulation is done in MATLAB 2017b using the static C-code generated from the ACADO toolkit [26]. These results have been obtained on a $2.8$ GHz core i5 CPU with 16 GB RAM. In the simulation scenario, there are two vehicles; the own vehicle and the encountered one. It is assumed that the encountered vehicle has the priority and therefore its collision avoidance algorithm was not triggered, and it sends its future prediction states to the own vehicle. In

Table 1. Vehicle and NMPC parameters.

$a_{max}=1\mathrm{m}/{\mathrm{s}}^2$	${\delta }_{max}=20deg$	$a_{do{t}_{max}}=0.4\mathrm{m}/{\mathrm{s}}^3$
${\delta }_{do{t}_{max}}=4deg/\mathrm{s}$	$N=80$	$T_s=0.1\mathrm{s}$
$T_p=8\mathrm{s}$	$m=1573\mathrm{k}\mathrm{g}$	$l_f=1.1\mathrm{m}$
$l_r=1.57\mathrm{m}$	$r_1=2.2\mathrm{m}$	$r_2=1.6\mathrm{m}$
$Q=\mathrm{diag}(1.0,10,0.2,0.002)$	$R=\mathrm{diag}(0.001,0.001,0.002)$

, the parameters of the vehicle Model (1), which are obtained from [28], and the NMPC parameters are given.

The vehicle starts from the initial position $(x,y),=(0\mathrm{m},0\mathrm{m})$ and the NMPC steers it to follow the reference quadratic Bézier path with the control points $P_0={[0,0]}^T$, $P_1={[100,50]}^T$, and $P_2={[300,0]}^T$ As shown in Figure 5, the NMPC steers the vehicle to follow the reference path, and when the slower vehicle impedes this path, the NMPC steers the vehicle to avoid collision with the encountered vehicle by overtaking it, while respecting the control input constraints and the road corridors. In Figure 6a, it is shown that the vehicle could achieve the desired speed except during the collision avoidance maneuvering. It is clear in Figure 6e,f that maneuvering of the vehicle does not have any abrupt change in the steering angle and therefore it is consistent with the steering system dynamics. In Figure 6c,d, it is shown that the maximum allowed acceleration is obeyed while avoiding any jerky actions due to the constraints on the acceleration rate of change. The distance between the centers of both vehicles at the closest point of approach is about $3.4\mathrm{m}$ which is quite smaller compared to selecting circular disks, as presented in [14], which gives a minimum distance of at least $4.4\mathrm{m}$. The maximum execution time of the presented scheme is about $50\mathrm{m}\mathrm{s}$ which qualifies it to be used for experimental evaluation, and the maximum number of iterations is 100.

5. Conclusions

An NMPC scheme is presented in this paper for the problem of path-following of autonomous vehicles with integrated collision avoidance based on ellipse geometry. A bicycle model is used with both acceleration and steering angle as control inputs. Employing a quadratic cost function that penalizes the deviation from the path, a dynamic optimization problem is formulated with linear inequality state and control constraints to achieve comfort driving. Collision avoidance is achieved by adding nonlinear time-varying state constraints based on the separation condition between elliptic disks. To guarantee staying within the road boundaries, implicit Bézier constraints are added to the OCP. The optimization problem is parametrized and solved using ACADO toolkit and is evaluated on MATLAB platform. The NMPC shows good performance for following of the predefined path while avoiding jerky behavior and collision with nearby vehicles.

Our future aim is to increase the safety levels of the presented scheme by considering robust NMPC techniques for uncertain nonlinear models. The upper bound of the path-following error is parametrized with the aid of an auxilary control law, and then included into the optimization problem as a decision variable to make it as small as possible. Moreover, a maximum permissible error could be added as a reference to the optimization problem.