# Autonomous Emergency Collision Avoidance and Stabilisation in Structured
Environments

Shayan Taherian, Shilp Dixit, Umberto Montanaro and Saber Fallah S. Taherian,
S. Dixit, U. Montanaro and S. Fallah are with the Department of Mechanical
Engineering at University of Surrey, Guildford GU2 7XH, UK e-mail:
(s.taherian,s.dixit,u.montanaro,s.fallah@surrey.ac.uk).Manuscript received
April 19, 2005; revised August 26, 2015.

###### Abstract

In this paper, a novel closed-loop control framework for autonomous obstacle
avoidance on a curve road is presented. The proposed framework provides two
main functionalities; (i) collision free trajectory planning using MPC and
(ii) a torque vectoring controller for lateral/yaw stability designed using
optimal control concepts. This paper analyzes trajectory planning algorithm
using nominal MPC, offset-free MPC and robust MPC, along with separate
implementation of torque-vectoring control. Simulation results confirm the
strengths of this hierarchical control algorithm which are: (i) free from non-
convex collision avoidance constraints, (ii) to guarantee the convexity while
driving on a curve road (iii) to guarantee feasibility of the trajectory when
the vehicle accelerate or decelerate while performing lateral maneuver, and
(iv) robust against low friction surface. Moreover, to assess the performance
of the control structure under emergency and dynamic environment, the
framework is tested under low friction surface and different curvature value.
The simulation results show that the proposed collision avoidance system can
significantly improve the safety of the vehicle during emergency driving
scenarios. In order to stipulate the effectiveness of the proposed collision
avoidance system, a high-fidelity IPG carmaker and Simulink co-simulation
environment is used to validate the results.

###### Index Terms:

Autonomous vehicle, trajectory planning, Nominal MPC, robust MPC, Offset free
MPC, obstacle avoidance, torque-vectoring.

##  I Introduction

Autonomous vehicles capable of providing safe and comfortable point-to-point
transportation have been an active area of research in academia and automotive
industry for the past three decades. In addition to performing normal driving
tasks, the capability of autonomous vehicles to perform emergency manoeuvres
is paramount for their acceptance as a safe and robust mode of end-to-end
transportation [1, 2]. On detecting the possibility of a collision, an
autonomous collision avoidance system needs to perform two main tasks; (i)
calculate a safe and feasible path to avoid collision and (ii) track the
calculated path accurately [3, 4] while ensuring the lateral-yaw stability [5,
6] of the vehicle is maintained.
Since, collision avoidance forms such a critical part of any autonomous
driving feature-set, many different implementations for such an autonomous
system have been proposed in literature. Heuristic searching algorithm such as
Diskstra algorithm, $A^{*}$ and $D^{*}$ algorithm have been proposed for
planning a safe trajectory [7, 8]. If a collision free path exists, these
search based algorithms can find it. However, a vehicle dynamics and kinematic
constraints are not considered while computing the path which can cause safety
and feasibility issues for medium and high-speed driving. Incremental search
based algorithms like "Rapidly exploring Random Trees"(RRT) have been
developed as a trajectory planner [9]. Although this algorithm includes
vehicle kinematic model, the planned trajectory can be jerky, resulting in
uncomfortable driving situation. Another method that is widely used among
researchers is Model Predictive Control (MPC). In this algorithm the path-
planning task is formulated as a finite horizon optimisation problem with
vehicle dynamics and collision-avoidance constraints modeled as constraints
for the optimisation. However, collision avoidance constraints usually take
the form of non-convex constraints. In order to tackle this problem,
researchers come across of techniques such as convexification [10], changing
the reference frame [11, 12] and affine linear collision avoidance constraints
[13].
A successful collision avoidance system does not end with trajectory planning
but extends to the capability of the system to maintain the lateral-yaw
stability of the vehicle while performing an evasive manoeuvre especially
under challenging scenarios such as curved road, wet/icy roads, etc. Torque
vectoring with its ability to generate corrective yaw moment to stabilise the
planar dynamics of a vehicle provides a proven technique to improve the
stability of a vehicle in limit-handling situations that might be encountered
while performing some of the extreme evasive manoeuvres. As a result, a large
body of literature is dedicated for the development of controller for torque-
vectoring, i.e., the control of traction and braking torque of each wheel to
generate a direct yaw moment. Torque-vectoring can increase the cornering
response, enhance the agility and guarantee stability in emergency and
transient manoeuvre [14, 15]. In this work, a torque vectoring controller is
designed to ensure that the vehicle maintains its stability while performing
limit handling evasive manoeuvres even under challenging conditions such as
curved roads and/or icy surface.

Figure 1: Control structure

In this paper a closed-loop architecture for collision avoidance and lateral-
yaw stabilisation is proposed. The main components of this architecture are
(i) reference generation block, (ii) a trajectory planning/control block, and
(iii) torque-vectoring control block, see Fig. 1. The reference generation
block uses the information from the road/map data to provide the reference for
the lane centre. The trajectory generation block based on MPC formulation
computes feasible trajectories to safely evade an obstacle in an emergency
situation. This reference generation and trajectory planning framework is an
extension of our previous work in [16] which was applicable only for straight
roads. In this paper, this framework has been enhanced to compute safety and
collision avoidance constraints for any generic curved road. Furthermore, in
adverse conditions (e.g., low friction, wind, etc.) the collision free
trajectories generated by the trajectory planner might result in the vehicle
operating on the limit of stability. Consequently, to ensure lateral/yaw
stability for the subject vehicle while performing the collision avoidance a
torque-vectoring controller is used. This controller ensures that the vehicle
maintains lateral-yaw stability while performing collision avoidance
manoeuvre.
The modular structure of the architecture allows to host different
implementations of MPC with the same collision avoidance constraints. In this
paper, three different MPC techniques (nominal MPC, offset free MPC, and
robust MPC) for trajectory planning are tested. The control architeture in
Fig. 1 is tested in a Matlab/IPG-CarMaker co-simulation environment to gain
insight on its ability to perform emergency evasive manoeuvres on curved
roads. The contribution of this paper are summarised as:

  * •

A mathematical framework to design convex constraints for collision avoidance
when travelling on any generic curved road.

  * •

A hierarchical motion planning, control and lateral-yaw stability architecture
for autonomous collision avoidance in medium and high-speed driving
conditions.

  * •

Comparison of three different MPC techniques for trajectory planning for
collision avoidance and their impact on the overall performance of the
proposed hierarchical control framework, through the definition of the key
performance indexes.

The paper is organized as follows: Section II introduces the system models
capturing the relevant vehicle and path dynamics for autonomous highway
driving that are used for the MPC controllers. Section III is dedicated to
designing collision avoidance constraints along with method of considering
convex optimization while driving on a curve. In Section IV three different
MPC approaches for trajectory planning are discussed. In Section V, torque
vectoring algorithm is introduced. The effectiveness of the framework to
support collision avoidance system is numerically demonstrated in Section VI.
Finally, conclusions are presented in Section VII.

Figure 2: Kinematic bicycle model

##  II System Model

This section provides an overview of the system models that have been used in
this paper to describe the vehicle dynamics along with the physical and design
constraints. In this paper, two system models are designed and used, (i) the
single track kinematic vehicle model, which is a common model for trajecory
planning algorithm [17] in Section II-A, and (ii) a system model that
incorporates curvature into dynamics of the system to enhance the tracking
performance of the controller [18] in Section II-C.

###  II-A Single track vehicle model

The single track kinematic model (also known as bicycle model) of a vehicle is
illustrated in Fig. 2. This model captures the planar dynamics of a vehicle in
terms of kinematic constraints. Under small angles approximation the system
dynamics are expressed as:

$\displaystyle\dot{x}$ $\displaystyle=v_{x}$ (1a) $\displaystyle\dot{y}$
$\displaystyle=v_{x}\psi+\frac{l_{r}}{l_{f}+l_{r}}v_{x}\delta_{f}$ (1b)
$\displaystyle\dot{v}_{x}$ $\displaystyle=a_{x}$ (1c)
$\displaystyle\dot{\psi}$ $\displaystyle=\frac{v_{x}\delta_{f}}{l_{f}+l_{r}}$
(1d)

Where $x$ and $y$ are the longitudinal and lateral displacement of the vehicle
in inertial coordinates, $\psi$ is the inertial heading angle of the vehicle,
$v_{x}$ is the longitudinal velocity of the vehicle, $l_{f}$ is the distance
of the front axle from centre of gravity (C.G), and $l_{r}$ is the distance of
the rear axle from centre of gravity. The control inputs are steering angle
$\delta_{f}$ and longitudinal acceleration $a_{x}$. For a given nominal
velocity $v_{x,nom}$, the system in (1) can be expressed using a LTI system
using the compact notation given below.

$\dot{\xi}=A\xi+Bu$ (2)

where $\xi\triangleq[x,y,v_{x},\psi]^{T}\in\mathcal{X}\subseteq\mathbb{R}^{4}$
is the state vector and
$u\triangleq[\delta_{f},a_{x}]^{T}\in\mathcal{U}\subseteq\mathbb{R}^{2}$ is
the input vector with $\mathcal{X}$ and $\mathcal{U}$ polyhedron regions
depicting state and input constraints respectively. The structure of the
system matrix $A$ and input matrix $B$ are as follows:

$\begin{bmatrix}\dot{x}\\\ \dot{y}\\\ \dot{v}_{x}\\\
\dot{\psi}\end{bmatrix}=\underbrace{\begin{bmatrix}0&0&1&0\\\
0&0&0&v_{x,\textrm{nom}}\\\ 0&0&0&0\\\
0&0&0&0\end{bmatrix}}_{A}\begin{bmatrix}x\\\ y\\\ v_{x}\\\
\psi\end{bmatrix}+\underbrace{\begin{bmatrix}0&0\\\
\frac{v_{x,\textrm{nom}}l_{r}}{(l_{r}+l_{f})}&0\\\ 0&1\\\
\frac{v_{x,\textrm{nom}}}{(l_{r}+l_{f})}&0\end{bmatrix}}_{B}\begin{bmatrix}\delta\\\
a_{x}\\\ \end{bmatrix}\,$ (3)

Where $v_{x,nom}$ is the nominal longitudinal velocity of the vehicle. Then
system (2) is descretised with a sampling time $t_{s}$ to obtain linear time
invariant discrete system shown below:

$\xi(k+1)=A_{d}\xi(k)+B_{d}u(k)$ (4)

###  II-B Path dynamics

The classical kinematic model presented in (1) formulates the vehicle dynamics
in reference to an initial coordinate system. However, it is beneficial to
plan the future vehicle motion along the reference curve $\Omega$ in Fig. 2.
For this reason the classical kinematic model has been modified by introducing
new system states $e_{y}$ as the lateral position error and $e_{\psi}$ as the
heading angle error. In Fig. 2, $e_{y}$ is defined as lateral distance of the
centre of gravity of the vehicle from the desired path and $e_{\psi}$ is
defined as the difference between the actual orientation of the vehicle and
desired heading angle. These states can be mathematically expressed as [19]:

$\displaystyle e_{y}$
$\displaystyle=(y-y_{ref})\cos\psi_{ref}-(x-x_{ref})\sin\psi_{ref}$ (5a)
$\displaystyle e_{\psi}$ $\displaystyle=v_{x}\kappa-\psi$ (5b)

Where $\kappa$ referred as curvature of the path, $y_{ref}$ is the desired
lateral distance, $x_{ref}$ is the desired longitudinal distance and
$\psi_{ref}$ is the desired heading angle of the vehicle. Using system (4),
the path error dynamics can be expressed as the following discrete LTI system:

$\displaystyle\eta(k+1)$
$\displaystyle=A_{\eta}\eta(k)+B_{\eta}u(k)+B_{\eta,d}d(k)$ (6a)
$\displaystyle y(k)$ $\displaystyle=C_{\eta}\eta(k)+C_{\eta,d}d(k)$ (6b)

Where $d(k)$ is disturbance in the system. The states and control inputs of
this model denoted as
$\eta=[x,v_{x},y,\psi,e_{y},e_{\psi}]^{T}\in\widetilde{\mathcal{{X}}}\subseteq\mathbb{R}^{6}$
and
$u\triangleq[\delta_{f},a_{x}]^{T}\in\widetilde{\mathcal{{U}}}\subseteq\mathbb{R}^{2}$
respectively, with $\widetilde{\mathcal{X}}$ and $\widetilde{\mathcal{U}}$
being state and input convex constraint sets respectively.

###  II-C Augmented system

Driving on curved roads can reduce the tracking performance of the vehicle due
to the curvature of the path which is not captured in the dynamics of the
system (4). Hence, incorporating curvature as a disturbance into the dynamics
of the system, results in reduction of the tracking error while driving on a
curve road. To achieve this, the system model (6) is augmented with the
disturbance $d(k)$ to predict the mismatch between the measured and predicted
state. The augmented system becomes:

$\begin{gathered}\begin{bmatrix}\eta(k+1)\\\
d(k+1)\end{bmatrix}=\underbrace{\begin{bmatrix}A_{\eta}&B_{\eta,d}\\\
0&I\end{bmatrix}}_{\widetilde{A}}\underbrace{\begin{bmatrix}\eta(k)\\\
d(k)\end{bmatrix}}_{\Xi}+\underbrace{\begin{bmatrix}B_{\eta}\\\
0\end{bmatrix}}_{\widetilde{B}}u(k)\\\ \begin{bmatrix}y(k+1)\\\
d(k+1)\end{bmatrix}=\underbrace{\begin{bmatrix}C_{\eta}&0\\\
0&C_{\eta,d}\end{bmatrix}}_{\widetilde{C}}\begin{bmatrix}\eta(k)\\\
d(k)\end{bmatrix}\end{gathered}$ (7)

The system model (7) is an enhancement of the system (4). The purpose of this
transformation is to depict the effectiveness of adding the disturbance into
the kinematic vehicle model in collision avoidance manoeuvre.

##  III Cosntraint design

In this section, the mathematical framework for computing convex regions that
can be used to plan trajectories on curved roads of varying radii is
presented. In the absence of traffic vehicles, the convex region is designed
to ensure that the planned vehicle trajectories remain within the boundaries
of the road. On the other hand, in the presence of obstacles, the convex
region is designed to ensure that the planned trajectories remain within the
road boundaries and maintains a safe distance from the obstacle.

###  III-A Road Boundary Constraints

Using the edges of a curved road as constraints within the MPC framework
results in non-convex constraints which is not suitable for Quadratic
Programming (QP) framework [20]. Consequently, boundary constraints are
designed by assuming that the road segment in the immediate vicinity of the
subject vehicle is part of a straight road (Fig. 3). The edges of this virtual
straight road are obtained where at each time step the task of the MPC
controller is to ensure that the planned trajectories lie within the edges of
this virtual road. The point $P_{1}$ is located at the intersection of the
left (outer) road edge with an imaginary line passing through the subject
vehicle’s centre of gravity (CG) and perpendicular to the subject vehicle’s
longitudinal axis. The equations of outer lane boundary and intersected line
is:

$\begin{cases}y_{upper}=f_{upper}(x)\\\ ax+by+c=0\end{cases}$ (8)

The line passing through P1 with a slope $\psi_{ref}$ forms the left (outer)
edge of the virtual straight road with equation of:

$m_{upper}x-y+P_{1y}-m_{upper}P_{1x}<0$ (9)

Similarly, the line passing through $P_{2}$ with a slope $\psi_{ref}$ forms
the right (inner) edge of the virtual straight road. The inner edge of the
virtual straight road can be expressed as:

$\begin{cases}y_{lower}=f_{lower}(x)\\\ ax+by+c=0\end{cases}$ (10)

Consequently in the same way as the upper bound, the lower hyperplane can be
defined as:

$m_{lower}x-y+P_{4y}-m_{lower}P_{4x}>0$ (11)

To decouple the orientation of the virtual straight road with the subject
vehicle’s orientation, the edges are parameterised using $\psi_{ref}$. It is
noted that, this technique is suitable to be implemented for different range
of curvature of the road. Moreover, it can be ensured that the optimization
problem in MPC will always be feasible, and guarantee the convexity on the
curve road.

Figure 3: The hyperplanes (red lines) indicate the constraints for
optimization. The points on the borders are the projection of the center line
of the subject vehicle on the borders

###  III-B Collision Avoidance Constraints

As mentioned above the collision avoidance constraints are designed to ensure
the subject vehicle evades the obstacle vehicle without any collisions. In
literature it is common to design ellipsoids/rectangles around the obstacle to
obtain the collision avoidance constraints [21, 22]. However, these techniques
result in non-convex optimisation problem which is a challenging problem to
solve. This paper offers a method to obtain a convex collision avoidance
constraints in the form of two affine linear inequalities. The technique is
inspired from [20], where the collision avoidance constraints are used for
only straight driving conditions. In this paper, a general technique has been
developed using linear affine collision avoidance constraints that can be
implemented for both straight and curve driving conditions. The collision
avoidance constraints can be generated using two lines entitled as forward
collision avoidance constraints (FCC) and rear collision avoidance constraints
(RCC) as shown in Fig. 4. The purpose of generating the FCC is to prevent
front end collision while performing a collision free lane change and the
purpose of the RCC is to prevent rear end collision while returning to the
original lane. The activation of FCC is when the subject vehicle is behind the
lead vehicle while RCC is activated when the subject vehicle crosses the lead
vehicle. In this framework, three points ($P_{3},P_{4},P_{5}$) are required to
generate collision avoidance constraints. These points can be calculated by
intersecting longitudinal and lateral axis of the lead vehicle (grey and
yellow lines respectively) with a circle centred at the lead vehicle’s
geometrical center, providing the location of the points $P_{3}$, $P_{4}$, and
$P_{5}$. The following subsections explain the procedure for generating
collision avoidance constraints.

Figure 4: Schematic to construct forward and rear collision avoidance
constraints line. Note: $\psi_{L}$ is lead vehicle orientation, $L_{x}$ and
$W$ indicate the safety distance from the lead vehicle

####  III-B1 Forward Collision Avoidance Constraints

The virtual line on the road segment representing FCC is the line that passes
through the points $(P_{3},P_{4})$ in Fig. 5. Point $P_{3}(P_{3x},P_{3y})$ is
obtained as a results of the intersection of longitudinal axis of the lead
vehicle (grey line in Fig. 4):

$a_{Lon}x_{L}+b_{Lon}y_{L}+c_{Lon}=0$ (12)

with a circle centred at the the lead vehicle geometrical centre and radius of
$L_{x}+l_{rL}$:

$(x-x_{L})^{2}+(y-y_{L})^{2}=(L_{x}+l_{rL})^{2}$ (13)

Where $L_{x}$ is safety distance which can be defined as $L_{x}=v_{x}t+L_{c}$.
The longitudinal velocity of the subject vehicle expressed as $v_{x}$, $t$ is
the desired time gap of subject vehicle when approaching lead vehicle and
finally $L_{c}$ is the lead vehicle length. Moreover, Parameters
$l_{rL},a_{Lon},b_{Lon},c_{Lon}$ represent the distance of center of gravity
of the vehicle to rear wheels and coefficients of the intersected line
respectively. It is noted that at every time instant of MPC optimization,
$L_{x}$ updated according to the current value of the subject vehicle velocity
($v_{x}$). Therefore, depending on the subject vehicle speed, $L_{x}$ will
define the safety distance from the lead vehicle. For generating FCC, another
set of points is required $(P_{4x},P_{4y})$. These points are generated by
means of intersection of lateral axis of the lead vehicle (yellow line in Fig.
4) with a circle of radius $W$:

$(x-x_{L})^{2}+(y-y_{L})^{2}=W^{2}$ (14)

After defining two set of points, it would be trivial to find the equation of
the line for forward collision avoidance constraints, where the final equation
will be:

$\underbrace{m_{FCC}}_{a_{FCC}}x+(\underbrace{-1}_{b_{FCC}})y+\underbrace{P_{1y}-m_{FCC}P_{1x}}_{c_{FCC}}>0$
(15)

Where $m_{FCC}$ is the slope of the forward collision avoidance constraint.
The generated FCC divides the $(x,y)$ plane into two regions. Region 1:
$a_{FCC}x+b_{FCC}y+c_{FCC}>0$ which is the safe region, Region 2:
$a_{FCC}x+b_{FCC}y+c_{FCC}<0$ which represent the unsafe region. FCC forces
the vehicle to be in a safe region while performing a lane change on a
curve/straight road. Moreover, (15) represents a linear affine constraint
approximation which can be formulated in QP format.

####  III-B2 Rear Collision Avoidance Constraints

Similar to FCC, RCC can be generated with the same structure. The procedure
for calculating the intersection point $P_{3}$, is identical to FCC, thus
resulting in generating the equation of the RCC line (Fig. 6):

Figure 5: Schematic to calculate the FCC. Note: orange car-subject vehicle,
green car-lead vehicle, red line RCC, $L_{x}$ and $W$ indicate the safety
distance from the lead vehicle Figure 6: Schematic to calculate RCC. Note:
orange car is subject vehicle, green car is lead vehicle, $L_{x}$ and $W$
indicate the safety distance from the lead vehicle

$\underbrace{m_{RCC}}_{a_{RCC}}x+(\underbrace{-1}_{b_{RCC}})y+\underbrace{P_{3y}-m_{RCC}P_{3x}}_{c_{RCC}}<0$
(16)

It is noteworthy that the aforementioned collision avoidance constraints can
be adopted for the scenarios where $(i)$ the subject vehicle needs to change
the lane while performing the collision avoidance manoeuvre and $(ii)$ where
more traffic member on the road are presented which require multiple
hyperplanes.
Remarks

  * •

One of the benefits of the proposed approach lies into the fact that there is
only one parameter to be tuned (desired time gap) and the rest of the
parameters are based on the geometry of the lead vehicle.

  * •

The design of the collision avoidance constrains is based on basic
mathematical operation, therefore the aforementioned constraints for each
traffic vehicle can be generated without any major computation overhead, thus
it is suitable for real time implementation.

It is noted that, by using boundary constraints and the appropriate FCC/RCC
based constraints a convex region representing safe zones on a curved road can
be computed. When applied to an MPC formulation, these constraints guarantee
that the planned trajectories are collision free.

##  IV Trajectory Planning Controllers

In general, autonomous highway driving involves motion planning and control of
a subject vehicle in order to maintain the velocity and keeping the vehicle
away from lane boundaries while avoiding possible collisions. The choice of
manoeuvre to perform is the results of the decision making process with the
objective of following the desired trajectory $x_{ref},y_{ref}$ (Section IV-A)
while respecting physical and design limitations of the subject vehicle. In
this section, using the definition of the linear prediction model in Section
II and linear system constraints in Section III, a constrained linear
quadratic optimal control problem for three different MPC’s is formulated over
a prediction horizon $N$.

###  IV-A Reference Trajectory Generator

The reference trajectory generator provides longitudinal and lateral positions
($x_{ref}(s),y_{ref}(s)$) as well as orientation ($\psi_{ref}(s)$) of the road
for the vehicle to follow. The reference trajectories are formulated using
clothoids method which expressed by Fresnel integrals [23, 24]. The
trajectories are parametrized by curvature $\kappa$ as a function of distance
$s$ along the path as follows:

$\psi_{ref}(s)=\int_{0}^{s}\kappa(s)dx$ (17)
$x_{ref}(s)=\int_{0}^{s}\cos\psi_{ref}(x)dx$ (18)
$y_{ref}(s)=\int_{0}^{s}\sin\psi_{ref}(x)dx$ (19)

###  IV-B Trajectory Planning: Nominal MPC

For the trajectory planning using nominal linear MPC, System (3) is subjected
to the following state and input constraints:

$\xi\in\mathcal{X},u\in\mathcal{U}$ (20)

where
$\mathcal{X}=\\{\xi\in\mathbb{R}^{4}:\xi_{min}$$\leq$$\xi$$\leq$$\xi_{max}\\}\subset\mathbb{R}^{4}$
and
$\mathcal{U}=\\{u\in\mathbb{R}^{2}:u_{min}$$\leq$$u$$\leq$$u_{max}\\}\subset\mathbb{R}^{2}$
are states and inputs polytope admissible regions (subscripts $min$ and $max$
are the minimum and maximum of the corresponding values). The following cost
function is formulated:

$\begin{gathered}\mathcal{J}=\sum_{k=0}^{N-1}[\parallel\xi_{ref}-\xi_{t+k\mid
t}\parallel_{Q}^{2}+\parallel u_{t+k\mid t}\parallel_{R}^{2}]\\\
+\parallel\xi_{ref}-\xi_{t+N\mid t}\parallel_{P}^{2}\end{gathered}$ (21)

where $\xi_{t+k|t}$ is the predicted state trajectory at time $t+k$ obtained
by applying the control sequence
${U}_{t}=[{u_{t}^{T},\dots,u_{t+N-1}^{T}}]^{T}$ to the system (4), starting
from initial state of $\xi_{t|t}$. The parameter $N\in\mathbb{N}^{+}$is the
prediction horizon while $Q\in\mathbb{R}^{4\times 4}$,
$P\in\mathbb{R}^{4\times 4}$ and $R\in\mathbb{R}^{2\times 2}$ are weighting
matrices. The performance index in (21) consists of the stage cost, input cost
and the terminal cost, respectively. The desired state $\xi_{ref}$
representing the reference state for the subject vehicle and is defined as
$\xi_{ref}=[x_{ref},y_{ref},v_{xref},\psi_{ref}]^{T}$. It is noted that in
this paper $v_{xref}$ is taken as the initial value of the subject vehicle’s
velocity. The following constrained optimization problem, for each sampling
time, is formulated as:

$\displaystyle\underset{U_{t}}{\text{min}}\mathcal{J}(u_{t};\xi(t),\xi_{ref})$
(22a) subject to $\displaystyle\eqref{Dis:model},\eqref{equation
upper},\eqref{equation lower},\eqref{equation FCC},\eqref{equation
RCC},\eqref{sys:constr}$ (22b)

In this framework, at every time step, the problem (22a), under the
constraints (22b) are calculated based on the current state $\xi(t)$, over a
shifted time horizon. The control input is calculated as
$u(x(t))=U_{t}^{*}(0)$, which is a solution to the problem (22a). As the sets
$\mathcal{X}$ and $\mathcal{U}$ are convex, then the MPC problem (22a) is
solved as a standard QP [20] optimisation problem.

###  IV-C Trajectory Planning: Offset-free MPC

For trajectory planning using offset-free MPC system model (7) is used. This
system is subjected to the following state and input constraints:

$\eta\in\widetilde{\mathcal{X}},u\in\widetilde{\mathcal{U}}$ (23)

where
$\widetilde{\mathcal{X}}=\\{\eta\in\mathbb{R}^{6}:\eta_{min}$$\leq$$\eta$$\leq$$\eta_{max}\\}\subset\mathbb{R}^{6}$
and
$\widetilde{\mathcal{U}}=\\{u\in\mathbb{R}^{2}:u_{min}$$\leq$$u$$\leq$$u_{max}\\}\subset\mathbb{R}^{2}$
are states and inputs polytope. According to this model the following cost
function can be written:

$\begin{gathered}\mathcal{J}=\sum_{k=0}^{N-1}[\parallel\Xi_{ref}-\Xi_{t+k\mid
t}\parallel_{Q_{\Xi}}^{2}+\parallel u_{t+k\mid t}\parallel_{R}^{2}]\\\
+\parallel\Xi_{ref}-\Xi_{t+N\mid t}\parallel_{P_{\Xi}}^{2}\end{gathered}$ (24)

At each sampling time, the following constrained finite-time control problem
is solved:

$\displaystyle\underset{U_{t}}{\text{min}}\mathcal{J}(u_{t};\Xi(t),\Xi_{ref})$
(25a) subject to $\displaystyle\eqref{eq:Offset_MPC},\eqref{equation
upper},\eqref{equation lower},\eqref{equation FCC},\eqref{equation
RCC},\eqref{off:const}$ (25b)

Remarks

  * •

$\eta$ and $d$ are parameters while $u$ is the decision variable of the
optimisation (25a).

  * •

The terms of the cost function under the summation in (24), penalize the state
for deviation of reference state as well as penalty on control input, and the
last term is penalty on terminal state.

###  IV-D Trajectory Planning: Robust MPC

This section provides an outline of the robust tube based MPC approach
provided in [25]. Robust MPC concerns control of the systems that are
uncertain, such that the predicted behaviour based on the nominal system is
not identical to the actual one. The dynamics of the lateral and yaw motion of
the vehicle with its longitudinal velocity have a non-linear relationship.
This non-linear relation between the states can be expressed as linear
additive disturbance as in [26]. The uncertainty in this formulation is
considered as parametric uncertainty in linear constraint system. System
dynamics in (3) are rewritten as a linear time invariant system subject to an
additive disturbance and can be recast as a linear parameter varying (LPV)
system

$\dot{\xi}_{p}=A_{p}(v)\xi_{p}+B_{p}(v)u$ (26)

System (26) can be discretised to generate a LPV discrete system:

${\xi}_{p}(k+1)=A_{d}(v)\xi_{p}(k)+B_{d}(v)u(k)$ (27)

In which the parameter $p=(A_{d}(v),B_{d}(v))$ can take any value in the
convex set $\mathcal{P}$ defined by

$\mathcal{P}=co\\{(A_{d,j}(v_{j}),B_{d,j}(v_{j}))|j\in\mathscr{J}\\}$ (28)

Where $\mathscr{J}=\\{1,2,...,J\\}$. The system is subject to the bounded
convex sets:

$\xi\in\mathcal{X}\subset{R^{4}},u\in\mathcal{U}\subset{R^{2}}$ (29)

With $\mathcal{X}$ and $\mathcal{U}$ are assumed to be compact and polytopic
and contains the origin in their interior. According to the dynamics of LPV
system (27), the nominal system:

$\bar{\xi}(k+1)=\bar{A}\bar{\xi}(k)+\bar{B}\bar{u}(k)$ (30)

in which the pair ($\bar{A},\bar{B}$) is obtained by the terms given below
[25]:

$\bar{A}=(\frac{1}{J})\sum_{j=1}^{J}A_{dj}(v_{j}),\bar{B}=(\frac{1}{J})\sum_{j=1}^{J}B_{dj}(v_{j})$
(31)

The system difference equation (27) can be expressed as:

$\xi(k+1)=\bar{A}\xi(k)+\bar{B}u(k)+w(k)$ (32)

Where the disturbance $w$ is defined as

$w=(A(v)-\bar{A})\xi+(B(v)-\bar{B})u$ (33)

thus the disturbance $w$ is bounded by the set $\mathcal{W}$ defined as:

$\begin{gathered}\mathcal{W}=\\{(A_{d}(v)-\bar{A})\xi(k)+(B_{d}(k)-\bar{B})u(k)|\\\
(A_{d}(v),B_{d}(v))\in\mathcal{P},(\xi,u)\in\mathcal{X}\times\mathcal{U}\\}\end{gathered}$
(34)

The set constraints for the nominal model are selected such that if the
closed-loop solution for the nominal system satisfies
$(\bar{\xi}(k),\bar{u}(k))\in\bar{\mathcal{X}}\times\bar{\mathcal{U}},\forall{k}$,
then $(\xi(k),u(k))\in\mathcal{X}\times\mathcal{U}$ ,where $\bar{\mathcal{X}}$
and $\bar{\mathcal{U}}$ are tightened states and inputs constraints used for
robust MPC. Essentially the idea is to steer the uncertain system (32) towards
a given reference state, using MPC approach with modified state and input
constraints ($\bar{\mathcal{X}},\bar{\mathcal{U}}$). The tightened constraints
for the nominal model can be expressed as:

$\bar{\mathcal{X}}=\mathcal{X}\ominus\mathcal{Z},\,\bar{\mathcal{U}}=\mathcal{U}\ominus
K\mathcal{Z},$ (35)

Where $K\in{\mathbb{R}^{2\times 4}}$ such that $A_{k}=(\bar{A}+\bar{B}K)$ is
hurwitz, and $\mathcal{Z}$ is robust positively invariant set [27] for the
system $e(k+1)=A_{k}e(k)+w(k)$ with $e=(\xi-\bar{\xi})$, such that

$A_{k}\mathcal{Z}\oplus\mathcal{W}\subseteq\mathcal{Z}$ (36)

In [25] it is proven that if $\bar{\mathcal{X}}$ and $\bar{\mathcal{U}}$ are
non-empty sets, they contain the set-points and control inputs that can be
robustly imposed to the system (32) when $e(0)=\xi(0)-z(0)$, under the control
action:

$u=\bar{u}+Ke,\,\bar{u}\in\bar{\mathcal{U}}$ (37)

These definitions above can be used to formulate the optimisation framework
for a tube-based MPC problem [28]. The cost function can be written as :

$\begin{gathered}\mathcal{J}=\sum_{k=0}^{N-1}[\parallel\xi_{ref}-\bar{\xi}_{t+k\mid
t}\parallel_{Q}^{2}+\parallel\bar{u}_{t+k\mid t}\parallel_{R}^{2}]\\\
+\parallel\xi_{ref}-\bar{\xi}_{t+N\mid t}\parallel_{P}^{2}\end{gathered}$ (38)

Where $\bar{\xi}_{t+k\mid t}$ denotes the predicted state at time $t+k$
calculated by applying control sequence
$\bar{U}_{t}=\\{\bar{u}_{t,t}...,\bar{u}_{t+k,t}\\}$ to the system (27) with
$\bar{\xi}_{t,t}=\bar{\xi}(t)$. The matrices $Q$,$R$ and $P$ are weights of
appropriate dimension penalising the state tracking error, control action and
terminal state. The optimisation problem is defined as:

$\displaystyle\begin{gathered}\underset{\bar{U}_{t}}{\text{min}}\mathcal{J}(\bar{u}_{t};\bar{\xi}(t),\xi_{ref})\end{gathered}$
(39b)

subject to

$\displaystyle\eqref{equation lower},\eqref{equation FCC},\eqref{equation
RCC},\eqref{RMPC-model_nom},\eqref{const:Robsut}$ (39c)

Figure 7: Closed-loop framework for collision avoidance system

In QP framework, at each time instant $k$ the optimization problems (22a),
(25a) and (39b) are solved subject to the physical and design constraints of
the vehicle as well as collision avoidance constraints (15),(16),(9) and (11).
This will lead to obtain the optimal trajectory
$\xi^{*}=[x^{*},y^{*},v_{x}^{*},\psi^{*}]$ by simulating the vehicle model
Fig. 2 with optimal inputs $u^{*}=[\delta^{*},a_{x}^{*}]$ from the solution of
the MPC problems. The optimal trajectories are then passed to the torque
vectoring controller (lower-level control) presented in next session as
reference signals. The closed-loop structure shown in Fig. 7 summarizes all
the steps to perform a collision avoidance in this paper.

##  V Torque vectoring

A torque-vectoring controller was developed to maintain lateral/yaw stability
of the vehicle even in limit-handling situations. A proportional controller is
used to calculate the braking/traction torque required to eliminate the error
between optimal velocity from the MPC and actual velocity of the vehicle [29].
The following equation represents the required torque which is equally
distributed among the wheels:

$T_{ff}=K_{p}(v_{x}^{*}-v_{x})$ (40)

where $K_{p}$ is the proportional gain. The generated torque $T_{ff}$ and
steering $\delta_{f}^{*}$, will be fed to the Command Interpreted Modeule
(CIM) block to develop the desired C.G forces. The details of the torque-
vectoring controller and functionality have been presented in [30]. In this
formulation the optimal steering and velocity from the MPC are considered as
two inputs for the torque-vectoring controller. The output of the controller
is the driving/braking torque correction required for the stabilization of the
vehicle. The desired C.G. forces derived from the CIM block are defined as:

$F_{des}=[F_{x}^{*},F_{y}^{*},G_{z}^{*}]^{T}$ (41)

where $F_{x}^{*}$,$F_{y}^{*}$ and $G_{z}^{*}$ are the desired C.G.
longitudinal and lateral forces and yaw moment, respectively. The actual C.G.
forces of the vehicle are:

$F=[F_{x},F_{y},G_{z}]^{T}$ (42)

where $F_{x}$, $F_{y}$ and $G_{z}$ are the actual C.G. longitudinal and
lateral forces and yaw moment of the vehicle, respectively. Each C.G. force
components is a function of longitudinal and lateral tire forces (see Fig. 8)
as:

$\displaystyle F_{x}=F_{x}(f_{x1},\dots,f_{x4},f_{y1},\dots,f_{y4})$ (43)
$\displaystyle F_{y}=F_{y}(f_{x1},\dots,f_{x4},f_{y1},\dots,f_{y4})$ (44)
$\displaystyle G_{z}=G_{z}(f_{x1},\dots,f_{x4},f_{y1},\dots,f_{y4})$ (45)

where, $f_{xi}$ and $f_{yi}$,$(i=1,\dots,4)$ are longitudinal and lateral tire
forces on each wheel of the vehicle. The corresponding adjusted C.G. forces to
minimize the error between the actual $F$ and desired, $F_{des}$, is
represented by:

$F(f+\theta f)\approx F(f)+\nabla F(f)\delta f$ (46)

where, $\nabla F(f)$ is the Jacobian matrix and $f$ is the total longitudinal
and lateral forces on each wheel. The control action needed to minimize the
error is:

$\theta f=[\theta f_{x1},\dots,\theta f_{x4},\theta f_{y1},\dots,\theta
f_{y4}]^{T}$ (47)

and is formulated as:

$\begin{gathered}\theta f=[W_{df}+W_{f}+(\nabla F(f)^{T}W_{E})\nabla
F(f)]^{-1}\\\ .[\nabla F(f)^{T}(W_{E}E)-W_{f}f]\end{gathered}$ (48)

Where $W_{df}$, $W_{f}$ are weighing matrices. The applied corrective torque
on each wheel is $\delta T=R_{eff}\times\theta f$. For brevity of the paper,
further details about calculating control actions and weighting metrics are
available in [30, 31].

Figure 8: Force conventions

##  VI Numerical Results

In this section, the closed-loop framework presented in Fig. 7 is studied in a
high-fidelity environment. The scenarios used to perform this study is as:
$1)$ an obstacle avoidance when the subject vehicle is travelling at
velocities in the range [60,80]
$\text{\,}\mathrm{km}\text{\,}{\mathrm{h}}^{-1}$ with road radius of
$750\text{\,}\mathrm{m}$, and $2)$ the application of torque-vectoring
controller to evaluate the performance of the collision avoidance manoeuvre
under different radius of the road with low surface friction. The purpose of
the two tests is to illustrate the effectiveness of the proposed collision
avoidance system on curve roads subject to environment constraints and
disturbances using three MPC strategies discussed in Section IV. The vehicle
parameters are tabulated in Table I. The simulation results are conducted
using combine IPG carmaker and Matlab/Simulink software.

TABLE I: Design parameters Symbols | Value(Unit)
---|---
$l_{f}$ | $1.43\text{\,}\mathrm{m}$
$l_{r}$ | $1.21\text{\,}\mathrm{m}$
$M$ (Subject Vehicle mass) | $1360\text{\,}\mathrm{kg}$
$I_{z}$ (Yaw moment of inertia) | $2050\text{\,}\mathrm{k}\mathrm{g}\mathrm{m}^{2}$
$w_{lane}$ (Lane width) | $5\text{\,}\mathrm{m}$
$l_{f}$ (lead vehicle) | $1.5\text{\,}\mathrm{m}$
$l_{r}$ (lead vehicle) | $1.7\text{\,}\mathrm{m}$
$t_{s}$ (sampling time) | $0.1\text{\,}\mathrm{s}$

###  VI-A Collision Avoidance Under Different Velocities

The simulation environment is initialized with the subject vehicle behind the
lead vehicle. Then the system plans the trajectory at each sampling time while
applying FCC and RCC constraints based on the position of subject vehicle and
the obstacle. The simulation results conducted for different velocities to
assess the ability of the vehicle to perform the collision avoidance
manoeuvre. Moreover, all predefined MPCs are initialized with short-sighted
prediction horizon ($N=14$), for imitating more extreme and critical
conditions in a collision avoidance scenario. The projection of the state and
input constraints of kinematic vehicle model used in all MPCs are represented
in Fig. 9 where $\mathcal{X}$ represents the state constraints of the nominal
MPC and offset-free MPC while $\bar{\mathcal{X}}$ is tightened state which is
used for robust MPC as state constraints.

Figure 9: State and input polyhedron and resulting tighten state and input

Figure 10: Simulation results for collision avoidance test, Note: The orange
line represents offset free MPC, Green line normal MPC and grey line robust
MPC

Subsequently $\mathcal{U}$ is the input constraint for nominal MPC and offset-
free MPC whereas $\bar{\mathcal{U}}$ is the result of tightened input set. For
offset-free MPC the extra state constraints $ey$ and $e_{\psi}$ are chosen as
$e_{y}$ = [-4,4]$\text{\,}\mathrm{m}$ and $e_{\psi}$ = [-0.15, 0.15]$rad$. It
is noteworthy that the structure of robust positively invariant set
($\mathcal{Z}$) introduced in section IV-D, has a dependence on size of
disturbance set $\mathcal{W}$ and the matrix $A_{K}$. Since the only degree of
freedom available to design $\mathcal{Z}$ is by means of matrix $A_{K}$,
careful choose of the appropriate controller $K$ is necessary to ensure stable
error dynamics, leading to calculate the tightened input and state
constraints. An interested reader can refer to [26] for detailed explanation
of the procedure. Fig. 10, illustrates the obstacle avoidance scenario with
initial speed of $80\text{\,}\mathrm{km}\text{\,}{\mathrm{h}}^{-1}$ with
assumption of high friction surface road condition. As can be seen from this
figure, all MPC strategies successfully avoid the collision from the lead
vehicle (red car) and perform a complete collision avoidance manoeuvre. It is
noteworthy, despite the fact that the nominal MPC and offset-free MPC can be
effective for generating trajectory in collision avoidance scenario with a
fixed longitudinal velocity, their performance can be degraded when the
longitudinal speed is varying due to braking and acceleration while performing
the manoeuvre. This can be confirmed in Fig. 10 where offset free MPC and
nominal MPC generate a trajectory very close to the lead vehicle during
initial lane change. On the other hand, the robust MPC based trajectory
maintain the safety margins to the lead vehicle during the initial lane
change. It is noteworthy, the parameters in MPCs (i.e $Q$, $R$ and $P$) can be
tunned to adjust the aggressiveness of collision avoidance manoeuvre. To show
the aggressiveness of all controllers, we compare the control efforts for the
time span of collision avoidance manoeuvre as shown Fig. 11. The control
efforts are compared using the integral of the absolute value ($IACA$) of the
steering and acceleration. The equations related to the performance indicator
($IACA$) can be written as [19]:

$\displaystyle IACA_{\delta}$
$\displaystyle=\frac{1}{t_{fin}-t_{in}}\int_{t_{in}}^{t_{fin}}\left|{\delta}\right|dt$
(49) $\displaystyle IACA_{a_{x}}$
$\displaystyle=\frac{1}{t_{fin}-t_{in}}\int_{t_{in}}^{t_{fin}}\left|{a_{x}}\right|dt$

Figure 11: Performance indicators of :$(a)$ Steering, and $(b)$ Longitudinal
acceleration

It is worth mentioning that as the velocity increases, the control effort to
perform the collision avoidance manoeuvre is less compared to lower speed for
all controllers. It is well known that the steady-state yaw-rate response to
steering angle of a vehicle increases as the vehicle’s velocity increases.
Hence, as the velocity rises, smaller steering angle inputs are required to
generate the same yaw-rate (for evading the fixed size obstacle) and this can
be observed in Fig. 11(a). The Table II illustrates the trend, where
$IACA_{\delta}$ is at the maximum when the vehicle speed is
$60\text{\,}\mathrm{km}\text{\,}{\mathrm{h}}^{-1}$, and reaches to its minimum
value in high speed for all controllers. The result of the steering effort is
reflected on the heading angle of the vehicle (in Fig. 12(b)) where large
evolution of heading angle is shown for robust MPC during either of the lane
changes. It is noted that Fig. 12 is the time series of velocity and heading
angle of the vehicle for all controllers for a velocity value of
$75\text{\,}\mathrm{km}\text{\,}{\mathrm{h}}^{-1}$ as an example. The second
performance indicator in (49) represents control effort on longitudinal
acceleration. Fig. 11(b) compares the acceleration needed to perform collision
avoidance manoeuvre for all controllers. It can be seen that, the longitudinal
acceleration for nominal MPC and offset-free MPC is almost the same, whereas
the longitudinal acceleration effort for robust MPC is lower than the rest.
This can be confirmed in Table II where the longitudinal acceleration for
robust MPC is almost 40% less than the rest of the controllers for all
velocity values. The significant of this analysis can be seen in Fig. 12(a)
where the longitudinal velocity profile for offset-free and nominal MPC show
larger evolution with overshoot during braking and accelerating. On the other
hand, robust MPC demonstrates smooth profile without any high-frequency
oscillation or overshoot. This can be inferred, where the robust MPC tackles
the variations of the longitudinal vehicle speed, resulting in a smooth and
jerk free braking/accelerating while performing collision avoidance manoeuvre.
This analysis has been carried out to demonstrates the benefits of utilizing
each controllers based on the users need, and the trade-off between control
actions required for smoothness and handling of the vehicle in collision
avoidance manoeuvre.

Figure 12: $(a)$ Longitudinal velocity, and $(b)$ Heading angle Note: red dash
line represents reference heading angle

It is beneficial to observe the maximum lateral error between the generated
trajectory and the reference trajectory that vehicle needs to track. Fig. 13
represents maximum lateral errors for all controllers during and after
performing obstacle avoidance. Fig. 13(a) indicates that the lateral errors
are gradually increasing as velocity of the vehicle increases. It can be seen
that the maximum lateral error for nominal MPC, offset-free MPC and robust MPC
reaches to the maximum of $0.34\text{\,}\mathrm{m}$, $0.18\text{\,}\mathrm{m}$
and $0.28\text{\,}\mathrm{m}$ when the speed is at
$80\text{\,}\mathrm{km}\text{\,}{\mathrm{h}}^{-1}$. It also shows the superior
performance of offset-free MPC in path tracking compared to other controllers.
This suggests that the accurate tracking requires the incorporation of path
curvature into the system dynamics. Fig. 13b shows the distance of the subject
vehicle from the reference line during the obstacle avoidance manoeuvre for
different velocities to demonstrates the safe distance from outer road
boundary. In this figure, the capability of all MPC’s in maintaining safe
distance from the outer road boundary while performing collision avoidance is
shown.

TABLE II: Performance indicators NomMPC |  $60km/h$ |  $65km/h$ |  $70km/h$ |  $75km/h$ |  $80km/h$
---|---|---|---|---|---
$IACA_{\delta}(rad)$ |  $0.1207$ |  $0.09987$ |  $0.09977$ |  $0.09183$ |  $0.09183$
$IACA_{a_{x}}(\frac{m}{s^{2}})$ |  0.6296 |  0.97 |  1.31 |  1.5 |  1.67
OffMPC |  |  |  |  |
$IACA_{\delta}(rad)$ |  0.068 |  0.066 |  0.0647 |  0.0643 |  0.05
$IACA_{a_{x}}(\frac{m}{s^{2}})$ |  0.68 |  1.04 |  1.35 |  1.58 |  1.7
RobMPC |  |  |  |  |
$IACA_{\delta}(rad)$ |  0.1373 |  0.1337 |  0.1326 |  0.1324 |  0.1307
$IACA_{a_{x}}(\frac{m}{s^{2}})$ |  0.35 |  0.61 |  0.89 |  1.16 |  1.31

###  VI-B Collision Avoidance using Torque Vectoring

Torque-vectoring controller enhances vehicle’s handling and safety in extreme
manoeuvres such as low surface friction or high road curvature. To show the
need for torque-vectoring controller, this section evaluates the performance
of all MPCs when they are combined with torque vectoring controller to
maintain the stability of the vehicle under different curvature as well as
environment conditions. For evaluation purposes, the performance of trajectory
planning controllers is compared with the case of vehicle without having
torque-vectoring controller.

Figure 13: Maximum lateral error $(a)$Tracking error, and $(b)$ Maximum
lateral error during obstacle avoidance, Note: Red dash line indicates outer
road edge

As an example, Fig. 14, shows the performance of the offset-free MPC with and
without torque-vectoring controller. In this example, simulation result has
been carried out under tight curvature with radius,
$R=$500\text{\,}\mathrm{m}$$ and friction surface of $\mu=0.4$. This scenario
represents a harsh driving condition for performing collision avoidance
manoeuvre. As can be seen, controller with torque-vectoring, stabilizes the
vehicle during collision avoidance manoeuvre with initial speed of
$v_{x}=$75\text{\,}\mathrm{km}\text{\,}{\mathrm{h}}^{-1}$$. The figure also
shows the incapability of the controller without torque-vectoring in
stabilizing the vehicle in such harsh driving manoeuvre.
A parametric analysis carried out to verify the efficacy of the proposed
combined framework after performing collision avoidance manoeuvre to
demonstrate the tracking capability of the controllers under low surface
friction for different radius of the path. In this analysis, The maximum
lateral and heading error (Fig. 15) between the reference and actual
trajectory of the vehicle is investigated. The maximum lateral error for all
controllers increases gradually as the radius of the road decreases (curvature
increase). Among all controllers, offset-free MPC experiences a better
reference tracking. This can be confirmed in Table III where the largest
lateral error can be seen with radius of $R=$500\text{\,}\mathrm{m}$$ with
$e_{y}=$0.27\text{\,}\mathrm{m}$$, following with nominal and robust MPC with
$e_{y}=$0.37\text{\,}\mathrm{m}$$ and $e_{y}=$0.48\text{\,}\mathrm{m}$$
respectively. Fig. 15 indicates that, MPCs incorporating torque-vectoring
controller maintain vehicle from further deviation of reference trajectory and
keep the vehicle within the lane limits. However, vehicle without torque-
vectoring controller (blue dashes in Fig. 15a) starts to deviate from
reference path with above $1\text{\,}\mathrm{m}$ deviation.

Figure 14: Collision avoidance test under low friction surface and tight
curvature. The orange line represents subject vehicle with torque-vectoring
controller, and blue line without torque-vectoring controller Figure 15:
Collision avoidance test under low friction surface for different value of
curvature. $e_{y}$ represents lateral error and $e_{\psi}$ lateral heading
angle error, Note: Dash blue lines represent the vehicle without torque-
vectoring

Similarly, for the heading angle error ($e_{\psi}$) in Fig. 15b, all
controllers maintain the directionality of the vehicle by keeping the heading
angle error to be small, allowing subject vehicle to follow the reference
heading angle. On the other hand, controllers without torque-vectoring control
experience large lateral and heading error resulting in loss of stability and
directionality (blue dash lines in Fig. 15). It is noted that when the vehicle
travels on a moderate driving condition, i.e., low speed or low-curvature, the
path tracking objectives are fulfilled. However, as the vehicle travels in an
extreme conditions i.e., high-curvature or high speed, the required heading
angle may deteriorate the tracking capability. The reason of this phenomena is
twofold: (1) there would be unavoidable tire sliding effects, (2) presence of
sideslip angle of the vehicle which is not included in the kinematic vehicle
model. Therefore, as the radius of the road decreases (curvature increases),
the maximum heading angle error will increase. It is noted that, although the
largest heading error happens when the radius of the road is
$500\text{\,}\mathrm{m}$ where curvature is tight, the MPC’s with
incorporation of torque-vectoring controller maintain the directionality of
the vehicle. This can be seen in Table III where the largest heading error for
nominal MPC, offset-free MPC and robust MPC is 0.0075rad, 0.0066rad and
0.09rad respectively.
The net control action from all tire forces can be seen in Fig. 16. In this
figure, The control effort ($IACA_{M_{z}}$) needed for stabilizing the vehicle
while perform the collision avoidance manouvre, for offset MPC and nominal MPC
is larger repsect to robust MPC. Robust MPC incorporates the worst-case
disturbance realizations while solving the optimisation problem. In the robust
MPC optimization problem, the performance index is being optimized allows MPC
to operate between lower and upper bound of disturbance set. As a consequence,
control law maintain the system within an invariant tube around nominal MPC
and attempt to execute the control action more aggressively to steer the
uncertain system towards the reference system. The results of this steering
control action, allows the intervention of the torque-vectoring controller,
for applying additional torque, to be limited. On the other hand offset-free
MPC and nominal MPC require extra torque actuation for stabilisation. This can
be confirmed in Fig. 17 where less steering action is required in both low
frequency and high frequency range compared to robust MPC.

Figure 16: Net control action from torque-vectoring controller Figure 17:
Spectrum of (a) steering (b) front-right torque

The tabulated results from the performance indicator for the torque-vectoring
actuation as well as maximum lateral and heading error values are reported in
Table III. It is shown that, in driving scenario where radius is 750m, the
combined controllers can enhanced vehicle stability while performing collision
avoidance with minimum lateral and heading errors. However, as radius of the
road decreases, extra additional yaw moment from torque-vectoring controller
is needed in order to stabilize the vehicle. This table summarized the
performance of the combined controllers in terms of tracking the reference
values from the road. It can be inferred from the table the need of torque
vectoring controller for stabilization under extreme driving condition and
safe driving condition on different curvature.

TABLE III: Performance indicator and maximum lateral and heading error NomMPC |  $500m$ |  $550m$ |  $600m$ |  $650m$ |  $700m$ |  $750m$
---|---|---|---|---|---|---
$IACA_{M_{z}}(N.m)$ |  567.5 |  556.1 |  539.1 |  537.1 |  515.9 |  506
$e_{y}(m)$ |  0.3745 |  0.328 |  0.316 |  0.253 |  0.233 |  0.214
$e_{\psi}(rad)$ |  0.0075 |  0.0073 |  0.0074 |  0.0049 |  0.00456 |  0.00454
OffMPC |  |  |  |  |  |
$IACA_{M_{z}}(N.m)$ |  719.1 |  665.8 |  651.7 |  651.9 |  640.4 |  602.2
$e_{y}(m)$ |  0.27 |  0.28 |  0.22 |  0.17 |  0.154 |  0.147
$e_{\psi}(rad)$ |  0.0066 |  0.0057 |  0.0053 |  0.0050 |  0.0043 |  0.0038
RobMPC |  |  |  |  |  |
$IACA_{M_{z}}(N.m)$ |  326.7 |  320.5 |  308.2 |  302 |  293.4 |  286.6
$e_{y}(m)$ |  0.48 |  0.44 |  0.40 |  0.31 |  0.29 |  0.22
$e_{\psi}(rad)$ |  0.09 |  0.089 |  0.064 |  0.041 |  0.015 |  0.0036

##  VII Conclusion

This paper, compared the performance of three different MPC’s for trajectory
planning/control, combined with torque vectoring controller for collision
avoidance tests. A novel path planning was designed based on model predictive
control algorithm. This formulation, deals with non-convex collision avoidance
constraints on curve road, allowing the use of effective optimisation solvers
for computing vehicle trajectory through MPC based strategies. In order to
implement these constraints, a technique was presented to have feasible and
convex free optimization on a curve road. This allows, the trajectory planning
controller to generate safe and feasible trajectories with admissible inputs
while performing collision avoidance manoeuvre. Furthermore, Three different
MPCs, entitled as nominal MPC, robust MPC and offset-free MPC has been
compared. It was seen from the simulation results that offset-free MPC
outperform the rest of the controllers in terms of driving smoothness and path
tracking. However, robust MPC add further benefit for designing collision
avoidance system, by providing extra safety margin. Additionally, torque
vectoring algorithm has been combined with all MPCs through its separate
implementation. To assess the performance of the combined controller,
different value of curvature as well as low surface friction have been used to
simulate the controllers under extreme driving conditions. The numerical
results in Simulink/IPG CarMaker co-simulation environment demonstrated that,
the combined controllers improved vehicle performance under extreme and
dynamic environment, compare to controllers without torque-vectoring, and
fulfil the safety considerations for collision avoidance manoeuvre.

## References

  * [1] J. Funke, M. Brown, S. M. Erlien, and J. C. Gerdes, “Collision Avoidance and Stabilization for Autonomous Vehicles in Emergency Scenarios,” _IEEE Transactions on Control Systems Technology_ , vol. 25, no. 4, pp. 1204–1216, 2017\.
  * [2] S. Cheng, L. Li, H.-Q. Guo, Z.-G. Chen, and P. Song, “Longitudinal Collision Avoidance and Lateral Stability Adaptive Control System Based on MPC of Autonomous Vehicles,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. PP, pp. 1–10, 2019.
  * [3] S. Xu and H. Peng, “Design, Analysis, and Experiments of Preview Path Tracking Control for Autonomous Vehicles,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. PP, pp. 1–11, 2019.
  * [4] A. Boyali, V. John, Z. Lyu, R. Swarn, and S. Mita, “Self-scheduling robust preview controllers for path tracking and autonomous vehicles,” _2017 Asian Control Conference, ASCC 2017_ , vol. 2018-Janua, pp. 1829–1834, 2018.
  * [5] Z. Wang, U. Montanaro, S. Fallah, A. Sorniotti, and B. Lenzo, “A gain scheduled robust linear quadratic regulator for vehicle direct yaw moment Control,” _Mechatronics_ , vol. 51, no. January, pp. 31–45, 2018. [Online]. Available: https://doi.org/10.1016/j.mechatronics.2018.01.013
  * [6] S. Khosravani, M. Jalali, A. Khajepour, A. Kasaiezadeh, S. K. Chen, and B. Litkouhi, “Application of lexicographic optimization method to integrated vehicle control systems,” _IEEE Transactions on Industrial Electronics_ , vol. 65, no. 12, pp. 9677–9686, 2018.
  * [7] A. Stentz, “The Focussed D* Algorithm for Real-Time Replanning,” in _14th international joint conference on artificial intelligence_ , San Francisco, California, 1995, pp. 1652–1659.
  * [8] M. L. Dave Ferguson, Thomas M.howard, “Motion Planning in UrbanEnvironments,” _Journal of Field Robotics_ , vol. 25, pp. 939–960., 2008.
  * [9] Y. Chen, H. Ye, and M. Liu, “Hierarchical Trajectory Planning for Autonomous Driving in Low-speed Driving Scenarios Based on RRT and Optimization,” 2019\. [Online]. Available: http://arxiv.org/abs/1904.02606
  * [10] G. Franze and W. Lucia, “A Receding Horizon Control Strategy for Autonomous Vehicles in Dynamic Environments,” _IEEE Transactions on Control Systems Technology_ , vol. 24, no. 2, pp. 695–702, 2016.
  * [11] Y. Gao, A. Gray, J. V. Frasch, T. Lin, E. Tseng, J. K. Hedrick, and F. Borrelli, “Spatial Predictive Control for Agile Semi-Autonomous Ground Vehicles,” _Proceedings of the 11th International Symposium on Advanced Vehicle Control_ , vol. VD11, no. 2, pp. 1–6, 2012.
  * [12] J. Karlsson, N. Murgovski, and J. Sjöberg, “Temporal vs. Spatial formulation of autonomous overtaking algorithms,” _IEEE Conference on Intelligent Transportation Systems, Proceedings, ITSC_ , pp. 1029–1034, 2016.
  * [13] J. Nilsson, P. Falcone, M. Ali, and J. Sjöberg, “Receding horizon maneuver generation for automated highway driving,” _Control Engineering Practice_ , vol. 41, pp. 124–133, 2015. [Online]. Available: http://dx.doi.org/10.1016/j.conengprac.2015.04.006
  * [14] L. De Novellis, A. Sorniotti, P. Gruber, J. Orus, J. M. Rodriguez Fortun, J. Theunissen, and J. De Smet, “Direct yaw moment control actuated through electric drivetrains and friction brakes: Theoretical design and experimental assessment,” _Mechatronics_ , vol. 26, pp. 1–15, 2015. [Online]. Available: http://dx.doi.org/10.1016/j.mechatronics.2014.12.003
  * [15] Q. Lu, P. Gentile, A. Tota, A. Sorniotti, P. Gruber, F. Costamagna, and J. De Smet, “Enhancing vehicle cornering limit through sideslip and yaw rate control,” _Mechanical Systems and Signal Processing_ , vol. 75, pp. 455–472, 2016. [Online]. Available: http://dx.doi.org/10.1016/j.ymssp.2015.11.028
  * [16] S. Taherian, U. Montanaro, S. Dixit, and S. Fallah, “Integrated Trajectory Planning and Torque Vectoring for Autonomous Emergency Collision Avoidance,” _International Conference on Intelligent Transportation Systems (ITSC)_ , 2019.
  * [17] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design,” in _Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design_ , 2015.
  * [18] F. Borrelli and M. Morari, “Offset free model predictive control,” _Proceedings of the IEEE Conference on Decision and Control_ , pp. 1245–1250, 2007.
  * [19] C. Chatzikomis, A. Sorniotti, P. Gruber, M. Zanchetta, and D. Willans, “Comparison of Path Tracking and Torque- Vectoring Controllers for Autonomous Electric Vehicles,” _IEEE_ , vol. 3, no. 4, pp. 559–570, 2018\.
  * [20] J. Nilsson, Y. Gao, A. Carvalho, and F. Borrelli, “Manoeuvre generation and control for automated highway driving,” _IFAC Proceedings Volumes_ , vol. 47, no. 3, pp. 6301–6306, 2014. [Online]. Available: http://linkinghub.elsevier.com/retrieve/pii/S1474667016426005
  * [21] Y. Gao, A. Gray, A. Carvalho, H. E. Tseng, and F. Borrelli, “Robust nonlinear predictive control for semiautonomous ground vehicles,” _Proceedings of the American Control Conference_ , pp. 4913–4918, 2014.
  * [22] M. G. Plessen, P. F. Lima, J. Martensson, A. Bemporad, and B. Wahlberg, “Trajectory planning under vehicle dimension constraints using sequential linear programming,” _IEEE Conference on Intelligent Transportation Systems, Proceedings, ITSC_ , vol. 2018-March, pp. 1–6, 2018.
  * [23] A. Scheuer and T. Fraichard, “Collision-free and continuous-curvature path planning for car-like robots,” _Proceedings - IEEE International Conference on Robotics and Automation_ , vol. 1, no. April, pp. 867–873, 1997\.
  * [24] D. Shin and S. Singh, “Path Generation for a Robot Vehicle Using Composite Clothoid Segments,” Tech. Rep., 1990.
  * [25] J. B. R. Mayne and D. Q., “Model Predictive Control: Theory and Design,” in _Model Predictive Control: Theory and Design_ , 2015, pp. 237–241.
  * [26] S. Dixit, U. Montanaro, M. Dianati, D. Oxtoby, T. Mizutani, A. Mouzakitis, and S. Fallah, “Trajectory Planning for Autonomous High-Speed Overtaking in Structured Environments Using Robust MPC,” _IEEE Transactions on Intelligent Transportation Systems_ , pp. 1–14, 2019.
  * [27] S. V. Rakovic, E. C. Kerrigan, K. I. Kouramas, and D. Q. Mayne, “Invariant Approximations of the Minimal Robust Positively Invariant Set,” _IEEE TRANSACTIONS ON AUTOMATIC CONTROL_ , vol. 50, no. 3, pp. 406–410, 2005.
  * [28] Y. Gao, A. Gray, H. E. Tseng, and F. Borrelli, “A tube-based robust nonlinear predictive control approach to semiautonomous ground vehicles,” _Vehicle System Dynamics_ , vol. 52, no. 6, pp. 802–823, 2014. [Online]. Available: https://doi.org/10.1080/00423114.2014.902537
  * [29] K. Jalali, S. Lambert, and J. McPhee, “Development of a Path-following and a Speed Control Driver Model for an Electric Vehicle,” _SAE International Journal of Passenger Cars - Electronic and Electrical Systems_ , vol. 5, no. 1, pp. 100–113, 2012.
  * [30] S. Fallah and A. Khajepour, “Vehicle Optimal Torque Vectoring Using State-Derivative Feedback and Linear Matrix Inequality,” _IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY_ , vol. 62, no. 4, 2013.
  * [31] S.-k. Chen, S.-k. Chen, Y. Ghoneim, N. Moshchuk, and B. Litkouhi, “Tire Force-based Holistic Corner Control Tire Force-based Holistic Corner Control,” no. November, 2012.

|  Shayan Taherian received the M.Sc. degree in automation and control
engineering from the Polytechnic of Milan University, Italy, in 2016. He is
currently pursuing the Ph.D. degree with the Centre of Automotive Engineering,
University of Surrey, Guildford, U.K. His research interests are vehicle
dynamics and control, trajectory planning for autonomous vehicles, and
intelligent vehicles.
---|---
|  Shilp Dixit received the M.Sc. degree in automotive technology from the
Eindhoven University of Technology, Eindhoven, The Netherlands, in 2015. He
currently finished the Ph.D. degree with the Centre of Automotive Engineering,
University of Surrey, Guildford, U.K. His research interests are vehicle
dynamics and control, trajectory planning and tracking for autonomous
vehicles, and intelligent vehicles.
---|---
|  Umberto Montanaro received the Laurea (M.Sc.) degree (cum laude) in
computer science engineering from the University of Naples Federico II,
Naples, Italy, in 2005, and the Ph.D. degrees in control engineering and in
mechanical engineering from the University of Naples Federico II, in 2009 and
2016, respectively. From 2010 to 2013, he was a Research Fellow with the
Italian National Research Council (Istituto Motori). He served as a temporary
Lecturer in automation and process control with the University of Naples
Federico II. He is currently a Lecturer in control systems for automotive
engineering with the Department of Mechanical Engineering Sciences, University
of Surrey, Guildford, U.K. The scientific results he has obtained till now
have been the subject of more than 60 scientific articles published in peer-
reviewed international scientific journals and conferences. His research
interests range from control theory to control application and include:
adaptive control, with special care to model reference adaptive control,
control of piecewise affine systems, control of mechatronics systems,
automotive systems, and connected autonomous vehicles.
---|---
|  Saber Fallah is currently an Associate Professor with the University of
Surrey. He is also the Director of the Connected and Autonomous Vehicles Lab
(CAV-Lab), Department of Mechanical Engineering Sciences. Since joining the
University of Surrey, he has been contributing to securing research funding
from EPSRC, Innovate UK, EU, KTP, and industry.
He has coauthored a textbook Electric and Hybrid Vehicles: Technologies,
Modeling and Control-A Mechatronics Approach (John Wiley Publishing Company,
2014). The book addresses the fundamentals of mechatronic design in hybrid and
electric vehicles. His work has contributed to the state-of-the-art research
in the areas of connected autonomous vehicles and advanced driver assistance
systems. So far, his research has produced four patents and more than 40 peer
reviewed publications in high-quality journals and well-known conferences. He
is the Co-Editor of the book of conference proceedings resulted from the
organization of the TAROS 2017 Conference (Springer, 2017).
---|---