development of a mpc controller for an autonomous vehicle
Post on 01-Oct-2021
8 Views
Preview:
TRANSCRIPT
End of Master's project
ETSEIB - MU ENG INDUSTRIAL
Development of a MPC
Controller for an autonomous vehicle
REPORT
Autor: CARTRÓ BENAVIDES, JAUME Director: DORIA CEREZO, ARNAU Date: JANUARY 2019
Escola Tècnica Superior d’Enginyeria Industrial de Barcelona
Page 2 Report
MPC controller for Autonomous vehicle Page 3
Summary
This thesis has as its main objective the development of a controller for an autonomous
vehicle. Among all possible control strategies, the methodology chosen follows a linear MPC
structure in order to achieve the goal. Basically a MPC tries to predict future behaviour of the
vehicle and anticipated the actions to perform smooth movements.
In addition, a methodology to find a feasible speed profile was developed as it was a prior
requirement to test correctly the controller. This planning for speed profile was done also by
using MPC structure and does not take into account vehicle dynamics but kinematics.
The procedure to evaluate the performance of the controller goes on building different
scenarios and making simulations on them. Simulations have been done in MATLAB and VI-
CarRealTime. This last software is very important to achieve realistic dynamic response.
The results show that the controller is a very valid option to consider in autonomous vehicles
and has a big potential behind. Only in very extreme situations where the limits of the tyres
are reached it presents some difficulties to operate.
Page 4 Report
Index
INDEX _______________________________________________________ 4
LIST OF FIGURES _____________________________________________ 6
1. GLOSSARY ______________________________________________ 9
2. PREFACE _______________________________________________ 11
2.1. Origin of the project ...................................................................................... 11
2.2. Motivation ..................................................................................................... 11
2.3. Previous requirements ................................................................................. 11
3. INTRODUCTION __________________________________________ 13
3.1. Project objectives ......................................................................................... 13
3.2. Scope of the project ..................................................................................... 14
3.3. Collaborators ................................................................................................ 14
4. VEHICLE MODEL _________________________________________ 15
5. SCENARIOS _____________________________________________ 19
5.1. Urban scenario ............................................................................................. 19
5.2. Racing scenario ........................................................................................... 20
6. VEHICLE CONTROL ______________________________________ 21
6.1. Controller discussion .................................................................................... 21
6.2. Model predictive control ............................................................................... 22
6.2.1. Formulation MPC ............................................................................................. 23
6.2.2. Linear MPC ...................................................................................................... 25
6.3. Implementation ............................................................................................ 27
6.3.1. Simulink model ................................................................................................. 27
6.3.1.1. Reference generator ............................................................................ 27
6.3.1.2. Controller .............................................................................................. 29
6.3.1.3. Vehicle plant ......................................................................................... 30
6.3.2. MPC controller ................................................................................................. 30
6.3.2.1. MPC tuning .......................................................................................... 33
7. REFERENCES GENERATION _______________________________ 34
7.1. Urban references ......................................................................................... 36
7.2. Racing references ........................................................................................ 37
MPC controller for Autonomous vehicle Page 5
8. SIMULATIONS AND RESULTS ______________________________ 38
8.1. Urban scenario ............................................................................................. 38
8.2. Racing Scenario ........................................................................................... 41
8.3. Real-time feasibility ...................................................................................... 43
8.3.1. Sample time and horizon of prediction affectation ........................................... 43
9. BUDGET ________________________________________________ 45
CONCLUSIONS ______________________________________________ 46
FUTURE WORK ______________________________________________ 47
ACKNOWLEDGMENTS ________________________________________ 48
BIBLIOGRAPHY ______________________________________________ 49
Page 6 Report
List of figures
Figure 3-1: Modular architecture framework ......................................................................... 13
Figure 3-2: Mid-size Sedan on the left and Racecar on the right side .................................. 14
Figure 4-1: Vehicle scheme with acting forces ..................................................................... 15
Figure 4-2: Lateral force vs tire slip angle ............................................................................. 17
Figure 5-1: Urban scenario (blue: trajectory, green: direction) .............................................. 19
Figure 5-2: Overall Idiada's tracks ........................................................................................ 20
Figure 5-3: Dry Handling track and specifications ................................................................ 20
Figure 6-1: Shceme of MPC predictions on a desired path .................................................. 22
Figure 6-2: Graphic on relation between weights ................................................................. 23
Figure 6-3: Curve trajectory in open loop ............................................................................. 25
Figure 6-4: Reference generator blocks ............................................................................... 28
Figure 6-5: Discretized track with his speed profile .............................................................. 28
Figure 6-6: Controller block .................................................................................................. 29
Figure 6-7: Vehicle plant block ............................................................................................. 30
Figure 6-8 : 𝐴𝑒𝑞 and 𝑏𝑒𝑞 matrices ........................................................................................ 32
Figure 6-9: Total cost function (red), terms included (blue) .................................................. 33
Figure 7-1: Offline nonlinear MPC planner example on a curved trajectory. ......................... 35
Figure 8-1: Urban scenario with desired path highlighted in red ........................................... 38
Figure 8-2: Speed,acceleration and jerk references to selected path ................................... 38
Figure 8-3: Map of path with zoom in 6 curved sections (reference in red and real path in
blue) ..................................................................................................................................... 39
Figure 8-4 Data obtained in simulution ................................................................................. 40
MPC controller for Autonomous vehicle Page 7
Figure 8-5: Reference curvature vs real curvature in simulation ........................................... 41
Figure 8-6: Graphics comparision between laps 1,2,4 and 6 in Dry Handling ...................... 42
Figure 8-7: Time used from MPC controller each 𝑇𝑠 iteration .............................................. 43
Figure 8-8: Affectance of changing sample time................................................................... 44
Figure 8-9: Affectance on changing horizon of prediction ..................................................... 44
MPC controller for Autonomous vehicle Page 9
1. Glossary
IRI: The ‘Institut de Robòtica i Informàtica Industrial’ is a Joint Research Centre of the
Spanish Council for Scientific Research (CSIC) and the Technical University of Catalonia
(UPC).
MPC: Model Predictive Control is a control technique that finds control inputs considering
future states in a horizon of prediction while minimizing a cost function.
Slip: is generally giving as a percentage of the difference between the surface speed of the
wheel compared to the speed between axis and road surface.
Slip ratio: Slip ratio is a measure of tire slipping with respect to the road along the
longitudinal direction.
Side slip angle: is defined as the angle made by the velocity vector to longitudinal axis of
the vehicle at the center of gravity in an instantaneous frame.
Pitch motion: rotation around the side-to-side axis
Yaw motion: rotation around the vertical axis
Roll motion: rotation around the front-to-back axis
Cornering stiffness: when a tire is orientated at an angle not equal to its direction of motion,
a side force acts perpendicular to the plane of the wheel. The relation is nearly linear for
small slip angles. The relationship that defines side force as a function of slip angle is the
cornering stiffness.
VI-CarRealTime: software developed by VI-Grade used for modelling and simulating
vehicles in real-time.
VI-Grade Compact Simulator: driving simulator provided by VI-grade that features a
driver’s seat, steering wheel and pedals.
Jerk: is the rate of change of acceleration; that is, the time derivative of acceleration.
LPV: Linear parameter-varying control (LPV control) deals with the control of linear
parameter-varying systems, a class of nonlinear systems which can be modelled as
parametrized linear systems whose parameters change with their state.
MPC controller for Autonomous vehicle Page 11
2. Preface
2.1. Origin of the project
During the last year of my master course one the professors explained that he was
collaborating with Idiada giving them support in technical aspects and suggested that if
someone had interest in doing projects related to our specialization, they were looking to hire
people.
So, I contacted with the human resources of Idiada and achieved an interview with the
project manager of the dynamical department in Idiada. In the interview, they showed me
their actual projects which called my attention and hopefully I ended up joining them.
Currently I’m doing an internship in Idiada where at the same time I have developed my
thesis.
2.2. Motivation
Nowadays, autonomous vehicles are becoming more present in many leading automotive
companies and everything points to the fact that autonomous vehicles will form an important
part of the mobility of the future. For that reason, there is a technology race to get the best
autonomous vehicle where companies such as Tesla, Ubber, Google, Zucks, etc. stand out.
Idiada, as a leading company in the automotive sector, is no different from the rest and has a
great interest in having technologies of this type. That is why during the last few years,
research projects are beginning to deepen their knowledge and be able to offer related
services in the future.
In regards to myself, as an engineer I find autonomous driving as a very attractive field where
lots of challenges are waiting to be solved. In addition, not only I appeal strictly to the control
side but also others aspects like planning or perception of the environment.
2.3. Previous requirements
In order to carry out a task like the one explained before requires several conditions which all
of them can be accomplished in Idiada’s facilities.
First of all, the phase of simulation is very important. In the development of an autonomous
car is really useful (or even mandatory) to test its capabilities before starting with a real car.
Page 12 Report
For instance, you can see how the vehicle reacts to a sudden pedestrian in the street or a
front car breaking very strongly without putting the lives of any person in danger.
To do such tests is required to have a model of the vehicle very close to reality which can be
able to simulate the behaviour of the tyres with the ground, the mass distribution in every
moment or any other dynamics involved in the driving process. In this fields there are several
software doing this job like CarMaker, CarSim, Adams, etc. In this project the dynamical
department of Idiada provided me with VI-CarRealTime to perform realistic simulations.
Secondly, once the simulations are giving good results, it is the moment to combine
simulations with real experiments to move on. In this section Idiada is also very prepared as
it has one of the best track parks in Europe. Also, a fully actuated vehicle with the necessary
sensors is needed to perform correctly.
Finally, remark the fact that Idiada has incorporated a very sophisticated GPS system inside
its facilities which will be very useful for the purpose of evaluating a control path following
strategy. Having this help, which normally an autonomous car will rely on its sensors and
perception algorithms to locate itself, will help to decouple the perception phase from the
control one.
MPC controller for Autonomous vehicle Page 13
3. Introduction
3.1. Project objectives
This thesis is involved in a bigger project which aims to develop an autonomous vehicle that
will circulate within the facilities of Idiada and be able to perform as a Taxi travelling between
its buildings.
This bigger project, which we can refer as Project Taxi L4, has been designed and planned
by different independent blocks so the changes in each one does not affect the others.
The structure is the one showed in Figure 3-1:
Figure 3-1: Modular architecture framework
It is very important to define the inputs and outputs of every block to achieve a good
communication between them.
This thesis will only take care of the control block. So, the first objective is to define all the
inputs and outputs very carefully.
The inputs required are the following from each block:
- Planning block: will be in charge of generating a speed profile and a trajectory
reference.
- Localization and mapping: will find out the current position of the vehicle which from
the control perspective will be compared with the references from planning.
- Car sensors: the control block will receive the vehicle states captured by the sensors
Page 14 Report
installed such as yaw rate or longitudinal acceleration.
The outputs of the control block will be the position of the steering wheel and the percentage
of throttle or brake pressure applied. More specifically the control block will generate the
references of these signals for lower controller actuators which won’t be treated in here. Also,
to simplify the simulations instead of using a pedals system longitudinal tyre forces will be
our output so far.
Finally, to enrich the testing of the controller, one of the objectives is to see how the same
controller is able to act for two different types of environments: urban and racing. Each one
will be tested with a different car. The cars chosen are:
- Mid-size Sedan vehicle: this car will be used for urban scenarios.
- General race car: this car will be used for racing scenarios.
Figure 3-2: Mid-size Sedan on the left and Racecar on the right side
Both of them are modelled inside our dynamics simulator CarRealTime.
With this it is the objective to see how good it can perform in each scenario and realise the
current lacks which must be strengthen in the future.
3.2. Scope of the project
As it has been said before, the objective is making a controller for an autonomous taxi that
will work inside Idiada’s facilities. However, the scope of this thesis is only aiming to the
simulation phase.
3.3. Collaborators
Currently, there is collaboration between Idiada and the Institute of Robotics and Industrial
Computing (IRI). We are getting support from researchers in IRI that contribute giving us
knowledge about control and planning algorithms.
MPC controller for Autonomous vehicle Page 15
4. Vehicle model
In order to perform a good controller, a vehicle model is required for its design. However, a
vehicle is a quite complex mechanism and can be described with lots of models depending in
the degree of details that are included. Then, due to the fact that the controller must work as
a real-time application with a refresh of about dozens of milliseconds, we are restricted to
pick a computational efficient model which can represent all the essential dynamics of a
vehicle.
The most common model used for this type of applications is the bicycle model which will
also be used in this thesis. This model simplifies a four-tyre model into two-tyres assuming
symmetry between both left and right sides. As a result, while yaw load transfer is
considered, load transfer caused by roll motion is neglected. Pitch will not be considered in
the equations either.
Taking into account that mid-size Sedan is a front tractive vehicle and Racecar is a rear
tractive vehicle (like most competition vehicles), the model will slightly differ for each car. The
scheme that represents the system is:
Figure 4-1: Vehicle scheme with acting forces
𝜃
𝑣
𝐹𝑥𝐹
𝐹𝑥𝑅
𝑦𝑊
𝑥𝑊
𝑌
𝑋 𝑊
𝑥𝐵
𝑏
𝑦𝐵 𝛼
𝛿
𝑎
𝐵
𝜔
𝐹𝑦𝑅
𝐹𝑦𝐹
𝐹𝑑𝑓
Page 16 Report
Where 𝑎 is the distance from the Centre of gravity (CoG) to the front axis, 𝑏 the distance
from the CoG to the rear axis, 𝐹𝑖𝑗 are the forces acting on the tyres (i: y= lateral, x =
longitudinal; j: R: rear, F: front) plus 𝐹𝑑𝑓 as the resistance force composed by drag and
friction. Then 𝑣 is the global speed with 𝛼 angle deviation from the longitudinal axis of the
vehicle, 𝜔 is the yaw rate, 𝜃 is the yaw angle and 𝛿 is the wheel toe angle. Also, two different
axis coordinates are differentiated: 𝑊 is the world reference and 𝐵 the body reference.
The nonlinear equations that describe the longitudinal (𝑉𝑥) and lateral (𝑉𝑦) local body
velocities and yaw motion (𝜔), for each car respectively and using Newtown’s third law of
motion are:
Mid-size Sedan (Front tractive):
(Eq. 4-1)
RacerCar (Rear tractive):
(Eq. 4-2)
In the equations, 𝛿 and 𝐹𝑥𝑗 are the inputs and only lateral forces on tyres are to be
determined. Some research was made to find out the real values of lateral forces including
several papers like [1], [2] and also a very complete book [3] from where a lot tyre models
are studied. Lateral forces have a lineal behaviour around zero slip angle where the slope
corresponds to the cornering stiffness of the tyre. Then the behaviour becomes nonlinear
reaching a saturation point that corresponds to the maximum lateral force that a wheel can
generate. This maximum is given by product of the ground friction coefficient and vertical
force on the tyre. After this the lateral forces decays.
𝑣�̇� =−𝐹𝑦𝐹 sin(𝛿) + 𝐹𝑥𝐹 cos(𝛿) − 𝐹𝑑𝑓
𝑀+ 𝜔 𝑣𝑦
𝑣�̇� =𝐹𝑦𝑅 + 𝐹𝑦𝐹 cos(𝛿) + 𝐹𝑥𝐹 𝑠𝑖𝑛(𝛿)
𝑀− 𝜔 𝑣𝑥
𝜔 ̇ =𝐹𝑦𝐹𝑎 cos(𝛿) + 𝐹𝑥𝐹𝑎 𝑠𝑖𝑛(𝛿) − 𝐹𝑦𝑅𝑏
𝐼
𝑣�̇� =𝐹𝑥𝑅−𝐹𝑦𝐹 sin(𝛿) − 𝐹𝑑𝑓
𝑀+ 𝜔 𝑣𝑦
𝑣�̇� =𝐹𝑦𝑅 + 𝐹𝑦𝐹 cos(𝛿)
𝑀− 𝜔 𝑣𝑥
𝜔 ̇ =𝐹𝑦𝐹𝑎 cos(𝛿) − 𝐹𝑦𝑅𝑏
𝐼
MPC controller for Autonomous vehicle Page 17
As an example, from [2] for one tyre at different friction coefficients the lateral forces
according to the tire slip angle are:
Figure 4-2: Lateral force vs tire slip angle
In the references mentioned before two types of models are given to represent this curves.
On one side physic algoriths as the brush model and on the other side empiric models where
Paceka’s it’s the most fomous. However, for our control purpose instead of using a nonlinear
version, one can estimate the forces in the linear region assuming that side slip and slip
angles stay relatively low (this assumption will be more analized in detail in the results and
conclusions). The lateral force will be represented as:
𝐹𝑦𝐹 = 𝐶𝑓𝛼𝑓
𝐹𝑦𝑅 = 𝐶𝑟𝛼𝑟
(Eq. 4-3)
Where 𝐶𝑓 and 𝐶𝑟 are the cornering stiffness of the front and rear wheels respectively.
The slip angles 𝛼𝑓 and 𝛼𝑟 are defined as:
𝛼𝑓 = 𝛿 − 𝛼 −𝑎𝜔
𝑣
𝛼𝑟 = −𝛼 +𝑏𝜔
𝑣
(Eq. 4-4)
Finally, from the velocities in each direction of the local axis, the trajectory of the vehicle
centre of gravity in global axis coordination will be determined as:
�̇� = 𝑉𝑥 cos(𝜃) − 𝑉𝑦 sin(𝜃)
�̇� = 𝑉𝑥 sin(𝜃) + 𝑉𝑦 cos(𝜃)
(Eq. 4-5)
Page 18 Report
All vehicles parameters are defined in Table 4-1:
Parameter Mid-size Sedan Racecar
𝒂 [𝒎] 1.172 1.14
𝒃 [𝒎] 1.64 1.53
𝑴 [𝒌𝒈] 1894 1594
𝑰 [[𝒌𝒈 𝒎𝟐] 3541 2374
𝑪𝒇 [𝑵/𝒓𝒂𝒅] 56239 63421
𝑪𝒓[𝑵/𝒓𝒂𝒅] 81114 74723
𝑨𝒓[𝒎𝟐] 1.91 1.75
Table 4-1: Vehicles parameters
So far model equations from (Eq. 4-1) to (Eq. 4-5) are the ones used for the controller. In the
case of the vehicle plant VI-CarRealTime uses a much more complex system that includes
dynamics like differentiation of sprung and non-sprung masses, steering column, tyre models
and much more to achieve realistic simulations.
MPC controller for Autonomous vehicle Page 19
5. Scenarios
In this section the scenarios used to test the controller are presented.
5.1. Urban scenario
In order to have an urban scenario where the vehicle cannot overpass 40 kph, the following
streets, with total dimensions of 200x250 meters, have been created:
Figure 5-1: Urban scenario (blue: trajectory, green: direction)
The way of making this small city was done imitating what a real autonomous car will use to
locate itself and search a path to move between one origin and a destination. Using the
options given by MATLAB, a digraph (graph with oriented edges) has been created using a
total of 54 nodes and 66 edges. Nodes have information about global position and edges
have properties such as intermediate waypoints, maximum speed or length. Then MATLAB
offers methods (which are based on Dijkstra’s algorithm) to determine the fastest way to
move from one node to another taking into account edges properties like how much time
takes to travel through one edge.
Page 20 Report
5.2. Racing scenario
In the case of racing, advantage from Idiada’s facilities is taken. Figure 5-2 shows all of
Idiada’s tracks in the division of Tarragona:
Figure 5-2: Overall Idiada's tracks
From all of them number 5, Dry Handling has been chosen. The main reason to pick this
particular track is that in the past was scanned and now we possess a digital version of it
which is already in our VI-Grade Compact Simulator.
Figure 5-3: Dry Handling track and specifications
MPC controller for Autonomous vehicle Page 21
6. Vehicle control
In this section the controller formulation and implementation is exposed.
6.1. Controller discussion
Before starting to explain the MPC formulation, let us see why MPC was chosen instead of
other types of controllers. See the benefits of MPC as well as possible lacks.
First of all let us see which the main highlights of the MPC strategy are:
- Using an MPC you are able of manipulating multivariable systems.
- MPC accepts constraints in its design which is very useful as you can set your
physical limits of your steering angle for example.
- MPC actuates as an open loop optimization problem with initial conditions and
updated each sample time.
- Uses a cost function that allows emphasizing whatever you like.
Gathering all we get an optimal constrained control which is a very powerful tool.
In the disadvantages side one of the main difficulties that MPC introduces into the
robustness question is the open-loop nature of the optimal control problem. The high
frequency of the updated solutions helps to deal with this issue. Also, to use MPC having a
fidelity model of your system is highly required otherwise the control will be very poor.
In contrast of MPC, we could apply other techniques. One way to go could be using
traditional controllers like a PID. However a PID controller is not able to predict future events
like MPC does. Also is not able to set constraints.
Another option to consider are neuronal networks (NN). In [4], [5] and [6] some examples of
NN applied in autonomous driving applications is demonstrated. With NN you can get a
vehicle model by just doing a lot of testing and driving. This model will include all nonlinear
dynamics without having to know them. One problem that you may encounter is that you
face a situation that you have never tried before and the controller it is no adjusted for it.
Then it is very important to test any situation that one can imagine.
So far NN looks very good, however a vehicle and tracks are required so the NN can learn
which at this moment is not an option for my personal case. In spite of it, a combination of
MPC formulation and NN would able to predict futures states using a nonlinear model fitted
with empirical data. This approach does look like a real option for the future.
Page 22 Report
Another dilemma encountered reading about MPC is that a lot of works present MPC only for
lateral movement considering a constant speed and making a separate controller for
longitudinal movement (for example a PID [11] or a Lyapunov controller [13]).
On one hand, this decoupled strategy is less heavy computationally. However, as seen in [2]
where both strategies are compared, lateral and longitudinal dynamics are dependent on
each other and considering a model for both at the same time results in a more stable and
reliable control.
6.2. Model predictive control
Model predictive control (MPC) is generally used for complex dynamical systems. In our
case of study a vehicle is represented with a highly nonlinear model.
In order to provide adequate inputs for the vehicle, the MPC solves and optimization problem
every sample time (which includes a cost function restricted to a model and other
constraints). This optimization considers a horizon of prediction (Hp) and finds all the control
inputs and future states of the variables of the model for each iteration till the horizon that
minimizes the cost function. Then, only the control inputs from the first iteration are applied to
the vehicle. The next step is to measure the vehicle states and repeat the process. So at
each sample time an optimal problem is solved considering the actual circumstances.
Figure 6-1: Shceme of MPC predictions on a desired path
In Figure 6-1 the distribution of the reference waypoints that are chosen is represented.
Notice the fact that the distance between them is time variant. So the distance equals the
speed multiplied by the time travelled where the time is a constant value, the controller
sample time (𝑇𝑠) , and the speed for each point is given in the reference speed profile by the
planner.
Desired path
1
2
3
…
0
Hp
𝑣0,𝑟𝑒𝑓 · 𝑇𝑠
𝑣1,𝑟𝑒𝑓 · 𝑇𝑠
𝑣𝐻𝑝−1,𝑟𝑒𝑓 · 𝑇𝑠 …
MPC controller for Autonomous vehicle Page 23
6.2.1. Formulation MPC
The general formulation of an MPC problem is defined as follows:
-Cost function to minimize:
𝑚𝑖𝑛 𝐽 = 𝜔𝑒𝐽𝑒 + 𝜔𝑢𝐽𝑢 + 𝜔𝑐𝐽𝑐
-Subjected to:
−𝐷𝑦𝑛𝑎𝑚𝑖𝑐𝑎𝑙 𝑚𝑜𝑑𝑒𝑙 −𝑂𝑝𝑒𝑟𝑎𝑡𝑖𝑜𝑛𝑎𝑙 𝑙𝑖𝑚𝑖𝑡𝑎𝑡𝑖𝑜𝑛𝑠
(Eq. 6-1)
where 𝜔𝑖 is the weight on each term 𝑖 of the cost function. Then the typical terms involved
are 𝐽𝑒 as the error committed in tracking some reference, 𝐽𝑢 as control effort penalizing
aggressive inputs and 𝐽𝑐 stability or comfort parameters.
In vehicle application for example one could apply the knowledge acquired through
experience and try to minimize comfort parameters such as lateral acceleration or pitch
movement (if you use a model that considers this state).
Another related behaviour to review is that by normalizing the weights for each variable, the
sum of them will be 1. This means that by increasing one value you are decreasing the
others by default. In Figure 6-2 we can see that with a high value on minimizing control
actions the error will be very high and on the opposite way, having a high value on error
variables will increase the cost on actuators. In addition, sudden strong changes in actuators
can degrade them or even make your system become unstable. For all these reasons
becomes very important to find a compromise between the weights.
For further documentation in MPC technique and its variants one can refer to [7], [8] and [9].
Control effort, Ju
𝝎𝒆 = 𝟎 𝝎𝒖 = 𝟏
Tra
ckin
g e
rror,
Je
𝝎𝒆 = 𝟏 𝝎𝒖 = 𝟎
Figure 6-2: Graphic on relation between weights
Page 24 Report
So far, this was a very general MPC formulation. Let us see more precisely how it looks for
our system:
𝑚𝑖𝑛𝑢
∑ ( �̃�𝑘𝑇𝑄𝑥𝑘�̃�𝑘 + ∆𝑢𝑘
𝑇𝑅∆𝑢𝑘) + 𝑥𝐻𝑝𝑇 𝑄𝑥𝐻𝑝
𝐻𝑝−1
𝑘=0
Subjected to:
𝑥0 = 𝑥𝑐𝑢𝑟𝑟𝑒𝑛𝑡
�̃�𝑘+1 = 𝑥𝑘 − 𝑥𝑟𝑒𝑓
𝑥𝑘+1 = 𝑓(𝑥𝑘, 𝑢𝑘) 𝑘 = 0,… ,𝐻𝑝 − 1
𝑢𝑘+1 = 𝑢𝑘 + ∆𝑢𝑘 𝑘 = 0,… ,𝐻𝑝 − 1
𝑢𝑘 ∈ [�̅�, 𝑢]
(Eq. 6-2)
Where 𝑥 = [𝑥 𝑦 𝜃 𝑣𝑥 𝑣𝑦 𝜔]𝑇 and 𝑢 = [𝐹𝑥 𝛿]
𝑇 are the state and the control inputs vectors
respectively. The weighting matrices are 𝑄 = 𝑄𝑇 ≥ 0, 𝑅 = 𝑅𝑇 ≥ 0 (traditionally last states
weighting matrix is noted as 𝑃 and is considered to be more important than intermediate
states having a higher value but in this application of path tracking all states are considered
equally important). 𝑥𝑐𝑢𝑟𝑟𝑒𝑛𝑡 corresponds to the measures of the states at current time. Then
also because we are tracking a path instead of minimizing states, we minimize the
differences between them and the references. In the case of control actions we are
interested in minimizing the difference between the current and previous actions to get
smooth movements.
Notice that in state vector there are more variables than the ones in (Eq. 4-1) and (Eq. 4-2).
It has been added the position states 𝑥 (longitudinal position), 𝑦 (lateral position) and 𝜃 (yaw
angle) so the difference between them and the reference trajectory can be minimized. These
variables are, as speed variables, in local references to respect to vehicle local axis and are
obtained by integrating the speed variables (in section 6.2.2 is explained in more detail).
As a parenthesis, in this thesis it has been decided to use these position variables. In other
works it has been chosen to use lateral deviation and relative yaw angle which instead of
using the 𝑥 and 𝑦 coordinates of the waypoints, requires the curvature for each reference
waypoint.
Yet, another correction was made. When minimizing the difference between the trajectory
reference angle and the vehicle yaw angle, it was noticed that when the vehicle has a lateral
speed different from zero, the direction of the global speed is not matching the direction of
the longitudinal axis. So this correction allows the vehicle to track better in pronounced
curves:
MPC controller for Autonomous vehicle Page 25
�̃�𝑘 = 𝜃𝑘 − (𝜃𝑟𝑒𝑓 + 𝛼𝑘)
(Eq. 6-3)
Where 𝛼 is the side slip angle and represents the angle between the vehicle longitudinal axis
and the global speed. To see more clearly this effect, a simulation in open loop was done
(without a controller), forcing to steer and reaching side slip angles up to 15 degrees. Figure
6-3 shows this simulation where it is easy to see the mismatch between angles. The
trajectory done by the vehicle is marked in black.
In Figure X in black its showed the trajectory made by a vehicle and as
6.2.2. Linear MPC
In the restrictions of (Eq. 6-2), futures states appear as a function of previous states and
control inputs. This function in this particular context means that the relation is nonlinear.
Solving this MPC problem as a nonlinear MPC in real time it is unfeasible. As the space of
solutions is not convex, it becomes very hard to find the optimal solutions in few iterations.
One possible approach is to use a linear MPC which in this application of vehicle path
following means that one has to linearize the vehicle models before solving the MPC
[2],[11],[12].
𝑣
𝑣𝑦
𝑣𝑥
𝛼
Figure 6-3: Curve trajectory in open loop
Page 26 Report
Then, restrictions on vehicle model:
𝑥𝑘+1 = 𝑓(𝑥𝑘 , 𝑢𝑘) 𝑘 = 0,… ,𝐻𝑝 − 1
(Eq. 6-4)
become:
𝑥𝑘+1 = 𝐴𝑑,𝑘𝑥𝑘 + 𝐵𝑑,𝑘𝑢𝑘 𝑘 = 0,… ,𝐻𝑝 − 1
(Eq. 6-5)
Matrices 𝐴𝑑,𝑘 and 𝐵𝑑,𝑘 are defined in discrete time space and are obtained by discretizing
the continuous models:
�̇� = 𝐴𝑐,𝑘𝑥𝑘 + 𝐵𝑐,𝑘𝑢𝑘 𝑘 = 0,… , 𝐻𝑝 − 1
(Eq. 6-6)
Continuous matrices 𝐴𝑐,𝑘 are obtained by deriving the nonlinear equations respect the state
variables and 𝐵𝑐,𝑘 respect the control action inputs and evaluating in each 𝑘 iteration:
𝐴𝑐,𝑘 =𝜕𝑓(𝑥, 𝑢)
𝜕𝑥|𝑥=𝑥𝑘𝑢=𝑢𝑘
𝑘 = 0,… ,𝐻𝑝 − 1
𝐵𝑐,𝑘 =𝜕𝑓(𝑥, 𝑢)
𝜕𝑢|𝑥=𝑥𝑘𝑢=𝑢𝑘
𝑘 = 0,… ,𝐻𝑝 − 1
(Eq. 6-7)
To make the partial derives an online mathematic tool was used [14] .
For 𝑘 = 0 current measured states and control inputs are used to find the linear model. For
the rest, predicted states and control inputs obtained in previous iterations of the MPC
controller are used.
The resultant continuous state space of (Eq. 6-6) looks like:
[ �̇��̇�
�̇�𝑣�̇�
𝑣�̇�
�̇� ]
=
[ Ø
1 0 00 1 00 0 1
Ø
𝑎44 𝑎45 𝑎46
𝑎54 𝑎55 𝑎56
𝑎64 𝑎65 𝑎66]
[ 𝑥𝑦𝜃𝑣𝑥𝑣𝑦
𝜔]
+ [
Ø𝑏41 𝑏42
𝑏51 𝑏52
𝑏61 𝑏62
] [𝐹𝑥𝛿]
(Eq. 6-8)
To add the position states variables 𝑥, 𝑦 and 𝜃 one has only to add a identity matrix.
MPC controller for Autonomous vehicle Page 27
Then using Euler method discretization is obtained. It is the most basic explicit method for
numerical integration but at the high frequency of the controller, the approximation works fine
and it is easy to implement. Euler is a first-order method that considers the slope between
two operating points as follow:
𝑥𝑘+1 = 𝑥𝑘 + (𝐴𝑐,𝑘𝑥𝑘 + 𝐵𝑐,𝑘𝑢𝑘)𝑑𝑡 = (𝐼 + 𝐴𝑐,𝑘𝑇𝑠)𝑥𝑘 + (𝐵𝑐,𝑘𝑇𝑠)𝑢𝑘
(Eq. 6-9)
From where the discrete matrices are deducted as:
𝐴𝑑,𝑘 = 𝐼 + 𝐴𝑐,𝑘𝑇𝑠
𝐵𝑑,𝑘 = 𝐵𝑐,𝑘𝑇𝑠
(Eq. 6-10)
Another possible approach to find the matrices, where our collaborators from IRI stand out
[16], is to get the matrices with the LPV method (linear parameter varying control). This
method, mentioned before, consists on embedding the model non-linearities inside model
parameters that depend on some variables, called scheduling variables. This method
enables to reduce drastically the cost of calculating the matrices and will probably be
implemented in the future.
6.3. Implementation
First of all let us have a view at the overall Simulink blocks scheme workflow and later on the
MPC controller itself.
6.3.1. Simulink model
The scheme can be divided in 3 main parts:
The reference generator
The controller
The vehicle plant
The reference generator and the controller work at the sample time of the controller. The
plant works at a lower sample time of 1 millisecond so the response is more realistic.
6.3.1.1. Reference generator
It is composed with 2 blocks (Figure 6-4). The 1st block receives as inputs the current
position and the references found offline (explained in Section 7) and as output has all the
references (waypoints position, longitudinal speeds, yaw angles… for each prediction).
Page 28 Report
Figure 6-4: Reference generator blocks
To understand it better let us have a look at the following example:
Figure 6-5: Discretized track with his speed profile
Left part of Figure 6-5 shows a curve trajectory where blue points are the discretized
trajectory, the red point is the actual position of the vehicle and the green ones are the
prediction references. So first the algorithm finds the closest blue point to actual position and
from this point goes to speed profile and searches the reference speed for this particular
waypoint. Then, with this information calculates the predicted distance travelled and chooses
MPC controller for Autonomous vehicle Page 29
the next references waypoint. Iteratively waypoints are chosen till the horizon prediction.
The 2nd block just converts the references that are in global axis to local vehicle axis.
6.3.1.2. Controller
Controller (Figure 6-6) has as inputs all outputs from reference generator plus current vehicle
states. The outputs are wheel toe angle and longitudinal tyre forces.
Figure 6-6: Controller block
Resistance forces have been decoupled from the MPC controller and are compensated later.
As Figure 6-6 shows, a very simplified friction force estimator is added that considers
aerodynamic friction loses, rolling friction of tyres and hill climbing force. One future
improvement will be to implement an observer to estimate correctly this value (for example
Extended Kalman Filter EKF which is able to work with nonlinear models).
Page 30 Report
6.3.1.3. Vehicle plant
Vehicle plant is represented by a block that connects to VI-CarRealTime software. The
inputs are tyre longitudinal forces, rack displacement and several other inputs. These last
inputs allow us to control the car the way we want and for example deactivate the internal
steering column or the pedals.
Figure 6-7: Vehicle plant block
Outputs from the controller are adapted as Figure 6-7 shows. Tyres force from the controller
is divided into left and right wheels and is only applied in front or rear wheels. Then wheel toe
angle calculated in the controller is converted into rack displacement by using a transmission
ratio.
6.3.2. MPC controller
MPC controller is implemented using 𝑞𝑢𝑎𝑑𝑝𝑟𝑜𝑔 solver from MATLAB optimization toolbox.
This implies that MPC equations from (Eq. 6-2), including the linear model restriction
changes in Section 6.2.2, have to be rearranged into a quadratic programing structure.
The problem now will have a minimizing cost function and restrictions like:
𝑚𝑖𝑛𝑥
1
2𝑋𝑇𝐻𝑋 + 𝑓𝑇
𝑆𝑢𝑏𝑗𝑒𝑐𝑡𝑒𝑑 𝑡𝑜: 𝐴𝑋 ≤ 𝑏,
𝐴𝑒𝑞𝑋 = 𝑏𝑒𝑞 ,
𝑙𝑏 ≤ 𝑋 ≤ 𝑢𝑏.
(Eq. 6-11)
MPC controller for Autonomous vehicle Page 31
Where now cost function has a quadratic part 𝐻, that is a diagonal with the cost of the states
defined, and 𝑓 for the lineal part (in our case it is null). Then vector of states 𝑋 is not only the
vehicle states (𝑥, 𝑦, 𝜃, 𝑉𝑥 , 𝑉𝑦, 𝜔) but also includes predicted control actions (𝐹𝑥 , 𝛿), variables
that we define to minimize like gradient of control inputs (∆𝐹𝑥 , ∆𝛿) and difference between
vehicle states and references (�̃� = 𝑥 − 𝑥𝑟𝑒𝑓 , �̃� = 𝑦 − 𝑦𝑟𝑒𝑓 , �̃� = 𝜃 − 𝜃𝑟𝑒𝑓 , �̃�𝑥 = 𝑣𝑥 − 𝑣𝑥 𝑟𝑒𝑓). 𝐴
and 𝑏 matrices are defining inequality contraints (which in our case are null) and
𝐴𝑒𝑞 and 𝑏𝑒𝑞matrices defining the equality contraints. And finally 𝑙𝑏 and 𝑢𝑏 define the lower
and upper bounds of 𝑋 states.
More precisely cost function is:
min𝑋
[𝐹𝑥, 𝛿, 𝑥, 𝑦, 𝜃, 𝑉𝑥 , 𝑉𝑦 , 𝜔, ∆𝐹𝑥 , ∆𝛿, �̃�, �̃�, �̃�, �̃�𝑥] [
08 0 00 𝑅2 00 0 𝑄4
] [𝐹𝑥 , 𝛿, 𝑥, 𝑦, 𝜃, 𝑉𝑥 , 𝑉𝑦 , 𝜔, ∆𝐹𝑥, ∆𝛿, �̃�, �̃�, �̃�, �̃�𝑥]𝑇
(Eq. 6-12)
Where 𝑅2 was a 2𝑥2 matrix with the gradient control actions weights and 𝑄4 a 4𝑥4 matrix
with the references error weights.
Then 𝐴𝑒𝑞 and 𝑏𝑒𝑞matrices, as Figure 6-8 shows, are for each predicton the vehicle model
constraints and variable to minimize in cost function. Notice that each prediction uses
diferent state space models.
To sum up, basically quadratic programming consist on stacking all states and equations to
make a compact linear system.
Page 32 Report
Figure 6-8 : 𝐴𝑒𝑞 and 𝑏𝑒𝑞 matrices
1𝑠𝑡 iteration
2𝑛𝑑 to 𝐻𝑝 − 1
iterations
𝐻𝑝 iteration
Model equations
Definition of the variables to
minimize
Aeq beq
x
MPC controller for Autonomous vehicle Page 33
6.3.2.1. MPC tuning
Once MPC logic is built, one has to adjust the weights in 𝑅 and 𝑄. The way to proceed has
been the following:
- The first step consisted on trying to normalize the value of the terms in the cost
function evaluated as we are comparing speeds, position, forces or angles and the
rang of values and units vary very much.
- The second step was to consider which is the degree of importance for each term
and give a higher value according to it.
- Finally, the weights were calibrated manually by modifying them and evaluate the
controller behaviour.
The last step was done using the data obtained by evaluating the cost function (total cost
and each term individually) every MPC solved at each sample time and extract useful
information from it.
Figure 6-9: Total cost function (red), terms included (blue)
The matrices Q and R obtained are:
𝑅 = [ 1 · 10−6, 2 · 103 ]
𝑄 = [ 1, 10, 180, 1 ]
As a conclusion, remark that if the vehicle model used in the restrictions is perfectly
according with the real behaviour, it would as easy as making the matrices weights as
identity matrices. However, this is never the case so it is important to calibrate your matrices
but if the results obtained are not the expected, one could try to improve a little more the
model instead of spending a lot of time in the calibration.
Page 34 Report
7. References generation
Going back to Figure 3-1, we can see again that before control block there is a planning
block which must ensure to generate the correct references. This block can be called also a
planner.
The planner receives from the perception block all the obstacles along the road and the
centre and boundaries of the lanes on it. With this information first on a higher level the
planner decides what type of manoeuvre must perform (advance a vehicle, change lane,
etc.). Secondly, find a trajectory and a speed profile for a certain horizon distance or time.
This new trajectory does not have to strictly follow the centre of the road but can differ to a
more feasible path always respecting the lateral limits. This means that the planner should
also use a dynamical model of a vehicle.
Though, this project is not aiming to get these references but to follow them. Nevertheless,
it’s necessary to generate them.
As already mentioned before, partner collaboration with IRI research center is being carried
out. IRI has been developing very interesting planners which are also using the MPC
structure. Because of that this planners are much related to the controller developed in this
thesis so I would like to review a couple of them and see the possibilities behind MPC
(unfortunately their work has not been uploaded yet and no reference is available at this
moment).
One of the planners proposed by them consist on making a MPC with a horizon that covers
the whole track. However, it is no easy task to predict how many sample times you need as
you still don’t know the speed profile. That is why they propose to switch to a spatial
representation instead of a time one. This means that model equation will know depend on
travelled distance instead of time and therefore every iteration corresponds to a fixed interval
distance 𝑑𝑠, and total iterations you will need to reach the horizon is the total track length
divided by this 𝑑𝑠 interval. Then a state to represent the time needed to travel 𝑑𝑠 is added.
The main difference between this planner and the controller explained in section 6.2 is that
controller cost function wants to minimize speed and lateral deviation error while the planner
will focus in other goals. For instance, in a racing application planner will try to minimize final
time to travel the whole track or for a planner more focused in comfort, the cost function will
be interested in minimizing the accelerations during the travel. Also, the planner includes a
restriction on lateral deviation in order to not overpass lateral limits of the road.
The main drawback of this planner is that linear techniques are not applicable and you are
forced to use nonlinear equations. In spite of getting the optimal solution, the time needed to
MPC controller for Autonomous vehicle Page 35
get there is exponential to the total distance and can take hours to solve a complete track
round making it incompatible with real time applications. One of the possible applications
would be to get an optimal path and speed profile for a race track and use it as a reference
with online planners. As an example in Figure 7-1 is showed an optimized trajectory which
took around 5 minutes to complete.
Figure 7-1: Offline nonlinear MPC planner example on a curved trajectory.
Another technique used by IRI developers consist in a LPV-MPC strategy. This time thanks
to LPV formulation (a technique that instantiates the nonlinear model at a precise moment)
allows to work with lineal equations making it feasible to work in real-time. This planner
linearizes the predictions in smaller horizon distance that cover the next 50-100 meters and
is updated every sample time. The function cost attributes is very similar to the nonlinear
planner. To linearize the predicted iterations, the model uses the predictions found in
previous MPC planner (remember the planner gets updated every Ts).
One possible disadvantage that one could think is that closer predictions to current position
can be accurate but the furthest are very different. Although this is partially true, when you
reach these further positions, the planner has already been updated lots of times. So in the
end only closer planner predictions are used to be followed by the controller.
Unlike the nonlinear planner, the LPV-MPC does not have to return the optimal solution but
very close one and with the capability of working online and being able to incorporate
unexpected events like it occurs in real driving cases.
Unfortunately, this last planner is still in development and will not be used for this thesis.
Nevertheless, with all these strategies presented for planning and before for the controller,
one can realise how powerful MPC formulation is and some of its uses.
Page 36 Report
In relation to this thesis, it was finally discarded to use advanced planners and the following
solutions to test the controller were adopted (one for every scenario).
7.1. Urban references
As explained in Section 5.1, in the urban scenario an origin and a destination will be chosen
and automatically the centre road path will be obtained. If it is true that the most normal thing
would be to have a planner that adapts this centre line path to a smoother one, in a urban
circuit one drives very close to the centre line (vehicle’s width is very similar to rail’s width).
Then assuming the centre line as the reference is quite reasonable.
In regard to the speed profile, a method based on MPC has been developed. This method
does not consider vehicle dynamics and is calculated in a decoupled way from the trajectory.
For this reasons in a real case it is not the most suitable solution although it would probably
be enough for making manoeuvres that are not very demanding.
It consists on finding a speed profile which maximizes the speed but at the same time
minimizes the acceleration and jerk.
In (Eq. 7-1) is detailed the MPC structure is defined:
𝑚𝑖𝑛𝑢
∑ 𝛼𝑣(−𝑣𝑘) +
𝐻𝑝
𝑘=0
∑ 𝛼𝑎(𝑎𝑘2) +
𝐻𝑝−1
𝑘=0
∑ 𝛼𝑗(𝑗𝑘2)
𝐻𝑝−1
𝑘=1
Subjected to:
𝑣0 = 𝑣𝑐𝑢𝑟𝑟𝑒𝑛𝑡
𝑣𝑘+1 = 𝑣𝑘 + 𝑎𝑘 𝑘 = 0,… ,𝐻𝑝
𝑗𝑘+1 = 𝑎𝑘+1 − 𝑎𝑘 𝑘 = 0,… ,𝐻𝑝 − 1
0 ≤ 𝑣𝑘 ≤ 𝑣𝑚𝑎𝑥
𝑎𝑚𝑖𝑛 ≤ 𝑎𝑘 ≤ 𝑎𝑚𝑎𝑥
𝑗𝑚𝑖𝑛 ≤ 𝑗𝑘 ≤ 𝑗𝑚𝑎𝑥
(Eq. 7-1)
Where vk, ak and jk are the speed, acceleration and jerk and αi is the weight on each term.
Hp is the horizon of prediction, vcurrent the initial speed.
The problem formulated uses a quadratic cost function, this time includes also a lineal part,
and lineal constraints. This allows us to use a linear solver like ‘quadprog’ and obtain the
resulting speed profile very fast being able to work for real time applications.
MPC controller for Autonomous vehicle Page 37
Respect the constraint on the maximum speed in straight sections the maximum speed will
be set as 40kph and in the circular sections a very generic formula (Eq. 7-2) has been used.
This formula specifies the maximum speed to travel depending on the road curvature:
𝑉𝑚𝑎𝑥 = √𝑔 𝜇 𝑅
(Eq. 7-2)
Where 𝑔, 𝜇 and 𝑅 are gravity, friction coefficient and road radius of curvature. This simplified
formula approximately gives a speed so you reach 1g of lateral acceleration as is deducted
from rearranging (Eq. 7-2) to (Eq. 7-3).
𝑎𝑦 ≡𝑉𝑚𝑎𝑥
2
𝑅 = 𝑔𝜇 ≈ 10
𝑚
𝑠2= 1𝑔
(Eq. 7-3)
Then for having a demanding speed profile but not too challenging the value has been
reduced to half of it.
7.2. Racing references
In this case, having previous knowledge about race driving, it is obvious that the optimal path
has nothing to be with the road centre line. Also, for the speed profile it is not sensible to use
the approach developed for urban scenarios as now the manoeuvres will be very demanding
and it becomes mandatory to use a dynamical model.
The solution implemented is to use a real driver telemetry and try to follow it. This concept
has been used for example in Roborace previous stages. Roborace will be a competition
with autonomously driving electrically powered vehicles that looks like it is going to be a
reference competition in autnomous vehicles field [15].
But how can this telemetry will be achieved? As explained in section 2.3, Idiada provided me
with VI-Grade CarRealTime software. But this is not everything, not only I can run this
sofware like a block representing my vehicle inside MATLAB but, very recently, Idiada has
adquired VI-Grade Compact Simulator that allows to control the steering wheel and pedals
manually. Then, racing circuit Dry Handling introduced before will be driven and telemetry
obtained will be used as an offline planner.
Page 38 Report
8. Simulations and results
In this section the MPC controller will be tested for each scenario created.
8.1. Urban scenario
To evaluate the performance in a typical urban scenario where speed cannot overpass
40kph, the trajectory in Figure 8-1 has been chosen.
Figure 8-1: Urban scenario with desired path highlighted in red
From all possible routes this one, which is the resulting shortest path to get from this origin
selected to this destination, most of the features available are included.
Then applying the method described in Section 7.1 the speed profile obtained is presented in
Figure 8-2. One can see how smooth are the transitions between road sections as well as
accelerations and jerks along.
Origin
Destination
Figure 8-2: Speed,acceleration
and jerk references to selected
path
MPC controller for Autonomous vehicle Page 39
So, using the mid-size Sedan as our vehicle model in VI-CarRealTime, a simulation is done
obtaining the following results.
Figure 8-3: Map of path with zoom in 6 curved sections (reference in red and real path in blue)
5
6
4
1
2
3
3 1
2
4 5 6
Page 40 Report
Figure 8-3 shows the curved sections zoomed to see the deviation and Figure 8-4 several
data obtained like speed error, lateral deviation, wheel toe angle and accelerations.
Figure 8-4 Data obtained in simulution
It can be observed that in sections 1, 3 and 5 where radius of curvature is around 5 meters
the vehicle has more trouble to follow perfectly reaching lateral deviations of 50 cm. On the
other way, sections 2, 4 and 6 the radius of curvature is between 15 and 30 meters and does
not suffer so much reaching lateral deviations around 10 cm.
Among possible causes generating these errors we find the fact that the speed profile has
been selected to reach lateral accelerations around 0.5g on purpose to see how it reacts in
demanding cases. Also see that wheel toe angle in some cases reaches 20º which is a very
high value adding more complexity to that manoeuvres.
Then the most remarkable fact is that looking carefully all 6 sections one can see how the
real path is always trying to make a more feasible one that the reference. If we remember the
trajectory was done coupling straight sections with circular ones so the resulting reference
curvature has steps between each transition then vehicle controller tries to do it more
smoothly. This would explain why in very curved sections the lateral deviation is that high as
it needs to deviate this much to smoothen the curvature. Figure 8-5 shows this behaviour:
MPC controller for Autonomous vehicle Page 41
Figure 8-5: Reference curvature vs real curvature in simulation
Another way of looking it is that the controller is acting a bit as a planner adapting the
trajectory.
So, how can one improve this lack? If one thinks in MPC, it is based in minimizing a cost
function with terms smoothing control inputs and terms minimizing states with its references.
The way to go would be to decrease the weight in minimizing control inputs and giving more
importance to follow correctly the states. In our case, by lowing more these values oscillating
behaviour was starting to appear in wheel steering input which is not very desirable.
However, it is difficult to say as one would have to try it in a real vehicle to get a better feeling
and see whether the response is natural or not.
The conclusion is that the planner should generate paths that are feasible with smooth
curvatures and the controller should rely on it and try to follow it the best it can without
making modifications.
8.2. Racing Scenario
To test the controller in our Racing scenario, Dry Handling, a sophisticated planner involving
vehicle dynamics would be required. However, as explained in Section 7.2, instead of a
planner, telemetry from a real driver has been used.
So, an experimented driver has done up to 6 laps using Racecar in Dry Handling so that
each lap is taking more to the limit the vehicle till his best performance.
In Table 2 it is represented the time needed to do each lap by the driver and time required for
the Racecar controlled by MPC algorithm. Then also some data like maximum and average
lateral deviation and maximum lateral acceleration. Figure 8-6 shows how some of these
data is evolving over time for some of the laps.
Page 42 Report
Reference
time
Simulation
time
Max. lateral
deviation
Average
lateral
deviation
Max. lateral
acceleration
Lap 1 62.9 sec 62.52 sec 40.9 cm 13 cm 1.41 G’s
Lap 2 51.1 sec 51.54 sec 1.21 m 33.74 cm 2.01 G’s
Lap 3 48.3 sec 48.81 sec 1.25 m 36.31 cm 2.22 G’s
Lap 4 47 sec 47.64 sec 1.35 m 39.85 cm 2.34 G’s
Lap 5 45.8 sec 46.35 sec 1.75 m 44.51 cm 2.51 G’s
Lap 6 44.6 sec 45.3 sec 2.11 m 59.39 cm 2.75 G’s
Table 2: Comparision between 6 Laps simulated in Dry Handling
The results show that controller is able to follow but not with a proper precision. Average
lateral deviation goes from 13 to 60 cm and maximum values up to 2 meters. In these last
cases in video simulations it is observed how the vehicle is drifting consequently losing grip
but no adapting to the circumstances. As the model in the controller only considers the lineal
region of tyres, when the vehicle finds itself out of this region the predictions are not good
enough and understeering happens.
Figure 8-6: Graphics comparision between laps 1,2,4 and 6 in Dry Handling
MPC controller for Autonomous vehicle Page 43
8.3. Real-time feasibility
In this section real-time feasibility will be analysed.
To see how much time the controller takes to make all calculations, a clock has been used at
the beginning and at the end of the algorithm. In Figure 8-7 we can see that average time
needed to perform is 24 milliseconds. Taking into account that sample time used is 30
milliseconds, real-time performance is barely achieved. However, this simulation is running in
MATLAB code and when time arrives to implement the controller, code will be compiled with
programming language like C++ or Python which speeds up the process up to 10 times.
Then real-time feasibility is assured.
Simulations have been running in a HP computer with processor: Intel core i7-6700HQ CPU
@ 2.60GHzx2).
Figure 8-7: Time used from MPC controller each 𝑇𝑠 iteration
8.3.1. Sample time and horizon of prediction affectation
Lap 2 references from Table 2 has been selected to analyse the affectation on changing
sample time and horizon of prediction.
Figure 8-8 shows the resulting time needed by the controller and lateral deviation when
controller is running at different frequencies. By changing the sample time the problem to
solve does not change its complexity and timing does not get affected. However lateral
deviation increases according to sample time. Average deviation goes to 29, 59 and 69
centimetres.
On the other side, Figure 8-9 shows the same results but now changing the horizon of
prediction. By doing such a think the MPC problem increases exponentially as it is observed
and deviation seems to not be that much affected.
Also, one must consider that by reducing sample time, using the same horizon, controller is
Page 44 Report
embracing less distance so both parameters are highly related. So, to choose your settings
one should try to find the equilibrium between minimizing the sample time and maximizing
the horizon of prediction considering the power of computing available. Sample time and
horizon chosen in this thesis are 30 milliseconds and 15 steps respectively.
Figure 8-8: Affectance of changing sample time
Figure 8-9: Affectance on changing horizon of prediction
MPC controller for Autonomous vehicle Page 45
9. Budget
To estimate the budget destined to finance this thesis the main things to consider are the
hours spent for the development and licenses of the software used. Remember that this
project only aims to the simulation phase and vehicle and other cost involved in real tests are
not part of this work.
In Table 3 are depicted all costs except from the VI-Grade products which are confidential
and cannot be shared in here.
Origin Breakdown Concept (€)
Mathworks license Matlab (annual license) 800
Optimization Toolbox (annual
license)
460
Simulink (annual license) 1200
VI-Grade software and
hardware
VI-CarRealTime -
VI- Grade Compact Simulator -
Hours of work 900 h (6,5 € / 1 hour) 5850
Total 8310 €
Table 3: Budget table
Impact on environment is not analysed as is not applicable because of the simulation nature
of the project.
Page 46 Report
Conclusions
Good behaviour in urban scenarios has been achieved although in most demanding curves
the controller smoothens the original path to a more feasible one which can be bad in some
situations. However, the lack of path planning strategy is probably the responsible of this
behaviour.
In racing scenarios it has been demonstrated to being able to follow although the lateral
deviations obtained are a bit far. Considering lateral accelerations reach 2G and longitudinal
1G, it is understandable as the model used, the bicycle model, is not taking into account
pitch or roll motions that really affect the vehicle. Also, bringing the vehicle to its tyre limits
shows how it starts to drift.
In spited of all this, improving the model and complementing the MPC controller with active
systems like an anti-lock braking system (ABS) or a traction control system (TCS) which are
expected to be functionalities of an autonomous car, would result in a much better control in
a racing scenario.
Another important conclusion is the importance of the planner which is the one that must
include all comforts features like maximum acceleration or jerk achieved. This means that
planner is aware if you are going to do a safe manoeuvre or on the contrary, a very risky and
demanding manoeuvre (forced by current circumstances) while the controller must be
prepared to follow the more correctly the references from the planner without changing the
reference trajectory.
Finally, one of the conclusions arrived is that MPC strategy can be applied for control
purposes but rearranging the problem and increasing the horizon it can work also as a
planner. Its capability to predict the future make it very suitable for autonomous driving as
continuous changes in real driving occurs.
MPC controller for Autonomous vehicle Page 47
Future work
In reference to future work there are lot of features that can be upgraded.
First of all, during the project has been observed the big importance of having a good
planning strategy which is fully related to final performance and can generate very useful
data for the controller to predict future states. So, developing a planner that can work in real
time and consider the vehicle dynamics is one of the topics to work with in the future.
Secondly, regarding the controller several upgrades are pending. For example the model can
be improved. One could see if it is viable to use a 4 wheel model and add pitch and roll
motions. Also improve the tyre forces including the limits and loss of grip would be very
interesting for very demanding situations.
Thirdly, a more efficient and automatic way of adjusting weights on cost function would be
very interesting to accelerate the whole process of testing and may be even adjust in real
time depending on situations.
Finally, about the resistance force it is mandatory to implement a good estimator as it affects
strongly in the model like for example when you change to a road with different friction
coefficient or a day with a lot of wind. And, not only is important to estimate the resistance
force but also estimators that update parameters of the model that may vary in each
situation. For example the mass of the vehicle depending on how many people is inside and
situational additional weights or the cornering stiffness of the tyres that is degraded over long
periods of time.
Page 48 Report
Acknowledgments
I would like to thank the people who have helped me to do this project. Starting with my tutor
in Idiada, Guido Tosolin, who first gave me the opportunity to make this research project and
later on supported me technically and with guidance. Then inside Idiada I would like to thank
also Ivan Mula, who helped me driving the Racecar in racing scenario and rest of colleagues
that at some point supported me especially in matters related to vehicle dynamics like tyres
behaviour.
Also I would like to thank our collaborators in IRI research centre. Especially to Eugenio
Alcala who gave me a lot of knowledge about optimization tools, control and planning
strategies.
Finally, I would like to thank my tutor in university, Arnau Doria, who for the second time has
led me in a final-title project and always has been available to solve any problem.
MPC controller for Autonomous vehicle Page 49
Bibliography
[1] Ren, H., Shim, T., Ryu, J., and Chen, S., "Development of Effective Bicycle Model for
Wide Ranges of Vehicle Operations," SAE Technical Paper 2014-01-0841, 2014,
doi:10.4271/2014-01-0841.
[2] Christian Olsson, “Model Complexity and Coupling of Longitudinal and Lateral Control
in Autonomous Vehicles Using Model Predictive Control”, Degree project in automatic
control second level, Stockholm, Sweden 2015
[3] Joop P.Pauwelussen,”Essentials of vehicle dynamics”,February 1,2015,Eastbourne UK
[4] M. Acosta, S. Kanarachos, “Teaching a vehicle to autonomously drift: A data-based
approach using Neural Networks”, Knowledge-Based Systems (2018),
https://doi.org/10.1016/j.knosys.2018.04.015
[5] Lin, Y., Tang, P.,Zhang, W.J. and Yu, Q. (0000) “Artificial neural network modelling of driver
handling behaviour in a driver vehicle environment system”, Int. J.Vehicle Design, Vol. 00,
Nos. 0/0, pp.000000.
[6] Yingzi Lin, P. Tang, W. J. Zhang,”A rule-based neural network approach to model
driver naturalistic behavior in traffic”, September 2012,
http://dx.doi.org/10.1016/j.trc.2012.09.011
[7] James B. “Rawlings, Tutorial Overview in of Model Predictive Control”, June 2000
[8] Dale E.Seborg, Thomas F.Edgar, Duncan A. Mellichamp, “Process Dynamics and
control, Chapter 20”, 10/12
[9] Bo Bernhardsson, Karl Johan Åström, Presentation with topic: “Model Predictive
Control (MPC)”, Department of Automatic Control LTH, Lund University
[10] Pedro F. Lima, “Predictive control for autonomous driving with experimental
evaluation on a heavy-duty construction truck”, Licenciate Thesis Stockholm,
Sweden 2016
[11] Guo H. Y.,Ji Y.,Qu T. Chen H., “Understanding and Modeling the Human Driver
Behavior Based on MPC”, 7th IFAC Symposium on Advances in Automotive Control
The International Federation of Automatic ControlSeptember 4-7, 2013. Tokyo,
Japan
Page 50 Report
[12] Gonçalo C. Pereira, “Model Predictive Control for Autonomous Driving of Over-
Actuated Research Vehicle”, Degree project in electrical engineering second cycle,
Stockholm, Sweden 2016.
[13] Rachid Attia, Rodolfo Orjuela, Michel Basset. “Combined longitudinal and lateral
control for automated vehicle guidance”. Vehicle System Dynamics, Taylor &
Francis, 2014, 52 (2), pp.261-279.<10.1080/00423114.2013.874563>. <hal-
01027591>
[14] Online Derivator calculator: https://www.numberempire.com
[15] Official Roborace website: https://roborace.com
[16] E.Alcala, V. Puig, J.Quevedo, T.Escobet, “Gain Scheduling LPV Control for
Autonomous Vehicles including Friction Force Estimation and Compensation
Mechanism”, Advanced Control Systems Group, Automatic Control Department,
Universitat Politècnica de Catalunya (UPC).
top related