SlideShare a Scribd company logo
Université Joseph
Fourier
CHALMERSUniversity
Internship Report
Development and implementing
algorithms for autonomous overtaking
Author :
M. Jeremy Bernard
Supervisors :
M. Emmanuel Witrant
M. Jonas Sjöberg
M. Nikolce Murgovski
June 23, 2016
Contents
Introduction 4
1 Car Model 6
1.1 Basics of Vehicle Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.2 Frame conversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3 Lateral Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.1 Lateral States . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.2 Beta angle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.4 Longitudinal Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2 Optimal Trajectory Generator 12
2.1 Optimization overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.2 Constraints Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.3 Mathematical formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.3.1 Longitudinal and lateral dynamics . . . . . . . . . . . . . . . . . . 14
2.3.2 State and control constraints . . . . . . . . . . . . . . . . . . . . . 14
2.3.3 Problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.4 Optimal Trajectory example . . . . . . . . . . . . . . . . . . . . . . . . . 15
3 Controller 16
3.1 Longitudinal Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3.1.1 Speed Control (Cruise Control) . . . . . . . . . . . . . . . . . . . 17
3.1.2 Spacing Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2 Lateral Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
4 Global Simulink Environment 19
5 Desktop Simulation 20
5.1 Single Re-Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
5.2 Multiple Re-Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . 21
5.2.1 Innite time Simulation . . . . . . . . . . . . . . . . . . . . . . . 22
Références 23
1
List of Figures
1 Denition of All dierent vehicles . . . . . . . . . . . . . . . . . . . . . . 3
2 Advanced Driver Assistance Systems . . . . . . . . . . . . . . . . . . . . 4
3 Global Simulink Environment . . . . . . . . . . . . . . . . . . . . . . . . 5
4 Global Simulink Environment (Focused on Car Model) . . . . . . . . . . 6
5 Two wheels vehicle model . . . . . . . . . . . . . . . . . . . . . . . . . . 7
6 Tire slip angle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
7 Global Simulink Environment (Focused on Optimal Trajectory) . . . . . 12
8 Optimal Solver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
9 Constraints Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
10 Constraints Example obtained . . . . . . . . . . . . . . . . . . . . . . . . 13
11 Optimal Trajectory example . . . . . . . . . . . . . . . . . . . . . . . . . 15
12 Adaptive Cruise Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
13 Cruise Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
14 Adaptive Cruise Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
15 Lateral Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
16 Global Simulink Environment with Signals . . . . . . . . . . . . . . . . . 19
17 Single Re-Optimization Trajectory . . . . . . . . . . . . . . . . . . . . . 20
18 Single Re-Optimization Longitudinal Acceleration (Input) . . . . . . . . 20
19 Single Re-Optimization Steering (Input) . . . . . . . . . . . . . . . . . . 20
20 Single Re-Optimization Longitudinal Speed . . . . . . . . . . . . . . . . . 21
21 Single Re-Optimization Lateral Speed . . . . . . . . . . . . . . . . . . . . 21
22 Multiple Re-Optimization Trajectory . . . . . . . . . . . . . . . . . . . . 21
23 Multiple Re-Optimization Longitudinal Acceleration (Input) . . . . . . . 22
24 Multiple Re-Optimization Steering (Input) . . . . . . . . . . . . . . . . . 22
25 Multiple Re-Optimization Trajectory (70s simulation) . . . . . . . . . . . 22
2
Table 1: Nomenclature.
Symbol Description
X, Y Coordinates in the inertial frame
x, y Coordinates in the vehicle frame
˙X, ˙Y Longitudinal and lateral velocity in the inertial frame
˙x, ˙y Longitudinal and lateral velocity in the vehicle frame
ψ Yaw (heading) angle
˙ψ Yaw rate
Acc Acceleration (input)
δ Steering angle (input)
m Vehicle mass
Iz Moment of inertia z-axis
Cαf Tire constant of the front wheel
Cαr Tire constant of the rear wheel
lf Distances from CoG to the front wheel
lr Distances from CoG to the rear wheel
Vocabulary denition:
1. Ego vehicle (controlled vehicle)
2. Leading vehicle
3. Adjacent vehicle
4. Oncoming vehicle
Figure 1: Denition of All dierent vehicles
3
Introduction
The vehicles we drive are becoming increasingly more complex and many electronic tech-
nologies that had once no link whatsoever with the automotive industry are changing our
driving experience. Our cars have started to assess the situation in trac much faster
than humans can do thanks to many ingenious sensor-based driving assistance systems.
As a next step, exchanging dedicated technical information between vehicles or between
vehicles and the outside world is also within practical reach. Other participants in trac
like pedestrians and bicyclists could benet as well if detected by in-car advanced driving
assistance systems (ADAS 2) and avoided when an accident becomes imminent, or simply
warned the driver [[3]].
Figure 2: Advanced Driver Assistance Systems
Even in uncritical trac situations and decent weather conditions Many vehicle roadway
accident happens. According to accident data over the last 10 years from German road-
ways [[2]], 14.5% of all accidents were due to unintended lane departure, 35% of these
with deadly consequences.
Reasons for lane departure are many and range from the driver's inability to control
the vehicle (for example, on account of severe weather conditions) to lack of attention,
drowsiness, and intoxication or illness leading to the loss of lane-keeping control.
4
In this thesis, the focus is made on a function able to perform an automated overtaking.
The idea is to create a Simulink environment were dierent algorithm can be tested.
The approach in the current tested algorithm is to create an optimal trajectory of the
overtaking and then to build the controller which makes the car follow this trajectory.
In the following chapter, a complete description of the used overtaking algorithm, the
modeling of the car, the generator of the optimal trajectory and the nal controller will
be provided.
The nal objective is to provide a complete Simulink environment with both a car model,
a controller (lateral and longitudinal) and a optimal trajectory generator, as described in
Figure 3.
Figure 3: Global Simulink Environment
As it is depicted, the Car Model provides information both to generate the optimal
trajectory and to control the vehicle. The Optimal Trajectory Generator sends this
trajectory to the Controller. Finally the Controller computes the input signal of the car
and with those inputs the Car Model can update the current states of the car. A more
detailed explanation the global Simulink environment is provided in chapter 4.
5
1 Car Model
The rst step, before any test drive on an actual vehicle, is to perform desktop simulation.
To do so, a Car Model is needed to apply the overtaking algorithm on it and observe the
car behavior. This approach is used in every engineering process on complex systems,
it is called Model-Based Design. Model-Based Design is transforming the way engineers
and scientists work by moving design tasks from the lab and eld to the desktop. It
allows the engineers to do system verication, saving time and avoiding the introduction
of manually coded errors. In the global Simulink environment created the Car Model is
used as displayed in Figure 4.
Figure 4: Global Simulink Environment (Focused on Car Model)
In this section, an explanation of the used Car Model will be provided. All the theory
of this chapter is coming from [1]. The developed model is named the bicycle model. It
is a dynamic model which describes the lateral and longitudinal motions of the vehicle.
Such a model provides a mathematical description of the vehicle motion. The equations
of motion are based purely on geometric relationships governing the system. In this
model the car is seen as a bicycle, with one rear wheel (without steering) and one front
wheel (with steering) and a Center of Gravity (CoG) between them (Figure 5). All the
dimensions used in our examples are taken from the Volvo S60 car's dimensions.
1.1 Basics of Vehicle Dynamics
The inuence of road bank angle will not be considered here, we assume that our vehicle
are moving along a straight road every time. Applying Newton's second law for motion
6
Figure 5: Two wheels vehicle model
along the y axis we obtain:
may = Fyf + Fyr (1)
Where ay is the inertial acceleration of the vehicle at the CoG in the direction of the y
axis and Fyf and Fyr are the lateral tire forces of the front and rear wheels respectively.
The inertial acceleration is made of two term : the acceleration ¨y which is due to motion
along the y axis and the centripetal acceleration ˙x ˙ψ. Such as:
ay = ¨y + ˙x ˙ψ (2)
By substituting from equation (2) into equation (1):
m(¨y + ˙x ˙ψ) = Fyf + Fyr (3)
Furthermore, moment balance about the z axis yields the equation for the yaw dynamics
as:
Iz
¨ψ = lf Fyf − lrFyr (4)
Where lf and lr are the distances of the front tire and the rear tire respectively from the
CoG of the vehicle.
7
Now, it is need to model the lateral tire forces Fyf and Fyr. To do so, let's introduce the
slip angle αf and αr of each wheels as:
αf = δ − θV f
αr = −θV r
(5)
Where θV f is the angle that the velocity vector makes with the longitudinal axis of the
vehicle (called front tire velocity angle) and δ is the front wheel steering angle. The rear
slip angle is dened dierently since there is no steering for this wheel. The slip angle of
a tire and velocity angle are clearly explained on Figure 6.
Figure 6: Tire slip angle
Now, the lateral tire forces can be written as:
Fyf = 2Cαf (δ − θV f )
Fyr = 2Cαr(−θV r)
(6)
Where the proportionality constants Cαf and Cαr are called the cornering stiness of
each front and rear tire.
To compute θV f and θV r, the following relations can be used:
tan(θV f ) =
˙y + lf
˙ψ
˙x
tan(θV r) =
˙y + lr
˙ψ
˙x
In this case, small angle approximation can be used:
θV f =
˙y + lf
˙ψ
˙x
θV r =
˙y + lr
˙ψ
˙x
(7)
8
Now, all the basics dynamics of the vehicle are explain, a description of the lateral and
longitudinal model will be provide.
1.2 Frame conversion
The dierence between the inertial frame and the vehicle frame is showed by the dierence
in the writing of the signals: capitals letters are for the inertial frame and small ones are
for the vehicle frame. The dierence is depicted in Figure 5. To go from one frame to
the other the following relationships are used [6].
˙X = cos(ψ) ˙x − sin(ψ) ˙y
˙Y = sin(ψ) ˙x + cos(ψ) ˙y
˙x = cos(ψ) ˙X + sin(ψ) ˙Y
˙y = − sin(ψ) ˙X + cos(ψ) ˙Y
1.3 Lateral Model
Here, it is described either the mathematical denition of the lateral model and the way
it is computed on the global Simulink environment.
1.3.1 Lateral States
The lateral states are:




y
˙y
ψ
˙ψ




By Substituting from equations (5) and (7) into (3) and (4) we obtain the following lateral
model:




˙y
¨y
˙ψ
¨ψ



 =





0 1 0 0
0 −
2Cαf +2Cαr
m ˙x
0 − ˙x −
2Cαf lf −2Cαrlr
m ˙x
0 0 0 1
0 −
2Cαf lf −2Cαrlr
˙xIz
0 −
2Cαf lf
2
+2Cαrlr
2
˙xIz









y
˙y
ψ
˙ψ



 +




0
2Cαf
m
0
2Cαf lf
Iz



 δ
Where δ is the Steering input of our Model.
The focus is made on the states ¨y and ¨ψ by removing the lines 1 and 3 of the previous
model.
¨y
¨ψ
=
−
2Cαf +2Cαr
m ˙x
− ˙x −
2Cαf lf −2Cαrlr
m ˙x
−
2Cαf lf −2Cαrlr
˙xIz
−
2Cαf lf
2
+2Cαrlr
2
˙xIz
˙y
˙ψ
+
2Cαf
m
2Cαf lf
Iz
δ (8)
By developing the matrix computation of equation (8) we obtain:
9
¨y = −
2Cαf + 2Cαr
m ˙x
˙y + − ˙x −
2Cαf lf − 2Cαrlr
m ˙x
˙ψ +
2Cαf
m
δ
¨ψ = −
2Cαf lf − 2Cαrlr
˙xIz
˙y + −
2Cαf lf
2
+ 2Cαrlr
2
˙xIz
˙ψ +
2Cαf lf
Iz
δ
To compute numerically the updated states ( ˙y, ˙ψ) in the model (each sampling time Ts),
an Euler approximation is used:
¨yt =
˙yt − ˙yt−1
∆t
⇔ ˙yt = ˙yt−1 + ¨yt∆t
¨ψt =
˙ψt − ˙ψt−1
∆t
⇔ ˙ψt = ˙ψt−1 + ¨ψt∆t
The resulting ˙yt and ˙ψt are:
˙yt = ˙yt−1 + −
2Cαf + 2Cαr
m ˙x
˙yt−1 − ˙x +
2Cαf lf − 2Cαrlr
m ˙x
˙ψt−1 +
2Cαf
m
δt ∆t
˙ψt = ˙ψt−1 + −
2Cαf lf − 2Cαrlr
˙xIz
˙yt−1 −
2Cαf l2
f + 2Cαrl2
r
˙xIz
˙ψt−1 +
Cαf lf
Iz
δt ∆t
Same Euler 1st order approximation for y and ψ
˙yt =
yt − yt−1
∆t
⇔ yt = yt−1 + ˙yt∆t
˙ψt =
ψt − ψt−1
∆t
⇔ ψt = ψt−1 + ˙ψt∆t
NB: Here we computed y and ˙y which are in the vehicle frame, but we might need to
convert them into Y and ˙Y in the inertial frame, Cf 1.2.
1.3.2 Beta angle
The Beta Angle (β) is the slip angle of the vehicle at the CoG, as depicted in Figure 5.
It is used to control the lateral motion of the vehicle as explained further in chapter 3.2.
The trigonometrical formula of β is:
βt = arctan
lr
lr + lf
tan(δt)
10
1.4 Longitudinal Model
The longitudinal state are only longitudinal position and longitudinal velocity. The
method is the same than with the lateral state, taking the longitudinal acceleration and
integrate it with rst order Euler approximation (once to obtain the velocity and twice
to obtain the position). The Longitudinal Acceleration is the input of our model, but we
need to consider the fact that when the vehicle is steering (δ = 0), the global longitudinal
acceleration of the vehicle is decreasing.
Applying Newton's second law for motion along the x axis we obtain:
max = Fxf + Fxr (9)
Where Fxr = 0.
By substituting from equation (7) into equation (6):
Fyf = 2Cαf δ −
˙y + lf
˙ψ
˙x
(10)
Projecting equation (10) on the x axis with small angle approximation:
Fxf = 2Cαf δ −
˙y + lf
˙ψ
˙x
δ (11)
By substituting from equation (11) into equation (9):
ax =
1
m
2Cαf δ −
˙y + lf
˙ψ
˙x
δ (12)
Therefore lateral motion is:
¨x = Acc − ax (13)
To compute numerically the updated state ( ˙x in the model (each sampling time Ts),
an Euler approximation is used:
¨xt =
˙xt − ˙xt−1
∆t
⇔ ˙xt = ˙xt−1 + ¨xt∆t (14)
By substituting from equations (12) and (13) into equation (14):
˙xt = ˙xt−1 +

Acc −
1
m
2Cαf ∗

δt −
lf
˙ψt + ˙yt
˙xt−1

 ∗ δt

 ∆t
11
2 Optimal Trajectory Generator
In this part, an explanation of the optimal trajectory generator will be provided. The
optimal trajectory is used, afterward in the lateral control as a reference, as depicted in
Figure 7.
Figure 7: Global Simulink Environment (Focused on Optimal Trajectory)
In order to nd the best trajectory, an optimization problem needs to be solve. After
formulated the mathematical problem, a tool named FORCES PRO has been used. This
tool can generate code, that solves it orders of magnitude faster than available optimizers
[[4]]. The exact formulation of the optimization problem will be provide in 2.3.
2.1 Optimization overview
The optimal trajectory computation takes into account: all the lateral states of the ego
vehicles and the constraints imposed by the surrounding vehicles and the road.
Figure 8: Optimal Solver
12
NB: The lateral states of the ego vehicle are lateral: position, velocity, acceleration and
jerk; And they are coming straightly from the Car Model.
2.2 Constraints Function
To provide the constraints to the optimization solver, we integrated a Matlab function
inside our Optimal Trajectory Generator Block as depicted in Figure 8. The constraints
3 dierent constraints are: ymin,ymax and yref , which are respectively the right limit,
the left limit and the lateral reference position for the Ego vehicle. The goal of those
constraints is to nd the trajectory which respect the two limits ymin and ymax and be as
close as possible to the yref .
As an example, in the case of one leading vehicle the constraints would be as:
Figure 9: Constraints Example
NB: This constraints are dened for the all prediction horizon (40s in our case) and with
the assumption that all longitudinal vehicle's speed are constant.
The real constraints obtained, with our function, in the case of a simple overtaking is
depicted in Figure 10.
0 5 10 15 20 25 30 35 40
Relative longitudinal position (m)
0
1
2
3
4
5
6
7
8
9
10
Lateralposition(m)
ymin
ymax
yref
Figure 10: Constraints Example obtained
13
2.3 Mathematical formulation
The problem is formulated to minimize deviation from a reference velocity and position
trajectory, while keeping the vehicle on the road and avoiding collision with surrounding
vehicles. We show that the optimization problem can be formulated as a convex program,
by providing convex modeling steps that include change of reference frame, change of
variables, sampling in relative longitudinal distance and linearization. All the details are
explained in [5].
2.3.1 Longitudinal and lateral dynamics
The ego vehicle is modeled as a point mass system, with state and control vectors dened
as
xE(t) = [XE(t), ˙XE(t), YE(t)]T
uE(t) = [ ¨XE(t), ˙YE(t)]T
Where XE and YE denote longitudinal and lateral position in the inertial frame.
2.3.2 State and control constraints
Constraints are imposed on the state and control signals of the ego vehicle
xE(t) ∈ [xmin, xmax]
uE(t) ∈ [umin, umax]
With the limits dened as
xmin = [0, vL + , ymin]
xmax = [free, vxmax, ymax]
umin = [axmin, vymin]
umax = [axmax, vymax]
Where axmin, axmax, vymin, vymax ad vxmax are constants dened by our scenarios, and
vL + represents the fact that the minimum longitudinal speed of the ego vehicle needs
to be higher than the leading vehicle's speed (  0).
2.3.3 Problem statement
We can now formulate the optimization problem as:
min
uE
J(xE, uE, ˙uE) (15)
14
2.4 Optimal Trajectory example
As a matter of example, this is a perfect trajectory:
0 5 10 15 20 25 30 35 40
Relative longitudinal position (m)
0
1
2
3
4
5
6
7
8
9
10
Lateralposition(m)
ymin
ymax
yref
Optimal Trajectory
Figure 11: Optimal Trajectory example
A trajectory such as the one depicted in Figure 11 is the reference use to control laterally
our vehicle. In the next section we will see how this controller is made.
15
3 Controller
As it was explained previously, in order to overtake the car needs to be control both in
its lateral and longitudinal motion. The controller is divided in two cases, which are:
either we need to control the steering or either not. In the rst case, we will control only
the Longitudinal Acceleration and use an ACC (Adaptive Cruise Control) or even only
a CC (Cruise Control) if there is no other car on the road. In the second case where we
need to move the car laterally, we will use the optimal trajectory computed previously
and follow it.
3.1 Longitudinal Control
An ACC system is an extension of the standard cruise control system. An ACC equipped
vehicle has a radar or other sensor that measures the distance to other preceding vehicles
(downstream vehicles) on the highway. In the absence of preceding vehicles, the ACC
vehicle travels at a user-set speed, much like a vehicle with a standard cruise control
system (see Figure 12). However, if a preceding vehicle is detected on the highway by
the vehicle's radar, the ACC system determines whether or not the vehicle can continue
to travel safely at the desired speed.
Figure 12: Adaptive Cruise Control
If the preceding vehicle is too close or traveling too slowly, then the ACC system switches
from speed control to spacing control. In spacing control, the ACC vehicle controls both
the throttle and brakes so as to maintain a desired spacing from the preceding vehicle.
Assumption: In all our scenarios we assume the road is straight, without any curves.
According to this assumption, the longitudinal control consist only on controlling one of
the input of the car: the Longitudinal Acceleration (Acc) and set the Steering to 0.
16
3.1.1 Speed Control (Cruise Control)
In the case of non-presence of any vehicle in front of the Ego vehicle, a cruise control is
used. The only goal of this controller is to make to car reach a certain speed. To do so,
a PID controller has been implemented as depicted in Figure 13
Figure 13: Cruise Control
3.1.2 Spacing Control
The spacing control is a expended way of the Cruise control seen previously. In this case
the Longitudinal Speed Desired is provided by another controller which take into account
the distance between the Ego vehicle and the Leading vehicle. When the Longitudinal
Speed Desired is determined, it is sent in the Cruise Controller described before, as
depicted in Figure 14.
Figure 14: Adaptive Cruise Control
3.2 Lateral Control
As explained previously, the lateral control is made on following an optimal trajectory
to do so, state feedback has been done. The feedback is made on the parameters named
Course Angle which is the angle between direction of the velocity V of the car and the
inertial frame, as depicted in Figure 5. The Course Angle (γ) value is:
γ = ψ + β (16)
The rst step was to extract the Course Angle Reference from the Optimal trajectory
and then compare it to the real Course Angle of the car (given by the model). Then a
17
PI controller has been used to correct the error in this comparison, as depicted in Figure
15.
Figure 15: Lateral Controller
The computation of the γref is made according to the following relation:
γref = arctan
˙Yref
˙Xref
(17)
Where ˙Yref and ˙Xref are respectively the lateral and longitudinal reference velocity (op-
timal trajectory).
18
4 Global Simulink Environment
After the denition of all the component of the Simulink environment, the summarise of
this is the following. The Car Model uses the control inputs (Acc and δ) to update the
current states of the Ego vehicles and the Car Model is also used to update the states of
the other surrounding vehicle (which are not controlled). The States computed are used
either by the Optimal Trajectory Generator and the Controller. The Controller uses the
Optimal Trajectory and the states of the Ego vehicle to compute the input to apply to
the car (model).
Figure 16: Global Simulink Environment with Signals
19
5 Desktop Simulation
The simulations are divided in two categories: single re-optimization of the trajectory
and multiple re-optimization of the trajectory.
5.1 Single Re-Optimization
The prediction horizon of the optimal trajectory is 40s in all of our simulations. Firstly we
tried to follow this trajectory and do the overtaking without re-optimizing the trajectory.
The simulation lasts 40s or less, and the trajectory is computed at the rst instant of the
simulation and then no other optimal trajectory are generated. In this case we obtained
the results depicted in Figure 17.
0 100 200 300 400 500 600
X position (m)
2
3
4
5
6
7
8
Yposition(m)
Real Trajectory
Reference Trajectory
Figure 17: Single Re-Optimization Trajectory
When the two inputs: Acc and δ are showed in Figure 18 and 19.
0 5 10 15 20 25 30
time (sec)
-0.02
-0.015
-0.01
-0.005
0
0.005
0.01
0.015
0.02
0.025
0.03
Acceleration(m/s
2
)
Real Acceleration
Figure 18: Single Re-Optimization Longitu-
dinal Acceleration (Input)
0 5 10 15 20 25 30
time (sec)
-8
-6
-4
-2
0
2
4
6
Steering(rad)
×10-3
Real Steering
Figure 19: Single Re-Optimization Steering
(Input)
20
The little change in the Longitudinal Acceleration is explained by the change in the
Longitudinal Speed (Figure 20). Same for the Steering and the Lateral Speed (Figure
21).
0 5 10 15 20 25 30
time (sec)
19.44
19.445
19.45
19.455
19.46
19.465
19.47
19.475
19.48
19.485
Vxspeed(m/s)
Real Longitudinal Speed
Figure 20: Single Re-Optimization Longitu-
dinal Speed
0 5 10 15 20 25 30
time (sec)
-0.015
-0.01
-0.005
0
0.005
0.01
Vyspeed(m/s)
Real Lateral Speed
Figure 21: Single Re-Optimization Lateral
Speed
5.2 Multiple Re-Optimization
The main objectives of this approach is to able to run indenite time simulation. To do
so we triggered the Optimal Trajectory Generator (every 2s in our case) and when a new
trajectory is generated it automatically replaces the previous reference in our controller.
In the exact same conguration than the previous simulation (single re-optimization) we
obtained the results depicted in Figure 22.
0 100 200 300 400 500 600
X position (m)
2
3
4
5
6
7
8
Yposition(m)
Real Trajectory
Reference Trajectory
Figure 22: Multiple Re-Optimization Trajectory
When the two inputs: Acc and δ are showed in Figure 23 and 24.
21
0 5 10 15 20 25 30
time (sec)
-0.02
-0.015
-0.01
-0.005
0
0.005
0.01
0.015
0.02
0.025
0.03
Acceleration(m/s
2
)
Real Acceleration
Figure 23: Multiple Re-Optimization Longi-
tudinal Acceleration (Input)
0 5 10 15 20 25 30
time (sec)
-10
-8
-6
-4
-2
0
2
4
6
Steering(rad)
×10-3
Real Steering
Figure 24: Multiple Re-Optimization Steer-
ing (Input)
5.2.1 Innite time Simulation
The new approach allow the simulation to be longer than the prediction horizon. For
instance in a scenario with two leading vehicle: one in the prediction horizon at the rst
instant and one outside of this zone. It is showed that the Ego vehicle is now able to run
innite time scenario since the rst prediction (in red in Figure 25) has been followed
rstly and then the new trajectory computed detected the second vehicle and so on.
0 10 20 30 40 50 60 70
time (s)
2
3
4
5
6
7
8
Yposition(m)
Real Trajectory
Reference Trajectory
Figure 25: Multiple Re-Optimization Trajectory (70s simulation)
22
References
[1] Rajesh Rajamani Vehicles Dynamics and Control. Springer Edition 2012
[2] J. Pohl, W. Birk, and L. Westervall A driver-distraction-based lane-keeping assis-
tance system. Proc. IMechE Vol. 221 Part I: J. Systems and Control Engineering
2007
[3] M. van Schijndel-de Nooij, B. Krosse, T. van den Broek, S. Maas, E. van Nunen,
H. Zwijnenberg, A. Schieben, H. Mosebach, N. Ford, M. McDonald, D. Jeery, J.
Piao, and J. Sanchez, Denition of necessary vehicle and infrastructure systems
for automated driving. European Commission 2011
[4] https://www.embotech.com/FORCES-Pro.
[5] N. Murgovski, J. Sjoberg Predictive cruise control with autonomous overtaking.
IEEE Conference on Decision and Control 2015
[6] Paolo Falcone , H. Eric Tseng , Francesco Borrelli , Jahan Asgari and Davor Hrovat
MPC-based yaw and lateral stabilization via active front steering and braking.
Vehicle System Dynamics 2009
23

More Related Content

Viewers also liked

Revolución energética y la sostenibilidad
Revolución energética y la sostenibilidadRevolución energética y la sostenibilidad
Revolución energética y la sostenibilidad
Joel Corteggiano
 
Derecho Procesal Penal II
Derecho Procesal Penal IIDerecho Procesal Penal II
Derecho Procesal Penal II
margelis Muñoz
 
Portfolio
PortfolioPortfolio
Portfolio
Julia Shearin
 
SmarArt
SmarArtSmarArt
Manual de instalacion arranque dual linux windows
Manual de instalacion arranque dual linux windowsManual de instalacion arranque dual linux windows
Manual de instalacion arranque dual linux windows
Jaime Villamil
 
animaciones
animacionesanimaciones
Hipervinculo
HipervinculoHipervinculo
Bachelor_thesis_Jeremy_Bernard
Bachelor_thesis_Jeremy_BernardBachelor_thesis_Jeremy_Bernard
Bachelor_thesis_Jeremy_BernardJeremy Bernard
 
Revolución energética y la sostenibilidad
Revolución energética y la sostenibilidadRevolución energética y la sostenibilidad
Revolución energética y la sostenibilidad
Joel Corteggiano
 
Emich Qualifications Summary Dec2016
Emich Qualifications Summary Dec2016Emich Qualifications Summary Dec2016
Emich Qualifications Summary Dec2016Chuck Emich
 
Margelis muñoz m.docx tributario
Margelis muñoz m.docx tributarioMargelis muñoz m.docx tributario
Margelis muñoz m.docx tributario
margelis Muñoz
 
Guide to Entering things in QB By Gionni Paul Taxes
Guide to Entering things in QB By Gionni Paul TaxesGuide to Entering things in QB By Gionni Paul Taxes
Guide to Entering things in QB By Gionni Paul TaxesGionni Paul
 
Problema4.leccion3
Problema4.leccion3Problema4.leccion3
Problema4.leccion3
Hans Sáez
 
Revolución energética y la sostenibilidad
Revolución energética y la sostenibilidadRevolución energética y la sostenibilidad
Revolución energética y la sostenibilidad
Joel Corteggiano
 

Viewers also liked (16)

Revolución energética y la sostenibilidad
Revolución energética y la sostenibilidadRevolución energética y la sostenibilidad
Revolución energética y la sostenibilidad
 
Derecho Procesal Penal II
Derecho Procesal Penal IIDerecho Procesal Penal II
Derecho Procesal Penal II
 
shahab cv (1)
shahab cv (1)shahab cv (1)
shahab cv (1)
 
Portfolio
PortfolioPortfolio
Portfolio
 
SmarArt
SmarArtSmarArt
SmarArt
 
Manual de instalacion arranque dual linux windows
Manual de instalacion arranque dual linux windowsManual de instalacion arranque dual linux windows
Manual de instalacion arranque dual linux windows
 
animaciones
animacionesanimaciones
animaciones
 
Hipervinculo
HipervinculoHipervinculo
Hipervinculo
 
Bachelor_thesis_Jeremy_Bernard
Bachelor_thesis_Jeremy_BernardBachelor_thesis_Jeremy_Bernard
Bachelor_thesis_Jeremy_Bernard
 
Richard Martinez #1 PDF
Richard Martinez #1 PDFRichard Martinez #1 PDF
Richard Martinez #1 PDF
 
Revolución energética y la sostenibilidad
Revolución energética y la sostenibilidadRevolución energética y la sostenibilidad
Revolución energética y la sostenibilidad
 
Emich Qualifications Summary Dec2016
Emich Qualifications Summary Dec2016Emich Qualifications Summary Dec2016
Emich Qualifications Summary Dec2016
 
Margelis muñoz m.docx tributario
Margelis muñoz m.docx tributarioMargelis muñoz m.docx tributario
Margelis muñoz m.docx tributario
 
Guide to Entering things in QB By Gionni Paul Taxes
Guide to Entering things in QB By Gionni Paul TaxesGuide to Entering things in QB By Gionni Paul Taxes
Guide to Entering things in QB By Gionni Paul Taxes
 
Problema4.leccion3
Problema4.leccion3Problema4.leccion3
Problema4.leccion3
 
Revolución energética y la sostenibilidad
Revolución energética y la sostenibilidadRevolución energética y la sostenibilidad
Revolución energética y la sostenibilidad
 

Similar to Report_Jeremy_Berard

Advances in Automobile Safety Systems - Prashant Kumar
Advances in Automobile Safety Systems - Prashant KumarAdvances in Automobile Safety Systems - Prashant Kumar
Advances in Automobile Safety Systems - Prashant Kumar
Prashant Kumar
 
Autonomous cargo transporter report
Autonomous cargo transporter reportAutonomous cargo transporter report
Autonomous cargo transporter report
MuireannSpain
 
ME75-2014-myan076-report
ME75-2014-myan076-reportME75-2014-myan076-report
ME75-2014-myan076-reportMicky Yang
 
Towards Digital Twin of a Flexible manufacturing system with AGV
Towards Digital Twin of a Flexible manufacturing system with AGV Towards Digital Twin of a Flexible manufacturing system with AGV
Towards Digital Twin of a Flexible manufacturing system with AGV
YasmineBelHajsalah
 
Vehicle Testing and Data Analysis
Vehicle Testing and Data AnalysisVehicle Testing and Data Analysis
Vehicle Testing and Data AnalysisBenjamin Labrosse
 
Electric_FSAE_Race_Car_MS_Thesis
Electric_FSAE_Race_Car_MS_ThesisElectric_FSAE_Race_Car_MS_Thesis
Electric_FSAE_Race_Car_MS_ThesisQuinn Sullivan
 
Design of a regional aircaft
Design of a regional aircaftDesign of a regional aircaft
Design of a regional aircaft
AlexGuerrero117
 
Cranfield mixte hybrid
Cranfield mixte hybridCranfield mixte hybrid
Cranfield mixte hybrid
Josep M Carbonell Oyonarte
 
development-accurate-position
development-accurate-positiondevelopment-accurate-position
development-accurate-positionJames Petrie
 
Report
ReportReport
Team Omni L2 Requirements Revised
Team Omni L2 Requirements RevisedTeam Omni L2 Requirements Revised
Team Omni L2 Requirements RevisedAndrew Daws
 
Tesis de posicionamiento
Tesis de posicionamientoTesis de posicionamiento
Tesis de posicionamiento
josesocola27
 
rc plane design guide
rc plane design guiderc plane design guide
rc plane design guide
CHARANBASIREDDY1
 
Video Games for Autonomous Driving
Video Games for Autonomous DrivingVideo Games for Autonomous Driving
Video Games for Autonomous DrivingArtur Filipowicz
 
1997 mitsubishi galant service repair manual
1997 mitsubishi galant service repair manual1997 mitsubishi galant service repair manual
1997 mitsubishi galant service repair manual
fjksekmdme
 

Similar to Report_Jeremy_Berard (20)

Alinia_MSc_S2016
Alinia_MSc_S2016Alinia_MSc_S2016
Alinia_MSc_S2016
 
finalReport
finalReportfinalReport
finalReport
 
Advances in Automobile Safety Systems - Prashant Kumar
Advances in Automobile Safety Systems - Prashant KumarAdvances in Automobile Safety Systems - Prashant Kumar
Advances in Automobile Safety Systems - Prashant Kumar
 
Autonomous cargo transporter report
Autonomous cargo transporter reportAutonomous cargo transporter report
Autonomous cargo transporter report
 
Dmitriy Rivkin Thesis
Dmitriy Rivkin ThesisDmitriy Rivkin Thesis
Dmitriy Rivkin Thesis
 
ME75-2014-myan076-report
ME75-2014-myan076-reportME75-2014-myan076-report
ME75-2014-myan076-report
 
Towards Digital Twin of a Flexible manufacturing system with AGV
Towards Digital Twin of a Flexible manufacturing system with AGV Towards Digital Twin of a Flexible manufacturing system with AGV
Towards Digital Twin of a Flexible manufacturing system with AGV
 
ProjectLatestFinal
ProjectLatestFinalProjectLatestFinal
ProjectLatestFinal
 
Vehicle Testing and Data Analysis
Vehicle Testing and Data AnalysisVehicle Testing and Data Analysis
Vehicle Testing and Data Analysis
 
Electric_FSAE_Race_Car_MS_Thesis
Electric_FSAE_Race_Car_MS_ThesisElectric_FSAE_Race_Car_MS_Thesis
Electric_FSAE_Race_Car_MS_Thesis
 
Design of a regional aircaft
Design of a regional aircaftDesign of a regional aircaft
Design of a regional aircaft
 
Cranfield mixte hybrid
Cranfield mixte hybridCranfield mixte hybrid
Cranfield mixte hybrid
 
development-accurate-position
development-accurate-positiondevelopment-accurate-position
development-accurate-position
 
Report
ReportReport
Report
 
Team Omni L2 Requirements Revised
Team Omni L2 Requirements RevisedTeam Omni L2 Requirements Revised
Team Omni L2 Requirements Revised
 
Tesis de posicionamiento
Tesis de posicionamientoTesis de posicionamiento
Tesis de posicionamiento
 
bachelors-thesis
bachelors-thesisbachelors-thesis
bachelors-thesis
 
rc plane design guide
rc plane design guiderc plane design guide
rc plane design guide
 
Video Games for Autonomous Driving
Video Games for Autonomous DrivingVideo Games for Autonomous Driving
Video Games for Autonomous Driving
 
1997 mitsubishi galant service repair manual
1997 mitsubishi galant service repair manual1997 mitsubishi galant service repair manual
1997 mitsubishi galant service repair manual
 

Report_Jeremy_Berard

  • 1. Université Joseph Fourier CHALMERSUniversity Internship Report Development and implementing algorithms for autonomous overtaking Author : M. Jeremy Bernard Supervisors : M. Emmanuel Witrant M. Jonas Sjöberg M. Nikolce Murgovski June 23, 2016
  • 2. Contents Introduction 4 1 Car Model 6 1.1 Basics of Vehicle Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.2 Frame conversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.3 Lateral Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.3.1 Lateral States . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.3.2 Beta angle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 1.4 Longitudinal Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 2 Optimal Trajectory Generator 12 2.1 Optimization overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.2 Constraints Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.3 Mathematical formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.3.1 Longitudinal and lateral dynamics . . . . . . . . . . . . . . . . . . 14 2.3.2 State and control constraints . . . . . . . . . . . . . . . . . . . . . 14 2.3.3 Problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.4 Optimal Trajectory example . . . . . . . . . . . . . . . . . . . . . . . . . 15 3 Controller 16 3.1 Longitudinal Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 3.1.1 Speed Control (Cruise Control) . . . . . . . . . . . . . . . . . . . 17 3.1.2 Spacing Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 3.2 Lateral Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 4 Global Simulink Environment 19 5 Desktop Simulation 20 5.1 Single Re-Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 5.2 Multiple Re-Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . 21 5.2.1 Innite time Simulation . . . . . . . . . . . . . . . . . . . . . . . 22 Références 23 1
  • 3. List of Figures 1 Denition of All dierent vehicles . . . . . . . . . . . . . . . . . . . . . . 3 2 Advanced Driver Assistance Systems . . . . . . . . . . . . . . . . . . . . 4 3 Global Simulink Environment . . . . . . . . . . . . . . . . . . . . . . . . 5 4 Global Simulink Environment (Focused on Car Model) . . . . . . . . . . 6 5 Two wheels vehicle model . . . . . . . . . . . . . . . . . . . . . . . . . . 7 6 Tire slip angle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 7 Global Simulink Environment (Focused on Optimal Trajectory) . . . . . 12 8 Optimal Solver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 9 Constraints Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 10 Constraints Example obtained . . . . . . . . . . . . . . . . . . . . . . . . 13 11 Optimal Trajectory example . . . . . . . . . . . . . . . . . . . . . . . . . 15 12 Adaptive Cruise Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 13 Cruise Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 14 Adaptive Cruise Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 15 Lateral Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 16 Global Simulink Environment with Signals . . . . . . . . . . . . . . . . . 19 17 Single Re-Optimization Trajectory . . . . . . . . . . . . . . . . . . . . . 20 18 Single Re-Optimization Longitudinal Acceleration (Input) . . . . . . . . 20 19 Single Re-Optimization Steering (Input) . . . . . . . . . . . . . . . . . . 20 20 Single Re-Optimization Longitudinal Speed . . . . . . . . . . . . . . . . . 21 21 Single Re-Optimization Lateral Speed . . . . . . . . . . . . . . . . . . . . 21 22 Multiple Re-Optimization Trajectory . . . . . . . . . . . . . . . . . . . . 21 23 Multiple Re-Optimization Longitudinal Acceleration (Input) . . . . . . . 22 24 Multiple Re-Optimization Steering (Input) . . . . . . . . . . . . . . . . . 22 25 Multiple Re-Optimization Trajectory (70s simulation) . . . . . . . . . . . 22 2
  • 4. Table 1: Nomenclature. Symbol Description X, Y Coordinates in the inertial frame x, y Coordinates in the vehicle frame ˙X, ˙Y Longitudinal and lateral velocity in the inertial frame ˙x, ˙y Longitudinal and lateral velocity in the vehicle frame ψ Yaw (heading) angle ˙ψ Yaw rate Acc Acceleration (input) δ Steering angle (input) m Vehicle mass Iz Moment of inertia z-axis Cαf Tire constant of the front wheel Cαr Tire constant of the rear wheel lf Distances from CoG to the front wheel lr Distances from CoG to the rear wheel Vocabulary denition: 1. Ego vehicle (controlled vehicle) 2. Leading vehicle 3. Adjacent vehicle 4. Oncoming vehicle Figure 1: Denition of All dierent vehicles 3
  • 5. Introduction The vehicles we drive are becoming increasingly more complex and many electronic tech- nologies that had once no link whatsoever with the automotive industry are changing our driving experience. Our cars have started to assess the situation in trac much faster than humans can do thanks to many ingenious sensor-based driving assistance systems. As a next step, exchanging dedicated technical information between vehicles or between vehicles and the outside world is also within practical reach. Other participants in trac like pedestrians and bicyclists could benet as well if detected by in-car advanced driving assistance systems (ADAS 2) and avoided when an accident becomes imminent, or simply warned the driver [[3]]. Figure 2: Advanced Driver Assistance Systems Even in uncritical trac situations and decent weather conditions Many vehicle roadway accident happens. According to accident data over the last 10 years from German road- ways [[2]], 14.5% of all accidents were due to unintended lane departure, 35% of these with deadly consequences. Reasons for lane departure are many and range from the driver's inability to control the vehicle (for example, on account of severe weather conditions) to lack of attention, drowsiness, and intoxication or illness leading to the loss of lane-keeping control. 4
  • 6. In this thesis, the focus is made on a function able to perform an automated overtaking. The idea is to create a Simulink environment were dierent algorithm can be tested. The approach in the current tested algorithm is to create an optimal trajectory of the overtaking and then to build the controller which makes the car follow this trajectory. In the following chapter, a complete description of the used overtaking algorithm, the modeling of the car, the generator of the optimal trajectory and the nal controller will be provided. The nal objective is to provide a complete Simulink environment with both a car model, a controller (lateral and longitudinal) and a optimal trajectory generator, as described in Figure 3. Figure 3: Global Simulink Environment As it is depicted, the Car Model provides information both to generate the optimal trajectory and to control the vehicle. The Optimal Trajectory Generator sends this trajectory to the Controller. Finally the Controller computes the input signal of the car and with those inputs the Car Model can update the current states of the car. A more detailed explanation the global Simulink environment is provided in chapter 4. 5
  • 7. 1 Car Model The rst step, before any test drive on an actual vehicle, is to perform desktop simulation. To do so, a Car Model is needed to apply the overtaking algorithm on it and observe the car behavior. This approach is used in every engineering process on complex systems, it is called Model-Based Design. Model-Based Design is transforming the way engineers and scientists work by moving design tasks from the lab and eld to the desktop. It allows the engineers to do system verication, saving time and avoiding the introduction of manually coded errors. In the global Simulink environment created the Car Model is used as displayed in Figure 4. Figure 4: Global Simulink Environment (Focused on Car Model) In this section, an explanation of the used Car Model will be provided. All the theory of this chapter is coming from [1]. The developed model is named the bicycle model. It is a dynamic model which describes the lateral and longitudinal motions of the vehicle. Such a model provides a mathematical description of the vehicle motion. The equations of motion are based purely on geometric relationships governing the system. In this model the car is seen as a bicycle, with one rear wheel (without steering) and one front wheel (with steering) and a Center of Gravity (CoG) between them (Figure 5). All the dimensions used in our examples are taken from the Volvo S60 car's dimensions. 1.1 Basics of Vehicle Dynamics The inuence of road bank angle will not be considered here, we assume that our vehicle are moving along a straight road every time. Applying Newton's second law for motion 6
  • 8. Figure 5: Two wheels vehicle model along the y axis we obtain: may = Fyf + Fyr (1) Where ay is the inertial acceleration of the vehicle at the CoG in the direction of the y axis and Fyf and Fyr are the lateral tire forces of the front and rear wheels respectively. The inertial acceleration is made of two term : the acceleration ¨y which is due to motion along the y axis and the centripetal acceleration ˙x ˙ψ. Such as: ay = ¨y + ˙x ˙ψ (2) By substituting from equation (2) into equation (1): m(¨y + ˙x ˙ψ) = Fyf + Fyr (3) Furthermore, moment balance about the z axis yields the equation for the yaw dynamics as: Iz ¨ψ = lf Fyf − lrFyr (4) Where lf and lr are the distances of the front tire and the rear tire respectively from the CoG of the vehicle. 7
  • 9. Now, it is need to model the lateral tire forces Fyf and Fyr. To do so, let's introduce the slip angle αf and αr of each wheels as: αf = δ − θV f αr = −θV r (5) Where θV f is the angle that the velocity vector makes with the longitudinal axis of the vehicle (called front tire velocity angle) and δ is the front wheel steering angle. The rear slip angle is dened dierently since there is no steering for this wheel. The slip angle of a tire and velocity angle are clearly explained on Figure 6. Figure 6: Tire slip angle Now, the lateral tire forces can be written as: Fyf = 2Cαf (δ − θV f ) Fyr = 2Cαr(−θV r) (6) Where the proportionality constants Cαf and Cαr are called the cornering stiness of each front and rear tire. To compute θV f and θV r, the following relations can be used: tan(θV f ) = ˙y + lf ˙ψ ˙x tan(θV r) = ˙y + lr ˙ψ ˙x In this case, small angle approximation can be used: θV f = ˙y + lf ˙ψ ˙x θV r = ˙y + lr ˙ψ ˙x (7) 8
  • 10. Now, all the basics dynamics of the vehicle are explain, a description of the lateral and longitudinal model will be provide. 1.2 Frame conversion The dierence between the inertial frame and the vehicle frame is showed by the dierence in the writing of the signals: capitals letters are for the inertial frame and small ones are for the vehicle frame. The dierence is depicted in Figure 5. To go from one frame to the other the following relationships are used [6]. ˙X = cos(ψ) ˙x − sin(ψ) ˙y ˙Y = sin(ψ) ˙x + cos(ψ) ˙y ˙x = cos(ψ) ˙X + sin(ψ) ˙Y ˙y = − sin(ψ) ˙X + cos(ψ) ˙Y 1.3 Lateral Model Here, it is described either the mathematical denition of the lateral model and the way it is computed on the global Simulink environment. 1.3.1 Lateral States The lateral states are:     y ˙y ψ ˙ψ     By Substituting from equations (5) and (7) into (3) and (4) we obtain the following lateral model:     ˙y ¨y ˙ψ ¨ψ     =      0 1 0 0 0 − 2Cαf +2Cαr m ˙x 0 − ˙x − 2Cαf lf −2Cαrlr m ˙x 0 0 0 1 0 − 2Cαf lf −2Cαrlr ˙xIz 0 − 2Cαf lf 2 +2Cαrlr 2 ˙xIz          y ˙y ψ ˙ψ     +     0 2Cαf m 0 2Cαf lf Iz     δ Where δ is the Steering input of our Model. The focus is made on the states ¨y and ¨ψ by removing the lines 1 and 3 of the previous model. ¨y ¨ψ = − 2Cαf +2Cαr m ˙x − ˙x − 2Cαf lf −2Cαrlr m ˙x − 2Cαf lf −2Cαrlr ˙xIz − 2Cαf lf 2 +2Cαrlr 2 ˙xIz ˙y ˙ψ + 2Cαf m 2Cαf lf Iz δ (8) By developing the matrix computation of equation (8) we obtain: 9
  • 11. ¨y = − 2Cαf + 2Cαr m ˙x ˙y + − ˙x − 2Cαf lf − 2Cαrlr m ˙x ˙ψ + 2Cαf m δ ¨ψ = − 2Cαf lf − 2Cαrlr ˙xIz ˙y + − 2Cαf lf 2 + 2Cαrlr 2 ˙xIz ˙ψ + 2Cαf lf Iz δ To compute numerically the updated states ( ˙y, ˙ψ) in the model (each sampling time Ts), an Euler approximation is used: ¨yt = ˙yt − ˙yt−1 ∆t ⇔ ˙yt = ˙yt−1 + ¨yt∆t ¨ψt = ˙ψt − ˙ψt−1 ∆t ⇔ ˙ψt = ˙ψt−1 + ¨ψt∆t The resulting ˙yt and ˙ψt are: ˙yt = ˙yt−1 + − 2Cαf + 2Cαr m ˙x ˙yt−1 − ˙x + 2Cαf lf − 2Cαrlr m ˙x ˙ψt−1 + 2Cαf m δt ∆t ˙ψt = ˙ψt−1 + − 2Cαf lf − 2Cαrlr ˙xIz ˙yt−1 − 2Cαf l2 f + 2Cαrl2 r ˙xIz ˙ψt−1 + Cαf lf Iz δt ∆t Same Euler 1st order approximation for y and ψ ˙yt = yt − yt−1 ∆t ⇔ yt = yt−1 + ˙yt∆t ˙ψt = ψt − ψt−1 ∆t ⇔ ψt = ψt−1 + ˙ψt∆t NB: Here we computed y and ˙y which are in the vehicle frame, but we might need to convert them into Y and ˙Y in the inertial frame, Cf 1.2. 1.3.2 Beta angle The Beta Angle (β) is the slip angle of the vehicle at the CoG, as depicted in Figure 5. It is used to control the lateral motion of the vehicle as explained further in chapter 3.2. The trigonometrical formula of β is: βt = arctan lr lr + lf tan(δt) 10
  • 12. 1.4 Longitudinal Model The longitudinal state are only longitudinal position and longitudinal velocity. The method is the same than with the lateral state, taking the longitudinal acceleration and integrate it with rst order Euler approximation (once to obtain the velocity and twice to obtain the position). The Longitudinal Acceleration is the input of our model, but we need to consider the fact that when the vehicle is steering (δ = 0), the global longitudinal acceleration of the vehicle is decreasing. Applying Newton's second law for motion along the x axis we obtain: max = Fxf + Fxr (9) Where Fxr = 0. By substituting from equation (7) into equation (6): Fyf = 2Cαf δ − ˙y + lf ˙ψ ˙x (10) Projecting equation (10) on the x axis with small angle approximation: Fxf = 2Cαf δ − ˙y + lf ˙ψ ˙x δ (11) By substituting from equation (11) into equation (9): ax = 1 m 2Cαf δ − ˙y + lf ˙ψ ˙x δ (12) Therefore lateral motion is: ¨x = Acc − ax (13) To compute numerically the updated state ( ˙x in the model (each sampling time Ts), an Euler approximation is used: ¨xt = ˙xt − ˙xt−1 ∆t ⇔ ˙xt = ˙xt−1 + ¨xt∆t (14) By substituting from equations (12) and (13) into equation (14): ˙xt = ˙xt−1 +  Acc − 1 m 2Cαf ∗  δt − lf ˙ψt + ˙yt ˙xt−1   ∗ δt   ∆t 11
  • 13. 2 Optimal Trajectory Generator In this part, an explanation of the optimal trajectory generator will be provided. The optimal trajectory is used, afterward in the lateral control as a reference, as depicted in Figure 7. Figure 7: Global Simulink Environment (Focused on Optimal Trajectory) In order to nd the best trajectory, an optimization problem needs to be solve. After formulated the mathematical problem, a tool named FORCES PRO has been used. This tool can generate code, that solves it orders of magnitude faster than available optimizers [[4]]. The exact formulation of the optimization problem will be provide in 2.3. 2.1 Optimization overview The optimal trajectory computation takes into account: all the lateral states of the ego vehicles and the constraints imposed by the surrounding vehicles and the road. Figure 8: Optimal Solver 12
  • 14. NB: The lateral states of the ego vehicle are lateral: position, velocity, acceleration and jerk; And they are coming straightly from the Car Model. 2.2 Constraints Function To provide the constraints to the optimization solver, we integrated a Matlab function inside our Optimal Trajectory Generator Block as depicted in Figure 8. The constraints 3 dierent constraints are: ymin,ymax and yref , which are respectively the right limit, the left limit and the lateral reference position for the Ego vehicle. The goal of those constraints is to nd the trajectory which respect the two limits ymin and ymax and be as close as possible to the yref . As an example, in the case of one leading vehicle the constraints would be as: Figure 9: Constraints Example NB: This constraints are dened for the all prediction horizon (40s in our case) and with the assumption that all longitudinal vehicle's speed are constant. The real constraints obtained, with our function, in the case of a simple overtaking is depicted in Figure 10. 0 5 10 15 20 25 30 35 40 Relative longitudinal position (m) 0 1 2 3 4 5 6 7 8 9 10 Lateralposition(m) ymin ymax yref Figure 10: Constraints Example obtained 13
  • 15. 2.3 Mathematical formulation The problem is formulated to minimize deviation from a reference velocity and position trajectory, while keeping the vehicle on the road and avoiding collision with surrounding vehicles. We show that the optimization problem can be formulated as a convex program, by providing convex modeling steps that include change of reference frame, change of variables, sampling in relative longitudinal distance and linearization. All the details are explained in [5]. 2.3.1 Longitudinal and lateral dynamics The ego vehicle is modeled as a point mass system, with state and control vectors dened as xE(t) = [XE(t), ˙XE(t), YE(t)]T uE(t) = [ ¨XE(t), ˙YE(t)]T Where XE and YE denote longitudinal and lateral position in the inertial frame. 2.3.2 State and control constraints Constraints are imposed on the state and control signals of the ego vehicle xE(t) ∈ [xmin, xmax] uE(t) ∈ [umin, umax] With the limits dened as xmin = [0, vL + , ymin] xmax = [free, vxmax, ymax] umin = [axmin, vymin] umax = [axmax, vymax] Where axmin, axmax, vymin, vymax ad vxmax are constants dened by our scenarios, and vL + represents the fact that the minimum longitudinal speed of the ego vehicle needs to be higher than the leading vehicle's speed ( 0). 2.3.3 Problem statement We can now formulate the optimization problem as: min uE J(xE, uE, ˙uE) (15) 14
  • 16. 2.4 Optimal Trajectory example As a matter of example, this is a perfect trajectory: 0 5 10 15 20 25 30 35 40 Relative longitudinal position (m) 0 1 2 3 4 5 6 7 8 9 10 Lateralposition(m) ymin ymax yref Optimal Trajectory Figure 11: Optimal Trajectory example A trajectory such as the one depicted in Figure 11 is the reference use to control laterally our vehicle. In the next section we will see how this controller is made. 15
  • 17. 3 Controller As it was explained previously, in order to overtake the car needs to be control both in its lateral and longitudinal motion. The controller is divided in two cases, which are: either we need to control the steering or either not. In the rst case, we will control only the Longitudinal Acceleration and use an ACC (Adaptive Cruise Control) or even only a CC (Cruise Control) if there is no other car on the road. In the second case where we need to move the car laterally, we will use the optimal trajectory computed previously and follow it. 3.1 Longitudinal Control An ACC system is an extension of the standard cruise control system. An ACC equipped vehicle has a radar or other sensor that measures the distance to other preceding vehicles (downstream vehicles) on the highway. In the absence of preceding vehicles, the ACC vehicle travels at a user-set speed, much like a vehicle with a standard cruise control system (see Figure 12). However, if a preceding vehicle is detected on the highway by the vehicle's radar, the ACC system determines whether or not the vehicle can continue to travel safely at the desired speed. Figure 12: Adaptive Cruise Control If the preceding vehicle is too close or traveling too slowly, then the ACC system switches from speed control to spacing control. In spacing control, the ACC vehicle controls both the throttle and brakes so as to maintain a desired spacing from the preceding vehicle. Assumption: In all our scenarios we assume the road is straight, without any curves. According to this assumption, the longitudinal control consist only on controlling one of the input of the car: the Longitudinal Acceleration (Acc) and set the Steering to 0. 16
  • 18. 3.1.1 Speed Control (Cruise Control) In the case of non-presence of any vehicle in front of the Ego vehicle, a cruise control is used. The only goal of this controller is to make to car reach a certain speed. To do so, a PID controller has been implemented as depicted in Figure 13 Figure 13: Cruise Control 3.1.2 Spacing Control The spacing control is a expended way of the Cruise control seen previously. In this case the Longitudinal Speed Desired is provided by another controller which take into account the distance between the Ego vehicle and the Leading vehicle. When the Longitudinal Speed Desired is determined, it is sent in the Cruise Controller described before, as depicted in Figure 14. Figure 14: Adaptive Cruise Control 3.2 Lateral Control As explained previously, the lateral control is made on following an optimal trajectory to do so, state feedback has been done. The feedback is made on the parameters named Course Angle which is the angle between direction of the velocity V of the car and the inertial frame, as depicted in Figure 5. The Course Angle (γ) value is: γ = ψ + β (16) The rst step was to extract the Course Angle Reference from the Optimal trajectory and then compare it to the real Course Angle of the car (given by the model). Then a 17
  • 19. PI controller has been used to correct the error in this comparison, as depicted in Figure 15. Figure 15: Lateral Controller The computation of the γref is made according to the following relation: γref = arctan ˙Yref ˙Xref (17) Where ˙Yref and ˙Xref are respectively the lateral and longitudinal reference velocity (op- timal trajectory). 18
  • 20. 4 Global Simulink Environment After the denition of all the component of the Simulink environment, the summarise of this is the following. The Car Model uses the control inputs (Acc and δ) to update the current states of the Ego vehicles and the Car Model is also used to update the states of the other surrounding vehicle (which are not controlled). The States computed are used either by the Optimal Trajectory Generator and the Controller. The Controller uses the Optimal Trajectory and the states of the Ego vehicle to compute the input to apply to the car (model). Figure 16: Global Simulink Environment with Signals 19
  • 21. 5 Desktop Simulation The simulations are divided in two categories: single re-optimization of the trajectory and multiple re-optimization of the trajectory. 5.1 Single Re-Optimization The prediction horizon of the optimal trajectory is 40s in all of our simulations. Firstly we tried to follow this trajectory and do the overtaking without re-optimizing the trajectory. The simulation lasts 40s or less, and the trajectory is computed at the rst instant of the simulation and then no other optimal trajectory are generated. In this case we obtained the results depicted in Figure 17. 0 100 200 300 400 500 600 X position (m) 2 3 4 5 6 7 8 Yposition(m) Real Trajectory Reference Trajectory Figure 17: Single Re-Optimization Trajectory When the two inputs: Acc and δ are showed in Figure 18 and 19. 0 5 10 15 20 25 30 time (sec) -0.02 -0.015 -0.01 -0.005 0 0.005 0.01 0.015 0.02 0.025 0.03 Acceleration(m/s 2 ) Real Acceleration Figure 18: Single Re-Optimization Longitu- dinal Acceleration (Input) 0 5 10 15 20 25 30 time (sec) -8 -6 -4 -2 0 2 4 6 Steering(rad) ×10-3 Real Steering Figure 19: Single Re-Optimization Steering (Input) 20
  • 22. The little change in the Longitudinal Acceleration is explained by the change in the Longitudinal Speed (Figure 20). Same for the Steering and the Lateral Speed (Figure 21). 0 5 10 15 20 25 30 time (sec) 19.44 19.445 19.45 19.455 19.46 19.465 19.47 19.475 19.48 19.485 Vxspeed(m/s) Real Longitudinal Speed Figure 20: Single Re-Optimization Longitu- dinal Speed 0 5 10 15 20 25 30 time (sec) -0.015 -0.01 -0.005 0 0.005 0.01 Vyspeed(m/s) Real Lateral Speed Figure 21: Single Re-Optimization Lateral Speed 5.2 Multiple Re-Optimization The main objectives of this approach is to able to run indenite time simulation. To do so we triggered the Optimal Trajectory Generator (every 2s in our case) and when a new trajectory is generated it automatically replaces the previous reference in our controller. In the exact same conguration than the previous simulation (single re-optimization) we obtained the results depicted in Figure 22. 0 100 200 300 400 500 600 X position (m) 2 3 4 5 6 7 8 Yposition(m) Real Trajectory Reference Trajectory Figure 22: Multiple Re-Optimization Trajectory When the two inputs: Acc and δ are showed in Figure 23 and 24. 21
  • 23. 0 5 10 15 20 25 30 time (sec) -0.02 -0.015 -0.01 -0.005 0 0.005 0.01 0.015 0.02 0.025 0.03 Acceleration(m/s 2 ) Real Acceleration Figure 23: Multiple Re-Optimization Longi- tudinal Acceleration (Input) 0 5 10 15 20 25 30 time (sec) -10 -8 -6 -4 -2 0 2 4 6 Steering(rad) ×10-3 Real Steering Figure 24: Multiple Re-Optimization Steer- ing (Input) 5.2.1 Innite time Simulation The new approach allow the simulation to be longer than the prediction horizon. For instance in a scenario with two leading vehicle: one in the prediction horizon at the rst instant and one outside of this zone. It is showed that the Ego vehicle is now able to run innite time scenario since the rst prediction (in red in Figure 25) has been followed rstly and then the new trajectory computed detected the second vehicle and so on. 0 10 20 30 40 50 60 70 time (s) 2 3 4 5 6 7 8 Yposition(m) Real Trajectory Reference Trajectory Figure 25: Multiple Re-Optimization Trajectory (70s simulation) 22
  • 24. References [1] Rajesh Rajamani Vehicles Dynamics and Control. Springer Edition 2012 [2] J. Pohl, W. Birk, and L. Westervall A driver-distraction-based lane-keeping assis- tance system. Proc. IMechE Vol. 221 Part I: J. Systems and Control Engineering 2007 [3] M. van Schijndel-de Nooij, B. Krosse, T. van den Broek, S. Maas, E. van Nunen, H. Zwijnenberg, A. Schieben, H. Mosebach, N. Ford, M. McDonald, D. Jeery, J. Piao, and J. Sanchez, Denition of necessary vehicle and infrastructure systems for automated driving. European Commission 2011 [4] https://www.embotech.com/FORCES-Pro. [5] N. Murgovski, J. Sjoberg Predictive cruise control with autonomous overtaking. IEEE Conference on Decision and Control 2015 [6] Paolo Falcone , H. Eric Tseng , Francesco Borrelli , Jahan Asgari and Davor Hrovat MPC-based yaw and lateral stabilization via active front steering and braking. Vehicle System Dynamics 2009 23