MPC Path-Planner For Autonomous Driving
MPC Path-Planner For Autonomous Driving
MPC Path-Planner For Autonomous Driving
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 bedifficult 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
Obs motion
Road co nnections
Wold
Self-localization Environment
Self-loc alization
4D Environment
reconstruction General information (at different level of detail)
Raw data Measures
processing
Information required/useful
In detail, on the right (blue background) physical (or sim- correspond to a reaction time of 0.05s: this is limited mainly
ulated) system and environment are represented: it represents
by considering the working frequency of camera sensors
the ”controlled” vehicle surrounded by the scenario (roads,
(usually around 30 Hz to which should be added the delay
other vehicles or pedestrians, . . . ) and sensor setup.
due to image processing). Robustness is intended mainly as
On the left (green background) software algortihms are rep- the capability of the controller to always generate a feasible
resented. It can be mainly divided into three main sequential
result (even if sub-optimal), to be capable of correctly operate
tasks:
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
enriched by traffic and behavior laws, where obstacles In detail, fig. 2 represents the different information required
are detected, tracked and their time evolution is predicted at every decisional tasks: in this work, Mission planner is
(time dimension); not further investigated as well as Mode planning (a specific
reference trajectory as output of these two tasks is considered
. ”Decisional Algorithm” groups all decisional driving
known).
tasks. starting from environment reconstruction and driver
requests, it’s aim is to define and actuate a decisional ”Vehicle dynamic data” (model parameters), ”Self-
strategy. localization”, ”Road geometry information” and ”Obstacle’s
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. P ATH - 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
be defined as shown in 1.
of performing trajectory planning and vehicle control tasks
ensuring:
min Ltot (x (t ), u(t ))
. a real-time feasible implementation; u(t)
. a sufficient robustness of the solution calculated; (1)
s.t. ẋ(t) = f (x(t), u(t))
. the capability of correctly handle with multiple and
x(t 0 ) = x 0
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 ●
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 ~T *i
U = { } i = 0, . . . , N — 1 (18)
through an evolutionary process allow to stop the process when *i
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
The design of the initial population is an important aspect
classical GA schemes reoprted in [31], the specific formulation
that highly affects the possibility to find a feasible solution and
of each task is described in detail in the following.
the convergence speed to find sub-optimal solutions close to
the best possible. The implementation proposed is composed
by three sub-sequential phases:
a) Previous solution: the first candidate solution is de-
fined as equal to the sub-optimal solution calculated at the
previous iteration of the algorithm;
Fitness calculation Uopt
Next gen b) Variational solutions: starting from ”previous solu-
tion” defined as in (17), a set of ”variational solutions” is
obtained considering small variations for the zero-order terms
(b1 , b2 ). The variations considered are [—var, 0, +var], where
var is relative variation respect to the nominal value (fig. 8).
”Variational solutions” are obtained considering all possible
+var
b2
-var
+var
C. Fitness calculation been better than the new ones just generated). This drawback
can slow down convergence to expected sub-optimal solution.
Fitness calculation is defined by mean of cost function
This reason crossover algorithm designed also implements an
evaluation over the horizon for each solution candidate in
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 a) Elitism: the first candidate solution of the new popu-
fitne ss i = (20)
L tot (U i ) lationis 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 fitness tot in (21). IV-B, a set of ”variational solutions” around the best candidate
solution is computed considering the 9 possible combinations.
fitness to t = fitness i (21) c) Continuous crossover: the rest of new population
is calculated by a linear combination of parents solution
candidates. The linear combination is computed by a weighted
mean where the weight coefficient is randomly defined as
D. Selection
2 [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 x ne w 1 = (α x )x ol d 1 + (1 - α x )x ol d 2
(23)
driven by fitness-ranking evaluation by means of a specific x ne w 2 = (1 - α x )x ol d 1 + (α x )x ol d 2
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
fitn e s si time a too high mutation rate may reduce the convergence
width = (22) process to optimalityin 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 2 [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 ≥ β t h )
x ne w = (1 + α mu t )x ol d
1
candidate 2
candidate 4
end
0,5
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”.
i n If CH S (u k jS 0 ) - p^ k I 2 (25)
vi .
vN
×106
5.0387
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
state + U
estimation
30 Fig. 16. Complete lap simulated: where red line represents the computed
trajectory,blue lines represent lane limits and colored areas represents turns
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.
15
In fig. 17 evolution of the simulation parameters of interest
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
1 ×106
0
-1 5.0386
0 50 100 150 200 300 350 400 450
s [m] 250
5.0386
5
0 5.0386
0 50 100 150 200 s [m]250 300 350 400 450
10
0 5.0386
-10
-20
-30
0 50 100 150 200 s [m]250 300 350 400 450 5.0386
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
6
×10
0.05 5.0386
0 5.0386
5.0386
-0.05
5.0386
-0.1
x
y
9.2 m 5.0386
-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. 2 1. Stationary vehicle scenario: re e ipse is a parked vehicle on h e
right s ide of the roa d while blue ellip se i c ntro lled vehicle at the beginn n g
of the simulation
1 CoG Limit
respectively and it’s lateral position is -1.3m respect to the
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.
6
4
Fig.22 presents an extensive comparison between simulation
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
-20
spatial predictive horizon and Vy , δ̇ , τ̇ cost terms generate
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
turn. Even if all simulation succeed, high speed simulations
0
corner as expected.
-1000
0
CoG Limit side (1.3m) at 2m/s and finally vehicle C is driving on right
-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
8
by means of a sequence of figures representing different
time frames (shown in figs. ??) in order to clarify controller
6
4
behavior. In all figures blue ellipse represents controlled
10
175 180 185 190 195 200 205 210 215
vehicle, green line is the optimal trajectory planned by the
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
-200
0
section V-B. it’s assumed that controlled vehicle is allowed to
-400 overtake regardless its position on the road (even during a turn)
-600
or according to road rules (overtaking on left/right). This is
175 180 185 190 195 200 205 210 215
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
0 5.0386 A
CoG Limit
-2 5.0386
10 20 30 40 50 60 70 80 90
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
200
0
Fig. 24. Moving obstacles’s scenario - frame 1: overtaking maneuver in
-200
V 4 V 8 V 12 V 18 progress. Green line represents planned trajectory over the predictive horizon,
x x x
-400 x
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
0 ×106
5.0386
-1
-2
5.0386
2 5.0386
C
0
-2
5.0386
-2
5.0386
2
0
5.0386
-2 B
20 30 40 50 60 70 80 90 100 110 120 130
0
V 4 V 8 V 12 V 18 Fig. 25. Moving obstacles’s scenario - frame 2:Overtaking and turning
-2 x x x x
5.0386
×106
5.0386
5.0386
5.0386
5.0386
5.0386
5.0386
C
5.0386
5.0386
5.0386
5.0386 5.0386
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] ×10 5
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] 5
×10
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
5.0386
5.0386
5.0386
5.0386
5.0386
×106
5.1227 5.1227 5.1228 5.1228 5.1229 5.1229 5.1230 5.123 5.1231 5.1231
X (UTM) [m] ×10 5
Fig. 27. Moving obstacles’s scenario - frame 4: overtaking and speed increase
VI. C ONCLUSION
In this paper a NMPC trajectory planner for autonomous vehicles based on a direct approach has been presented.
Thanks to the proposed modified slip calculation, the dynamic single-track vehicle model can be employed at both high
and low velocities. This allows to have one single model that presents a kinematic-like behaviour at low velocities
while keeping the standard dynamic single-track vehicle model behaviour at higher speeds. Obstacles’ uncertainties have been
taken into account through the implementation of coupled soft and hard constraints. The correlation between the simulated
and perceived spatial horizon lengths has been enforced with a spatial dependent velocity profile that has been imposed
as an extra inequality constraint. The numerical solution is carried out using ACADO toolkit, coupled with the QP solver
qpOASES. The trajectory planner performances have been checked in simulation, applying the controller to a realistic
nonlinear multibody model in CarMaker environment. Two significant driving scenarios have been reported assessing the
trajectory planner performances in terms of feasibility of the generated trajectories and passengers’ comfort. The analysis
of the computational cost has confirmed that the proposed trajectory planner can be implemented in real-time.
In this paper a novel decisional algorithm for trajectory planning & tracking is proposed. In detail the algorithm
is expected to ensure: a real-time feasible implementation, a sufficient robustness of the solution provided respect to
modeling errors and being able to deal with multiple and moving obstacles in urban-like scenarios.
The algorithm consists mainly in a optimal constrained problem formulated as a MPC problem and numerically
solved by means of a novel genetic algorithm approach. In particular, thanks to GA implementation for numerical
optimization compared to other approaches in literature, the proposed algorithm prevents the solution to get stuck in local
minima and the specific mathematical formulation proposed bound computational cost and generates continuous control
commands at the same time. Especially, the specific vehicle model adopted and the choice of optimization variables force
the evasive maneuver calculated over the predictive horizon of MPC to present continuous acceleration and jerk, while
trajectory computed by the closed loop presents continuous acceleration in time. on the other hand, this approach limits the
search of a solution to a suboptimal subset (due to hypothesis on control variables as linear equation). This assumption is
considered reasonable thanks to the short predictive horizon and the short sampling time of the controller.
Numerical simulations of a typical urban area (made of sharp curves and narrow lanes) are shown to test the algorithm
Computational time measured during the simulations confirms the feasibility of a real-time implementation of the controller
for working frequency up to 20Hz, as well as sufficient robustness respect to modeling errors and its capability to deal
with multiple and moving obstacles in urban-like scenarios is shown.