1

MPC path-planner for autonomous driving solved

by genetic algorithm technique
S. Arrigoni, F. Braghin, F. Cheli Politecnico di Milano, Mechanical Department
via G. La Masa, 1 - 20156 Milano Italy
Email: [email protected]

Abstract—Autonomous vehicle’s technology is expected to be to receive incremental improvements in term of additional

disruptive for automotive industry in next years. This paper pro- functionalities in next years.
arXiv:2102.01211v1 [cs.RO] 1 Feb 2021

poses a novel real-time trajectory planner based on a Nonlinear Academia involvement in this field is well documented in
Model Predictive Control (NMPC) algorithm. A nonlinear single
track vehicle model with Pacejka’s lateral tyre formulas has been literature. Just to cite a few: Stanford has several ongoing
implemented. The numerical solution of the NMPC problem is projects on autonomous vehicle prototypes. In [6] the so-
obtained by means of the implementation of a novel genetic called VW Passat prototype ”Stanley”, which participated the
algorithm strategy. Numerical results are discussed through DARPA Grand Challenge of 2005, is described and in [7] the
simulations that shown a reasonable behavior of the proposed Audi research vehicle ”Shelley” is presented. MIT worked on
strategy in presence of static or moving obstacles as well as in
a wide rage of road friction conditions. Moreover a real-time ”Talos” [8] for DARPA Urban Challenge 2007. Vislab (Parma
implementation is made possible by the reported computational University) developed several prototypes over last decades
time analysis. obtaining impressive results during VisLab Intercontinental
Autonomous Challenge (VIAC) [9].
This paper, as most of the works in literature, is based on
I. INTRODUCTION a hierarchical control structure composed by different layers
to decouple the complete driving problem into a sequence of
Autonomous vehicle appeal grown exponentially in last tasks [10], [11].
years and it is expected to be disruptive for automotive in- In this paper, motion planning task is mainly investigated.
dustry involving not only traditional OEMs, but also suppliers, In literature, this problem is tackled by different techniques
technology providers, academic institutions, municipal govern- as graph search methods which generate a path by means of
ments, and regulatory bodies. Considering that worldwide ev- an optimization of an objective function defined on nodes and
ery year dies around 1.35 million people due to road accidents edges in the discretized configuration space of the problem,
caused mainly by wrong behavior of the drivers as distracted A-star is an improved version of the famous “Dijkstra’s algo-
driving, exceeding speed limits and driving under the influ- rithm” obtained adding an heuristic term in objective function
ence of alcohol or drugs among the others [1], Autonomous that estimates the cost of the cheapest path from a given node
driving that aims at reducing human control on driving tasks, to the target. It is proven that A-star is usually faster than
could be considered a solution to increase road safety. Apart Dijkstra and in the worst case they have te same efficiency.
from safety, Autonomous driving will lead to several possible In [10], [12] some example are provided and A∗ extended
advantages: pollution, traffic, space consumption and mobility versions are grouped into sub-families related to the innovative
cost reduction just to cite a few as reported by [2]. aspects improved.
As suggested in [3], Autonomous Vehicle’s development Incremental search methods iteratively increment a graph
not only involves technological aspects, but it’s intrinsically of feasible configurations and transitions in the C-space until
connected to societal, legal, and regulatory issues related to the target region is reached. These methods try to remove
them and analysts (and most industry players and experts the dependence of the solution from the discretization of
agrees on that) predicts that ”The car of the future is electrified, configuration space of the problem. Among several imple-
autonomous, shared, connected and yearly updated” [4]. mentation proposed, like i.e. expansive spaces tree (EST)
The most of industrial carmakers already revealed their [13], Rapidly-exploring Random Tree (RRT) technique [14]
pilot programs and prototypes of accurate localization and rapidly spread and gained importance becoming one of the
environment perception, highly automated or fully autonomous most used methods thanks to its high efficiency in handling
vehicles. high-dimensional non-holonomic problems. RRT∗ , presented
Significant examples are Daimler’s Bertha prototype that in [15], is an ”asymptotically optimal” improved version of the
autonomously revisited the ”Bertha Benz Memorial Route” original RRT producing an increasingly finer discretization of
125 years after Bertha Benz’s first cross-country automobile the configuration space.
journey in history [5] as well as Tesla, leading company in Potential field approach can be easily explained by means
term of media exposure in ADAS development, that already of an electric charge moving in a potential field. Firstly intro-
commercialized an highly automated semi-autonomous driving duced by [16], it gained popularity for its simple implemen-
system called ”autopilot” mounted on it’s vehicles expected tation and extremely low computational demanding suitable
 2

for real-time application. Several improved implementation trajectory quality reducing low level tracking error. The same
were studied during the years in order to solve local solution formulation was used in [19] where implementing a single
convergence problem as reported in the survey [17] and it’s level NMPC based on a complex model (four wheels model 6
nowadays an extremely interesting and wide spread on-line dof with Pacejka’s tyre model) merging trajectory generation
path planning technique. and tracking tasks, tracking issue has been solved. The result
Model Predictive Control (MPC) is an important tool for was tested in simulation with static obstacles on low friction
optimal control problem. Thanks to progress in term of surfaces and impleented in real-time. Unfortunately spatial
mathematical formulation and a progressive increment of reformulation presents important limitations as inability to
computational power available, it is possible to extend its manage velocity equal to zero and dramatically reduces its
application to real time motion planning and tracking tasks advantages in presence of moving obstacles.
[18], [19]. Another dynamic single stage implementation is reported in
MPC formulation has some interesting features that deter- [18] where single-track vehicle model with Pacejka’s lateral
mined its increasing success among other techniques for com- tyre model is adopted. Calculation is speed up thanks to
plex control problems: it can handle a multivariable process linearization around current configuration and considering
subjected to inequality constraints on input and state variables, moving obstacles as static within every MPC calculation. An
high delays and highly nonlinear systems; control strategy can example of a kinematic single layer implementation is reported
vary dynamically in time in order to take into account changes in [23] in the form of a Linear Time-Varying MPC where static
in working condition (model, constraints or cost terms). and moving obstacles are considered, and the optimal control
On the other hand, the main drawback of MPC is an high problem (OCP) is solved by using a quadratic programming
computational demand for its implementation (at every time (QP) routine.
step the entire optimization problem needs to be solved). Vehicle modeling problem for MPC applications is inves-
Moreover the method relies on the open-loop integration of tigated in [24] where, after highlighting kinematic models
system dynamic over the horizon: this means that perfor- advantages as lower computational effort and the possibility
mances are highly dependent by model accuracy and reliability to handle stopped vehicles (singularity at zero velocity is
(i.e. vehicle model and obstacles’ motion prediction on the related to slip calculation affecting dynamic tyre models),
road). limitations as impossibility to consider friction limits at tyre-
Examples available in literature mainly differ in term of road interaction and modeling errors brings the authors to
vehicle model used, mathematical formulation of obstacles in suggest using dynamic models in presence of moderate driving
the optimization problem and mathematical implementation speeds.
of numerical solving method for the constrained optimization The aim of this work is to propose a single layer NMPC
problem. formulation for trajectory generation and tracking problems
A common approach adopted in literature to reduce overall with a special focus on real-time capability and robustness
complexity, is to split trajectory generation and tracking tasks of the algorithm in a urban-like scenarios. The vehicle model
into a hierarchical structure composed by subsequent MPCs, considered is a single-track vehicle model where lateral tyre
where higher level aims to define optimal trajectory over a dynamics are modelled through Pacejka’s simplified Magic
long time horizon based on a simplified model, while lower Formula. The algorithm is capable of handling multiple mov-
control level is in charge of tracking it by means of a more ing obstacles. The numerical solution of the OCP is obtained
sophisticated vehicle model and a shorted time horizon. by means of the implementation of a novel genetic algorithm
As an example in [20] high level controller is designed as strategy described in the paper. For testing purposes, the
a NMPC based on a point mass model capable of deal with resulting algorithm has been implemented in a Simulink sim-
static obstacle on a slippery road, and a lower level control ulation environment where robustness to model uncertainties
based on a double track vehicle model coupled with Pacejka’s and obstacle avoidance has been tested.
tyre model. The overall resulting controller is real-time but Section II introduces assumptions and aim of the paper,
the high discrepancy between the models used are leading to in particular the hierarchical approach adopted to define the
high tracking errors. An improvement is presented in [21], decisional algorithm involving trajectory planning and vehicle
where higher level control is modifed as a NMPC based on control. In Section III the optimization problem is deeply
precomputed motion primitives based on a complex vehicle described and the MPC formulation considered is presented.
model. The new approach allows a reduction on tracking error Section IV describes the genetic algorithm strategy adopted in
but limit the solution to a limited set of suboptimal trajectories order to solve the problem previously presented. In section V
and requires an online implementation of the mixed-integer numerical simulations are shown in order to evaluate the
problem that can be difficult when a large number of precom- behavior of the algorithm in term of performances, robustness
puted maneuvers is considered. and computational time required Finally in section VI the
As an improvement, in [22] a two-level NMPC with a High conclusion are drawn.
level base on a nonlinear dynamic single-track vehicle model
defined by mean of a spatial reformulation that considers a
II. A SSUMPTIONS AND AIM OF THE PAPER
simplified Pacejka’s tyre model is presented. this formulation
allows to speed up calculation in presence of static obstacle According to prototype and survey papers [11], [25], [26]
and the more sophisticated tyre modeling improved reference available in literature, fig. 1 reports a general framework
 3

grouping all the main sub-tasks required for autonomous Decisional Algorithm
driving development. Mission Planner Mode Planner Motion Planner Control Planner

Software Architecture System Simulated / Real Physical System

Vehicle dynamic data

Obs position and motion estimation

Vehicle
Road structure Dynamic
Decisional Algorithm Road geometry information
Obs motion
Road connections
Wold
Self-localization Environment
Self-localization

4D Environment
reconstruction General information (at different level of detail)
Raw data Measures
processing
Information required/useful

Fig. 2. Decisional Algorithm

Fig. 1. sub-tasks for Autonomous driving

In detail, on the right (blue background) physical (or sim-

ulated) system and environment are represented: it represents correspond to a reaction time of 0.05s: this is limited mainly
the ”controlled” vehicle surrounded by the scenario (roads, by considering the working frequency of camera sensors
other vehicles or pedestrians, . . . ) and sensor setup. (usually around 30 Hz to which should be added the delay
On the left (green background) software algortihms are rep- due to image processing). Robustness is intended mainly as
resented. It can be mainly divided into three main sequential the capability of the controller to always generate a feasible
tasks: result (even if sub-optimal), to be capable of correctly operate
although moderate modeling errors or external disturbances
• ”Raw data processing”, where all data directly provided
and finally to prevent to get stuck in local minima. Moreover
by sensors need to be processed in order to compute
the algorithm is required to correctly compute vehicle control
useful information relevant for decision making process
inputs that generate a safe and collision free trajectory in
or environment reconstruction;
presence of several multiple and moving obstacle. It’s finally
• ”4D environment reconstruction”. Starting from pro-
required to operate in a urban-like environment. It is char-
cessed data it’s essential to reconstruct surrounding of the
acterized by the presence of sharp curves, narrow lines and
vehicle using redundant measures. In detail: accurate and
medium-low driving speed required.
reliable self-localization of the vehicle in a 3D digital map
In detail, fig. 2 represents the different information required
enriched by traffic and behavior laws, where obstacles
at every decisional tasks: in this work, Mission planner is
are detected, tracked and their time evolution is predicted
not further investigated as well as Mode planning (a specific
(time dimension);
reference trajectory as output of these two tasks is considered
• ”Decisional Algorithm” groups all decisional driving
known).
tasks. starting from environment reconstruction and driver
”Vehicle dynamic data” (model parameters), ”Self-
requests, it’s aim is to define and actuate a decisional
localization”, ”Road geometry information” and ”Obstacle’s
strategy.
position and motion estimation” are considered known and
”Decisional Algorithm”, as suggested, incorporates the
with a sufficient accuracy level than the designed algorithms
complex decisional process of human driving. As proposed
considers these values as deterministic.
by previous works [10], [11], it is possible to split the entire
process into 4 sub-tasks in a hierarchical scheme as shown in
fig.2. III. PATH - PLANNER SCHEME
The main novelty contribution of this research work is
represented by the design of a decisional algorithm capable The trajectory planning & control problem investigated can
of performing trajectory planning and vehicle control tasks be defined as shown in 1.
ensuring:
min Ltot (x(t), u(t))
• a real-time feasible implementation; u(t)
• a sufficient robustness of the solution calculated; s.t. ẋ(t) = f (x(t), u(t)) (1)
• the capability of correctly handle with multiple and
x(t0 ) = x0
moving obstacles in urban-like scenarios.
These requirements can be translated into specifics that can where Ltot represents the cost function of the problem in-
be evaluated and verified. cluding also soft constraints formulation, while f (x(t), u(t))
In particular real-time implementation means to define a represents the numerical model of the vehicle: in the following,
minimum working frequency for the controller that allows optimization constraint problem is described in all its parts. In
to dynamically react fast enough to unexpected (dangerous) detail road and vehicle models, constraints and cost function
situation that can occur. This limit is set around 20 Hz, that formulations are reported.
 4

A. Map model
”Road geometry information” are assumed as known data: Y
at every time step the designed algorithm can rely on road
structure’s knowledge of an area equal or greater that the
X4,Y4
spatial horizon considered by the optimization problem. Road X2,Y2
information are defined as function of the curvilinear abscissa
of centerline of the road. Geometrical data and additional data t1
can be distinguished. Additional data include: number of lanes;

t
left and right width respect to centerline of the lane (byh ,byl );

s
X3,Y3
and other information that can be spatially located and defined X1,Y1
as function of curvilinear abscissa (i.e. traffic lights or traffic t0
signs position, intersections, speed limits,. . . ). Geometrical
data include: global coordinates of centerline of the road X
0
(X,Y ), heading angle of the road (θc ) and it derivatives (θcs ).
In order to: Fig. 3. Cubic Hermit Spline example
• reduce as much as possible the amount of data required
to fully describe the geometrical structure of the road; where the terms Xs0 , Ys0 , Xs00 , Ys00 are first and second deriva-
• minimize the computational effort required to perform a tives respect to s of X(s) and Y (s).
self-localization of the vehicle from curvilinear reference
system (electronic map) to global coordinate;
B. Vehicle Description And Modeling
• minimize the computational effort required to evaluate
”Geometrical data” from curvilinear coordinate. In fig. 4 a schematization of vehicle model and relative
reference systems used is shown. State function describing
The mathematical structure of the road is defined as a Cubic
system dynamics is composed by eight state variables (x(t) ∈
Hermite Spline (CHS) as reported in [27].
R8 ) and two input variables (u(t) ∈ R2 ). State function can
CHS is a continuous function defined as a piecewise, where be compactly written as ẋ(t) = f GA (x(t), u(t)). In detail,
each sub-function is defined as a cubic polynomial in Hermite as reported in (4), state x(t) is composed by curvilinear
form (2). coordinate and lateral displacement respect to road centerline
p(t) =H00 (t)p0 + H10 (t)d0 + H01 (t)p1 + H11 (t)d1 (s,y), absolute heading angle ψ, longitudinal and lateral speed
in the body frame (Vx ,Vy ), yaw rate (ω), front wheel steering
for [sk , sk+1 ]
angle (δ) and finally torque applied to the rear wheels (τ ).
s − sk
where: t ∈ [0, 1] t= Input variables u(t) are the derivatives of wheel steering angle
sk+1 − sk and torque applied, defined for clarity as u1 = δ̇,u2 = τ̇
∂p respectively.
p0 = p(t0 ) d0 = (t0 ) (2)
∂t
∂p x(t) = [s, y, ψ, Vx , Vy , ω, δ, τ ]T
p1 = p(t1 ) d1 = (t1 ) (4)
∂t u(t) = [u1 , u2 ]T
H00 = 2t3 − 3t2 + 1 H10 = t3 − 2t2 + t
Dynamic vehicle’s formulation shown in (5) is based on a
H01 = −2t3 + 3t2 H11 = t3 − t2 single-track simplification.
where p(t) are respectively X,Y global coordinates of the 
centerline of the road and s is the curvilinear coordinate 
 ṡ =[cos(θc )(Vx cos(ψ) − Vy sin(ψ)) + sin(θc )(Vx sin(ψ)+
0

(or operatively the traveled distance on the path calculated 


 + Vy cos(ψ))]/(1 − θcs y)
from an arbitrary initial point) as shown in fig.3. In order 


 ẏ =[− sin(θc )(Vx cos(ψ) − Vy sin(ψ)) + cos(θc )(Vx sin(ψ)+
to define each polynomial parameters, a set of boundary 

+ Vy cos(ψ))]


conditions (values and derivatives) in sk positions must be 



imposed. In that way is possible to define X(s), Y (s) as 

 ψ̇ =ω

continuous functions and with at least second order continuous 2Frl 2Ff c sin(δ) Faero
derivatives. The decisional algorithm designed requires also V̇x =ωVy + − −


 M M M
absolute heading angle of the road and its first derivative 
 2Frc 2Ff c cos(δ)
V̇y = − ωVx +

respect to s (curvilinear coordinate). Considering CHS road

 +


 M M
representation, these information can be numerically defined 


 ω̇ =2/J(−F rc lr + Ff c l f cos(δ))
by means of geometrical formulas in (3): 




 δ̇ =u1
Ys0 (s)

 τ̇ =u
θc (s) = arctan( ) 2
Xs0 (s) (5)
(3) 0
0 X 0 Y 00 − Ys0 Xs00 Where θc and θcs are absolute heading angle of road’s cen-
θcs (s) = s 02s terline and the derivative respect to curvilinear coordinate s
(Xs + Ys02 )3/2
 5

(defined by mean of a special formulation deeply described

f
V
̀

f
Vc
in next section).

x
V
State constrants
̀
byl ≤ y ≤ byh
y
V

f
Vl

fc
Y bδl ≤ δ ≤ bδh

F
bτ l ≤ τ ≤ bτ h
̀
y Inputs constrants (8)

fl
l
Fr

bδ̇l ≤ δ̇ ≤ bδ̇h
c
Fr

bτ̇ l ≤ τ̇ ≤ bτ̇ h
lr

s ̀c(s) Obstacles constrants

distreli ≥ 0

X Each variable is constrained within a feasible range bounded

by a set of lower and upper values (i.e on y variable byl and
Fig. 4. Vehicle model reference system
byh correspond to lower and upper maximum lateral width of
the lane respect to centerline). Constraints considered in the
proposed formulation act on lateral displacement of the vehicle
(maintain the lane), on maximum steering angle allowed,
(these values are considered known), M and J are total mass
maximum accelerating and braking torque, on steering angle
and inertia respect to z axis of the vehicle, lf and lr are relative
and torque ratio allowed and finally on relative distance of
distances between front and rear axles respect to CoG. in (5):
the vehicle respect to obstacles present on the lane. The set
• Faero represents aerodynamic resistance defined as func- of constraints presented are evaluated for each discretized
tion proportional to Vx2 ; time-step which compose the time horizon of the optimization
• Frl corresponds to longitudinal force applied to the problem.
vehicle assuming that only rear wheels are actuated. In Constraints are mathematically formulated as equality equa-
detail, vehicle powertrain is neglected and longitudinal tions by means of slack variables as in (9): each state and
force is simplified as Frl = τ /Rr , where Rr represents input variable is constrained by two equality equations (and
wheel radius; slack variables), while for each obstacle constraint equation is
• Ff c and Frc are lateral forces generated by tire-road defined by mean of the relative distance between vehicle and
interaction modeled by Pacejka’s non-linear numerical i-th obstacle. All constraints are evaluated as soft constraints
model (”Magic formula”), as reported in (6): in the cost function as shown in next section.
Z1 = y − byl
Z2 = −y + byh
(9)
F∗c (α∗ ) = −D sin(C arctan(Bα∗ +E(arctan(Bα∗ )−Bα∗ ))) ...
(6) ZObsi = distreli
where B, C, D, E are Pacejka model parameters and α∗ is slip
angle of the wheel. Front and rear slip angles are computed
by (7), where the additional term  is defined to numerically D. Obstacles’ constraints
prevent indetermination due to stationary vehicle condition While state and inputs constraints are directly defined
(Vx = 0) and it’s mathematically implemented as  = 0 e−Vx . on x, u of the problem as reported in (8), obstacles con-
straints require the evaluation of relative distance between
(αf ) =(Vy + lf ω)/(Vx + ) − δ vehicle and obstacles (distreli ). That value depends by self-
(7) localization of the vehicle (x) and obstacle’s position (xObs =
(αr ) =(Vy − lr ω)/(Vx + )
[sObs , yObs , θObs ]T ) on the curvilinear reference system eval-
uated for each time step considered distreli (x, u, xObs ).
Vertical load Fz is considered as a constant value defined Considering both measurements (x, xObs ) as deterministically
by an equal distribution of vehicle’s mass on each wheel as known, obstacles and vehicle are simplified as ellipses, where
Fz = M g/4. vehicle dimensions are known, while obstacles’ dimensions
are assumed as available (i.e. estimated). In order to reduce
computational effort, relative distance calculation is performed
C. Constraints in curvilinear reference system as shown in fig. 5.
In order to evaluate the relative distance distreli , an approx-
The optimization problem presents a set of inequality con- imate strategy is implemented and shown in fig 6:
straints (8). In detail, constraints are implemented on state • a straight line connecting centers of the two ellipses is
variables, inputs and relative position respect to obstacles defined;
 6

Y cost terms associated to state variables deviation respect to

the reference and their relative importance is defined by
means of a set of weights represented by matrix W; LUk are
s
X cost terms associated to optimization variables magnitude and
weighted by means of matrix S; finally Lcnstrk are penalty
functions associated to the constraints dependent by weighting
y coefficients P and slack variables Z (in the form defined in
̀-̀c(s1) the previous section).
y1 -̀c(s2) More in detail, Lstatek (shown in (11)) considers several
s aspects of the maneuver:
s1 2 • deviation respect to reference maneuver with terms: y
lateral displacement respect to centerline and Vx −Vx ref
Fig. 5. Vehicle-Obstacle projection from global to curvilinear reference
system.
speed error respect to reference;
• comfort-like aspect of the performed maneuver with
terms: Vy lateral speed in body frame reference system,
• a finite family of parallel lines (i.e. five in fig 6) is ω yaw rate, δ steering angle and τ applied torque.
generated. Line relative distances are selected in order Lstatek =w2 y 2 + w4 (Vx − Vx ref )2 + w5 Vy2 + w6 ω 2 +
to pass through 1/3 and 2/3 of semi-minor axis (or + w7 δ 2 + w8 τ 2
semi-major depending on the relative position of the two (11)
bodies); LUk (shown in (12)) considers the terms related to inputs
• intersections between ellipses and lines are computed; (the derivatives of δ and τ ) that directly affects comfort of the
• relative distances between intersection nodes are calcu- maneuver (their value affect the jerk of the maneuver).
lated;
LUk = s1 u1 2 + s2 u2 2 (12)
• shortest distance is chosen as distreli value.
All terms present in Lstatek and LUk are defined by means of
y weighted quadratic terms.
Lcnstrk terms are penalty function terms associated to the
constraints. As already stated, all constraints are evaluated
distreli
as soft constraints. In detail in (13), all equality constraints
associated to state and inputs variables, are introduced in
s
the cost function by means of exponential functions. This
mathematical formulation is implemented in order to obtain
negligible values when constraints are satisfied, while high
Fig. 6. Relative distance calculation procedure: blue lines represent straight
parallel lines generated, while yellow line is the shortest distance chosen as values (and with a high gradient) when close to constraint
distreli . border or breaking it.

For every time step obstacles’ position is required. This Lcnstrk =e(1−pyl Z1 ) + e(1−pyh Z2 ) + e(1−pδl Z3 ) + e(1−pδh Z4 ) +
information is measured just at first time-step and estimated in + e(1−pτ l Z5 ) + e(1−pτ h Z6 ) + e(1−pu1 l Z7 ) + e(1−pu1 h Z8 ) +
the rest of predictive horizon. Assuming that actual obstacles’ + e(1−pu2 l Z9 ) + e(1−pu2 h Z1 0) + LObs
speed is provided, obstacles’ position in time-steps subsequent (13)
the first is simply obtained by assuming them moving at In order to obtain the desired behavior, parameters of weight-
constant speed. This assumption is made considering that ing matrix P are accordingly tuned. In (14) penalty term
working frequency of the algorithm is high enough (so also related to obstacle is fully described. It differs from other
obstacles’ measures) and predictive horizon of the logic is penalty functions because it’s defined as a reciprocal function.
limited (less than 3 seconds). nObs
X pObs
LObs = (14)
E. Cost function i=1
ZObsi + Obs
The total cost function of the optimization problem (Ltot ) In order to avoid division by zero when relative distance is
is composed by several terms as presented in (10): equal to zero (ZObsi = 0), an additional parameter (close to
N −1 zero) (Obs ) is considered. Moreover this term is composed
1 X by contribution of relative distance evaluation respect to all
Ltot =LN + {Lstatek (W, x)}dt+
2 obstacles present on the road. This mathematical formulation
k=0
N −1 N −1 guarantees negligible values when far from zero relative dis-
1 X X
+ {LUk (S, u)}dt + {Lcnstrk (P, Z)}dt tance, while extremely high values when close to zero (that is
2 minimum value possible for ZObsi ).
k=0 k=0
(10) Finally the terminal cost LN is presented in (15).
In detail LN is the terminal cost evaluated in the last dis- 1
cretized time-step of optimization horizon; Lstatek are the LN = {LstateN (W, x)}dt + {LcnstrN (P, Z)}dt (15)
2
 7

IV. G ENETIC A LGORITHM IMPLEMENTATION This formulation reduces the independent variables that can
Genetic Algorithm (GA) can be classified as an adaptive freely vary from 2N to 4 with a dramatic reduction of
heuristic search technique loosely inspired by the evolution- computational complexity of the optimization problem. More-
ary ideas of natural selection and genetics. They can be over a continuous input sequence (linear dependency) force
considered a smart exploitation of a random search applied the dynamics to have a continuous jerk trend (with benefits
on optimization problems [28]–[30]. Although a randomized in term of comfort). On the other hand, this assumption
mechanism, GAs uses ”evolutionary information” to direct the intrinsically limit the system to sub-optimal solution. However
search to the optimal solution contained within the search high working frequency of the controller and a reduced length
space. GAs family of search algorithms are designed to of optimization horizon limits the sub-optimality. Another
simulate the natural evolutionary process through steps similar implementing decision is the continuous value encoding of
to the principles introduced by the scientist Charles Darwin. the variables (instead of the most commonly used binary
GA presents several advantages over classical optimization encoding). This choice is done in order to eliminate the
techniques as to be applicable to discontinuous, non-linear discretization effect due to the ”number of bits” of encoding
and not differentiable cost functions (its implementation does process and to include zero value within the search space.
not involve differential calculation or gradient evaluation). A The iterative process is stopped after a fixed number of
classical implementation of GA technique (high order popu- ”Generations” Ngen : the result of the process are the coeffi-
lation and unbounded number of generations stopped when a cients of the ”best” solution candidate. The sub-optimal control
condition has been reached) does not fit with fast calculation sequence resulting is computed as (18):
requirements, but on the other hand the iterative optimization a˜1 ∗ i + b˜1
 
through an evolutionary process allow to stop the process when ŨT = i = 0, . . . , N − 1 (18)
a˜2 ∗ i + b˜2
the maximum allowed calculation time is reached obtaining
a feasible suboptimal solution. Starting from a general GA B. Initial population
architecture as shown in the scheme of fig. 7 and based on
classical GA schemes reoprted in [31], the specific formulation The design of the initial population is an important aspect
of each task is described in detail in the following. that highly affects the possibility to find a feasible solution and
the convergence speed to find sub-optimal solutions close to
the best possible. The implementation proposed is composed
by three sub-sequential phases:
Initial Population (special design)
a) Previous solution: the first candidate solution is de-
fined as equal to the sub-optimal solution calculated at the
Fitness calculation Uopt previous iteration of the algorithm;
Next gen
b) Variational solutions: starting from ”previous solu-
tion” defined as in (17), a set of ”variational solutions” is
New Population
obtained considering small variations for the zero-order terms
Selection (roulette)
(b1 , b2 ). The variations considered are [−var, 0, +var], where
Crossover var is relative variation respect to the nominal value (fig. 8).
”Variational solutions” are obtained considering all possible
Mutation

+var
b1 b2
-var
+var

Fig. 7. General GA architecture -var

N N
Fig. 8. Variational solutions
A. Mathematical formulation of the problem
Each candidate solution U of the optimization problem combinations. In the specific implementation 32 = 9;
is defined as a sequence of N discretized values of input c) Random candidate solutions: The rest of candidate
variables (u1 ,u2 ) as in (16). solutions composing the initial population are calculated by
 1 randomly define set of values inside a feasible search space
u1 u21 . . . uN

T 1
U = (16) defined by boundaries as in (19).
u12 u22 . . . uN
2
b• = k ∗ limu• • ∈ (1, 2) k ∈ [1; 1]
GA implementation designed, is based on the assumption of a (19)
linear trend of each actuation variable over the horizon (time a• = k ∗ limu• − b• /(N dt) k ∈ [1; 1]
continuity of actuation commands). This allows to define the where limu• is a limit value for input variables: it can be
candidate solution as in (17). coincident or more conservative respect to constraint value
defined in section III-C and N dt corresponds to total time
 
a1 ∗ i + b1
UT = i = 1, . . . , N (17) horizon of the optimization.
a2 ∗ i + b2
 8

C. Fitness calculation been better than the new ones just generated). This drawback
Fitness calculation is defined by mean of cost function can slow down convergence to expected sub-optimal solution.
evaluation over the horizon for each solution candidate in This reason crossover algorithm designed also implements an
actual population. In detail, fitness calculation is defined as the ”elitism” mechanism. Elitism technique consists in maintain-
reciprocal of cost function Ltot because minimization of cost ing the best candidate solution (or a few of them) into the
function correspond to maximization of fitness performance new population. In detail crossover algorithm implemented is
(as typically is implemented in GA). composed by three phases (similar to the ones presented in
”Initial population” sect. IV-B).
1
f itnessi = (20) a) Elitism: the first candidate solution of the new popu-
Ltot (Ui ) lation is defined as equal to the best candidate solution present
Fitness calculation allows to rank each candidate solution in in actual population (best fitness value).
term of ”adaptation” and to define the global fitness of the b) Variational solutions: as already described in sect.
population f itnesstot in (21). IV-B, a set of ”variational solutions” around the best candidate
Npop solution is computed considering the 9 possible combinations.
f itnesstot =
X
f itnessi (21) c) Continuous crossover: the rest of new population
i=1
is calculated by a linear combination of parents solution
candidates. The linear combination is computed by a weighted
D. Selection mean where the weight coefficient is randomly defined as
∈ [0; 1]. The process presented in (23) for a generic variable
Selection process has the main purpose to select the best x is repeated for each optimization variable (a1 , b1 , a2 , b2 ).
candidates among population to be the ”genetic material”
used to generate a new population. The selection is usually xnew1 = (αx )xold1 + (1 − αx )xold2
(23)
driven by fitness-ranking evaluation by means of a specific xnew2 = (1 − αx )xold1 + (αx )xold2
mathematical method. There exist many different approaches
where xnew1 , xnew2 are the new variable values, xold1 , xold2
in literature as i.e. Boltzman selection, rank selection, steady
are ”parents” values of the variable and finally αx is the weight
state selection, . . . ??
coefficient associated.
The proposed implementation of the decisional algorithm
designed is based on ”Roulette Wheel Selection”. The math-
ematical principle can be easily explained by roulette wheel F. Mutation
analogy: all solution candidates of population have a ”slice” Mutation is implemented as a random change of an opti-
of the wheel, whose width is depending by its relative fitness mization variable after crossover task is completed. This task
respect to the total as in (22): allows to better explore the entire search space, but at the same
f itnessi time a too high mutation rate may reduce the convergence
width = (22) process to optimality in favor of a completely random search.
f itnesstot
As in Crossover task, elitism is implemented: first candidate
Defined a cumulative function of slice widths, using a ran- solution of the generated population from crossover task
domly selected value ∈ [0; 1], it’s possible to associate a remains equal to the best candidate solution present in actual
solution candidate from actual population. The process, de- population. All other solution candidate optimization variables
scribed by means of an example in fig. 9, is iterated until are evaluated and modified by (24).
Npop candidates are selected.
if (βx ≥ βth )
xnew = (1 + αmut )xold
1

candidate 1 else (24)

0,75

candidate 2 xnew = xold

candidate 4
0,5
end
candidate 3
0,25
Mutation is applied if a random parameter βx is greater
0 than a threshold βth . Mutation is computed by means of a
candidate candidate candidate candidate
1 2 3 4 multiplication (random) factor αmut selected within a limited
mutation range.
Fig. 9. Roulette Wheel Selection example After mutation stage a new population is finally generated
and represents initial population of a new ”generation”.

E. Crossover V. S IMULATION RESULTS

Crossover (and also mutation) generates a completely new A. Testing Area and Map generation
population considering as ”parents” the solution candidates In order to simulate the proposed algorithm in a realistic
chosen by ”Selection” process. This approach eliminates good scenario that can represent typical urban traffic conditions,
solution candidates from actual population (that could have experimental campaigns have been conducted. The testing
 9

As suggested in [27], positions are used as locations of

control points of spline in Hermite form. Tangent vectors
(vi ), instead, are computed by means of a minimization
of error between generated curve and reference measured
trajectory defined (considering Hausdorff distance calculation
as accuracy index) as in eq.25.
Np̂
X
min kfCHS (uk |S0 ) − pˆk k2 (25)
vi ,...,vN
k=1

where fCHS (uk |S0 ) represents the CHS curve parameterized

by S0 {(ui p̄i vi )|i ∈ [1, Np̄ ]} where pi are control points
locations, ui are knots of CHS, vi are the tangent vectors to
be determined and finally Np̂ and Np̄ are total collected and
resampled points respectively.

×106

Fig. 10. Mechanical Engineering Department area Y UTM

5.0387

area selected, is the area surrounding Mechanical Engineering 5.0386

Department of Politecnico di Milano as shown in fig.10. GPS Data

resampled data
Thanks to SINECO S.p.A support [32], an experimental approximated curve
5.0386
campaign was conducted in order to obtain accurate mea-
surements of GPS track of road centerline. The experimental
vehicle (shown in fig. 11) is equipped with RIEGL VMX- 5.0386

450 measurement system. This system is composed by laser

scanners, IMU/GNSS equipment, and cameras. IMU/GNSS 5.0386
system is capable of position measurements at 200 Hz, with
an extremely high accuracy (measurement uncertainty about
2cm). Moreover laser scanners and cameras can provide ac- 5.0386

curate 3D point clouds and images of the surrounding area but 5.1228 5.123 5.1232 5.1234 5.1236 5.1238 5.124
×105
are not of interest for th experiment. The experimental cam- X
Fig. 12. Simplified Mechanical Engineering Department map, where blue
dots are raw GPS data, red dots are resampled GPS points and green line is
the approximate resulting curve

Applying the simplification method described, it’s possible

to obtain the map shown in fig. 12. In detail blue and red dots
represent raw and resampled GPS data, while the green line
is the resulting CHS curve. In particular the simplified CHS
curve presents a Hausdorff maximum distance of about 0.03m
respect to the original raw GPS collected data (considered as
Fig. 11. SINECO S.p.A experimental vehicle equipped with RIEGL VMX- the ”correct” centerline curve). The mapped area consists in a
450 measurement system
two lanes urban road (used as single lane in out tests). Lane
width is fixed in 2.75m, so total road width is around 5.5m.
paign was performed by manually driving the instrumented
All simulations proposed in the following section consider this
vehicle in the center of the road in order to obtain a GPS
value as constant road width inside problem constraints and
sequence of the centerline of the street. The mapped area
vehicle reference is set to be the center of the road itself.
presents four straights and four sharp curves. The total distance
of an entire lap is around 450m and total GPS points collected
(representing road centerline) are about 12000. B. Numerical simulations
In order to define a CHS structure suitable for the decisional The numerical simulations presented in the following are
algorithm starting from GPS row data collected, lane geometry obtained by the closed-loop system architecture defined in
approximation is performed as presented in [27] by the author. fig.13. A numerical function (”Generated Obstacles”) respon-
The points considered in CHS interpolation are selected by sible of generation of sensors-like data regarding obstacles
resampling GPS data collected considering a fixed traveled (global position sObs ,yObs ,θObs and speed VsObs ,VyObs in
distance between subsequent points. ds is set to 4m that curvilinear reference frame) is defined. Within the designed
generates a reduction of overall considered points to just 114. replanner, these data are used to estimate obstacles’ motion
 10

over time horizon of the controller by means of a simple

linear extrapolation (”estimation”) as proposed in III-D. The
resulting physical actuation controls (δ, τ ) are set as inputs of
”Vehicle’s Dynamic”. It represents simulated vehicle behavior
and it’s defined by the same vehicle’s model presented in
(5) and used in the decisional algorithm. ”MPC Replanner”

state + U
Generated
Obstacles

MPC Replanner δ,τ Vehicle s Dynamic

estimation

Fig. 13. Simulation system architecture

(comprehensive of GA Replanner + obstacle’s motion estima-

tion) works at 20Hz while ”Vehicle’s Dynamic” is internally
Fig. 15. Prototype vehicle under development [33]
evaluated with a discretization time step of 0.01s.
An optimization time horizon of (N = 20) steps with a time
discretization of ∆t = 0.12s but state evolution is computed under development [33](fig.15). Higher speed is defined as a
with a discretization of ∆tstate = 0.06s is considered. Vehicle realistic value for the area selected (urban area among building
dimensions are assumed as 4m (length) and 1.9m (width), in a university campus). In fig.16 a top view of the numerical
similar to subcompact car segment.
The algorithm formulation is maintained fixed (in term
×106
5.0387
of cost function weights and GA parameters) for all the
simulation proposed in the following. Thanks to its numerical
5.0387
structure based on a fixed number of iterations, the compu-
tational time required is almost fixed. The testing machine 5.0386
considered is a laptop with in Intel-i7 CPU and 4 GB of RAM.
Y (UTM) [m]

The software is developed on MATLAB environment using 5.0386

Microsoft Windows OS.

5.0386
Fig. 14 shown the algorithm working frequency during a
generic simulation: the working frequency due to compu- 5.0386
tational effort required is always higher 20Hz confirming
that it’s possible to apply it in a real-time application. The 5.0386
computational effort is not scenario dependent.
5.0385
35
5.1226 5.1228 5.123 5.1232 5.1234 5.1236 5.1238 5.124 5.1242
X (UTM) [m] ×105

30 Fig. 16. Complete lap simulated: where red line represents the computed
trajectory,blue lines represent lane limits and colored areas represents turns
Frequency Hz

25
simulation performed at 4m/s is shown. Cornering areas are
Working frequency the most challenging part of the scenario and are highlighted
20
by colored circles in order to identify them in the following
Controller frequency graphs reported.
In fig. 17 evolution of the simulation parameters of interest
15
0 200 400 600
iterations [-]
800 1000 1200
are reported. In the order, rows represent: lateral displacement
respect to road centerline y, longitudinal speed in vehicle’s
Fig. 14. Algorithm frequency: blue dots represent algorithm estimated
maximum working frequency at each time step, while red line is working
reference frame Vx and finally physical actuation commands
frequency adopted for the controller (steering angle δ and applied torque τ ). In detail blue lines
represent slower speed simulation, while red higher speed one.
1) Complete lap: The first simulations presented consist in To clearly compare the simulations, all parameters are reported
a full lap of the testing scenario. respect to traveled distance s used as x-axis. Moreover turns
The simulations are performed by defining vehicle reference are highlighted with the same colors used in fig. 16. Both cases
speed equal to 4m/s and 8m/s respectively. Lower speed are correctly handled by the controller: lateral displacement
(4m/s) is proposed in order to compare simulation results to remains below boundary limits imposed to vehicle’s CoG
future experimental tests scheduled using a prototype vehicle (1.75m). As expected, high speed maneuver generates an
 11

y [m] 1 ×106
0
-1 5.0386
0 50 100 150 200 250 300 350 400 450
s [m]
5.0386
V x [m/s]

0 5.0386
0 50 100 150 200 s [m] 250 300 350 400 450

Y (UTM) [m]
10
0 5.0386
δ [°]

-10
-20
-30
0 50 100 150 200 s [m] 250 300 350 400 450 5.0386
τ [Nm]

200
0 5.0386
-200
0 50 100 150 200 250 300 350 400 450
s [m] 5.0386

Fig. 17. Complete lap simulation results, where red and blue lines represent 5.0386
time evolution of relevant parameters for high and low reference speed 5.1228 5.1228 5.1229 5.1229 5.1230 5.123 5.1231 5.1231
respectively X (UTM) [m] ×105
(a) global reference

higher lateral displacement due to lateral speed cost term

Vy and inputs rate terms δ̇, τ̇ . On the other hand an higher
speed increases the corresponding spatial predictive horizon
of the controller: this is reflected by anticipated maneuvers
approaching turns. In detail steering maneuvering anticipate
the turn by generating a trajectory that ”cuts the corner”
maintaining an average higher speed and reducing steering
rate. This effect is also visible from fig.18a. Both figures in
18 show the turning approach from a time point of view:
colored lines represent the optimal trajectories computed by
the planner at each time-step, while red dots represent the
closed loop overall trajectory traveled by the vehicle. In detail,
fig. 18a is defined in the global coordinate system and shows
the ”cut the corner” behavior, while fig.18b is represented in
curvilinear reference as internally used by the controller to
compute the optimization. In both figures is possible to notice
(b) curvilinear reference
how at each time-step a new solution of the problem generates
Fig. 18. Optimized trajectories at turn 1: lines represent optimal trajectories
a slightly different solution: this is mainly due to the shift of computed at each time-step, while red dots represent the closed loop overall
the predictive horizon. trajectory traveled by the vehicle
2) modeling error response: in order to evaluate control
behavior in presence of modeling errors, in this section, tyre
to road adhesion coefficient of vehicle model used inside = 5.3m).
decisional algorithm and the one used in ”Vehicle’s Dynamic” Results are shown in fig.20, where time evolution of lateral
of the scheme shown in fig.13 are different on purpose. This displacement, longitudinal speed, steering angle and applied
mismatch it’s a common but dangerous situation in particular torque. In detail, fig.20a represents the first turn. Simulation
during raining or snowing road conditions. The common at µ = 0.2 (snow) presents a loose of vehicle’s stability as
approach in ADAS algorithms is to define a conservative (or well shown by lateral displacement though opposite steering
estimated) constant value for it. This can lead to a mismatch command applied, while heavy rain condition (µ = 0.3) cause
between real and expected behavior of the vehicle. a violation of road borders constraints. Sharp turn in fig.20b
In detail a common and conservative value of friction confirmed the bad behavior shown in turn 1 empathizing
coefficient for dry asphalt surface defined as µ = 0.7 is constraints violation in heavy rain scenario as well shown
internally set. Tyre to road adhesion coefficient of simulated by lateral displacement and an uncomfortable variation of
vehicle (”vehicle’s dynamic”) instead is set to values varying applied torque. The decisional algorithm behave well in the
in a range defined as µ ∈ [0.2, 0.7]. The simulations are range µ ∈ [0.4, 0.7] showing a good rejection to parameter
performed imposing a reference speed equal to 8m/s. In order estimation errors.
to clearly present a comparison of the different simulation 3) Stationary vehicle: The simulations proposed in this
results, only limited areas of the scenario are shown. section involve a dangerous urban situation where a vehicle
First and second turns are chosen as most significant areas is parked on the right side of the road (i.e. picking up a
considering the graph in fig. 19 representing curvature of the pedestrian or waiting for a parking spot). In fig. 21 the scenario
track : turn 1 (green) is the smoothest one (curvature radius = considered is shown. Obstacle vehicle is simplified by an
9.2m) while turn 2 (red) is the sharpest one (curvature radius ellipse whose minor and major semi-axis are 1m and 2m
 12

×106
0.05 5.0386

0 5.0386

5.0386
curvature [m -1]

Y (UTM) [m]
-0.05

5.0386
-0.1
x
y
9.2 m 5.0386 50
-0.15

5.3 m 5.0386

-0.2
0 50 100 150 200 250 300 350 400 450 500
s [m] 5.1235 5.1236 5.1237 5.1238 5.1239 5.124 5.1241 5.1242
X (UTM) [m] ×105
Fig. 19. Curvature values of testing scenario’s turns
Fig. 21. Stationary vehicle scenario: red ellipse is a parked vehicle on the
right side of the road, while blue ellipse is controlled vehicle at the beginning
of the simulation

2
respectively and it’s lateral position is −1.3m respect to the
y [m]

1 CoG Limit

0 centerline of the road. The scenario presented is investigated

-1
100 105 110 115 120 125 130
by considering four increasing reference speeds. In detail
8
Vx ref is set to [4, 8, 12, 18] m/s.
Vx [m/s]

6
Fig.22 presents an extensive comparison between simulation
4

2
results. In particular lateral displacement y satisfy the limits
100
0
105 110 115 120 125 130
in all cases. In detail, as noticed also in section V-B1, longer
spatial predictive horizon and Vy , δ̇, τ̇ cost terms generate
[°]

-20
a smoother evasive maneuver for high speed simulations as
-40
100 105 110 115 120 125 130 shown in fig.22a. Fig.22b also considers the subsequent right
0
turn. Even if all simulation succeed, high speed simulations
[Nm]

-500
0.2 0.3 0.4 0.5 0.6 0.7 (12, 18) require a sudden speed reduction approaching the
-1000
100 105 110 115 120 125 130
corner as expected.
s [m]
4) Multiple moving obstacles: Last simulation proposed in-
(a) turn 1 volves a complex urban traffic scenario with multiple obstacles
3 moving on the road. As shown in fig. 23, vehicle A is driving
2 on right side (−1.3m) at 3m/s, vehicle B is driving on left
y [m]

1 CoG Limit side (1.3m) at 2m/s and finally vehicle C is driving on right
0

-1
side (−1.3m) at 3m/s. The simulation is performed imposing
175 180 185 190 195 200 205 210 215 a reference speed equal to 8m/s. The simulation is presented
Vx [m/s]

8
by means of a sequence of figures representing different
6
time frames (shown in figs. ??) in order to clarify controller
4
behavior. In all figures blue ellipse represents controlled
175 180 185 190 195 200 205 210 215
vehicle, green line is the optimal trajectory planned by the
10
0 controller over the predictive horizon considered and red line
[°]

-10
-20
is the closed-loop trajectory followed by the vehicle during the
0.3 0.4 0.5 0.6 0.7
simulation. Red ellipses represent obstacles and the black lines
-40
connected to them are expected obstacle motion of obstacles
175 180 185 190 195 200 205 210 215
200 estimated by means of linear extrapolation as explained in
[Nm]

0
section V-B. it’s assumed that controlled vehicle is allowed to
-200
-400 overtake regardless its position on the road (even during a turn)
-600
175 180 185 190 195 200 205 210 215
or according to road rules (overtaking on left/right). This is
s [m] done as a stress test of the algorithm: in a rel driving situation
(b) turn 2 higher driving tasks in the hierarchical scheme in fig.2, would
Fig. 20. Simulation using different adherence coefficients (reference speed prevent these situations. Moreover obstacle’s actual state are
equal to 8m/s) considered always known (regardless if they are visible or
not by on-board sensors). In fig. 24, controlled vehicle is
 13

×106

2
5.0386
y [m]
0 5.0386 A
CoG Limit
5.0386
-2
10 20 30 40 50 60 70 80 90

Y (UTM) [m]
Vx [m/s]

20 5.0386

15
10 5.0386

5
10 20 30 40 50 60 70 80 90 5.0386

2
[°]

0 5.0386

-2
-4 5.1235 5.1235 5.1236 5.1236 5.1236 5.1237
X (UTM) [m] ×105
10 20 30 40 50 60 70 80 90
200
[Nm]

0 Fig. 24. Moving obstacles’s scenario - frame 1: overtaking maneuver in

-200
Vx 4 Vx 8 Vx 12 Vx 18 progress. Green line represents planned trajectory over the predictive horizon,
-400 red line is the closed-loop trajectory performed and the black lines are
-600
10 20 30 40 50 60 70 80 90
estimated obstacles’ motion (reference speed equal to 8m/s)
s [m]
(a) Fixed obstacle

2
overtaking vehicle A. When approaching the first corner, as
1
y [m]

0 ×106
5.0386

-1 CoG Limit
-2 5.0386

20 30 40 50 60 70 80 90 100 110 120 130

2
V [m/s]

5.0386
C
0
x

Y (UTM) [m]

-2 5.0386

20 30 40 50 60 70 80 90 100 110 120 130

-2
5.0386

2
[°]

0
5.0386
-2 B
20 30 40 50 60 70 80 90 100 110 120 130

5.1227 5.1228 5.1229 5.123 5.1231 5.1232

2 X (UTM) [m] ×105
[Nm]

0
Vx 4 Vx 8 Vx 12 Vx 18 Fig. 25. Moving obstacles’s scenario - frame 2:Overtaking and turning
-2

20 30 40 50 60 70 80 90 100 110 120 130

s [m]
shown in fig.25, a cornering maneuver is defined considering
(b) Fixed obstacle + turn 1 (and correctly avoiding) vehicle B that is cornering too on
Fig. 22. Fixed obstacle avoidance + turn 1 at different speed: a comparison left side of the road. Concluded that maneuver, a slow vehicle
between simulation results obtained by different reference velocities of the (C) suddenly appears in the predictive horizon of the replanner
vehicle approaching a static obstacle and a sharp turn
forcing it to slow down and plan another overtaking maneuver
as visible in fig.26. Finally the resulting overtaking maneuver
×106

5.0386
×106
5.0386

5.0386
5.0386

5.0386
5.0386

5.0386
Y (UTM) [m]

5.0386
C
Y (UTM) [m]

5.0386 A 5.0386

100
5.0386 C
30 5.0386

85
B
5.0386 5.0386

5.0385
5.0386 B
5.1227 5.1228 5.1229 5.123 5.1231 5.1232 5.1233
X (UTM) [m] ×105

5.0385

5.123 5.1232 5.1234 5.1236 5.1238 5.124 Fig. 26. Moving obstacles’s scenario - frame 3: slow down and additional
X (UTM) [m] ×105 evasive maneuver planning.

Fig. 23. Multiple moving obstacles’ scenario: where the obstacles considered
are vehicle A (driving on right side), vehicle B (driving on left side) and is shown in fig.27 where predictive horizon spatial length
vehicle C (driving on right side)
implies that vehicle is expected to speed up.
 14

×106

search of a solution to a suboptimal subset (due to hypothesis

5.0386 on control variables as linear equation). This assumption is
considered reasonable thanks to the short predictive horizon
5.0386
and the short sampling time of the controller.
Numerical simulations of a typical urban area (made of
Y (UTM) [m]

5.0386
C sharp curves and narrow lanes) are shown to test the algorithm
5.0386
Computational time measured during the simulations confirms
the feasibility of a real-time implementation of the controller
5.0386
for working frequency up to 20Hz, as well as sufficient
robustness respect to modeling errors and its capability to deal
5.1227 5.1227 5.1228 5.1228 5.1229 5.1229
X (UTM) [m]
5.1230 5.123 5.1231 5.1231
×105 with multiple and moving obstacles in urban-like scenarios is
shown.
Fig. 27. Moving obstacles’s scenario - frame 4: overtaking and speed increase

ACKNOWLEDGMENT
VI. C ONCLUSION The authors would like to thank Sineco s.p.a for the
possibility to use an instrumented vehicle for high accuracy
In this paper a NMPC trajectory planner for autonomous measuments. The Italian Ministry of Education, University and
vehicles based on a direct approach has been presented. Research is acknowledged for the support provided through the
Thanks to the proposed modified slip calculation, the dynamic Project ”Department of Excellence LIS4.0 - Lightweight and
single-track vehicle model can be employed at both high Smart Structures for Industry 4.0”.
and low velocities. This allows to have one single model
that presents a kinematic-like behaviour at low velocities R EFERENCES
while keeping the standard dynamic single-track vehicle model
[1] “Global Status Report On Road Safety 2018,” World Health Organiza-
behaviour at higher speeds. Obstacles’ uncertainties have been tion (WHO), 2018.
taken into account through the implementation of coupled soft [2] N. Lang, M. Rugmann, A. Mei-Pochtler, T. Dauner, S. Komiya, X. Mos-
and hard constraints. The correlation between the simulated quet, and X. Doubara, “Self-Driving Vehicles, Robo-Taxis, and the
Urban Mobility Revolution,” Boston Consulting Group, july 2016.
and perceived spatial horizon lengths has been enforced with [3] X. Mosquet, T. Dauner, N. Lang, M. Rusmann, A. Mei-Pochtler,
a spatial dependent velocity profile that has been imposed R. Agrawal, and F. Schmieg, “Revolution In The Driver’s Seat,” Boston
as an extra inequality constraint. The numerical solution is Consulting Group, april 2015.
[4] F. Kuhnert, C. Sturmer, and A. Koster, “Five trends transforming the
carried out using ACADO toolkit, coupled with the QP solver Automotive Industry,” PwC (PricewaterhouseCoopers), 2018.
qpOASES. The trajectory planner performances have been [5] J. Ziegler, P. Bender, M. Schreiber, H. Lategahn, T. Strauss, C. Stiller,
checked in simulation, applying the controller to a realistic T. Dang, U. Franke, N. Appenrodt, C. G. Keller, E. Kaus, R. G. Her-
rtwich, C. Rabe, D. Pfeiffer, F. Lindner, F. Stein, F. Erbs, M. Enzweiler,
nonlinear multibody model in CarMaker environment. Two C. Knoppel, J. Hipp, M. Haueis, M. Trepte, C. Brenk, A. Tamke,
significant driving scenarios have been reported assessing the M. Ghanaat, M. Braun, A. Joos, H. Fritz, H. Mock, M. Hein, and
trajectory planner performances in terms of feasibility of the E. Zeeb, “Making Bertha Drive - An Autonomous Journey on a Historic
Route,” IEEE Intelligent transportation systems magazine, 2014.
generated trajectories and passengers’ comfort. The analysis [6] A. Teichman and S. Thrun, “Practical object recognition in autonomous
of the computational cost has confirmed that the proposed driving and beyond,” Advanced Robotics and its Social Impacts (ARSO),
trajectory planner can be implemented in real-time. 2011.
[7] J. Funke, P. Theodosis, R. Hindiyeh, G. Stanek, K. Kritatakirana,
In this paper a novel decisional algorithm for trajectory C. Gerdes, D. Langer, M. Hernandez, Bernhard, Muller-Bessler, and
planning & tracking is proposed. In detail the algorithm B. Huhnke, “Up to the Limits: Autonomous Audi TTS,” in 2012
is expected to ensure: a real-time feasible implementation, Intelligent Vehicles Symposium, 2012.
[8] Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How,
a sufficient robustness of the solution provided respect to “Real-time Motion Planning with Applications to Autonomous Urban
modeling errors and being able to deal with multiple and Driving,” in IEEE transactions on control systems technology, vol. 17,
moving obstacles in urban-like scenarios. 2009.
[9] A. Broggi, P. Medici, P. Zani, A. Coati, and M. Panciroli, “Autonomous
The algorithm consists mainly in a optimal constrained vehicles control in the VisLab Intercontinental Autonomous Challenge,”
problem formulated as a MPC problem and numerically in Annual Reviews in Control 36, 2012.
solved by means of a novel genetic algorithm approach. [10] B. Paden, M. Cap, S. Z. Yong, D. Yershov, and E. Frazzoli, “A Survey
of Motion Planning and Control Techniques for Self-driving Urban
In particular, thanks to GA implementation for numerical Vehicles,” IEEE Transactions On Intelligent Vehicles, Vol. 1, No. 1, 2016.
optimization compared to other approaches in literature, the [11] R. Okuda, Y. Kajiwara, and K. Terashima, “A Survey of Technical Trend
proposed algorithm prevents the solution to get stuck in local of ADAS and Autonomous Driving,” VLSI Technology, Systems and
Application (VLSI-TSA), 2014.
minima and the specific mathematical formulation proposed [12] O. Souissi, R. Benatitallah, D. Duvivier, A. Artiba, N. Belanger, and
bound computational cost and generates continuous control P. Feyzeau, “Path Planning: A 2013 Survey,” 5th IESM Conference,
commands at the same time. Especially, the specific vehicle 2013.
[13] D. Hsu, J. Latombe, and R. Motwani, “Path planning in expansive
model adopted and the choice of optimization variables force configuration spaces,” in Proceedings of the 1997 IEEE International
the evasive maneuver calculated over the predictive horizon Conference on Robotics and Automation, Albuquerque, New Mexico,
of MPC to present continuous acceleration and jerk, while USA, April 20-25, 1997, 1997, pp. 2719–2726.
[14] S. M. LaValle, “Rapidly-exploring random trees: A new tool for path
trajectory computed by the closed loop presents continuous planning,” Computer Science Dept., Iowa State University, Tech. Rep.
acceleration in time. on the other hand, this approach limits the 98-11, 1998.
 15

[15] S. Karaman and E. Frazzoli, “Optimal kinodynamic motion planning

using incremental sampling-based methods,” in Proceedings of the 49th
IEEE Conference on Decision and Control, CDC 2010, December 15-
17, 2010, Atlanta, Georgia, USA, 2010, pp. 7681–7687.
[16] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile
robots,” IEEE International Conference on Robotics and Automation,
1985.
[17] P. Raja and S. Pugazhenthi, “Optimal path planning of mobile robots:
A review,” International journal of physical sciences, 2012.
[18] A. Liniger, A. Domahidi, and M. Morari, “Optimization-based au-
tonomous racing of 1: 43 scale rc cars,” Optimal Control Applications
and Methods, vol. 36, no. 5, pp. 628–647, 2015.
[19] J. V. Frasch, A. Gray, M. Zanon, H. J. Ferreau, S. Sager, F. Borrelli,
and M. Diehl, “An auto-generated nonlinear mpc algorithm for real-time
obstacle avoidance of ground vehicles,” in Control Conference (ECC),
2013 European. IEEE, 2013, pp. 4136–4141.
[20] Y. Gao, T. Lin, F. Borrelli, E. Tseng, and D. Hrovat, “Predictive
control of autonomous ground vehicles with obstacle avoidance on
slippery roads,” in ASME 2010 dynamic systems and control conference.
American Society of Mechanical Engineers, 2010, pp. 265–272.
[21] A. Gray, Y. Gao, T. Lin, J. K. Hedrick, H. E. Tseng, and F. Borrelli,
“Predictive control for agile semi-autonomous ground vehicles using
motion primitives,” in American Control Conference (ACC), 2012.
IEEE, 2012, pp. 4239–4244.
[22] 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,” in Proceedings of the 11th international symposium
on advanced vehicle control, 2012.
[23] B. Gutjahr, L. Gröll, and M. Werling, “Lateral vehicle trajectory opti-
mization using constrained linear time-varying mpc,” IEEE Transactions
on Intelligent Transportation Systems, vol. 18, no. 6, pp. 1586–1595,
2017.
[24] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic and
dynamic vehicle models for autonomous driving control design.” in
Intelligent Vehicles Symposium, 2015, pp. 1094–1099.
[25] M. Montemerlo, J. Becker, S. Bhat, H. Dahlkamp, D. Dolgov, S. Et-
tinger, D. Haehnel, T. Hilden, G. Hoffmann, B. Huhnke, D. Johnston,
S. Klumpp, D. Langer, A. Levandowski, J. Levinson, J. Marcil, D. Oren-
stein, J. Paefgen, I. Penny, A. Petrovskaya, M. Pflueger, G. Stanek,
D. Stavens, A. Vogt, and S. Thrun, “Junior: The Stanford entry in the
Urban Challenge,” Journal of field robotics, 2008.
[26] S. Kammel, J. Ziegler, B. Pitzer, M. Werling, T. Gindele, D. Jagzent,
J. Schoder, M. Thuy, M. Goebl, F. von Hundelshausen, O. Pink, C. Frese,
, and C. Stiller, “Team AnnieWAY s Autonomous System for the DARPA
Urban Challenge 2007,” Journal of field robotics, 2008.
[27] T. Zhang, S. Arrigoni, M. Garozzo, D. ge Yang, and F. Cheli, “A
lane-level road network model with global continuity,” Transportation
Research Part C 71, 32-50, 2016.
[28] X. Yang, “Engineering optimization: An introduction with metaheuristic
applications,” 2010.
[29] ——, “Nature-inspired metaheuristic algorithms,” Luniver Press, 2010.
[30] S. Asadi and Z. W. Geem, “Sustainable building design: A review on
recent metaheuristic methods,” Recent Advances in Swarm Intelligence
and Evolutionary Computation. Studies in Computational Intelligence,
vol 585. Springer, 2015.
[31] A. Eiben and J. E. Smith, Introduction to Evolutionary Computing.
Springer, 2015.
[32] Sineco-s.p.a, “website http://www.sinecospa.com/.”
[33] M. Bersani, S. Mentasti, P. Dahal, S. Arrigoni, M. Vignati, F. Cheli, and
M. Matteucci, “An integrated algorithm for ego-vehicle and obstacles
state estimation for autonomous driving,” Robotics and Autonomous
Systems, 2020.