A Margin-based Approach to Vehicle Threat
Assessment in a Homotopy Framework for
Semi-Autonomous Highway Navigation
by
Alexandre Constantin
Ingenieur Diplom6 de l'Ecole Polytechnique, 2012
Submitted to the Department of Mechanical Engineering
in partial fulfillment of the requirements for the degree of
Master of Science in Mechanical Engineering
at the
CHUSETTS INSTITUTE
OFTECHNOLaGY
I
MASSACHUSETTS INSTITUTE OF TECHNOLOGY
June 2014
@ Massachusetts Institute of Technology 2014. All rights reserved.
Signature redacted
A uthor ..............................
Depar tmenht cf Mechanical Engineering
May 21, 2014
C
Certified by....................
ignature redacted
Karl Iagnemma
Principal Research Scientist
Thesis Supervisor
Signature redacted
Ac c ep t e d by . . . . . . . . . . . . . . . . . . . . . . .
-sH
asr dt
Professor David Hardt
Chairman, Committee on Graduate Studies
AUG15 2014
LI_1BRARIES(3__
A Margin-based Approach to Vehicle Threat Assessment in
a Homotopy Framework for Semi-Autonomous Highway
Navigation
by
Alexandre Constantin
Submitted to the Department of Mechanical Engineering
on May 21, 2014, in partial fulfillment of the
requirements for the degree of
Master of Science in Mechanical Engineering
Abstract
This thesis describes the design of an unified framework for threat assessment
and the shared control between a human driver and the onboard controller, based
on the notion of fields of safe travel. It allows to perform corridor navigation,
trajectory planning, threat assessment and driving assistance in hazardous
situations. A new approach to the threat assessment problem is introduced, based
upon the estimation of the control freedom afforded to a vehicle. Given sensor
information of the surrounding environment, an algorithm first identifies corridors of
travel through which the vehicle can safely navigate. The second stage then consists
in assessing the potential threat posed to the vehicle in each identified corridor
thanks to a metric associated with available control margin. For this purpose, the
fields of safe travel are associated with sets of homotopic trajectories generated
either from a lattice sampled in the vehicles input space or from a conformal state
lattice. This level of threat is the keystone of the system and serves as input to
influence autonomous navigation or driver support inputs. The semi-autonomous
control system aims to honor safe driver inputs while ensuring safe and robust
navigation properties. It ideally operates only during instances of significant threat:
it should give a driver full control of the vehicle in "low threat" situations but apply
appropriate levels of computer-controlled actuator effort during "high threat"
situations. This approach preserves the freedom of control of the human driver
when he/she remains within a safe navigable corridor, and adjust the vehicle
trajectory when its predicted future state falls out of a safe field, or when the lowest
threat exceeds some threshold. In fully autonomous mode, this human-inspired
motion planning approach ensures collision free navigation and driving comfort.
Thesis Supervisor: Karl Iagnemma
Title: Principal Research Scientist
3
4
Acknowledgments
First and foremost, I would like to thank Dr. Karl Iagnemma for his guidance
and advice through out my research and the writing of my thesis.
I have
particularly appreciated the careful blend between supervision and independence
which has afforded me a great confidence and motivation in my work. I would also
like to extend my gratitude to the other members of the Robotic Mobility Group,
Junghee Park, Marwan Hussein, Carmine Senatore, Sisir Kurumanchi, Ryota Ono,
Youzhi Liang, for the enriching discussions and their availability for driving tests.
Special thanks to Junghee, colleague and friends, for our fruitful collaboration. His
advice and insight have played a significant role in the progress of my research.
I would also like to thank the French Ministry of Defence for giving me the
opportunity to study at MIT and their financial support.
Last but not least, I would like to thank my roommates and close friends Remi
Lam and Louis Boulanger as well as my wonderful family for their full support during
this short but exciting research adventure in Boston.
5
6
Contents
17
Introduction
1.2
Background and Literature Review
. . . . . . . . . . . . . . .
17
. . . . . . . . . . . . . . . .
20
.
Problem Statement and Motivations
.
1.1
Collision Avoidance and Threat Assessment
. . . . . . .
20
1.2.2
Autonomous Planning . . . . . . . . . . . . . . . . . . .
21
1.2.3
Homotopy Representation . . . . . . . . . . . . . . . . .
22
1.2.4
Shared Control and Driver Intent Prediction . . . . . . .
24
Purpose and Outline of the Thesis . . . . . . . . . . . . . . . . .
25
2 Identification of Fields of Safe Travel - Homotopy Formulation
29
2.4
.
.
.
30
.
. . . . . . . . . .
Topological Definition
. . . . . . . . .
. . . . . . . . . .
31
2.1.2
Road Maps and Topological Graphs . .
. . . . . . . . . .
32
2.1.3
Cell Decomposition . . . . . . . . . . .
. . . . . . . . . .
35
Lane Structure and Target Zones . . . . . . .
. . . . . . . . . .
38
2.2.1
Spatial Constraint Decomposition . . .
. . . . . . . . . .
39
2.2.2
Target Zones
. . . . . . . . . . . . . .
. . . . . . . . . .
41
Static Graph Search Approach . . . . . . . . .
. . . . . . . . . .
43
2.3.1
Static Obstacles . . . . . . . . . . . . .
. . . . . . . . . .
44
2.3.2
Dynamic Obstacle
. . . . . . . . . . .
. . . . . . . . . .
47
2.3.3
A Few Words on Complexity . . . . . .
. . . . . . . . . .
50
Enumeration of Navigation Strategies Approach
. . . . . . . . . .
51
. . . . . . . . . .
51
.
.
.
.
.
.
.
2.1.1
2.4.1
.
2.3
. . .
.
2.2
From Work Space to Topological Search
.
2.1
Decision Tree . . . . . . . . . . . . . .
.
1.3
.
1.2.1
.
1
7
2.5.1
Geometric Properties . . . . . . . . . . . . . . . . . . . . . .
56
2.5.2
Optimality Approach - Single Representative
. . . . . . . .
57
2.5.3
Margin Approach . . . . . . . . . . . . . . . . . . . . . . . .
59
.
.
.
55
61
3.1
State-Space Sampling . . . . . . . . . . . . . . . . . . . . . . . . . .
62
3.1.1
Path Generation
. . . . . . . . . . . . . . . . . . . . . . . .
62
3.1.2
Velocity Profile and Trajectory Generation . . . . . . . . . .
67
3.1.3
Application to Highway Navigation with Traffic . . . . . . .
69
.
.
.
Input Space Sampling
70
3.2.1
Vehicle M odel . . . . . . . . . . . . . . . . . . . . . . . . . .
71
3.2.2
Construction of the Input Lattice . . . . . . . . . . . . . . .
72
3.2.3
Conformation to the Constrained Environment . . . . . . . .
76
.
.
.
.
. . . . . . . . . . . . . . . . . . . . . . . . .
Advantages and Drawbacks
. . . . . . . . . . . . . . . . . . . . . .
.
3.3
.
Classes of Feasible Trajectories from State and Input Lattices
3.2
A Margin Based Approach to Threat Assessment
4.3
4.4
81
81
4.1.1
Room for Maneuver . . . . . . . . . . . . .
. . . . . . . . . .
82
4.1.2
Notations
. . . . . . . . . .
85
4.1.3
Trajectory Sorting
. . . . . . . . . .
87
4.1.4
Input Space versus Acceleration Space
. .
. . . . . . . . . .
89
Metrics to evaluate the Threat . . . . . . . . . . .
. . . . . . . . . .
90
4.2.1
Convex Hull versus Chebyshev Ball . . . .
. . . . . . . . . .
90
4.2.2
Norm Comparison and Weighted Average
. . . . . . . . . .
91
4.2.3
Simulated Scenarios on Highway . . . . . .
. . . . . . . . . .
92
. . . .
. . . . . . . . . .
98
4.3.1
Definition of the Cost Function . . . . . .
. . . . . . . . . .
99
4.3.2
Cost Comparison . . . . . . . . . . . . . .
. . . . . . . . . .
100
.
.
. . . . . . . . . .
.
. . . . . . . . . . . . . . . . . .
.
.
.
.
. . . . . . . . . . . . .
.
4.2
77
Guiding Principle . . . . . . . . . . . . . . . . . .
Comparison with Minimal Cost Approach
.
4.1
.
4
53
. . . . . . . . . . . . .
.
3
Evaluation Methods to Characterize Threat
.
2.5
Heuristics to Prune the Number of Candidates . . . . . . . .
.
2.4.2
Application to Decision Making For Autonomous Driving
8
102
5
Better Synergies Between Driver and Control System
5.1
5.2
5.3
5.4
105
. . . . . . . . . . . . . . . . . . . . . . . . .
106
. . . . . . . . . . . . . . . . . . . . . . . .
106
. . . . . . . . . . . . . . . . . . . . .
108
Control Intervention Law Based on Maximal Margin Available . . . .
111
Evolution of the Threat
5.1.1
Traffic Slow Down
5.1.2
Lane Merging Scenarios
5.2.1
Hypothesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
5.2.2
Experimental Setup with CarSim . . . . . . . . . . . . . . . . 112
5.2.3
Intervention law - Matlab Simulation . . . . . . . . . . . . . .
115
Intervention Law Based on Probabilistic Driver Model and Expected
M argin . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
119
5.3.1
Driver M odel . . . . . . . . . . . . . . . . . . . . . . . . . . .
119
5.3.2
Construction of the Intervention Law . . . . . . . . . . . . . .
120
5.3.3
Human Testing with CarSim . . . . . . . . . . . . . . . . . . .
122
Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
131
A 4-Wheeled Vehicle Model
133
B Eye-tracking Information
135
9
10
List of Figures
1-1
Highway example with three identified navigation corridors . . . . . .
19
1-2
Design and block diagram of the semi-autonomous control system . .
26
2-1
From path planning to homotopy class planning . . . . . . . . . . . .
30
2-2
Voronoi Diagram (left) and visibility graph (right) . . . . . . . . . . .
33
2-3
Delaunay Triangulation - Direction of navigation: from left to right
.
35
2-4
Trapezoidal decomposition
. . . . . . . . . . . . . . . . . . . . . . .
37
2-5
Possible scenarios in highway navigation
. . . . . . . . . . . . . . . .
38
2-6
Decompositon of the space accounting for the lane structure and obstacles 39
2-7
Time varying decomposition when lead vehicle changes lane
. . . . .
40
2-8
Location of target zones . . . . . . . . . . . . . . . . . . . . . . . . .
41
2-9
Location of target region in lane exit and lane merging . . . . . . . .
43
2-10 Cell decomposition and graph construction . . . . . . . . . . . . . . .
45
2-11 Mechanism of the Breadth First Search . . . . . . . . . . . . . . . . .
46
2-12 All possible homotopies with topological length less or equal to two .
47
. . . . . . . . . . . . . . . .
48
2-14 Topological graph for lane merging . . . . . . . . . . . . . . . . . . .
49
. . . . . . . . . . .
50
2-13 Graph construction in the dynamic case
2-15 Topological graph when lead vehicle changes lane
2-16 Number of homotopies against the number of obstacles (limited to at
most two lane changes) . . . . . . . . . . . . . . . . . . . . . . . . . .
51
2-17 Left: partition of the space to identifiy strategy, Right: target regions
52
2-18 Left: Decision tree, Right: one homotopy candidate identified in the tree 53
11
2-19 Examples of computing minimum number of lane changes for homotopy
.
candidates . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . .
54
. . . . . . . . . . .
55
3-1
Grid points on the road
. . . . . . . . . . . . .
. . . . . . . . . . .
63
3-2
Generation of the edges of the lattice . . . . . .
. . . . . . . . . . .
66
3-3
Velocity Profile . . . . . . . . . . . . . . . . . .
. . . . . .
68
3-4
Modification of the grid to fit the current scenario
. . . . . .
70
3-5
Simple bicyle model . . . . . . . . . . . . . . . .
. . . . . .
71
3-6
Refined steering profile . . . . . . . . . . . . . .
. . . . . .
74
3-7
Two reasonable velocity profiles . . . . . . . . .
. . . . . .
75
3-8
Example of input lattices . . . . . . . . . . . . .
. . . . . .
75
3-9
Fitting the shape of the road . . . . . . . . . . .
. . . . . .
76
. . . . . .
78
.
.
.
.
.
.
.
.
most two lane changes) . . . . . . . . . . . . . .
.
2-20 Number of homotopies against the number of obstacles (limited to at
3-10 Occupancy grid: input-space (left) and state-space (right)
Road scenario example . . . . . . . . . . . . . . . . . . . . . . .
83
4-2
Evolution of the control freedom within prediction horizon . . .
84
4-3
Definition of a specific homotopy in both identification methods
86
4-4
Definition of a specific homotopy in both identification methods
86
4-5
Illustration of the sorting algorithm . . . . . . . . . . . . . . . .
88
4-6
Sorting based on minimal distance to vehicle . . . . . . . . . . .
88
4-7 Trajectory sorting . . . . . . . . . . . . . . . . . . . . . . . . . .
89
4-8
Chebyshev ball inscribed in the convex hull . . . . . . . . . . . .
91
4-9
Different norms for the normalization of the threat margin . . .
92
4-10 Homotopy candidates and associated margin . . . . . . . . . . .
93
4-11 Normalized steering input for all homotopy candidates
. . . . .
94
4-12 Normalized throttle input for all homotopy candidates
. . . . .
94
.
.
.
.
.
.
.
.
.
.
4-1
96
4-14 Homotopy candidates and associated control freedom . . . . . .
97
4-15 Normalized steering and throttle inputs for all candidates . . . .
98
.
.
4-13 Comparison of norms, Top: convex hull, Bottom: Chebyshev ball
12
4-16 Threat Assessment for various cost functions Top: static example,
Bottom: dynamic example . . . . . . . . . . . . . . . . . . . . . . . .
101
4-17 Simulation of a highway scenario. The host vehicle (blue) navigates
. . . . . . . . . .
104
5-1
Margin at three time steps . . . . . . . . . . . . . . . . . . . . . . . .
107
5-2
Evolution of the margin of feasible corridors . . . . . . . . . . . . . .
108
5-3
Margin at three time steps . . . . . . . . . . . . . . . . . . . . . . . . 109
5-4
Evolution of the margin of feasible corridors . . . . . . . . . . . . . .
5-5
Average distance to the closest lead vehicle among human subjects
5-6
Safety threshold defined as first decile for margin choice among human
through traffic with the Chebyshev center as input
subjects
110
. 113
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
114
5-7
Intervention law based on maximal margin or driver intent . . . . . . 115
5-8
Top: Evolution of the maximal margin available Bottom: Trajectory
of the host vehicle in both cases . . . . . . . . . . . . . . . . . . . . . 116
5-9
Extrapolation of the margin . . . . . . . . . . . . . . . . . . . . . . .
117
5-10 Top: Evolution of the maximal margin available Bottom: Trajectory
of the host vehicle in both cases . . . . . . . . . . . . . . . . . . . . .
118
5-11 Intervention law based on expected margin . . . . . . . . . . . . . . . 122
5-12 Relative error between the expected margin and the actual choice of
the driver in function of the tuning parameter a . . . . . . . . . . . .
124
5-13 Typical Receiver Operating Characteristic curve for one human subject 126
5-14 Optimal Alpha tuning parameter among subjects . . . . . . . . . . .
127
5-15 Comparison of optimal alpha and minimum acceleration maneuver .
128
5-16 Percentage of True Positive and False Positive for given requirements
129
5-17 Performance of the prediction and variance of lateral offset . . . . . .
130
A-1 Vehicle Slip M odel . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133
B-1 Decomposition of the image with respect to obstacles . . . . . . . . .
137
B-2 Correlation between sources of threat and gaze points . . . . . . . . .
138
13
14
List of Tables
3.1
Pros and cons of input space versus state space sampling . . . . . . .
4.1
Elements of the cost funcion . . . . . . . . . . . . . . . . . . . . . . . 100
A. 1 Vehicle Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
79
134
16
Chapter 1
Introduction
1.1
Problem Statement and Motivations
Recent traffic safety reports from the National Highway Traffic and Safety
Administration show that in 2012 alone, over 33,500 people were killed and 2.4
million injured in motor vehicle accidents in the United States [1].
Human error
accounts for more than 90 % of these road accidents (e.g. speed, distraction, driving
under the influence). In contrast, only 3 % were due solely to mechanical fault and
5 % were caused only by environmental factors (slippery road, visibility...).
presence
of passive safety
systems in motor vehicles,
combined
The
with the
ever-increasing influence of active systems, has contributed to a decline in these
numbers from previous years. Still, even if the active safety systems have begun to
play a major role in collision mitigation [2], the need for improved collision
avoidance
technologies
and threat
assessment
methods
remains significant.
Nevertheless, considering human superior judgment capacities and current legal
issues, fully autonomous cars are not the main focus of this research. The ultimate
goad is to build a seamless semi-autonomous active safety system that preserves the
freedom of control of the drivers and only intervenes in a transparent manner when
the predicted threat exceeds a specified threshold.
Recent developments in onboard sensing, lane detection and obstacle recognition
have facilitated active safety systems that share steering and braking control with
17
the driver [3]. However, these active systems don't make the most of the sensory
information related to the vehicle surroundings. Most are limited to warnings (when
the driver crosses a lane or is close to an obstacle for instance) or are used to improve
the vehicle's performance and stability (antilock breaking, yaw and roll stability).
They are fundamentally reactive in nature an do not anticipate the future states
of the vehicle. They have therefore limited ability to assess the threat of potential
accidents and cannot exert preventive actions to avoid them. Yet, automated and
predictive driving technologies such as Adaptive Cruise Control (ACC) and Rear-end
Collision Avoidance systems [4], [5] are gradually getting more mature, although their
range of application still remains limited to very specific situations.
The margin-based approach for threat assessment described in this thesis aims
to provide a general method for accident avoidance and semi-autonomous navigation
that can handle complex situations. It relies on the observation that human drivers
tend to operate vehicles within fields of safe travel, rather than along a specific path.
Based on this, we propose a framework that relies on identification and analysis
of candidate fields, each of which can be interpreted to contain a path homotopy
(figure 1-1). In this purpose, the planner combines sensory information related to
the vehicle surroundings (nearby vehicles, pedestrians, road edges, and other salient
features) and the vehicle's dynamics in order to assess the threat posed to the vehicle
for each navigation option identified in the environment. The planner can use this
crucial threat estimation information to plan the safest navigation option (in an
autonomous system) or correct the anticipated driver's trajectory when the threat
exceeds a specified threshold (whose control is shared with a human).
18
Figure 1-1: Highway example with three identified navigation corridors
Threat to a vehicle is assumed to be proportional to the freedom of control afforded
by a particular field. This implies that (for instance) drivers tend to prefer to navigate
through regions that are wide and exhibit low curvature. In figure 1-1 for example,
the green corridor offers more control freedom than the red corridor and therefore
seems to be a better choice is the driver wants to change lane. More generally, this
proposed approach infers that safe drivers tend to anticipate future hazardous events
in order to maximize their future leeway [6]. This assumption is part of a broader
concept than human usually operate so as to keep future options as open as possible.
To characterize threat in each candidate field, a state lattice is constructed via
sampling in the vehicle input space. This sampling method enables identification of
sets of feasible trajectories associated with each field, from which the metrics related
to control freedom can be derived. Lattice algorithms have been extensively studied
and are highly flexible tools for motion planning that can adapt to complex
environments.
Lattice structures can be adapted to parallel computing methods,
making them computationally efficient and implementable in real-time at high
resolution [7].
19
1.2
1.2.1
Background and Literature Review
Collision Avoidance and Threat Assessment
Historically, safety measures were mostly limited to artifacts to reduce the effects
of collisions on vehicle occupants using, for instance, seat belts, air bags or designing
carefully the deformation of the chassis during a crash. Nowadays, the focus turns
increasingly on driver assistance systems designed to prevent those accidents from
happening. These active safety systems can be sorted into two categories: reactive
systems that apply low-level control actions triggered when a safety criteria is critical,
and predictive systems that consider not only the current state of the host vehicle,
but also the predicted state evolution of the vehicle though environmental hazards
for earlier and more efficient safety interventions.
Some reactive systems are already integrated in modern cars, among others
antilock brakes,
traction controllers,
lane-assist systems [8], [9], [10].
electronic stability controllers or even
Predictive systems remain limited to specific
situations, but are experiencing a growing attention. Most common system include
Adaptive Cruise Control, which is a control system that automatically adjusts the
vehicle speed to maintain a safe distance from vehicles ahead using sensor
information from on-board sensors [4], [11]. Another growing class of safety systems
are the rear-end collision avoidance systems [12], [5].
However, most of these
systems use threat metrics based on time to collision (TTC) estimations with
constant
velocity/acceleration
assumptions
[13],
[14]
and
don't
take
into
consideration the potential strategic planning of a human driver. These approaches
usually assess the most imminent threat of a given situation based on a minimum
lateral or longitudinal acceleration (brake threat and steering threat numbers) that
a simple maneuver would require to avoid collision given the current states of both
host and hazard [15], [16].
While these metrics can provide simple yet useful information of imminent
threat, they are not well suited to consider multiple hazards, complex dynamics and
constrained environments, as in the case of highway navigation with a lane structure
20
and a dense traffic.
Other approaches to estimate the future threat posed to a
vehicle are based on a reachability analysis, which aims to assess whether there
exists a feasible path through an environment to reach a desired goal, considering a
driver model [17].
This approach provides a manner for determining the time of
intervention that doesn't only consider basic trajectories such as constant evasive
maneuvers.
However, it only provides a binary assessment of reachability, and is
therefore limited in its ability to compare the desirability (i.e. 'goodness') of various
candidate navigation choices.
field-based framework [18],
More recently, optimal-control-based methods in a
[19] propose to tackle the navigation problem by
dividing the space into homotopy classes (corridors of travel) and generating
optimal trajectories within a particular class by minimizing a chosen objective
function. The threat assessment metric is computed from metrics associated with an
optimal path within a particular field, for example the minimal lateral acceleration
or the slip angle. These methods allow to take into consideration the behavior and
tactics of the human driver and complex navigation though obstacles. In [20], the
method is extended and the threat metrics also includes heuristics with respect to
the desirability of the corridor such as the average width and curvature.
1.2.2
Autonomous Planning
Development in recent years in onboard sensing, obstacle recognition and
computational power have made possible autonomous planning and control of
ground vehicles that require no input from the human operator.
Many car
manufacturers have announced that the first autonomous cars available to the
public will arrive by 2020.
Among them, the Google car [21] is one of the most
emblematic projects. At the same time, competitions such as the DARPA Grand
Challenge of 2007 promote the progress made in this area and foster the
development of technologies needed to create a fully autonomous ground vehicle
capable of completing an urban course within a limited time [22], [23].
A typical architecture for theses autonomous vehicles combines a perception layer
(LIDAR, camera, radars), a strategic motion planning layer and a low-level controller
21
[24]. Within the environment mapped by the perception layer, the motion planner
computes a collision-free trajectory from an initial location to a target goal. In the
context of autonomous planning, typical path planning tools include rapidly exploring
random trees [25], [26], potential fields [27],[28], Model Predictive Control (MPC), [29]
and lattice-based approaches [30],[31],[32] and [33]. The resulting path traditionally
doesn't consider the dynamic or kinematic constraints of the vehicle. The low-level
controller, usually performed at a higher frequency, converts the path of the motion
planner into a sequence of dynamically feasible inputs. However, in the framework
of MPC or closed-loop RRT, planning and control are usually performed in the same
calculation [34], [18].
All these approaches have proven effective in autonomous navigation, however
their reliance on specific reference paths (rather than spatial fields) can potentially
lead to non-robust performance in the presence of various forms of uncertainty that
can cause deviation from the planned path. Also, for semi-autonomous applications,
forced adherence to a specific path can feel non-intuitive or over-restrictive to a human
operator.
1.2.3
Homotopy Representation
If path-based approaches are well-suited for autonomous vehicles, they can feel
non-intuitive to a human operator. This is the reason why the work described in
this thesis relies on the notion of fields of safe travel that are modeled by homotopy
classes of feasible trajectories, i.e. trajectories that are continuously deformed from
one another. This representation is arguably closer to the human perception of the
driving task [35].
Despite being mostly an uncharted research area, homotopy class constraints
often appear in path planning problems.
The trajectories often need to satisfy
constraints arising due to goal requirements or the presence of hazards, which
translates into restricting the solution to certain homotopy-like classes [36]. Besides,
topological approaches are usually more robust to sensor noise and small
environment changes than path-based methods and require less computational
22
resources [37].
In this context, homotopy approaches have traditionally been
employed to reduce the complexity of the planning task by guiding topologically
hierarchal motion planners.
Essentially, these methods divide the configuration
space into a union of simpler problems (divide and conquer) by attempting to
capture the topological skeleton of the environment.
In [38], the author uses
homotopy classes to simplify the shortest path problem.
After computing the
topological graph, he implements a modified version of Breadth-First Search (BFS)
to explore the graph and identify homotopies that are not self-crossing.
Later,
Hernandez [39] extends the previous method by using Rapidly-exploring Random
Tree (RRT) in each homotopy to find the shortest path. In [40], the authors first
use probabilistic roadmaps to capture homotopy classes and then compute the most
appropriate trajectory of each class.
This method provides a robust motion
planning technique for mobile devices and robots in the context of maintenance or
dismantling operations in the cluttered environment of nuclear power plants.
In
[36], the authors propose a way to represent homotopy classes using the Cauchy
Integral theorem and derive the optimal path constrained to certain homotopy class
using graph search techniques.
More recently, optimal-control-based methods in a field-based framework [18],
[19] propose to tackle the navigation problem by dividing the space into homotopy
classes and generating optimal trajectories within a particular class by minimizing a
chosen objective function. In [20] , homotopy classes are identified using a Delanay
Triangulation in the constrained environment.
Each homotopy candidates is
assimilated into a navigation strategy that is evaluated based on some heuristic. An
optimal path is then computed in the desirable corridor using a MPC controller.
The approach describes in this thesis uses a similar framework, although the
algorithms for homotopy identification are different and take into consideration the
lane structure of highways.
23
1.2.4
Shared Control and Driver Intent Prediction
In spite of the large proportion of human error in most road accidents and the
recent spectacular progress made in the field of fully autonomous car, it is very likely
that humans will be kept in the control loop, at least partially, in the near future.
Human-automation interactions have been widely discussed and most studies tend
to show that human and automated systems perform best at distinct tasks [41],
[42]. Incidentally, human and machines are usually not interchangeable but rather
complementary, and a good semi-autonomous system should therefore make the most
of human-automation synergies.
As a matter of fact, automated systems excel at responding quickly and precisely
to a well defined control objective and can handle complex scenario as long as it falls
within it scope of action. In addition, they are not concerned by problems related
to repetitive tasks and fatigue. Humans, on the other hand, have superior judgment
and robust reasoning capabilities which gives them the unique ability to assimilate
new types of information and adapt to new modes of operation. However, humans
tend to be overwhelmed in urgent situations or if the complexity of the task is too
high. A good semi-autonomous system ideally exploits advantages of both worlds.
In this respect, Loizou et Al. propose in [43] a method to incorporate human inputs
in navigation plans, based on the concept of almost input to state stability property
of a class of navigation function. It allows human intuition while guaranteeing safety
and convergence. Most current semi-autonomous systems rely on specific paths and
are restricted to specific types of action, such as lane keeping [44] or emergency
braking. In [45], the authors use a potential-field based approach to assist a human
in a wheelchair and reduce the workload while accommodating some human input.
The intervention law is based on the distance of the wheelchair to the closest obstacles
and the deviation from the current heading to the one computed by the planner.
In order to build a robust and efficient semi-autonomous system, an estimation
of the intent of the driver is often required. When little information is available,
trajectory prediction can be computed using only available state information provided
24
by IMU or GPS [46], although the accuracy of the prediction is relatively poor as
it doesn't take into consideration the environment or the behavior of the human
driver. In [18], the human driver is assumed to behave in an optimal manner and
MPC is employed to predict his future trajectory.
The command to the vehicle
is a combinaison of human and controller inputs modulated by the threat of the
prediction.
In [47], Dragan et al. provide a method to predict the user's intent
for better teleoperation. The planner identifies a set of relevant goals and associated
reaching trajectories. The planner assigns a cost to each trajectory and the probability
the human operator chooses one of them is assumed to be exponentially decreasing
with the cost.
In the context of vehicle navigation and assisting systems, hidden Markov
processes and Bayesian networks have been commonly employed to detect specific
actions such as lane changes [48], [49] and [50]. The classifier is trained to detect
with some anticipation when the driver is about to perform the action of interest.
This prediction can be improved by monitoring gaze points or head dynamics [51] or
even neurological factors like EEG measurements [52], [53]. One common drawback
of these approach is their focus on a specific aspect of the driving task.
The
framework described in this thesis aims at covering a wider range of situations by
providing a general method for threat assessment and control intervention.
1.3
Purpose and Outline of the Thesis
In this thesis, a unified framework for trajectory planning, collision avoidance and
semi-autonomous navigation based on the notion of threat assessment is developed.
Such a navigation system ideally operates only during instances of significant threat:
it should give a driver full control of the vehicle in low threat situations but apply
appropriate levels of computer-controlled actuator effort during high threat situations.
The long-term goal is to achieve seamless semi-autonomy, in other words a transparent
and unobtrusive active safety system that preserves the human control when possible
and that doesn't surprise the driver when the computer control is activated.
25
Internal
Look-ehead
Sensing
Sensing
a deton
det, e
Current vehicle
state (velocty
y
IdentificatIon
H
0
r
a
c
h
I
Predict
V/ Identification of homotopy candidates
Own perception of the environment
IV Evaluation of candidates
(threat asses ment)
1111i
Decide ifintervention
(based on driver intent prediction)
IntentZo
Act on
threat
Tksato
Driver Model
p
a
n
/ Compute
Optimal / Safest trajectory
Input Command
1I/ Controller to follow the path
Figure 1-2: Design and block diagram of the semi-autonomous control system
In addition, the semi-autonomous system conveniently finds a trade-off between
safety and human attention as frequent interventions can both annoy the human
driver and alter his/her focus on the driving task.
The semi-autonomous active
navigation system described in this thesis aims to satisfy all of the above requirements
and desired characteristics. Furthermore, the system's method for threat assessment
and computer-controlled intervention can potentially be modified in real time based on
the scenario, environmental conditions, driver preference, or past driver performance.
Figure 1-2 shows a block diagram of this system. The active safety system can
be decomposed into two hierarchical planners. The strategic motion planner is the
main focus of the research. Given the information of the surrounding environment
provided by the onboard sensing, this high-level planner identifies and evaluates
navigation options and determines when the controller has to take action and
override the human's command.
The low-level controller is a secondary aspect of this research.
Once the high
level planner has chosen the safest navigation strategy, the low-level controller then
26
computes the sequence of inputs of a good representative trajectory of the homotopy
class and makes sure the vehicle remains within the bounds of the corridor.
In parallel, the human driver perceives the environment in its own way and makes
Therefore, in order
his own driving decisions based on some internal reasoning.
to improve the performance of the safety system, one of the additional task of the
high-level planner is to predict the intent of the human driver. Finally, the system
modulates its intervention based on the identified threat and the predicted future
actions of the driver.
The three key points of the high-level planner addressed in this thesis are the
following:
" Identification of fields of safe travel (homotopy candidates)
" Evaluation of the candidates - threat assessment task
" Intervention law, based on a driver model
The first stage of the planner that is the identification of fields of safe travel
in the local vehicle surroundings is described in Chapter 2.
The characterization
of the desirability of each field in order to detect potential hazardous navigation
decisions is developed in Chapters 3 and 4. This second stage is, strictly speaking,
the threat assessment problem, which is addressed with a margin-based method that
relies on analysis of a state lattice.
Chapter 3 describes the construction of such
lattice both in input space and state space. The computation of the control freedom in
each homotopy candidate is detailed in Chapter 4. These methods are demonstrated
in simulated highways scenarios involving multiple tasks such as vehicle overtaking,
lane merging, or emergency braking. The threat assessment is also applied to fullyautonomous navigation in which the planner chooses the corridor that offers maximal
margin for a safe driving experience.
Finally, Chapter 5 describes an attempt to
build a reliable driver model in order to predict the future actions of the human
operator and build an efficient and robust intervention law for an improved active
safety system.
27
28
Chapter 2
Identification of Fields of Safe
Travel - Homotopy Formulation
The framework for semi-autonomous vehicle control in highway navigation
described in this thesis relies on the key notion of "homotopy" or travel corridor.
The reason behind this motivation is the assumption that human drivers tend to
operate vehicles within a field of safe travel rather than along a specific path. This
chapter aims at rigorously describing the methods and processes to identify these
fields of safe travel. Starting from the topological definition of homotopy classes, the
notion is extended to the framework of highway navigation which possesses inherent
characteristics such as a rigid lane structure and the prohibition to drive backward.
Two approaches are detailed. The first one relies on a cell decomposition and the
construction of a topological graph connecting the cells. The second one attempts
to identify all navigation strategies with respect to the obstacles and other vehicles
to build a decision tree.
Various scenarios involving both static constraints and
dynamic obstacles as well as lane merging and splitting lanes are presented to
Finally, the final section of the chapter
illustrate the the algorithm's operation.
introduces the crucial issue of finding a good metric to characterize the desirability
of each homotopy candidate.
29
Figure 2-1: From path planning to homotopy class planning
Figure 2-1 illustrates the founding principle of this thesis; instead of computing
the shortest path through obstacles, the planner identifies corridors of safe travel that
are assimilated to homotopy classes of trajectories (defined in the next section).
The methods introduced in this chapter assume that all information related to
the vehicle surroundings such as road edges, lane coordinates, position, velocity and
size of the obstacles are known over a finite preview horizon. In practice, these data
would be provided by onboard sensing, which is range-limited and subject to a relative
uncertainty.
2.1
From Work Space to Topological Search
We can find in literature as early as 1938 studies showing that human drivers tend
to drive their car within a field of safe travel rather than along a predefined path.
In [35], J. Gibson and L. Crooks describe these fields of safe travel as virtual objects
containing "... all possible paths, which the car may take unimpeded". This thesis is
based on the very same postulate and attempts to build efficient and reliable methods
to identify corridors that are as close as possible to the sets of paths described by the
two authors. Non trivial homotopies contain infinite number of feasible trajectories
and are therefore hard to identify or characterize as objects. The methods introduced
in this section aim at reducing the intractable problem of the continuous configuration
space into a tractable problem in a discrete topological space.
30
2.1.1
Topological Definition
In a topological space X, a path can be described by a continuous map (or
function)
f
from the unit interval [0, 1] to X. The initial point if the path is f(0)
and the terminal point is f(1).
f :I
X
8
s
X
Two continuous functions from one topological space to another are called
homotopic if one can be "continuously deformed" into the other.
This definition
implies that a homotopy of paths contains paths derived from continuously
deforming a reference path while keeping its endpoints fixed.
homotopy of paths, in X is a family of paths ft : I
-+
More precisely, a
X indexed by I such that:
" f t (0) = xo and ft(1) = x 1 are fixed
" The map F : I x I
-+
X given by F(s, t) = ft(s) is continuous
In the context of vehicle motion planning, homotopy classes of trajectories are
defined as sets of trajectories that can be transformed into each other by gradual
bending and stretching without colliding with obstacles [36]. Essentially, all paths
spanning from the vehicle's current position xO to a goal location Xg are said to be
homotopic if they pass through the obstacles in the same manner. Note that in this
case, Xg doesn't necessarily corresponds to a single point but can be a connected
surface of the space. Also, only feasible trajectories are considered, in the sense
they are compatible with the vehicle dynamic and the navigation rules. By abusing
the notation, we will refer to these classes of trajectories as "homotopy classes" or
"homotopies".
31
2.1.2
Road Maps and Topological Graphs
In robotics and more specifically in motion planning, homotopy approaches have
traditionally been employed to reduce the complexity of the planning task by
guiding topologically hierarchal motion planners. Essentially, these methods rely on
the partitioning of the configuration space into a union of simpler problems by
attempting to capture the topological skeleton of the environment. In [38], Jenkins
uses homotopy classes to simplify the shortest path problem.
To compute the
topological graph, he constructs lines joining random center points to every obstacle
within range.
These lines decompose the space into smaller regions.
The
connectivity between those regions form the graph. Finally, Jenkins implement a
modified version of Breadth-First Search (BFS) to explore the graph and identify
homotopies that are not self-crossing.
Later, Hernandez [39] extends the previous
method by using Rapidly-exploring Random Tree (RRT) in each homotopy to find
the shortest path.
Topological approaches such as road maps are efficient tools to tackle the motion
planning problem and rely on the construction of a graph-based abstraction of the
workspace.
In other words, road maps techniques attempt to convert the
N-dimensional configuration space path planning problem to a simpler graph search
problem in which the environment is represented by a reduced number of potential
states. More specifically, road maps represent the connectivity of the free space by a
network of one dimensional curves.
Traditionally, visibility graphs and Voronoi
Diagrams constitute well known class of algorithms in this regard.
In [54] for
instance, the authors implement families of path-wise connected balls and visibility
domains to form a topological base.
In [55], an extension of the Visibility
Probabilistic Roadmap Planner (PRM) methods is implemented to construct
efficient roadmaps which account for the connectedness of the configuration space.
Voronoi Diagrams are well-suited to generate the topological skeleton in workspace
populated with polygonal obstacles [56].
32
Figure 2-2: Voronoi Diagram (left) and visibility graph (right)
Visibility Graphs
Historically, one of the earliest and most widely used technique to identify the
topological connectivity of the free space is based on the construction of a visibility
graph (NJ Nilsson as early as 1969 [57]) . A visibility graph is a graph of intervisible
locations, typically for a set of points and obstacles in the configuration or work space.
Each node in the graph represents a point location, and each edge represents a visible
connection between them. That is, if the line segment connecting two locations does
not pass through any obstacle, an edge is drawn between them in the graph (see
Figure 2-2 right).
From an implementation perspective, the algorithm first draws lines of sight from
start and goal locations to all "visible" vertices and corners of the configuration space.
Then, at every time step, the algorithm draws the lines of sight from every visible
vertex from the previous time step to the newly visible vertices.
The process is
repeated until every vertex is explored at least once. The connections between the
vertices of the obstacles and the corners of the space form the topological graph. A
BFS algorithm can then be implemented to identify all homotopies starting from the
initial location and reaching a specific vertex.
Efficient algorithms can be implemented to build the visibility graph. If the brute
force methods lead to a complexity in 0(n') where n is the number of vertices,
more powerful algorithms can be run in O(n 2 logn) [58]. However, one of the main
drawbacks of visibility graph is that by construction, the trajectories are close to
the obstacle. More importantly, two distinct paths on the visibility graph can pass
33
through the obstacles in the same manner and are therefore homotopic according to
the definition. Therefore, although visibility graphs can be efficient in shortest path
problems, then often yield to redundancy during the homotopy identification step
and are ill-suited to the framework of this thesis. In Figure 2-2 for instance, the green
and blue paths are two distinct topological objects from the visibility graph but are
part of the same path homotopy according to the definition.
Voronoi Diagrams
Voronoi diagrams are another popular method to reveal the topological structure
(Figure 2-2 left) [59]. Considering a set of points P, usually representing the obstacles,
the Voronoi diagram splits the configuration space into a set of cells R, in a way such
that every point within cell Ri is closer to the sampled point P than to any other
point P from the sampling set P. The curved segments at the boundaries of the cells
trace out the topological skeleton of the workspace. Unlike visibility graph, Voronoi
Diagrams aim at maximizing the clearance between points points and obstacles. Note
that points along these axes are equidistant from the closest two or more obstacle
boundaries.
Voronoi diagram provides a complete method to capture the connectedness of a
workspace and design a roadmap that goes trough it [56]. Powerful methods have
been proposed to compute Voronoi diagrams such as Fortune's algorithm with a
complexity in O(nlogn) in time and O(n) in space [60].
Then, a BFS or a
shortest-path algorithm like Dijkstra's algorithm can be performed on the graph to
identify relevant homotopies. The advantage of this approach is that if the sampling
of the set of points P is adequate, the graph prevents redundancy in the homotopy
identification. However, similarly to the visibility graph, Voronoi diagrams are more
suited to path planning than to corridor planning as it is difficult to capture the
properties of the homotopies delineated by the partitioning.
Indeed, this method
allows to identify the center line of the corridors the but the boundaries are not
explicit.
34
2.1.3
Cell Decomposition
Although the road maps are well-suited to build the topological skeleton of the
configuration space, which is a mandatory step toward the identification step, they
are widely used to determine the shortest path to reach a goal location. The goal of
the planner detailed in this thesis is different since it aims at estimating the freedom
of control offered in each homotopy or field of safe travel. Both the visibility graph
and the Voronoi diagram fail address either the issue of redundancy or geometric
properties of the homotopies. Given this observation, the planner proposed in this
thesis is based on another branch of motion planning methods that are cell
decompositions.
Essentially, these techniques decompose the configuration space
containing convex polygonal obstacles into a set of constrained convex cells and
represent the connectivity by the adjacency graph of the cells.
9
7
25
10
4
Figure 2-3: Delaunay Triangulation - Direction of navigation: from left to right
Delaunay Triangulation
This
homotopy
decomposition
called
identification
Constrained
method
Delaunay
uses
a
well-known
Triangulation.
A
triangular
Delaunay
triangulation for a set P of points in a plane is a triangulation such as no point in P
is inside the circumcircle of any triangle of the decomposition. It is the geometric
dual of the Voronoi diagram: when connected by straight lines, the centers of the
35
Delaunay's triangulation's circumcircles form the medial axis of the Voronoi
diagram. In the context of highway navigation the edges of the surrounding vehicles
and the boundaries of the road impose constraints on the triangulation such as no
decomposed cells crosses constraints.
Once the triangular decomposition is performed, an associated topological graph
can be constructed.
Nodes are cells and edges indicate immediate connectivity
between nodes.
Given the graph structure, a source cell and a target cell, it is
straightforward
to
considered cells.
determine
the
lowest-cost
connections
between
the two
That is, any feasible homotopy containing the vehicle's current
position X0 , and the position of the goal location, may be defined as a sequence H'
of adjacent triangles (T1 ...
T)
extending from the triangle circumscribing the
vehicle's current position to that containing the goal location (from T
to tio
inFigure fig:Delaunay) [61], [20]. In this sense, the notion of homotopy is already
extended from the topological definition since the target location is not necessarily
.
reduced to a single point but is most of the time a convex region of R2
Figure 2-3 display one possible Delaunay triangulation of a space and the shaded
triangles correspond the sequence of convex regions forming one of the homotopy
candidate. Any paths starting and ending at the same points and contained in the
sequence can be derived from each other thanks to continuous deformations.
The
advantage of this approach is that the algorithms involved have been extensively
studied and that the decomposition provides full coverage of the environment with a
complexity of 0(nlogn) [62].
However, one drawback is that this triangular
decomposition
is
and
identifications.
Consequently, the second crucial task of the planner, which aims at
not
unique
can
therefore
yield
different
homotopy
characterizing the level of threat of each homotopy, can be biased, since the
properties can change according to the computed triangulation. Another drawback
is that this homotopy representation can look artificial, which first, contradicts the
goal of the planner that is to mimic the human perception of the corridors, and
second, is not well-suited for highway scenario since it reflects poorly the lane
structure.
36
Trapezoidal Decomposition
An alternative to the triangular decomposition is a trapezoidal decomposition, in
which the space is divided in line perpendicular to the direction of the vehicle. This
method also provides a full coverage of the environment and leads to a topological
graph with nodes being cells and connectivity given by immediate proximity. The
procedure for constructing a trapezoidal decomposition is similar to constructing a
triangular decomposition and several efficient algorithms also exist to perform the
decomposition.
Figure 2-4: Trapezoidal decomposition
Figure 2-4 displays the four homotopy candidates in the case of two obstacles if
the extreme right cell is the only destination cell.
Similarly to the triangular
decomposition, the trapezoidal decomposition is fully conservative, covers the entire
space and therefore contains all feasible trajectories, provided the vehicle can only
move forward and is not allowed to move backward.
The assumption is not
restrictive in the case of highway scenario. Besides, the decomposition provides cells
that are more realistic from a human driver point of view: then correspond to
real-life feasible maneuvers (stay in the region behind first vehicle, overtake first
vehicle from the right side and stay in the region between two vehicles).
One
drawback of this method is that the topological graph can change over time is the
37
obstacles
have different
velocities,
which prevent
the
planner
to identify
time-invariant homotopies and therefore characterize the level of threat in each
homotopy.
However, this key idea of decomposing the space in trapezoids with
respect to obstacles serves as one of the pillars of the final homotopy identification
algorithm for on-road navigation.
2.2
Lane Structure and Target Zones
Highway navigation requires a more robust method to identify homotopies so
that the planner can handle the various scenarios encountered. In particular, it has
to be able to manage multiple lanes boundaries, obstacles with time varying velocity
or change of lane position, as well as merging lanes or lane departure. Figure 2-5
presents different cases that can occur on a highway, such as a vehicle that enters
or exits the highway or another vehicle located on an adjacent lane suddenly decides
change lane. In the latter case, the human operator has usually two choices, either
slow down to allow the vehicle to merge or whenever possible, switch to the adjacent
lane to leave more "space" to the other vehicle. The host vehicle is the blue car on
the third lane and the grey vehicles are elements of the traffic.
-------------------------------------------
-----------------
----------------------------
Figure 2-5: Possible scenarios in highway navigation
In this context, it is important to distinguish hard constraints from soft
constraints.
On the one hand,
hard constraints cannot be violated in any
circumstance since they can lead to serious accidents. For instance, crossing another
vehicle boundary leads to a car crash and a departure from the road can also be
38
very hazardous. Soft constraints on the other hand, can be violated in some cases,
such as temporary maneuvers. In the framework of this thesis, lane boundaries are
considered as soft constraints. As a matter of fact, they have to be assimilated as
constraints since vehicles cannot stay in between two lanes for long periods of time.
However, a vehicle may cross them at some point to change lane to overtake a
slower vehicle or to reach an exit.
2.2.1
Spatial Constraint Decomposition
Similarly to the methods described in the previous section, the planner runs a
trapezoidal-inspired decomposition with respect to the lanes and the other vehicles.
Lane by lane, each area located between two vehicles or obstacles is considered as
a cell. If no vehicle is present in the lane, then the whole lane constitutes the cell.
The length of the portion of road considered depends on the range and accuracy of
the onboard sensing. In most cases, the prediction horizon is limited to either a few
seconds or a few hundred meters.
Figure 2-6 presents an example of decomposition in the case of a highway composed
of four lanes. The sensors detect two other vehicles within a sensing horizon. The
space is therefore decomposed into six cells. More generally if NL is the number
of lanes and Nv the number of vehicles, then the total number of regions is NE
=
NL + NV, since each vehicle contributes to the creation of a cell behind it.
1
m4
-5
6
Figure 2-6: Decompositon of the space accounting for the lane structure and obstacles
39
The general goal of a motion planner is to generate some control command over
some discretized time steps up to a definite prediction horizon. The command can be
in the form of direct control inputs when considering fully-autonomous vehicles, or
a threat-dependent recommendation when the control is shared between the human
operator and the controller. In the case of static obstacles, the cell decomposition is
time-invariant and the planner can thus use the same decomposition at every time
step. In highway scenarios however, the planner has to consider dynamic obstacles
and the boundaries of the each cell from the decomposition needs to be computed at
every time step between to and to + Tp where Tp is the prediction horizon.
This is the reason why the topological structure is important in homotopy
planning:
the connectedness of the graph, under some specific hypothesis, can
remain time-invariant over the prediction horizon.
More precisely, this cell
decomposition is valid as long as the surrounding vehicles don't change lanes. Even
if the vehicles go at different speeds, the relative size of each region may vary but
the general scheme of the decomposition remains time-invariant.
If one of the
vehicles from traffic changes lane within the prediction horizon, it creates a new cell
in the lane it reaches and the cell behind it in its former lane is no longer relevant
and the topological structure that support the homotopy identification becomes
time-dependent (Figure 2-7).
1
2
1
2E3
3
4_
5
----.----
4,5
----
6
-
7
to + at
to
Figure 2-7: Time varying decomposition when lead vehicle changes lane
40
2.2.2
Target Zones
As stated in the motivation section, the planner described in this thesis is designed
to come as close as possible to a human behavior in order to only intervene when the
global threat exceeds some threshold and to preserve the driving experience when
no critical intervention is required. In this respect, another layer is added to the
homotopy identification task with the introduction of the target regions.
General Case
Typically, on a highway, most human drivers tend to drive at the reference speed
which is close to the speed limit if the traffic allows it. If they encounter another slower
vehicle in the same lane, they have two choices: either slow down and stay in the same
lane or change lane to overtake the slower vehicle. In most cases, the driver is highly
unlikely to change lane and slow down to remain side by side with the slower vehicle.
This is why target regions come into play. In each homotopy candidate identified,
the sets of continuously deformed trajectories are restricted to subsets of trajectories
that reach the related target region within the prediction horizon. Besides, in the
framework of highway navigation, the requirement on the same termination point
for all trajectories from one set is relaxed: if all trajectories start at the current
location of the host vehicle, they don't necessarily end at the exact same location,
but somewhere inside the target zone. The requirement of continuously deformable
4
Figure
2-8:
--------------------------..
:--
..
Location
of
41
target
zones
..
.
..
.
1..
. 5 ..
.
.
.
Fg2-------------
.
paths remains mandatory.
In the lanes free of vehicles (the first one in the previous example), the target
region is located at the end of the prediction horizon, and is reached if the vehicle's
speed is close to the reference speed. It's a reasonable assumption that the driver will
try to reach this region if he chooses to change lane. Otherwise, the target zones are
located behind the lead vehicle, including a safety margin. Figure 2-8 displays the
location of the target in each region. It is straightforward to see that each cell from
the decomposition contains one and only one target region. The area of each target is
a tuning parameter and depends on the preferences of the human driver. The smaller
the target region, the closer the vehicle speed to the reference speed. On the contrary,
large target regions allow more freedom in terms of driving speed. Essentially, the
size of the target can range from a single point in the most restrictive case to the
whole cell in the most conservative approach.
From an implementation perspective, if the boundaries each cell from the spatial
decomposition are computed at every time step of the prediction, the target zones
are only computed at the last time step to + T since they capture the potential goals
the driver may want to reach at the end of the control horizon. They are independent
from the positions and the motions of the obstacles at the previous time steps. In both
homotopy identification methods described in sections 3 and 4, the homotopies are
defined as sequence of events, either sequence of cells to travel trough or navigation
actions to operate. In both cases, the target region constitutes the terminating step
of the sequence defining the homotopy class.
Lane departure and Lane Merging
Not only is the method to construct the target regions straightforward when the
lane structure is constant, it is also robust and flexible and allows one to tackle
situations where the number of lanes can change, for example if two lanes merge or
if one lane leads to an exit. The case of an exit lane (Figure 2-9) is not very different
from the standard case. Indeed, it is simply one of the possible goals the human
driver may want to reach. Therefore, the target region is also located at the end of
the sensing horizon, similar to the other lanes.
42
------
......
Figure 2-9: Location of target region in lane exit and lane merging
In the case of a lane merging with another one, it is slightly more complicated
since it is not necessarily a final goal but the human driver can still drive along it for
a short period of time (overtake another vehicle or wait for insertion in the adjacent
lane). The use of a target region allows us to solve this issue: when the end of the
terminating lane becomes close enough to the position of the host vehicle, the target
region associated with the merging lane is removed from the possible goals presented
to the planner. Therefore, the cells composing the merging lane can still be used in
the topological graph as a temporary travel solution, either as the initial nodes or as
transition nodes, but they cannot be terminal destinations.
Figure 2-9 shows the location of the target regions as the host vehicle approaches
the merging point of the two lanes. First, it is far enough from the singularity and the
soon-to-end lane is still a potential goal for the driver. As the vehicle comes closer,
traveling trough the terminating lane becomes less and less desirable and the size of
the target region decreases as one can expect. At some point, the driver has no choice
but to stay or go to the remaining lane and the target zone is removed from the set
of goals.
2.3
Static Graph Search Approach
This section details the first method employed to identify the homotopy
candidates thanks to a static graph search on the cell decomposition of the lane
structure introduced in the previous section. Generally speaking, motion planning
can be described as the task to plan a relevant sequence of configuration states for
the plant and a sequence of control inputs to achieve the desired configuration. The
43
length of the sequence
depends on the prediction
horizon and the time
discretization. As mentioned earlier, the first goal of the planner described in this
thesis is to identify corridors containing a set of homotopic trajectories that share
the same strategy of obstacle avoidance, whose sequence of inputs verify the vehicle
model constraints at each time step and whose configuration states remain inside
the homotopy constraints at each time step. The approach detailed in this section is
a constraint-based approach in the sense that at every time steps, the planner
computes the exact spatial boundaries that delineate every homotopy based on the
sequence of cells from the topological graph.
For representation sake, static obstacles are addressed in the first instance and
dynamic vehicles follow.
2.3.1
Static Obstacles
To begin the analysis of the algorithm, we detail the simplest case of static
obstacles where the spatial decomposition remains constant for all time steps
between to and to + Tp.
This leads to a simple and intuitive construction of the
topological graph, in particular with respect to the connectedness between the cells.
The same example is used throughout the section for consistency sake. It represents
a highway scenario in which the road is composed of four lanes and the traffic is
limited to two vehicles within range.
The first step is to run the cell decomposition as detailed in the previous section.
As the obstacles are static between to and to + T,, the planner is only required to
compute the decomposition once at to. In the example (Figure 2-10), there are six
cells and as many target regions. Two of the target regions are located behind the
leading vehicles. They correspond to braking maneuvers. The four other cells are
located at the end of the lane and correspond to overtake maneuvers.
44
Figure 2-10: Cell decomposition and graph construction
The construction of the graph is straightforward when the cell decomposition is
achieved: two cells are connected if they share a non-null edge. In the example for
instance, cells 2 and 5 or 4 and 6 are connected, which is not the case of cells 3 and
4. The graph is therefore composed of 6 nodes (cells) and 7 edges. The topological
graphs is assumed to be loop-less, which means that the sequence cannot contain
twice the same cell, which is a realistic assumption. Indeed, in real-life situations,
a human driver is unlikely to change lane and come back to the same lane if it is
without the purpose of overtaking another vehicle or avoiding an obstacle.
Incidentally, one interesting property of the topological graph in the static case,
where only adjacent cells are connected, is its property of being planar, meaning it
can be drawn on a plane in a way that that no edges cross each other. Efficient search
algorithms have been developed for this class of graphs. The property is however not
mandatory for the general homotopy identification task, which is fortunate since it is
no longer verified when dynamic vehicles are considered.
The homotopy identification task in itself consists of listing all feasible sequences
from the initial cell to one of the terminating cells containing a target region. In the
example, all cells contain a target region and can therefore be used as terminating
cells, even the initial one. In the later case, the homotopy is reduce to this single
cell. In order to list all possibilities, the graph can be explored using standard graph
search algorithms. One possibility is to employ a k-shortest path algorithm derived
from Dijkstra's single shortest past algorithm [63], [64]. The algorithm finds the K
45
shortest paths that have the lowest cost from one node to another, where K represents
an upper bound for the number of paths derived from the graph. The cost of each
edge is set to one so that no transition is favored compared to another. The algorithm
is run for every available terminating option, while always using the same initial cell.
The complexity of such algorithm is 0(kn(m + nlogn)) where n is the number of
nodes and m the number of edges.
However, as the topological graph is assumed to be unit-length graph, a Breadth
First Search algorithm (BFS), with a complexity of O(n) [65] is more suitable for
the current application. Besides, all homotopies may not be relevant to the driving
task regarding the number of lane changes. As a matter of fact, as the number of
lanes and obstacles grows, the number of lane changes involved in the homotopy can
be infeasible within a short prediction horizon. We can therefore restrict the search
to homotopy candidates that involve at most two or three lane changes which is a
reasonable assumption for highway driving.
Each lane change corresponds to a
transition from a cell to another in the topological graph.
BFS algorithms are
particularly well suited for this kind of application since at each time step, the
algorithm explores the neighbors of the cells visited at the previous time step. The
upper bound of lane changes authorized directly determines the number of steps in
the BFS algorithm.
3
3
5
4
6
6
Starting cell
First step
Second step
Figure 2-11: Mechanism of the Breadth First Search
46
Figure 2-11 illustrates the process of the BFS algorithm is the example.
The
algorithm is initialized at the initial lane. During the first step, the algorithm visits
the cells directly connected to the initial cells, and so on. At the end of the second
step, all cells have been visited and the algorithm can list all possibilities involving
at most two lane changes. There are exactly eight possibilities: 2, 2 -
1, 2 -÷ 4,
2 -+ 5, 2 -+ 1 -+ 3, 2 -* 4 -+ 6, 2 -÷ 5 -+ 3, 2 -* 5 -+ 6. The sequence of cells
forming the homotopy candidates are associated with spatial constraints derived from
the constraints of the cells. Not that the feasibility is not guaranteed at this point and
some homotopies may not contain any feasible trajectories. The bounded corridors
associated with the homotopy are plotted in Figure 2-12.
10
-5
0
10. ,
10
'-
-
50
-.--
0
-10
10
-5
50
5-
,
10
0
50
10
,
50
0
50
-
-5
0
10
0
50
0
50
0
50
Figure 2-12: All possible homotopies with topological length less or equal to two
2.3.2
Dynamic Obstacle
In the case of dynamic obstacles, the structure of the topological graph needs to be
slightly modified. As a matter of fact, the size of each cell of the decomposition will be
no longer be time invariant. Moreover, if the velocity of the vehicles is different, then
two cells that share a common edge at the initial time might be completely separated
a few time steps later, and the other way round (figure 2-13). In the example, the
vehicle on the third lane goes faster than the one on the second lane and eventually
overtakes it. At the initial time, cells 3 and 4 are connected but after a certain time,
they become distant and cells 2 and 5 eventually become connected.
47
3
2
...
*r
3
2..
..
2
5
tk
tk+1
Figure 2-13: Graph construction in the dynamic case
The identification method has to rely on a time-invariant spatial structure to
perform the graph search and find all relevant homotopies.
Therefore,
the
topological graph structure has to take into consideration the possible changes. A
more conservative approach is considered and two cells are now connected if they
belong to two adjacent lanes and not only if they share a common edge.
In the
example, the two cells 2 and 3 are both connected to cells 4 and 5.
Other
construction rules remain the same and cells in the same lane are still considered as
distant. Vehicles from traffic are still assumed to stay in the same lane within the
sensing horizon.
This method is conservative because the planner may identify
homotopies that will never be feasible (If both vehicles go at the same speed for
instance).
Nevertheless, even if it artificially increases the number of homotopies,
this issue is not a major drawback as these non-feasible homotopies can easily and
quickly be removed during the threat assessment phase.
From an implementation perspective, the same breadth first search algorithm on
the current structure is used to find all relevant homotopies (loop less sequence). The
search can also be restricted to paths whose length is smaller than a given number of
edges. In the example, where homotopies are limited to maneuvers involving at most
two lanes changes, only one homotopy candidate is added compared to the static case:
2
-
5 --+ 3. It doesn't look feasible on the snapshot at to but it does at to + Tp.
48
A few Words of the Case of Lane Merging
The lane merging case requires more refinement because the cells located in the
soon-to-end lane may not contain any target regions. In figure 2-14, cells containing
a target cell are surrounded by a green circle and can be considered as terminating
cells during the BFS search. On the contrary, cell 6 is only a temporary cell and is
surrounded by a red circle in the graph. The BFS search can not allow this cell as
one of a termination and has to continue the search if it gets to gets to this point. As
a consequence, the homotopy 2 -+ 4 -+ 6 is no longer valid but 2 -÷ 4 -+ 6
-+
5 can
be included is the planner allows maneuvers involving three lane changes.
-_-_--_----------------------55
Figure 2-14: Topological graph for lane merging
Vehicles from Traffic Changing Lane
A time invariant structure along the sensing horizon is crucial for the topological
search of feasible homotopies. The main issue when the other vehicles from traffic
are allowed to change lane is that the skeleton of cell decomposition can change over
time. One possible means to tackle the problem is to extend the previous method by
creating a yet more conservative decomposition of the space, which is not addressed
in this thesis (Figure 2-15). Another approach, based on the implementation of a
decision tree and detailed in section 4 is preferred.
49
]m
*
2"!
2
22
4-
t...
tk
5
tk+27
tk+1
Figure 2-15: Topological graph when lead vehicle changes lane
2.3.3
A Few Words on Complexity
Once all homotopies are identified, the high-level planner has to evaluate the
desirability of each candidate in the threat assessment task. It is important to check
that the number of candidates to evaluate doesn't blow up as the density of the
traffic increases. In the proposed approach, homotopy identification arise from the
topological graph. The number of homotopy candidates thus depends on the number
+
of nodes n and the number of edges m. If n is straightforward to calculate, n = NL
NV, m depends on the relative disposition of the vehicles in the lane structure. In the
best case scenario (lowest number of edges for a fixed number of vehicles), all vehicles
are located accumulated in one of the lanes on the side in order to minimize the
number of connections. In the worst case scenario, all vehicles are equally distributed
in the middle lanes and the connectedness of the graph is maximal.
There is no formula to give an estimation of the number of homotopies given NL
and NV for the reasons stated above, but the relation is exponential in the unbounded
case. When the number of lane change is bounded (2 for example), it is possible to
make statistics with respect to the number of obstacles. This restriction implemented,
Figure 2-16 shows the evolution of the number of homotopy candidates against the
number of obstacles. For each Nv, 1,000 scenarios are randomized. The number of
lanes is fixed to four but the initial lane is arbitrary. The relative disposition of the
vehicles is also randomized but not more than three vehicles can be in the same lane.
It arises from the graph that in this bounded case, the median number of candidates
is a linear function of the number of obstacles in first approximation for Nv < 10.
50
This guarantees that computation time doesn't blow up in typical highway situations.
40
~.35
0
S25
15
~-..
..
~..~~
~
.
(D 10
.0
1
2
3
4
5
6
7
8
number of vehicles
Figure 2-16: Number of homotopies against the number of obstacles (limited to at
most two lane changes)
2.4
Enumeration
of
Navigation
Strategies
Approach
The other homotopy indentification approach proposed in this thesis attempts to
enumerate of all navigation strategies with respect to the obstacles that verify the
properties of homotopy classes. Navigation decisions can be enumerated based on
the observation that each time the vehicle encounters an obstacle it faces a choice
between remaining in its current lane (by slowing) or moving to an adjacent lane.
It relies on the construction of a decision tree and doesn't attempt to compute the
boundaries of the each corridor as in the previous approach.
2.4.1
Decision Tree
The construction of the decision tree combines two types of actions:
e Actions taken each time the vehicle encounters and obstacle: overtake from
51
the left or right side or slow down and stay behind the vehicle. In compliance
with diagram in Figure 2-17, the host vehicle is defined as being behind another
vehicle or obstacle if and only if it is located behind it and in the same lane.
Otherwise, the host vehicle is considered to be either left or right.
The goal destinations at the end of the prediction horizon represented by the
target regions previously introduced.
Avoid vehicle from the left
I
Li____
Avoid vehicle from the right
Figure 2-17: Left: partition of the space to identifiy strategy, Right: target regions
A tree structure can be constructed by enumerating all the possible navigation
decisions associated with each obstacle, with each branch of the tree terminating
(not necessarily uniquely) in a target zone. A homotopy can then be identified as a
field constructed from connected regions associated with each branch of the decision
tree. For each homotopy candidate hi, 6ij is the decision taken at the
and
ri the chosen
target
zone.
By
construction,
Ti
E
jth
vehicle
{1, ... , NT}
and
6i, C {left, behind, right}. Each homotopy candidate can be written in the form of
a sequence hi = ( 6 j,1, ...
, 6
level of the decision tree.
,Nv, Ti) that represents the navigation decision at each
This method for homotopy candidate identification is
more general and contain less information that the previous one. In return, it is
more flexible as it allows various behaviors of the lead vehicles such as lane changes
Some ramifications of the decision tree are plotted in Figure 2-18 for the reference
example (four lanes and 2 vehicles). On the right is displayed one homotopy candidate
that corresponds to the sequence of decisions highlighted in blue in the tree.
52
Vehicle A
Vehicle B
Target
Figure 2-18: Left: Decision tree, Right: one homotopy candidate identified in the
tree
2.4.2
Heuristics to Prune the Number of Candidates
The number NH of homotopy candidates depends on the number of vehicles NV
and number of target zones NT, NH =
3 Nv
x NT which grows exponentially with
the number of vehicles. In the example, the number of homotopy candidates is 54.
However, some of these candidates are not feasible due to violation of constraints
associated with the vehicle dynamics, road and lane boundaries or even definition
contradictions. For instance, the host vehicle can't at the same time be behind two
adjacent vehicles, in the sense defined above.
Likewise, if the host vehicle stays
behind one vehicle, it can't reach target regions located in adjacent lanes.
The
related sequences of action are virtual objects that are irrelevant to the driving task
and have to be removed from the sets of homotopy candidates.
Furthermore,
homotopy candidates associated with multiple lane changes can be discarded as
practically undesirable similar to the previous case.
The identification of these unfeasible homotopy candidates can be performed using
the threat assessment methods described in Chapter 4 that relies on sets of trajectories
to compute the control margin within each corridor. However, this computation of the
margin can become computationally expensive as the number of candidates increases
quickly. Instead, it is possible to prune the number of candidates before performing
the threat assessment task. A fraction of the candidates can quickly be sorted out
based on heuristics such as the minimum number of lane changes associated with the
maneuver.
As an example, consider the scenario illustrated in Figure 2-19, with two obstacles
53
lane 1
lane 2
----------
lane 3
green
homotopy
left side of A
0 (1921
initial lane
right side of A
left side of B
(2)
(4)
(11
homotopy
+2
target lane
right side of B
initial lane
J2)
+1
+3
(3,4)
(3)
-
0
target lane
0
Figure 2-19: Examples of computing minimum number of lane changes for homotopy
candidates
and two identified candidate homotopies. We can see that bypassing vehicle A (from
left to right) requires passing through lane 1 or 2. Approaching behind vehicle A
requires travel in lane 3 and overtaking it from the right side requires travel in lane
4. Based on these observations and from a conservative perspective, we can then
calculate the minimal number of lane changes involved in the maneuver.
The red
homotopy requires at least 5 lane changes, whereas the green homotopy minimally
requires only one. We can thus discard the red option from the homotopy candidates
as it is unlikely that a) it contains any feasible paths, considering constraints arising
from vehicle dynamics, and b) the large number of required lane changes makes it
practically unlikely to be desirable to a driver.
Let Lj be the set of accessible lanes corresponding to a decision for the jth vehicle.
For example, when the jth vehicle is in lane 3 and decision is 'left', then Lj is
{1, 2}.
For notational convenience, let LO be the initial lane and let LN,+1 be the target
lane.
The minimum number of lane changes between decisions can be computed
as the minimum value of differences between elements of L and Lj+ 1 , which we
denote min-dist(Lj, Lj+ 1 ).
Then the total minimum number of lane changes can be
computed by summing them, as:
)
Min = ENv min-dist(Lj, Lj+1
This algorithm
candidates.
allows to significantly
reduce
the
number
of homotopy
Similarly to the graph-search approach, we can build some statistics
54
with respect to the final number homotopies against the number of vehicles, under
the same hypothesis (four lane structure, arbitrary initial lane...).
Figure 2-20
displays the results of 8 x 1, 000 randomized scenarios. The algorithm proves to be
efficient, especially as the number of obstacles becomes large.
In the case of 8
vehicles detected in the traffic, the median number of homotopy candidates is 150
instead of 38 x 10 ~ 65k. However, the restricted number is still higher than in the
graph-search approach because the heuristics are based on the minimum number of
lane changes instead of the exact count. For these reason the strategy enumeration
approach will be employed only when at least one vehicle is supposed to change lane
within prediction horizon.
In other situations, the graph-search approach will be
favored.
i5 2W
E
......................................................................................................
0
.2
.0
1
2
3
4
5
6
7
8
number of vehicles
Figure 2-20: Number of homotopies against the number of obstacles (limited to at
most two lane changes)
2.5
Evaluation Methods to Characterize Threat
In the previous sections, we have described two distinct approaches to identify
homotopy candidates.
The first is based on a static graph search on a cell
decomposition of the configuration space.
The homotopies are then defined as
sequences of cells the vehicle have to travel through. The constraints of the cells are
computed at every time step and the boundaries of the corridors are therefore
exactly known. In the second approach, the homotopies are defined as sequences of
55
actions derived from the construction of a decision tree. Although more flexible, this
method doesn't allow to characterize the spatial boundaries of the corridors.
Depending on how the information is employed by the planner, one of the two
methods can be more advantageous than the other.
In this thesis, the number one goal of the planner is to characterize the
desirability of each field of safe travel identified in the first instance. In other words,
the planners needs to assess the threat of the related maneuver according to
relevant and easily measurable criteria. We can then classify the feasible maneuvers
from the safest to the most hazardous so that the vehicle can either select the best
navigation option (fully-autonomous systems) or safely assist the human driver
(semi-autonomous system).
There exist several methods to tackle the threat
assessment task. They can be classified into three main categories: analysis of the
geometric properties of the corridor, analysis of a single trajectory representative of
the homotopy class and finally an estimation of the input freedom offered within the
corridor.
2.5.1
Geometric Properties
One of the most obvious means to characterize the desirability of a corridor is to
analyze its geometric properties, such as the effective travel length and the average
width of the corridor, the curvature of the road and other similar metrics. Generally
speaking, it is clear that drivers tend to prefer to travel through regions that are wide,
straight, and short, to reach a desired goal location. However, it is often difficult to
estimate those geometrical properties, as the shape of the corridor can be complex in
the presence of multiple obstacles. Should we consider the average or minimal width
of the road? How to properly define the curvature of a homotopic region? These
questions are hard to rigorously answer.
Some research has been oriented towards this approach.
In [20], the authors
propose to evaluate the geometric properties of the homotopy candidates based on
some heuristics computed at the scale of the cells from the Delaunay Triangulations.
These heuristics include among other the distance travelled in each triangle, the
56
width of the triangles perpendicularly to the direction of travel, and the minimal
avoidance acceleration required.
A graph search based on those heuristics using
Dijkstra's algorithm is then performed to calculate the optimal path homotopy.
This type of approach allows one to compute a rough estimate of the ease of
navigation and severity of the maneuver in each corridor, but is limited in his
applications due to rather restrictive hypothesis such as a constant speed during the
maneuver. It doesn't fully characterize the ease of navigation or room for maneuver
as it doesn't only derive from the corridor's geography but also from the vehicle's
dynamics.
2.5.2
Optimality Approach - Single Representative
Another category of threat assessment methods relies on the computation of a
single representative of each homotopy class (the shortest path for instance), as many
algorithms have been developed in this area of motion planning. One of the earliest
method to find the shortest path within a constrained corridor is to use an RRT/RRT*
algorithm. In the context of homotopy formulation, Hernandez computes the shortest
path in each identified homotopy using the h-RRT algorithm [39]. Although not the
concern in the article cited, the shortest path can be selected as the representative for
the threat assessment task. The criteria can include among others lateral acceleration,
slip angle, and distance to obstacle.
Recent approaches evolve around some optimality principle.
Indeed, some
studies have shown that human operators tend to behave in an optimal manner
such as to minimize an objective function [66], [67], [68], that takes into account
relevant aspects of the driving task.
This method relies on the assumption that,
while there may be many trajectories contained within a particular field of safe
travel, the optimal trajectory (with respect to a chosen optimality criteria)
represents the best case maneuver, and therefore can be viewed as a proxy for the
overall goodness of the field.
This optimal solution can be used to assess this
minimum threat posed to a vehicle navigating through the homotopic region.
From a practical perspective, if the optimal solution is associated with a high
57
threat level (as computed by a chosen metric) this suggests that the region is
undesirable, since there exists no other trajectories that have lower threat. So, such
a metric can be used to indicate the minimum threat of a given field of safe travel.
P
_LZ f
k=1
P
P
fR,30k + EZ Uu'Ruk+ E 1/XUTRAuAUk
k=1
k=1
J is an example objective function where p is the prediction horizon, u the sequence
of inputs and Au the input rate. The matrices Ri represent the weight given to
each of the considered variables. The optimal solution is the sequence of input
Uk
that minimizes this objective function J and that satisfies the constraints related to
geometrical or input bounds.
There are several implementation methods to find the optimal solution given an
objective function. They fall in two main categories: combinatorial (MPC, MIQP)
and sampling algorithms (RRT-based).
The Model Predictive Control approach
predicts the future vehicle state evolution and optimizes a set of inputs such that
this prediction satisfies constraints and minimizes the objective function [18], [19].
MPC handle explicitly environmental, performance and actuator constraints as long
as they are convex, which enables corridor navigation. When the constraints become
non convex, the model can be extended to Mixed Integer Quadratic Programming.
The second family of methods are sampling methods. In particular, lattice-based
approaches, sampled either in input space or state space are widely popular in motion
planning to find optimal path [31] and [30]. Each edge of the lattice is assigned a
cost, similar to the objective function, and a graph search can then be performed to
find the optimum.
The major drawback of this single representative approach is their reliance on an
ad hoc metric for evaluating the desirability of a candidate field. The great difficulty
lies in finding the best weight for all theses variables, not to mention that they are
often not homogenous and they need to be normalized before being incorporated in
the cost computation. Besides, these approaches can hardly capture the freedom of
control within a corridor and provide few information of the exposed risk if the vehicle
slightly deviates from the reference path.
58
2.5.3
Margin Approach
The last category of threat assessment methods attempt to characterize the
room for maneuver in each homotopy candidate.
One approach is to determine
whether the corridor contains at least one drivable path (and ideally many drivable
paths). This is a reachability analysis: the corridor is considered to be safe if the
planner can find at least one feasible path along it while considering both the
vehicle's dynamics and driver model [17].
This approach provides a manner for
determining the time of intervention, but provides only a binary assessment of
reachability (empty or nonempty reachable set), and is therefore limited in its
ability to compare the desirability (i.e. "goodness") of various candidate navigation
choices.
In principle, the more drivable paths a space contains, the more desirable that
space is, since it will be easier for the operator to navigate within it. This assumption
leads us to consider a more general margin approach. For each feasible homotopy
candidate, the planner computes a set of trajectories that covers well the relevant
area of the corridor.
The control freedom offered by the corridor is then derived
from these sets of trajectories. This approach is the main focus of this thesis. The
implementation of a lattice-based method is detailed in Chapter 3, and Chapter 4
describes how the threat metric is derived from the trajectories generated from the
lattice.
59
60
Chapter 3
Classes of Feasible Trajectories
from State and Input Lattices
Lattice discretizations were originally implemented for planetary rover navigation,
but they can easily be adapted to the structured environment of public roadways,
provided some small modifications [69]. Nowadays, the most common use of latticebased method is to induce a discrete graph search on a continuous state space while
respecting differential constraints on motion [70], [71]. Shortest path algorithms (A*
for instance) are then employed to find the lowest cost trajectory among the lattice.
In this thesis, the usage of the lattice structures is different. The purpose is not
to find the shortest path but to generate homotopy classes of trajectories in order to
estimate the room for maneuver in each field of safe travel. In this respect, the lattice
approach is very attractive for the homotopy framework. Indeed, as it is a samplingbased approach, each trajectory can be handled independently and assigned to the
homotopy candidate of which it complies with the constraints. It is a highly flexible
method that can handle complex scenarios and vehicle models, and can work with
both homotopy identification methods described in Chapter 2.
This chapter details the implementation of state and input space lattices and
discusses the advantages and drawbacks in either case.
In both approaches, the
discretization is two-dimensional: position and velocity for the state space and
steering and throttle for the input space.
61
3.1
State-Space Sampling
A state lattice is a structure where vertices that represent kinematic states of the
vehicle are connected by edges representing paths that satisfy the kinematic
constraints of the robot [72].
In the highly constrained environment of public
roadways [32], the vertices are placed in a regular pattern in the form of a grid that
fits the shape of the road, so that only states that are a priori feasible in highway
navigation are considered.
Traditionnaly, the construction of the lattice can be decomposed into two steps
[30]. First, paths are generated by connecting sampled endpoints using curvature
polynomials.
Second, candidate speed profiles are built for each individual path.
By combining the path edges with the speed edges, a trajectory set is obtained.
The method described in this thesis uses the same approach and applies it to the
framework of highway navigation with the lane structure and target regions.
3.1.1
Path Generation
Sampling of End Points
A vehicle actuated in curvature (steering) and speed (throttle) can be modeled by
the following four coupled, non linear equations:
J(t)
V(t)cos O(t), p(t) = V(t) sinO(t)
0(t)
=
r(t )V(t ), k(t) = u(t)
The vehicle state vector then consists of the position coordinates (x, y), heading
6 and curvature
i'.
The inputs are the speed V and the desired curvature u.
x = (x, y, 9, r)T
The sampling method for constructing the grid superimposed on the road is based
on the algorithm described by McNaughton in [31]. The road center line is used
as a reference for the construction of the whole lattice and is define by the same
four state vector, now a function of arc length or station s [x(s), y(s), 6(s), (s)].
62
We then define the grid points p(s, 1) with 1 being the lateral offset or latitude as
x(s, 1) = [x(s, 1), y(s, 1), 0(s, 1), r(s, 1)1 where:
x(s, 1)
=
x(s) + 1cos(6(s))
y(s, 1)
=
x(s) + 1sin(9(s))
6(s, 1) = 0(s)
r,(s, 1) = (r,(s) -' - 1) -I'(s) is the curvature of the path and 6(s, 1) = fo r'(s, 1) . To enumerate every node
from the grid, we define a linear mapping from discrete integers (i, j) to (s(i), 1(j))
s(i) = ai and 1(j) = 3 + -yj. The station is therefore monotonic increasing starting
from zero an the lateral offset is positive for nodes above center line and negative for
nodes below. An example of such a grid is shown in Figure 3-1 where the nodes are
plotted in cyan. The road is composed of the three lanes and number of samples for
the longitudinal and lateral discretizations are respectively 8 and 9.
Figure 3-1: Grid points on the road
Formulation
Once the endpoints are sampled, we use a common method [73] to compute the
paths that connect the nodes of the conformed lattice. In this iterative numerical
method, a path is defined as a cubic polynomial function of the arc length:
ri(s) = a + bs + cs2 + ds'
63
for which the parameters (a, b, c, d) can be found to define a path connecting
any pair of endpoints (x, y, 0, r,). Most work employs the same technique, and only
the order of the polynomial splines varies (usually cubic [31], quartic [30] or quintic
[74]). The requirements for the "smoothness" of the solution dictate the order of the
curvature polynomials. There are two sorts of paths to consider: ones connecting
pairs of sampled enpoints (cyan in Figure 3-2) and ones connecting the vehicle's
current state to one of the node (green in the Figure). In both cases, the same cubic
polynomial model is used to construct the paths.
For simplification, we express all coodinates in terms of initial posture so that
the initial position and heading coordinates are zero. We note x0 = (X 0 , yo,0,
(0, 0, 0, "o) is the initial state and
xf
o) =
= (xf, yf, Of, kf) is the terminal point the path
has to reach. Considering the model for the curvature, the initial equations can be
integrated and lead to the following expression of the state for any station s:
K(s) = a + bs + cs 2 + ds 3
b
8
0(S)
(s
c
2
d
)
=as + s + s3 + s4
4
3
102
X(s)
cos(O(s))
=
sin(0(s))
y(s) =
and the following constraints have to be satisfied:
r(0)
=o
x(sf)
= Xf
y(sf)
Yf
O(sf)
=
Of
I(sf)
=
rf
There are four parameters to estimate:
p
=
[b, c, d, sf] because given the
constraints, a is necessarily equal to K0. The previous state equations can be writen
in vector form:
x = f(p)
64
The system of equations can be linearized first order as follows:
Sufficient degrees of freedom to control the entire endpoint state (four equations
for four unknowns) implies that the Jacobian has sufficient rank to invert and we can,
in principle, solve for the parameters iteratively using:
_ap
Implementation of the Shooting Method
Initialisation
The first step is to initialize the parameters. Following [75], we use some heuristics,
based on an approximation of the observed average relationship between sf and the
total change in heading between the sample endpoints.
d = 0
Sf T
(
f
b=6
C=
3
of
) i
2
~5 [
f
2
s2
+4f
s
s
__
-(KO + 1f) + 6s
s
These heuristics have proven to be effective for initialisation.
However, in
replanning applications, the algorithm can be sped up significantly by reusing the
previous parameters from each cycle to seed the initial guess. The convergence is
faster, especially if the frequence of the planner is high since in that case, the
postures are in the same neighboorhood.
Termination conditions
The termination conditions depend on the requirement on the accuracy in the
destination posture. In this thesis we allowed a 1 mm error for both Xf and yf, a
0.1 rad for the heading error and finally 0.005 1/m for the curvature error. Theses
65
conditions lead to robust paths generation in the context of highway navigation while
maintaining a low computation cost since the average number of iterations ranges
from 3 to 5.
Forward/InverseSolution
The forward equations produce the state vector at any point of the curve given
the parameters (b, c, d, sf).
Numerical quadrature techniques such as Simpson's
formula are used to approximate the Fresnel Integrals since exact solutions are hard
to compute. At each time step, the distance from the computed endpoint to the
goal is computed.
If the distance is smaller than a predefined threshold, the
algorithm terminates.
Otherwise,
the inverse solution
is performed:
an
aproximation of the Jacobian using finite difference is computed and the new set of
parameters is obtained by multiplying the inverse of the Jacobian and the error.
The forward solution is then computed with the new set of parameters and the
algorithm repeats the process until termination conditions are met or the maximum
number of cycles is reached. Note that in some works, a symbolic differenciation is
favored over the finite difference approximation to compute the jacobian [31].
Figure 3-2 provides an example of a lattice with 3 sampled endpoints in lateral
and longitudinal directions. The green paths connect the vehicle's current posture to
the nodes and the cyan paths interconnect the pair of nodes.
10
0-
'000
0
10
so
20
40
F [in]
Figure 3-2: Generation of the edges of the lattice
66
W1
3.1.2
Velocity Profile and Trajectory Generation
Velocity Profile
Once the path edges are generated, candidate speed profiles are build for each
individual path. Several possibilities exist. In [31] for instance, speed profiles are
generated using forward methods by specifying the acceleration of travel. Here, we
favor an inverse method, also used in [30]. The speed is first discretized in a similar
manner to the position lattice and then a polynomial is generated to satisfy the vertex
constraints. Note that some accelerations may not be feasible due to the vehicle's
dynamics. Contrary to [30], the algorithm used in this thesis first checks if the two
sample points can be connected using a feasible acceleration before actually computing
the velocity edge in order to reduce computation time. In this context, a reasonable
estimate for the lower bound of deceleration is -1g and for acceleration is 0.5g. As a
matter of fact, the breaking power is stronger than the traction force and the velocity
profile should reflect this important dynamic observation.
Regarding the implementation, polynomial functions of the arc length s of the
following form are used:
v(s) = e + fs + gs
+ hs 3
2
A speed state can then be defined by the vector v = (s, v, a). The algorithm
computes every feasible path between pairs of sampled points, from (so, vo, ao) to
(s,, vi, ai). For each path, there are four parameters to dertermine e,
f,
g and h and
the corresponding constraints are vo, ao, vi and a1 . Except for the vehicle current
state, where vo and ao are obtained from the real vehicle sensors, ao and a, are set
to zero at every node. By setting so = 0, the parameters can be derived from the
following expressions:
e=V
f
=
ao
a,
v0
i-3-+3= -2aSi
Si
S
a0
a,
vO
h=
+
+2S
s 2 Si 2 i s3 s3
-
a0
67
v1
Si
v1
..........
.............
Figure 3-3 is an example of a velocity profile with nine sampled points for the speed
dimension. For consistency, the station discretization is the same as the one used to
build the road grid. As mentioned earlier, only paths that verify the acceleration
bounds are evaluated.
I
20....
.
U
.......
4-,
-1
4-
4.
S IM
s~m]
Figure 3-3: Velocity Profile
Trajectory Generation
Both paths from the lattice and edges of the velocity profile are functions of the
arc length s. We can therefore define families of trajectories by adding the velocity
component to each path stored in the lattice.
The final state is six-dimensional:
Xt (s) = (x(s), y(s), 0(s), K (s), v(s), a(s)). Trajectories are then evaluated at a higher
resolution, i.e. at not only on the grid points but also at discretized m stations
within the prediction horizon. To evaluate those trajectories, numerical quadrature
techniques such as trapezoidal integration of Simpson's formula are employed to
compute the local coordinates (x, y).
If Npaths depicts all travelling paths (all
feasible combinaisions of consecutive edges from the lattice) and Np,,eed the number
of authorized velocities profiles, then the total number of trajectories that the
algorithm can generate is Ntraj = Npaths x Npeed.
68
3.1.3
Application to Highway Navigation with Traffic
Latteral Acceleration Restriction
Although the contruction of the lattice is conformed to the shape of the road,
some trajectories generated may be unfeasible or very hazardous due to severe
curvature rate.
Indeed, except for the longitudinal acceleration that is bounded
when defining the velocity profile, other aspects of the vehicle's dynamics are not
taken into consideration during the construction of the lattice. In particular, the
lateral acceleration, which is a common metric employed to predict loss of control,
has to be analyzed after the construction step.
The lateral acceleration can be
computed at every sampled point of the trajectory. If the maximum value exceeds
one stability threshold, the trajectory is then deemed unfeasible or unsafe and is
disregarded during the threat assessment task.
Modification of the Grid with Target Zones
So far, the discretization of the state space was regular along the road. In the
homotopy framework described in the previous chapter, the sampling of the endpoints
can be slighlty adjusted to better synchronise with the location of the target regions,
which are goals the driver may want to reach. In the first Figure of 3-4, the enpoints
are sampled in a regular pattern within the prediction horizon. We can see that the
some of end points are located exactly at the same position of the vehicle, which can
substantially reduce the number of feasible trajectories.
One solution is to force the sampling of the nodes in the longitudinal direction so
as to fall into the target regions of the same vehicles (second Figure of 3-4). Note if
two vehicles are neighboors in terms of arc length, it is not relevant to create two sets
of nodes as they will be very close to each other. In his case, the vehicle that is closer
to the current host posture is dominant and the nodes are sampled so as to comply
with its associated target region.
69
4-
-2-
0
0
30
40
50
40
s0o0
40
s[ml
0
to
W03
S [ml
Figure 3-4: Modification of the grid to fit the current scenario
3.2
Input Space Sampling
The main drawback of the state-space lattice approach is the fact that the
dynamics of the vehicle is not taken into consideration while constructing the paths
connecting the nodes of the grid. The feasibiliy of the trajectories has to be tested
afterwards. In the input space approach, the trajectories are generated based on a
chosen vehicle model and the sampling of the input commands and are by
construction dynamically feasible.
In return, the constraints of the road are not
considered in first instance, and some trajectories may therefore depart the road
bounds.
Because of this limitation, input-space approaches have been somewhat forsaken
in favor the competing state-space implementation [32]. However, in this thesis, the
use of the lattice is not to compute the shortest path towards a specific goal as in most
motion planning problems. Trajectories from the lattice are employed to characterize
the room for maneuver in each homotopy candidate, which is directly connected
to the freedom of control. In this context, sampling the lattice in input space is
relevant.
Some methods, similar in spirit have beed developped for the DARPA
Urban Challenge [76]. This section details the implementation steps and explains the
70
asumptions to reduce the size of the lattice while ensuring dense coverage of space.
3.2.1
Vehicle Model
Lattice structures are flexible and efficient tools to generate feasible trajectories
and can easily accommodate any vehicle model provided the dimension of the input
space is reasonable as the the curse of dimensionality implies an exponential growth
with the number of inputs. Most vehicle model are indeed actuated by two inputs that
are the steering angle and the throttle (speed or acceleration). Only one of two model
implemented, the kinematic bicycle model, is detailed in this section for the sake of
clarity. During the simulation phase, in particular for the integration with Carsim
detailled in the last chapter of this thesis, the more accurate 4-wheeled Ackermannsteered vehicle was used. The associated equations of motion are detailled in the
appendices.
V
X
--
----
~~
--- - - - - --
Figure 3-5: Simple bicyle model
The bicycle model is a relatively simple mathematical model of a two-wheel inplane vehicle with two degrees of freedom, the steering angle 6 and the longitudinal
acceleration a,. The relevant variables are illustrated in Figure 3-5. The heading
angle or yaw angle is represented by the variable IF and 1 is the length between the
71
two wheels. Typically, I is close to 3 meters and 6 ranges from -/+ 0.1 rad in most
highway scenarios. The dynamics of the vehicle is given by the following equations:
1(t) = ai(t)
tan 6(t)
V(t) =V(t)
(t)
=
V(t) cos IF (t)
y(t)
=
V(t) sin T(t)
Once the sequence of inputs are specified within the prediction horizon, it is
straightforward to compute the trajectories by integration of the equations. These
sequences of inputs are precisely generated from the input lattice composed of a
steering and an acceleration or velocity profile.
V(t) = Vo + alt
'1(t)
=fo
x(t)=
V(t)tan 6 (t) dt
1
fJ V(t) cos TI(t)dt
y(t) = f V(t)sin I(t)dt
3.2.2
Construction of the Input Lattice
Steering Profile
The construction of the steering profile is similar to that of the velocity profile in
state-space discretisation. The same inverse method is used. The input space is first
discretized in a certain number of sampled points within the feasible bounds. Then a
polynomial is generated to satisfy the vertex constraints. Note that the polynomials
are no longer functions of the arc length s but the prediction time t. Incidentally,
we refer to the sampled point along time axis as "switching nodes".
The values of
the steering input at those switching nodes is chosen so that the input rate remains
smaller than the bound defined in the vehicle model
(0 <
max).
.
An edge of the steering profile is defined as 6(t) = a + bt + ct2 + dt3
Every node from the grid can then be defined by the vector I
=
(t, 6, 6).
The
algorithm computes every feasible path between pairs of sampled points, from
72
(to, 6o, 6o) to (ti, 61, 51). For each path, there are four parameters to determine a, b,
c and d and the corresponding constraints are 60, 6o,
61
and 61.
Except for the
vehicle current inputs, where 60 and 4 are set to the exact values, 60 and 6o are set
to zero at every node. This sampling method ensures smooth and continous steering
By setting to = 0, the parameters can be
commands from current set of inputs.
derived from the following expressions:
a
=0
b -=
c =-2
ti
ti
1
1
-3-+3
t2
t2
1
The following assumptions were made to build an efficient steering profile:
* The discretization of the prediction horizon is choosen so that all edges of the
steering profile are compatible with the steering rate limitations of the vehicle
" In most highway scenarios, the temporal discretization is the one used in
Figure 3-6, which allows the input strategies to change three times within the
prediction horizon, which is enough to perform most maneuvers from a simple
obstacle avoidance to a double lane change
Also, we observe the following:
" As one can expect, the curse of dimentionality applies to lattice structures and
the number of edges in the lattice is NN' where N6 is the number of sampled
points for the input discretization and NT the number of time steps within the
prediction horizon.
" The initial time steps in the sequence of inputs have more influence than the
final ones on the trajectory's shape: a small steering command at the begining
of the maneuver is more effecive that a more substantial action in the final
moments.
73
For those reasons, the nodes of the lattice are not exactly placed in a regular
pattern. As shown in Figure 3-6, fewer input choices are implemented at remote
times step than during the first moments. This trick allows us to reduce significantly
the number of trajectories to generate while preserving adequate coverage of the
corridors of navigation.
0.02-
0-
.
0
05I10
......
I
2
2.0
t [s)
Figure 3-6: Refined steering profile
Acceleration
/
Velocity Profile
The second component of the input-space lattice is the velocity or acceleration
dimension. Contrary to the state-space method in which the sampling nodes of the
position and velocity have to exactly coincide at the same arc length s, the constraint
is not mandatory for an input-space lattice. A fine discretization of the speed or
acceleration space is favored to a high number of "switching nodes" along the time
axis. Indeed, although it is relevant to allow the planner to compute a wide variety
of complex steering commands, most maneuvers are usually limited to steady
speed,
acceleration or braking phase. For this reason, the number of switching nodes is
usually limited.
Figure 3-7 displays two reasonable velocity profiles.
On the left
figure, no switching nodes are allowed; the velocity is monotonic for all edges. On
the right profile, one switching node is allowed
74
Figure 3-7: Two reasonable velocity profiles
Trajectory Generation
Once both the steering profile and velocity profile are computed, the planner can
extract all possible combinations of edges to define the feasible sequences of inputs.
To explore the lattice and identify all paths, a breath-first search algorithm similar to
the one used for the graph search of chapter 2 can be employed. For each sequence of
inputs derived from this lattice exploration, the planner computes one dynamically
feasible trajectory using the equations of motion of the vehicle model considered.
Figure 3-8 displays two examples of input-space lattices. The left plot represents
an arbitrary lattice in a unconstrained environment, for instance an exporation
robot operating at low speed. The right plot show a lattice applied to the highway
navigation task with a constrained environment (road edges with lane structure)
and high speed vehicles.
As expected, some trajectories leave the bounds of the
road as no spatial constraints are enforced in the construction of the lattice.
Figure 3-8: Example of input lattices
75
3.2.3
Conformation to the Constrained Environment
In state-space lattices, the dynamic feasibility of the trajectories has to be verified
afterwards. In input-space lattice, the planner has to check which trajectories verify
the constraints of the environment. More specifically, in the homotopy framework
detailed in Chapter 2, the constraints are even more restrictive with the addition of
a lane structure and target regions. In both identification methods, homotopies are
defined as some sequence of cells or actions that terminate in one of the target regions.
Consequently, the trajectories, whose purpose is to explore the corridor of safe travel,
have to reach one of the target regions within the prediction horizon (Figure 3-9 left).
In other regards, the input space lattice is dimensioned in such way to be
effective in low curvature highway scenarios. If the curvature of the road exceeds
some threshold, the lattice may not cover some relevant portions of the road. To
address this issue, the lattice is subjected to a transformation that is based on
properties of the road centerline.
An algorithm is employed to check (in a brute
force manner) that all trajectories remain consistent with input constraints and
stability vehicle bounds (Figure 3-9 right).
This case scenario is however rarely
encountered while driving on roadways, especially on highways. The scope of the
lattice usually allows to cope with reasonable road curvature situations.
Figure 3-9: Fitting the shape of the road
76
3.3
Advantages and Drawbacks
Useful Information for the Threat Assessment
The threat assesment task consists of evaluting the control freedom within each
corridor. In this respect, the input space approach has the edge over the state space
approach as the input information is directly stored in the lattice and is easily
accessible.
Trajectories are derived from sequences of vehicle input commands
instead of sampled kinematic states. As it is impossible to recover the inputs that
lead to a specific path in the state-space approach, the threat assessment can only
be performed in the acceleration space, which is directly mapped to the input space
though.
Computation Cost
The state-space implementation relies on a shooting method (inverse solution)
to compute the paths connecting the sampled endpoints. Although the convergence
is fast (usually less than five iterations), the computation time is greater than to
compute the input lattice.
Besides, if needed, the input lattice can be stored in
memory and re-used at every time step as it does not depend on the environment
constraints. Note that in this context, the algorithm detailed previously has to be
slightly adjusted as the initialization of the lattice is based on the vehicle current
input.
One possibility is to include several initial values for the inputs in the lattice.
Then at every time step, the planner can choose the closest choice to the real value
as initial node. The solution is less smooth than if the lattice was recomputed, but
it provides a robust method to skip the construction step. This is all the more useful
as the resolution of the lattice is high.
The benefit is however mitigated if the prediction horizon is long since the portion
of trajectories that leave the road edges increases. Also, if the curvature of the road
exceeds some threshold, the input lattice has to be shifted to ensure a good coverage.
77
In this case, another possibility is to store several lattices that correspond to specific
road curvatures to reduce the computation time.
Coverage of the Space
The main advantage of state-space lattices is the fact they are by construction
conformed to the shape of the road, which prevents from generating trajectories
that depart from the road edges.
In this sense, the state-space approach has a
significant benefit over the input-space approach.
However, the coverage of the
trajectories generated from the input lattice can still be better.
Indeed, in the
conformal case, many trajectories use the same edges they travel at different
velocities.
In the non-conformal case, if the velocities are different, then the
positions are necessarily different as low speed maneuvers allow more curvature than
high speed ones for a given steering command.
We can define an occupancy grid by constructing a grid superimposed on the
road. Each cell of the grid is considered occupied if one of the evaluation points of a
trajectory is located in the cell. In Figure 3-10, 50k trajectories were sampled in state
and input space. Even though some trajectories are ruled out in the input approach
because they leave the bounds, the coverage is denser for the reasons discussed. A
denser set of trajectories leads to a smoother estimation of the control margin and
limits the jumps induced by the discretization.
Figure 3-10: Occupancy grid: input-space (left) and state-space (right)
78
Table 3.1 shows a synthetic comparison of the advantages and drawbacks of state
space and input space lattice.
Table 3.1: Pros and cons of input space versus state space sampling
State Space
Input Space
+ Direct method
-
-
Shooting method
+ Memory storage possibility
Transformation if high road curvature
+ Conformal
+ All paths within bounds
- Multiple trajectories per paths
~ long predictions
- Only Acceleration information
- Non conformal
- Some paths out of bounds
+ Denser coverage
~ short predictions
+ Input and Acceleration information
79
80
Chapter 4
A Margin Based Approach to
Threat Assessment
The second stage of the high-level planner is to characterize the desirability of
each candidate homotopy that was previously identified. This task is called Threat
Assessment and provides crucial information to determine when the computer has
to take control of the vehicle and override the command of the human driver. As
seen in Chapter 2, several method can be employed to tackle this issue (geometric
considerations, single path representative...).
The threat assessment approach
described here relies on a metric that is based on the volume of the feasible input
space bounded by problem constraints, such as spatial boundaries or vehicle
dynamic constraints. The trajectories generated from the lattice allow to compute a
reliable estimation of the input space available.
4.1
Guiding Principle
The proposed margin-based approach to threat assessment relies on the
assumption that threat to a vehicle is inversely proportional to the available control
freedom, and therefore to the maneuverability within a candidate homotopy.
For
each candidate homotopy, the sequence of decisions from the decision tree or the
sequence of cells from a topological graph can be directly mapped to inequality
81
constraints in the vehicle's input space.
These inequality constraints yield
2-dimensional convex polytopes bounding the range of feasible vehicle inputs.
Essentially, if we can compute the volume inside this constrained region, we expect
that this will result in a rigorous metric to characterize the available control
freedom.
4.1.1
Room for Maneuver
The semi-autonomous system described on this thesis relies on the identification
and characterization of corridors of safe travel rather than on a single feasible path.
More specifically, the proposed approach presumes that the larger the control freedom,
the lower the threat posed to the vehicle. Figure 4-1 illustrates a potential highway
scenario including typical maneuvers. The host vehicle (blue car) is located in the
road center lane and approaches a slower vehicle navigating ahead. The driver has
several options :
" yellow homotopy: slow down and stay behind vehicle
" turquoise homotopy: go to left lane and overtake the slower vehicle
" pink homotopy: go to right lane (reduced width) and overtake vehicle
* green homotopy: overtake the vehicle from the left and come back to the center
lane
Some of these options may be safer than others. For instance, because of some
roadwork, the width of the right lane is reduced compared to the standard highway
width. Overtaking the vehicle from the right side therefore requires the human driver
to be more careful as a small mistake is more likely to lead to an accident than in
the left avoidance strategy. Likewise, the green maneuver requires good driving skills
since the operator has to accelerate to quickly overtake the vehicle and go back on
the same lane. A specific sequence of command has to be applied in order to safely
perform the maneuver. Finally, the desirability of the yellow strategy depends on the
82
distance between the two vehicles and the relative speed of the lead vehicle. As a
matter of fact, if the lead vehicle is remote and just slightly slower than the current
vehicle, the human driver only needs to apply a small pressure on the brakes and has
some relative margin with respect to amount of pressure and time of intervention.
On the contrary, if the lead vehicle is close to the host and suddenly brakes, then the
human driver has to apply a strong an immediate pressure on the brakes if he wants
to stay behind. In this case, a steering avoidance maneuver may be safer and offer
more margin as the required action on the brakes is less critical.
Figure 4-1: Road scenario example
All these maneuvers are defined within the prediction horizon, which is usually
equal to the range of the onboard sensing. The associated control freedom is a function
of time and can be evaluated at discrete time steps tk. Figure 4-2 schematically shows
the evolution of the control margin for every homotopy candidate of the previous
example. More precisely, it displays the bounded regions at three instants: initial
time step to, intermediary step to +
h
and the final prediction to + Th. The colored
ovoids represent the convex polytopes bounding the range of feasible vehicle inputs
and the dotted border the maximal bounds for both inputs due to physical limitations
of the vehicle.
With regards to the yellow maneuver, the associated region of input space is
constant over the prediction horizon: the driver has to remain in the center lane and
keep actioning the brakes to slow down to the speed of the lead vehicle. The volume of
the bounded region is relatively small, indicating the leeway for the driver is limited.
83
The turquoise homotopy presents the largest control freedom. Initially, the driver
has to initiate a left maneuver to perform a lane change. The bounded region is thus
concentrated in the left half plane. As the end of the maneuver, the driver needs to
counter-steer in order to align with the direction of the road and the bounded region
therefore shifts to the right half plane. From a velocity perspective, the vehicle can
navigate at the same speed as no obstacle stands in the way. The ovoid is aligned
on the center axis and the human driver is allowed to brake or accelerate reasonably
as he has the freedom to adapt his speed during the following time step to reach the
target region.
The pink homotopy is the symmetric maneuver and the evolution of the control
margin is very similar to that of the turquoise one. The only difference lies in the
more narrow width which results in a smaller volume of the polytope.
Finally, the green homotopy candidate requires a specific sequence of input with
little room for maneuver to be performed and the ovoid is concentrated around the
optimal trajectory. The convex polytope is in the upper half plane because the vehicle
has top speed up to perform the double lane change. The steering sequence is also
very precise: left to go to the left lane, right to fall back on the center lane and a
final left command to align with the road heading angle.
Throttle
Throttle
:Steering
Steering
to
Throttle
to + T
2
iern
tO+Th
Figure 4-2: Evolution of the control freedom within prediction horizon
From an implementation perspective, computing the exact volume of those
polytops is a very complex task as there is no analytical formula to derive the
84
bounds. For this reason, the technique proposed in this thesis relies on the analysis
of the trajectories sampled from the lattice.
If the density of the coverage is
sufficient, it allows to compute a good estimate of the volume of the bounded
region. Several techniques can then be employed to transpose the set of points from
the trajectories into a two-dimensional objects. Two main tools are employed here,
the convex hull of the set of points, and the Chebyshev ball (which is the largest
topological ball inscribed in the hull).
4.1.2
Notations
In this paragraph, we introduce the notation used in the rest of the thesis. We
note H = (hi, h 2, ...,
hnh)
the set of homotopy candidates identified in first instance
by the planner, where nh is the total number of homotopy candidate. Each candidate
hi is defined by a sequence of actions:
" Static graph search: hi
* Decision tree: hi
=
=
( 6 i,1,
(ci, 1 , ..., Ci,n, Ti)
...,
6
i,Nv, Ti)
(ci,11 ... , ci,n) is the sequence of cells composing the corridor, ni the number of
cells in the sequence and Ti is the target region terminating the ith homotopy. In the
decision tree approach, 6oj indicates the decision taken at the jth obstacle. Figure 4-3
illustrates the notation of a particular homotopy in both approaches.
We can then associate a margin metric mi to each corridor hi. The control freedom
is a function of time and takes discrete values within the prediction horizon mi,t, with
tk =
to + kAt ranging from to to tf = Thorizon. The final metric used to characterize
the threat is a normalization of this control freedom (euclidian norm, minimal norm,
weighted average...): mi = q(mi,to, ... , m,tf,).
The planner searches the lattice to identify trajectories that feasibly conform to
each unique homotopy candidate. In some cases, a homotopy candidate contains no
feasible trajectories, and thus the associated navigation decision is said to be
infeasible.
Ni is the number of feasible trajectories in the ith homotopy.
85
The
Cell Decomposition
Decision Tree
1
6
hk =(ckI,ck ,2tk)=(2,5,3)
hk =(6 k ,a,
2
,Tk)= ('left''right',3)
Figure 4-3: Definition of a specific homotopy in both identification methods
associated set of trajectories is called
4'j
and each trajectory from the set is referred
as ?Pi,1, with 1 ranging from 1 to Ni.
associated
with each
trajectory
The value
are
evaluated
4'i,l(tk).
at
The states and inputs
discrete
time
steps
tk:
xi,i = [xi,1, ye,](tk) and ui,i = 64; as,](tk).
Finally, instead of considering each trajectory 'z independently, we can consider
the scatter plot formed by the union of evaluation points of the trajectories from a
single set at a given time step tk:
1/i,1:N~
tk) (in state space or input space). In this
context, the volume of the convex hull circumscribing the scatter plot is denoted Vhsul
and the volume of the Chebyshev ball included in the hull is Vcheb. The Figure 4-4
shows the equivalence between the scatter plot in state space and the dual in input
space.
State Space
Input Space
Throttle
------------------
------ --------------
--- - - --- --
------------------------------ ------
SteWrQ
------------...............
............
tk
Figure 4-4: Definition of a specific homotopy in both identification methods
86
4.1.3
Trajectory Sorting
The first step towards computation of the control freedom is sorting of the
trajectories generated from the lattice according to their membership in a particular
homotopy.
For this purpose, the employed algorithm depends on the homotopy
identification method chosen in Chapter 2. Indeed, both homotopy representation
store the information in different forms.
Static Graph Search Approach
In this approach, one homotopy is represented as a sequence of cells whose
boundaries are exactly known at every time step within the prediction horizon. The
trajectories can also be evaluated at the same time steps for consistency.
The
algorithm then works as follows: at each time step, the planner identifies which cell
the evaluated points belong to. As the cells can be described by polygons, this task
boils down to a point-in-polygon (PIP) check which is a common problem in
computational geometry. Efficient algorithms such as ray casting algorithm allow a
quick computation of the answer. At the end of the process, every trajectory can be
defined as a sequence of presence in one of the cells (cf Figure 4-5). Note that the
last evaluated point is compared to the target region and not the whole cell in
accordance with the homotopy definition. Any repeated cell in the sequence is then
removed to bring up the sequence of transition from one cell to another.
This
sequence is finally compared to those of the homotopy candidates. As a homotopy is
uniquely defined by it sequence, the sorting is straightforward from this point.
Decision Tree Approach
In this approach, one homotopy is defined as a sequence of actions. Except for
the terminating constraint of the target region, the boundaries of the corridor are
not exactly known. For each trajectory, the planner needs to check the action taken
at each obstacle. The position and velocity of the other vehicles are known at every
time step the trajectories are evaluated. In order to determine the action taken with
87
N feasible Trajectories
C1 C-
C1- 1- C
-C
1- C
1- C
1- C]
1
l- l - Cl - -Cl -C-C1
-
I
C,
C2 -C2-C2
C,- I - C, --
C - C, - C
C1 -
Ci - C1
21 -1l-
C2
32
H homotopy candidates
2-c2 -
- CW
j
C3 -*~C
C
2
C2 - C 2 - C 2
C 2 - C2
C3 -C, -3
-
C3 - C3 -
3
C1
C3-
-C
3
C3 - C C3 - C 3 - C- C3
C3 -
1
C3
-C3 -C
C- C3
C
3
Cl
-Cl-c 3 -C 3 -C 3 -.- c
C - @ C 2 C - C - C1 C1 -C 1 - C1
C--CfC
C2
2
C2
--
> C2 --> C3
*C
C
->
C 3 --
C
sequence of cells
T sampled time steps
Figure 4-5: Illustration of the sorting algorithm
respect to one vehicle, the sorting algorithm searches the time step at which the host
vehicle is closest to the obstacle. The strategy identification is performed at this
particular time step according to the diagram introduced in Chapter 2 (Figure 2-17).
In Figure 4-6, this instant is encircled in red and the host vehicle bypasses the slower
vehicle from the left side. The process is repeated for every vehicle and the trajectory
is classified in the corresponding homotopy class.
Figure 4-6: Sorting based on minimal distance to vehicle
Figure 4-7 shows an example of a three lane highway with two static obstacles.
The lattice is sampled in input space and the static graph search is employed to
identify the homotopy candidates.
In this example, only five homotopies contain
feasible trajectories. Two of them correspond to a simple lane change, two others
involve a double lane change and the last one is a braking maneuver.
88
M
-
---- __________---o
ic
20
30
40
50
0
60
5
-
10
0
20
30
--------
---
40
50
5
20
10
30
40
5-
M
0
60
60
50
20
10
30
40
50
60
M10
0
20
30
40
50
60
Figure 4-7: Trajectory sorting
4.1.4
Input Space versus Acceleration Space
The threat assessment approach described here relies on a metric that is based
on the volume of the feasible input space bounded by problem constraints. When
the lattice is sampled in input space, the input information is directly stored in
the trajectory description.
The planner can therefore estimate the volume of the
polytope without further computation. On the contrary, the vehicle's dynamics are
not taken into consideration when sampling the lattice in state space. The feasibility
of rendered trajectories is tested afterwards, looking for example at the maximal
lateral acceleration, but the sequence of inputs can't be recovered.
In this case, the control freedom can be characterized in acceleration space
rather than input space. There exists a direct mapping between the inputs and the
acceleration so the two approaches are equivalent. The main difference is that the
margin is velocity dependent in this case: a given steering command yields a greater
lateral acceleration at high speed than at low speed.
This observation has to be
taken in consideration when using the threat information.
Note that the threat assessment can also be performed in acceleration space when
the lattice is sampled in input space.
89
4.2
Metrics to evaluate the Threat
This section tackles this issue of the choice of the margin metric with the
motivation to come up with a threat information that is as close as possible to the
human perception for better synergies with the controller.
The norms are first
detailed and then some examples derived from highway scenarios illustrate the
proper functioning of the method.
4.2.1
Convex Hull versus Chebyshev Ball
Recall that, at each time step tk, all trajectories from a set Oi can be evaluated in
the two dimensional input space or acceleration space. The corresponding set of points
is denoted Xi,k =
(ai,l(tk),
lateral acceleration and
/i,I(t)),
1 E [1 : Ni]} where a is the steering command or
# is longitudinal
acceleration. One possible approximation of
the volume of the feasible input space bounded by problem constraints is the area of
the convex hull of the set Xi,k. The convex hull of a set X is the smallest convex set
that contains X. With this approach, any points sampled within the hull is assumed
to verify the input constraints of the homotopy class at a given time step. Finding the
convex hull of a finite set of points in the plane is one of the fundamental problems
of computational geometry and efficient algorithms exist to derive the set with a
complexity of O(nlogn) [77].
Another metric that is relevant to consider is the volume of the Chebyshev ball,
that is the largest ball inscribed in the convex hull previously computed. This metric
is derived from the computation of the Chebyshev center, which is a famous convex
optimization problem [78]. The Chebyshev center maximizes the distance to the edges
of the convex hull. Since this particular point is the farthest from the boundary, it
can be considered as the most conservative navigation option in the sense that a small
variation of the convex hull in any direction is likely to contain this point. The radius
of the ball is proportional to the input margin within the corridor. Figure 4-8 displays
the convex hull and Chebyshev ball for an arbitrary set of points.
From a fundamental point of view, the area of the convex hull describes the global
90
Figure 4-8: Chebyshev ball inscribed in the convex hull
control freedom at a particular time within a corridor. It contains no information
regarding the distribution of this margin along the two input axes. Thus, a narrow
corridor with no velocity restriction can lead to a relatively large margin estimation.
On the other hand, the Chebyshev ball yields the minimal margin of both directions.
In the narrow corridor example, the limiting factor is the width and the resultant
margin metric is accordingly small. Any of these two metrics provide reasonable
estimation of the control freedom but the Chebyshev metric may be more suitable to
the threat assessment from a robustness perspective.
4.2.2
Norm Comparison and Weighted Average
Both the Chebyshev ball and the convex hull metric allow to compute the
control margin at a given time. The next question is how to compute a single global
metric that characterizes the threat within the prediction horizon, by merging the
information derived at each sampled time step. Simple approaches include average
norm or Euclidian norm, in which each time step is assigned the same weight for the
computation of the global metric. Depending on the threat assessment model used,
more appropriate normalizations can be employed.
One possible model is to
presume that the human driver only considers the minimal margin as relevant. For
instance, if there in a pinch in the middle of the corridor, the margin computed at
the time step corresponding to the crossing of the pinch dominates the computation
of the global margin. In this case, the infinity norm is appropriate.
Another class of norms addresses the issue of the preference of the driver for
91
present or short term actions. Indeed, similarly to the concern while constructing the
lattice, the first input command have a stronger influence on the global trajectory
than later ones.
For this reason, it appears relevant to attribute more weight to
the first margin computed during the earlier time steps. Possible mathematical laws
include linear decrease Wk ~ Wmax - a.k and exponential decrease; Wk ~ wmaxek-
Figure 4-9 presents three possibilities for the choice of the norm: unbiased average
(left), linear decrease (middle) and exponential decrease (right).
linear
average
exponential
0 .1 --- - -- -- - - --- -- - - --
0.04--.
.
. . .
-
0.08!-
0.09
0.07
-
.....
.....
0.06e 0.06;
. . .
.
.
.
.
- --
0.05:
-
- -- --
---E
M
004:
0.03-
0.015
....
0.06
....
0.06
....
0.04
..
0.03.
.. . . .
...
... .
.
. .. .
.
0.03-
0.07
0.02-
0.01
0.02
0.01;
0
0.5
1
1b5
t [5]
2
2.5
3
0
0.01
0.5
1
1.5
0
2
2.5
t Is]
3
0
0.5
1
1.5
2
25
3
t f5]
Figure 4-9: Different norms for the normalization of the threat margin
4.2.3
Simulated Scenarios on Highway
Static Obstacles
The first example is the same as the one introduced in the previous chapter, i.e.
travel on a four lane road with two static obstacles present in the second and third
lanes. The vehicle of interest is originally located in the third lane. In Figure 4-10, the
center of each disc corresponds to the possible location of the vehicle in the corridor
at a specific time step, and the radius is proportional to the computed freedom of
control.
For simplicity sake, only one representative of each set of trajectories is
plotted. This feasible path from the set corresponds to the location at the Chebyshev
center of the convex hull. Although the value of the metric assessing the margin of
control is time varying, the area of the discs of the same homotopy are here shown as
92
a constant (computed as the average over the entire horizon) to improve the clarity
of presentation.
6-------------------
-----------------------------
Figure 4-10: Homotopy candidates and associated margin
Clearly, the blue and purple homotopies represent fields of travel with the greatest
maneuver freedom.
As expected, the green strategy is the most hazardous as it
requires swerving between two obstacles that are near each other. The two other
choices appear less desirable due to a stronger restriction of the input space: strong
deceleration to stop behind the obstacle for the yellow homotopy, and a severe steering
maneuver for the red homotopy (to bypass the top-most obstacle on the left).
Figures 4-11 and 4-12 display the normalized input profiles for each homotopy
candidate. For instance, the blue corridor offers a large steering freedom all along
while the red corridor requires a sharp left turn followed by a strong counter-steering
to align with the road direction. The yellow homotopy is similar to the red one but
involves a single lane change and therefore offers more steering freedom. With respect
to the throttle input, most candidates offer a similar control freedom, except for the
yellow option that requires a strong deceleration at the beginning of the maneuver
to ensure collision avoidance. On the contrary, the green homotopy requires at least
a constant speed at first to ensure that the host vehicle can navigate in between the
two vehicles and reach the target region within the prediction horizon.
93
0.-
~o .05
0
0.5
....
-
.......
-
-
-as5-
1
15
2
25
3
0
0
0.....5
3
2.
-
-
-051
0
-
t[S]
0.5
1.5
2
2.5
3
2
2.5
3
2
25
3
2
2.5
3
-
t [s]
0.5
1
- -
0.5- --- -
1.5
2
3
25
05
0
15
0.5
t [s]
-
0
-a
0.5
0
1.5
1
2
3
2.5
t[s]
Figure 4-11: Normalized steering input for all homotopy candidates
0 .4 --
--
.-..
.--.
.-..
-----------..
- --
-0.2 t
0.4
0
0.5
1
1.5
2
3
2.5
2
t[s]
0.4
02 L
-- . . . . .... . . . ..-.---.-..
-.0_
-0
G9 e ----
-0.
0
0.5
1
1.5
t[sJ
2
2.5
- -
-
1
e-6
15
t [S]
0.4
-
M) -0.20F2
0.5
-
a)
---
3
2
-0.2
-0.A
0
0.5
1
1.5
t [s]
a)
0
0.5
1
1.5
2
2.5
3
t[s]
Figure 4-12: Normalized throttle input for all homotopy candidates
Figure 4-13 displays the computed overall margin considering either the area of
the convex hull (top) or the radius of the Chebyshev ball (bottom) for various norms
and weighted average. In this scenario, all metrics yield a relatively similar hierarchy
regarding the degree of control freedom afforded by each homotopy candidate. The
average norm and Euclidian norm yield similar results, while minimum norm tends
94
to favor the red option compared to yellow and green ones. With respect to the
metrics that involve a weighted average, synonymous with a preference for the first
time steps, the gap between the two most desirable candidates and the remaining
ones is increased. In the three most hazardous corridors, the restriction on the input
command is stronger at the beginning of the maneuver and is somewhat relaxed once
the maneuver is well underway. Again, this is due to the fact that the initial input
commands are decisive and mostly determine the global profile of the trajectory.
On the other hand, the Chebyshev metric also tends to increase the difference
between the two most desirable homotopy candidates and the next two, but for
different reasons. Indeed, as stated earlier, the Chebyshev metric depends on the
minimal margin in both input dimensions, whereas the convex hull metric relies on
the global volume, independent of the distribution.
In other words, homotopy
candidates four and five have a large margin in both inputs whereas candidates one
and two have a relatively comfortable margin only in one direction (either steering
for candidate 2 or throttle for candidate 1). It results from this brief analysis that a
weighted average combined with the use of the Chebyshev ball is a well-suited
metric for a conservative approach to the threat assessment, in the sense if focuses
on the initial input commands and the minimum control freedom in both axes.
95
Average Norm
eo
Minimum Norm
Euclidian Norm
25
60
so
20-
Go
15
~40
940,
10
20
20
0
1
2
3
5
4
1
Hlomotopy Candidate
3
2
4
0
5
1
Hornotopy Candidate
Unea Decrease
2
3
4
5
Homotopy Candidate
Strong Exponential
Sinall exponential
0
50-
so
50-
40
40-
40-
20
230
20
20
30
2010.
011
1
2
3
4
5
Homotopy Candidate
1
Average Normn
2
3
4
5
Homnotopy Candidate
1
50r
3
4
5
Euclidian Norm
Minimum Norm
0
2
Hoinotopy Candidate
70
10
40
.9
320
40
2D0
130
10
20
0
1
2
3
4
5
Homotopy Candidate
1
12
Lnear Decrease
50
2
3
4
5
1
Homnotopy Candidate
Sinall exponential
3
4
5
Strong Exponential
40
50
40,
2
Hoinotopy Candidate
4020-
~30.
~2D
~20
120
i2010
10
a
1
2
S
4
Homotopy Candidate
5
1
2
3
4
Homotopy Candidate
5
1
2
3
4
5
Homotopy Candidate
Figure 4-13: Comparison of norms, Top: convex hull, Bottom: Chebyshev ball
Dynamic Obstacles
This second example involves a more complex scenario that includes dynamic
obstacles, lane merging and a lane-shifting vehicle. In this scenario the fourth lane is
being eliminated, forcing the vehicle in the extreme right to merge to the third lane,
96
which is the current lane of the host vehicle. As in the static case, the homotopy
candidates are displayed along with the associated control margin. We here assume
perfect knowledge of the state evolution of the dynamic obstacles.
In the purple
case, the host vehicle travels straight and accelerates to pass ahead of the vehicle
in the fourth lane, before it merges into the third lane. In the turquoise navigation
strategy, the host vehicle brakes to allow the other vehicle to merge in front. The
green homotopy candidate offers the largest margin and corresponds to the case where
the vehicle merges to the second lane to avoid the merging vehicle entirely. The red
candidate offers comparatively little control freedom. In this scenario, as one of the
lead vehicles is changing lane, the decision tree approach is employed to identify all
homotopy candidates.
4-w
Y~
--~~-------
-----
x
[ml
Figure 4-14: Homotopy candidates and associated control freedom
Similarly to the previous example, Figure 4-15 displays the normalized input
commands for both steering and throttle.
The turquoise corridor offers a large
steering margin but requires a strong brake at the first instants in order to slow
down and allow the other vehicle to merge in front. On the contrary, the purple
corridor requires the vehicle to accelerate so that the other vehicle merges behind.
Besides, although the target region is located in the same lane, the host vehicles has
to perform a small lateral maneuver in order to avoid collision with the merging
vehicle which is coming from the right side. As the second lane is totally free from
vehicles, the green homotopy offers good margin both in steering and longitudinal
acceleration. Finally, the red candidate involves a sharp left maneuver to cross two
97
lanes coupled with some deceleration to merge behind the vehicle navigating within
the target lane.
05-000
-
-
-
0
.
..
-.-.
--.
-
0, -
0--
-0.5
0
0.5
1
1.5
2
25
-0.5
3
0
0.5
1
1.5
t [s]
2
2.5
3
2
2.5
3
t [s]
0-5;
0.5
Af
C
0,
0
0.5
1
1.5
2
2.5
-0-5
0
3
0.5
1
1.5
t [S]
0.4
t [s]
..--6-6, ----.
---------..-- ...- ....... .-..-.-.-
0.4 -..................
- 0) e e e-
a,
-
0 -0.2
-C
-0.8
0
0.5
-
1
1.5
a,
- -%-
2
25
-0.2-
-0.8
3
--------
0
0.5
1
1.5
t [S]
0-4 ..---
---- --.-..
--- ..
...
...
..-..
...
..
04-............G..0......
a,
8,
2
2 -0.2 -....... . ........
-C
-0.8
0
0.5
. -.
----... -..
-..
.....
1
1.5
2
2.5
3
t [S]
2
2.5
....-..
-'
3
-0.8-........... .....
0
0.5
t IS]
.......
1
4
-
0'-6.0...6.o
--.....
-
J................... .......
1.5
2
-- -
-
-0.5 L---------
-
0
VS)
I............. I
2.5
3
t [s]
Figure 4-15: Normalized steering and throttle inputs for all candidates
4.3
Comparison with Minimal Cost Approach
At this point, it is relevant to compare the threat information derived from the
margin metric to that of more traditional approaches with a single representative
such as the minimal cost path [18]. In that case, the minimal threat in each corridor
is assumed to be directly correlated to the cost of the optimal single path. In lattice
methods, the lowest cost path is traditionally derived from a graph search (A*)[72],
since each edge of the graph can be assigned a cost.
In our approach, sets of
trajectories are generated to explore the control freedom and evaluated at discrete
98
time steps within the prediction horizon. The minimal cost trajectory can then be
found as the one minimizing some discrete objective function.
4.3.1
Definition of the Cost Function
The first step of such approach is to build a cost function taking into account
relevant aspects of the driving task such as the input stress, slip angle, lateral
acceleration, relative speed... It is usually in the following form:
P
J=
where
/k
Z
k=1
P
P
R/3k + E _UT RuUk+
k=1
:
AU RAuAuk
k=1
is a vector containing some relevant states of the vehicle,
Uk = (U1,k, U2,k)
is the input vector (steering and throttle), Auk is the input rate vector with k ranging
from to to tf, the prediction horizon. The matrices Ri are diagonal weight matrices
and the same weight is attributed to each element of the diagonal (homogeneous
within the prediction horizon).
Another formulation of the objective function is to separate the N distinct sources
of threat that are considered relevant:
N
J
=
z wici
With ci is a threat source (lateral acceleration for instance), normalized along the
prediction horizon, and wi is associated weight. We can derive this formula by noting
P
that wici = 1
[R4]i,Ak. In addition, the weights can be chosen so that their
k=1
N
sum is equal to one:
Z wi
1.
For each trajectory we can define static and dynamic costs to evaluate the safety,
comfort, behavior and efficiency of the resulting maneuver.
Each trajectory is
evaluated in p sampled points within the prediction horizon. Some relevant threat
sources with their expression are listed in table 4.1.
Then, for each homotopy candidate, the planner determines the minimal cost
trajectory from the associated set of feasible trajectories. This representative serves
a reference for the estimation of the threat which is assumed to be proportional to
the cost.
99
Table 4.1: Elements of the cost funcion
Cost
Cst
Formula
P
Z
Physical sense
Category
U1,k
steering effort
efficiency
U2,k
throttle effort
efficiency
steering rate effort
comfort
throttle rate effort
comfort
kinetic energy
efficiency
k=1
P
Cth
k=1
P
Cstd
L
AUl,k
k=1
Cthd
LAU2,k
p
k=1
p
k=1
P
Cay
Z
ay,k
lateral acceleration
safety
L
ax,k
longitudinal acceleration
comfort
slip angle
safety
lateral lane offset
heading angle offset
behavior
behavior
k=1
p
Cax
k=1
P
cslip
Z
rk
k=1
4.3.2
Coff
o(p)
Cyaw
b(p)
Cost Comparison
In this section, the margin and the minimal cost approaches are briefly compared
as in to the two previous examples.
The goal is to show that the margin metric
is generally more robust and homogeneous when the single representative method
sometimes fails to address important issues and is highly dependent on the choice of
the weights.
Merging Inhomogeneous Threat Sources
As seen earlier, the estimation of the control freedom is relatively robust toward
the choice of the metric employed.
The discrepancy between the most and least
desirable candidates is usually more pronounced when using the Chebyshev radius
than the volume of the convex hull, but the global hierarchy is similar.
In the minimal cost approach, the results of the threat assessment is very sensitive
100
to the choice of the weights attributed to each source of threat in the objective
function. Although inverse methods can help [79], [80], it is often hard to define an
objective function that is truly representative of the behavior of a human driver or
to the real threat posed to the vehicle.
Figure 4-16 shows the cost of the homotopy candidates for the two previous
examples. On the left is recalled the estimation of the margin for reference. On the
right, three objective functions are analyzed: in the first one, all the weights are
equal, in the second, c, is multiplied by three (focus on the speed) and in the last
one, cay is multiplied by five (focus on the lateral acceleration for safety issues). The
computed threat varies greatly in the three cases as expected.
Cost Metric: smaller
Margin Metric: larger = better
Eqiml wagifa
45
jFwC
=
an VW
better
Facnsan Wftrd accdWrgon
B
S5
50
IHI
j40.
0
0
12 3 4
11
0
0
10
48
Equal we ts
Focus on speed
Focus on latra acceWaron
40,
120
ii
Homctopy Candidde
1234
Horatopy Candidate
1234
1234
HfMn
Candidate
Ho:otoy Candidate
Figure 4-16: Threat Assessment for various cost functions Top:
static example,
Bottom: dynamic example
Sensibility to Uncertainty
Another drawback of the lowest cost approach is that it can hardly capture the
control margin and is therefore more sensitive towards uncertainties. In the static
case for instance, the green corridor is arguably the most hazardous in the sense it
101
offers little control freedom. Depending on the choice of the weights, the minimal cost
approach may not take into account this threat since the maneuver doesn't require
a hard brake or a strong lateral acceleration.
Similarly, in the dynamic scenario,
the purple maneuver is relatively complex and doesn't offer much control freedom.
Nevertheless, the lowest cost approach is likely to ignore this fact as the related
optimal maneuver doesn't require much input effort.
Consequently, a small error on state estimation of the obstacles can lead to collision
when considering the optimal approach as the optimal trajectory can come close to
the obstacles and road edges. On the contrary, the margin approach yields more
robustness as the safest candidate is the one offering more margin.
In this case,
the safest and most certain trajectory would be the one using the Chebyshev center
as input command. As a conclusion, the margin approach seems both more robust
and less model-dependent and is well suited for autonomous and semi-autonomous
applications.
4.4
Application
to
Decision
Making
For
Autonomous Driving
Although the main purpose of the margin metric and the homotopy formulation
is to introduce a better framework for shared control between a human operator and
the computer, the general approach is also well suited for navigation in constrained
environments such as highways or smaller roads. At each time step, the planner can
perform the threat assessment as described in this chapter and choose the safest
corridor.
Once the most desirable homotopy is identified, the planner needs to
compute the input command to feed the controller.
There are different ways to compute this input. One option is to search for the
minimal cost trajectory within the corridor and keep the first input from the
sequence generating the trajectory.
This approach is similar in principle to the
MPC formulation developed in [20] In this case, the input is chosen so as to
102
minimize an objective function and ensures minimal lateral acceleration or slip
angle.
Another option is to compute the Chebyshev center of the convex hull
associated with the corridor at the first time step.
This input is not necessarily
optimal but it is the most conservative and offers most robustness against
uncertainty with respect to obstacle positions and sizes.
Figure 4-17 displays a simulation of an autonomous vehicle navigating in a
highway. The reference speed is set to 20 m.s-
1
for the host vehicle (blue car) but
the other vehicles navigate at a slower speed (15 m.s- 1).
In this example, the
bicycle model is used for the vehicle model and the input bounds are -/+ 0.1 rad
for the steering angle and -8/ + 4 m.s
2
for the acceleration.
The width of each
lane is 4 meters and every vehicle is assumed to be 2 meters wide and 4 meters long.
The length of the simulation is 30 seconds, divided into six sequences of 5 seconds
for representation.
Because the other vehicles are slower, the host vehicle has to change lane to
overtake them when the option is available. The planner manages to safely guide
the autonomous vehicle amongst the surrounding traffic, and keeps it within the
lane edges when no special maneuver is required. The computer fulfills its role and
the described approach is therefore suited for highway navigation with a constrained
environment, dynamic obstacles and a rigid lane structure.
103
10
5
---
0
-
-
- - --
~
- -..-
- - -..
-..--
mam
-
"
-5
40
50
60
-
-
70
80
90
100
"
110
"
" "" ""
120
130
t=0-5s
140
105-
0-
140
- - - - - - - - - - - - - - - - - - - - - - - - -
t=5- 10s
150
160
170
180
190
200
210
220
-
-
-5 L:
-----
-------.....-
-L-TTIT
24(0
230
10
10
-10
240
250
0- - -
260
-
270
-
280
290
300
310
- --- - - - - - - - -
320 -
.
-
-.......-...-
330-
t-=10-15s
-34(
-
....
---------
5
t= 15
340
350
360
370
380
390
400
410
420
430
-20s
44
....-...--..
- - -10
44
0
450
----
- - - - - - -
460
470
----- ...-
-
--
--
-
--
t =20-25 s
-
0
-
10
480
490
500
510
520
530
540
5
- - - - - - - - - - - - - - - - - - - - - - - - - - - -
-------------------------5jj ------------ M
1
1
1
I -1
_in
'940
550
560
570
580
590
-
10
t =25-30 s
-------------1
600
1
610
. I
620
I630
k
640
Figure 4-17: Simulation of a highway scenario. The host vehicle (blue) navigates
through traffic with the Chebyshev center as input
104
Chapter 5
Better Synergies Between Driver
and Control System
In this last chapter, two reasonable intervention laws are described for driving
assistance that arise from the homotopy framework and the margin-based approach
for threat assessment described in the previous chapters.
The goal is to build a
transparent and unobtrusive active safety system that ideally operates only during
instances of significant threat.
It should preserve the driver's full control of the
vehicle in low threat situations but apply appropriate levels of computer-controlled
actuator effort during high threat situations.
In addition, the semi-autonomous
system conveniently finds a trade-off between safety and human attention as long
and frequent interventions can distract his/her focus on the driving task.
First, the evolution of the threat in each homotopy candidate in canonical
scenarios is analyzed.
Based on this analysis, two approaches to the intervention
process are proposed. The first one relies on the assumption that human drivers
tend to always travel in corridors that offer a margin greater than a specific
threshold.
The second one relies on a probabilistic model of a human driver for
which we assume that the likelihood the human operator chooses one of the
homotopy candidates is proportional to the margin offered by the corridor.
105
5.1
Evolution of the Threat
This section aims to analyze how the control freedom in each candidate homotopy
evolves as the driver comes closer to a hazard. A naive model of the human driver
assuming constant input is implemented. This work helps defining safety threshold
standards as well as a relevant metric to determine ideal intervention time. Indeed,
at every iteration, the planner estimates the threat in each corridor and the final goal
is then to combine all the information into a single metric for decision making.
5.1.1
Traffic Slow Down
This first example is similar to the static case of Chapter 4. It can represent an
emergency slow down on a highway or hazardous debris on the road. In this radical
case, the two vehicles are stationary on the second and third lanes. We consider the
hypothetical case where the driver is either asleep or hasn't noticed that the two
lead vehicles have stopped. As a consequence, he keeps going straight at a constant
speed and is about to collide with one of the two vehicles.
Figure 5-1 shows the
control freedom associated with each homotopy candidates at different time steps:
1.6s, 1.0s and 0.4s before collision.
Initially, the homotopy associated with lane
keeping offers the largest margin (aquamarine homotopy), followed by the right lane
change maneuver (purple homotopy). As the vehicle nears the obstacle, changing
lanes becomes the most desirable navigation decision, as indicated by the larger
margin for avoidance maneuvers shown in the middle subfigure. There, remaining in
the lane requires a strong braking input and loses its attractiveness compared to the
(purple and green) candidate homotopies that involve lane-change maneuvers. The
green corridor exhibits reduced margin due to the presence of a second obstacle on
the second lane. Finally, when the vehicle reaches a distance of 25m to the obstacle,
it has no option but to change lanes because there is insufficient braking authority
to stop the vehicle and avoid collision. Note that the control margin offered by the
green homotopy first increases as the relative size of the gap between the two
vehicles in terms of visibility increases when the host gets closer.
106
e
-
e
10.,
1.6 s before collision
0
0
10
20
30
40
50
60
10
5
1.0 s before collision
0
-5
0
10
20
30
50
40
0
10
5
-
.-........-.......
------------09000000*0*e
.51
0
10
20
30
50
40
0.4 s before collision
.
0
60
Figure 5-1: Margin at three time steps
Figure 5-2 shows a more precise evolution of the control freedom in each navigation
corridor. The purple homotopy quickly overcomes the aquamarine one in terms of
desirability. At the end, the only feasible navigation option is the orange homotopy
which consists in crossing two lanes. Indeed, the left turn to avoid the obstacle is
so severe that the driver doesn't have enough steering margin to counter-steer on
the adjacent lane but has to terminate the maneuver of the top lane. Note that if
the slope is initially small, the control freedom tends to decrease very quickly as the
driver nears the obstacle. Ideally, the high level planner anticipates this drop soon
enough so that the controller has enough time to deflect the vehicle trajectory. Here
a safety threshold is arbitrarily set to 35. This is a normalized metric, 100 indicating
no restriction on the input command. This threshold corresponds to roughly a third
107
of the global input space available. In this example, it is particularly well suited as
the largest margin falls below this threshold before the slope of the threat variation
becomes too high. More precisely, it leaves one second to the planner to respond to
the threat.
70
0
2
04
0.6
08
11.2
IA
1.8
1A
tie'
Figure 5-2: Evolution of the margin of feasible corridors
5.1.2
Lane Merging Scenarios
This second example was also introduced in the previous chapter.
The fourth
lane is being eliminated, forcing the lead vehicle located on this lane to merge into
the current lane of the host vehicle.
Similarly, we still consider the hypothetical
case where the driver is either asleep or hasn't noticed the lane change of the lead
vehicle. Figure 5-3 displays the margins at three consecutive time steps. Initially,
the safest choice is to slow down and let the other vehicle merge in front. Changing
lane (left maneuver) is also safe as it offers comparable control freedom since no
vehicle is located in the second lane. It allows the lead vehicle to have more room
for maneuver to perform its lane change.
At this point, another possibility is to
accelerate so that the lead vehicle merges behind the host. Note that the decision
tree approach is employed to identify homotopy candidates. Thus, this last maneuver
can be assimilated to a double lane maneuver even though most of the path is located
in the same lane when the lead vehicle is still in the fourth lane. Then, as the
driver doesn't modify its course, the two homotopies that terminate in the third lane
108
2
(blue homotopies) lose their appeal while the green corridor remains a safe choice
(middle subfigure). Finally, if the driver hasn't taken action yet, the planner has
no other choice but to perform a left avoidance maneuver as there is insufficient
braking authority to stop the vehicle and avoid collision. This homotopy framework
only considers good navigation options. There might be another extreme maneuver
involving a sharp right turn that would lead to road departure (better than collision)
but the violation of constraints is not addressed here.
10-
~ ~
0
10
20
30
50
40
~.
0.5 s before collision
60
10
0.3 s before collision
-0
-5
)
10
20
30
50
40
60
10
5 --
0.1 s before collision
0
-5
10
20
30
40
50
60
Figure 5-3: Margin at three time steps
Figure 5-4 shows the temporal evolution of the margin in each homotopy
candidate.
The green homotopy soon becomes the safest option compared to the
two blue homotopies, as the lead vehicle merges into the third lane. Note that the
109
drop of the maximal margin is even more pronounced in this extreme scenario than
in the previous example. Once the the metrics falls below the threshold, the planner
has less than 0.1 seconds to correct the trajectory of the vehicle and avoid the
collision. This example is very unlikely as it assumes that both the host and the
lead vehicle ignore each other's behavior. However, in the eventuality such an event
occurs, the semi-autonomous system has to be able to detect such hazard.
It
becomes crucial that the planner anticipates this drop so that it can safely assist the
human operator.
When no information about the driver intent is available, one possibility is to
consider the latest variation of the threat metric and extrapolate its value for the next
one or two seconds to get a simple estimate of the future threat posed to the vehicle.
The expected threat can then be compared to the threshold instead of considering
only the current threat. However, if the planner can precisely predict the intent of the
driver, this estimation becomes less significant as the planner can respond sooner. In
the example, the blue homotopy falls below the threshold 0.15 s before the maximal
margin, which gives more leverage to the controller. This issue is further discussed
- -60
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
--
-
-
-
-
-
-
-
-
in the next sections introducing two approaches to the intervention law.
E
20
10
_________
0
0.1
0.2
0.3
0.4
t [s]
Figure 5-4: Evolution of the margin of feasible corridors
110
0.5
0.6
5.2
Control Intervention Law Based on Maximal
Margin Available
This section introduces a simple approach to shared control between a human
driver and onboard planner based on the computation of the maximal margin
available. The hypothesis is described first and the experimental setup for human
testing in the CarSim Simulator is then detailed. Finally, simulations in the Matlab
environment illustrate the implementation of such an intervention law based on this
assumption.
5.2.1
Hypothesis
In this approach, we assume that human drivers tend to travel through corridors
that offer a relatively high control margin (i.e.
threshold), whenever the choice is available.
specific homotopy candidate.
superior to some pre-defined
No strong preference is given to a
The driver is assumed to arbitrarily choose a
reasonable navigation strategy. In this context, the intervention law depends on the
sole computation of the maximal margin at every time step. Indeed, if the maximal
margin is above the predefined threshold, the planner can be sure that there exists
at least one and maybe several homotopy candidates that offer enough flexibility.
Given the hypothesis just stated, the semi-autonomous system grants full authority
to the human driver.
On the other hand, if the maximal margin falls below the
safety threshold, it implies that all homotopy candidates can be labeled as
dangerous and whatever the choice of the driver, the active safety system takes
control in order to make sure the vehicle safely overcome the temporary hazardous
situation.
In accordance with the analysis of the various margin metrics in Chapter 4, the
weighted average norm combined with the use of the Chebyshev ball is employed in
the following simulations for a conservative threat assessment and intervention.
One important comment is that this model presumes that the driver is focused
111
on the driving task and is therefore vigilant to the surrounding environment. There
is no prediction with respect to the intent if the human driver. In the opposite case
(the driver is asleep for instance), the vehicle eventually finds itself in a hazardous
posture and the semi-autonomous system then takes action. However, the response
delay is degraded as the hypothesis about the driver behavior is not necessarily valid.
In this situation, other sources of information such as gaze points or head movements
can alert the system when the driver is distracted and the onboard system can then
take up the reins even if the maximal margin computed if above the threshold.
5.2.2
Experimental Setup with CarSim
A driving simulator (CarSim) capable of simulating a motorway traffic
environment was used to measure driver behavior data from a dozen volunteers.
The scenario involved a three-lane highway with relatively dense traffic in which two
consecutive vehicles are on average spaced by approximately one hundred meters.
Subject were asked to drive naturally between 90 and 100 kph and preferably in the
center lane. In a random manner, some vehicles from traffic were programmed to
suddenly slow down in order to create a surprise effect and analyze how the human
operator responds to the resulting threat. After a training run to get familiar with
the simulator, subjects have were asked to take two driving tests of 5 minutes where
they had to perform lane changes or strong braking to avoid collision with vehicles
from traffic. The data was then post processed with Matlab using the margin based
approach previously introduced.
At each time step, the algorithm identified the homotopy candidates and computed
an estimate of the control freedom available. As the whole trajectory of the driver is
known, the algorithm can easily determine which navigation strategy the driver has
chosen at every iteration. The vehicle model used to sample the lattice in input space
is the four-wheel Ackerman model (described in Appendix) for better coherence with
Carsim model.
It arises from this study that human drivers tend to anticipate future threat by
maximizing their future room for maneuver and avoid potential hazardous situations.
112
For instance, they tend to maximize the distance to the closest lead vehicle in the
same lane. Incidentally, when a human operator comes close to a lead vehicle, he
is likely to change lane for a better safety margin (in case the lead vehicle suddenly
brakes for example). This level of anticipation varies among the human population
and some people need a clear horizon to feel safe when others can accommodate
closer lead vehicles. Figure 5-5 shows the average distance to the closest lead vehicle
among human subjects. It varies between 40 and 60 meters, a range which is rather
significant considering the reference speed (100 kph). The further away from the lead
vehicle, the more time to react in case of unexpected event.
70-
40-
So-
-
30
0
2
4
6
a
10
12
Human Subject
Figure 5-5: Average distance to the closest lead vehicle among human subjects
We define a safety threshold such that the human driver chooses a homotopy
candidate whose margin metric exceeds this threshold 90% of the time. This threshold
was computed for every human subject from the pool of participants and averaged
over the duration of the simulation. Two sets of data were distinguished: the first one
gathers instances of "typical high risk", which are defined as the time steps involved
during a lane change maneuver (-2 s before crossing the separating line to +1 s
after) or a strong braking (greater than 3 m.s- 2 in absolute value). The second set
gathers moments of "typical low risk" and includes the time steps that are not in
the first set. The control freedom is normalized with respect to the input bounds of
the vehicle. Therefore, in a completely open environment with no spatial constraint,
there is only one field of safe travel whose associated margin metric is 100. In the
113
case of highway driving, even in the absence of other vehicles, the maximum control
margin may be reduced due to the lane structure. In an open lane, the maximal
control margin ranges from around 55 (at 30 m/s) to about 75 (at 10 m/s).
0. .....
- ...... - -...... - -.-.-.-.-.-.-.-.-.-.-...-.-.-.-.- .-.
..............
020
5 ...........
. .....
........ ........... -......... ............ - --..... ......
1
----
-
26 ..........
......
2
...................................
......
-
3
...........
.
low threat
-
4
5
6
7
8
9
10
11
12
human subject nc
Figure 5-6: Safety threshold defined as first decile for margin choice among human
subjects
Figure 5-6 displays the computed safety threshold for high and low threat
situations for all human subjects.
In low threat situations, the safety threshold
varies between 36 and 40 (normalized value) whereas it varies between 27 and 34 for
high threat events. This disparity among human subjects can be attributed among
others to the safety margin with the lead vehicle or the level of aggressively of the
driving (sharp turns, strong accelerations/brakings...).
This safety threshold,
constructed such that 90% of the choices of the driver remain above this lower
bound appears to be a reasonable indicator for the high level planner to determine
when to intervene.
It follows from this preliminary study that a threshold set
between 35 and 40, which is approximately one third of the total control freedom,
can be used as the lower bound for intervention by the semi-autonomous system. In
other words, when the predicted choice of the driver falls below this threshold, the
onboard controller takes control of the vehicle and complete the maneuver.
114
5.2.3
Intervention law - Matlab Simulation
In this section, we implement some Matlab simulations based on the safety
threshold defined in the previous section to test the semi-autonomous system.
Figure 5.7 displays the evolution of the margin of the various homotopy candidates.
The scenario is similar to the first example of the chapter, except the obstacle are
now moving at 10 m/s. The safety threshold is set to 35. The maximal margin
available and the margin of the corridor chosen by the driver at every time step are
highlighted. While the metric is above the safety threshold (green highlight), full
authority is given to the human driver. When the maximal margin falls below the
threshold (red highlight), the internal computer takes control to ensure collision free
maneuver in hazardous situations. If the exact intent of the driver is known, the
controller can intervene sooner (ti in Figure 5-7); otherwise it has to wait for the
maximal margin to fall below the threshold to take action (t2 ). An intervention law
based on the extrapolation of the variation of the margin is also considered to better
anticipate future threat.
t
t2
70
Figure 5-7: Intervention law based on maximal margin or driver intent
115
Intervention Based on Current Margin
We compare here the performance of the semi-autonomous system when the intent
of the driver is available or not. As one can see in Figure 5-8, the semi-autonomous
system ensures collision free navigation in both cases, but it can intervene sooner when
the intent of the driver in terms of corridor navigation is known. The maximal margin
available at every time step is plotted in the top subfigure. It shows that although the
maximal margin approach allows safe navigation, the intervention might be a little
belated in this specific example, which results in a more hazardous obstacle-avoidance
maneuver. Indeed, the maximal margin can drop as low as 18 at the worst instant (red
trajectory). On the contrary, when the intent is known, the driving assistance system
can anticipate and reacts sooner, which guarantees a smoother and safer maneuver
(green trajectory). Knowing the intent of the driver is often very difficult. To improve
the assistance when the intent is unknown, we propose to extrapolate the variation
of the margin to predict future threat.
-
late intervention
intervention
-early
E
-
40-. ........
0
- - - --
-
.............
10
...
20
...
. ........
30
--
-
40
-
-- - - - - - -
...... ..
..
-
..........
60
50
-
-
2E
70
80
distance [m]
5-
0
- --
---
I
10
- - --
-----
20
I
30
--
I
40
-
I
50
- --
-----
--
--
- ----
I
60
70
80
90
100
x [m]
Figure 5-8: Top: Evolution of the maximal margin available Bottom: Trajectory of
the host vehicle in both cases
116
Intervention Based on Extrapolated Margin
When the intent of the driver is not known for sure, one possibility to anticipate
the future threat posed to the vehicle is to extrapolate the past variation of the
maximal margin available. If the prediction is short (typically less than one second),
the variation of the margin metric can be assumed to be constant. Figure 5-9 displays
the extrapolated maximal margin displays for different prediction horizon ranging
from 0.1 to 0.5 s. The extrapolated value is then compared to the safety threshold
instead of the current margin estimation.
The vertical black lines indicate when
the semi-autonomous system intervenes depending on the choice of the prediction
duration.
0
0.2
04
0.8
00
12
IA
IS
.I
t~aJ
Figure 5-9: Extrapolation of the margin
Similarly to the previous paragraph, it is possible to compare the trajectories of
the vehicle in each case, as well as the maximal margin available (Figure 5-10). In
this example, the longer the prediction (towards green), the sooner the intervention
and the safer the intervention. As the prediction shortens (towards red), the lowest
maximal margins decreases to the value of 18 (as in Figure 5-8). This extrapolation is
a useful tool for the active safety system to intervene sooner and ensure a smoother a
safer maneuver. However, this technique is only meaningful for short predictions: as
the control freedom can evolve quickly, longer predictions could lead to unnecessary
intervention since the extrapolated value of the margin would fall below the safety
threshold more often than expected.
117
7
0 -. -
70
---.-
-.--..--
-. -
-- - --
-..-.-.-.- - -
-.--.-.--.-
-.-.-
--.. .. . . .. . . . --... . .. . .. .-.
- -05speito
--
~-
-
E
04speito
-0.5 s prediction
-- --
5 -------------------
------
---
- - - - - - - - - -
-
distance [in]
0
-
10
-
-0. 8 predictIon
0.3 s prediction
- - - - - - no prediction
-
E 0
10
20
30
40
50
60
70
80
90
100
x[m]
Figure 5-10: Top: Evolution of the maximal margin available Bottom:
the host vehicle in both cases
118
rajectory of
5.3
Intervention
Law
Based
on
Probabilistic
Driver Model and Expected Margin
5.3.1
Driver Model
In this approach, the likelihood that the driver chooses one of the homotopy
candidates is assumed to be proportional to the control freedom offered by the
corridor. The hypothesis is similar, in principal, to the one used in [47] where the
probability is exponentially decreasing with the cost associated with the identified
target goal.
We introduce the tuning parameter a that represents the level of
prudence of a human driver and can therefore vary from one driver to another:
p ~,.) m. This expression has to be normalized so that the sum of the probabilities is
equal to one.
associated
If we note H = (hl, ... , hN) the set of N feasible homotopies with
margin (Mi,
distribution p = (p1,
... ,
..., imN),
we can then assign the travel probability
PN) and the formula to derive its value is:
N
k=1
In the previous model we assumed that the human driver would always try to
travel in a corridor that offers a margin above some threshold, without any preference
among the safest margin. In this probabilistic approach, each homotopy candidate is
assigned a likelihood, and the larger a, the stronger the preference of the driver for
the safest corridor.
The goal is to anticipate the future threat posed to the vehicle given the human
model. For this purpose, we propose to compute the expected margin based on the
probability distribution of driver choice.
N
Em
pim
=
i=1
By substituting the value of the probability, we derive the final expression of the
expected margin and variance:
119
N
m
Em
2
+l
and ori =(
pimi
N2
-Zpimi
k=1
It follows from the expression of the probability that the larger the tuning
parameter a, the more pronounced the preference for the corridors offering a large
control freedom.
The computation of the expected value of the margin enables to
merge the threat information of all homotopy candidates into one global metric that
is assumed to mimic as closely as possible the behavior of the human operator. The
variance or the standard deviation
- aims to characterize the confidence in the
prediction of the margin. Indeed, if all homotopy candidates exhibit similar control
freedom, the variance is obviously small and the threat prediction is therefore
accurate,
which is logical as whatever the choice of the driver is, the margin
associated with the chosen travel corridor is close to the expected value.
On the contrary, if the threat assessment task identifies multiple homotopy
candidates with an uneven probabilistic distribution, the expected value of the
margin can possibly falls far from the actual margin value associated with the
corridor followed by the human driver.
Consequently, the intervention law of the
semi-autonomous system should take into consideration both the expected value as
well as the variance of the margin in order to ensure a robust driving assistance.
5.3.2
Construction of the Intervention Law
We propose to build an intervention law that takes into consideration both the
expected margin and the associated variance based on the previous observation.
When a good value of the tuning parameter a is identified, the expected margin is
usually close to the actual choice of the human operator, but it also means that 50%
of the actions of the driver are below this expectation.
The semi-autonomous
system has to be robust and ideally intervenes even when the driver chooses to
travel
within a
corridor
whose
margin
is
smaller than
the expected
value.
Nevertheless, it shouldn't be too intrusive and take control whenever the slightest
120
hazard is detected either. The planner has to find a trade-off between safety and
frequency of intervention.
To tackle the problem, we propose to introduce the
following extended threat metric:
Qthreat(t) = Q(t) =
Em(t) - /O-m(t)
Where Em(t) and am(t) are respectively the expected value and the variance of
the margin at a given instant and
# a second tuning parameter that
enables to adjust
the frequency of intervention of the planner. The larger 3, the lower the value of
Qthreat
and therefore the more likely the active safety system intervenes. In the end,
the semi autonomous system works as follows:
1. Identify corridors of safe travel (homotopy candidates) based on the mapping
of the surrounding environment
2. Estimate the control margin within each corridor thanks to the lattice
3. Compute the extended threat metric Q(t) for a pair (oz, 3) that depends on the
behavior of the driver (assumed to be known)
4. If Q(t) > threshold, full authority is given to the human driver. If Q(t) <
threshold, the active safety system overrides the commands to complete the
maneuver.
The process is repeated at every time step. Whenever the active system starts
to intervene, we suggest that the duration of the intervention be set to at least 3
seconds, i.e. the prediction horizon so that the system guarantees the host vehicle
safely complete the maneuver, which corresponds to the corridor with the largest
margin.
Figure 5-11 displays the evolution of various threat metrics during a time window
that corresponds to a lane change. It is extracted from one of the test runs of CarSim.
The magenta curve represents the margin of the corridor chosen by the driver at
every instant. The yellow and cyan curves are respectively the maximal and expected
margin at the same instants. When the yellow and cyan curves overlap, it means
121
the driver chooses the corridor with the largest control freedom. The two envelopes
correspond to two values of the tuning parameter of the intervention law: /3 = 1 for
the dark grey and / = 2 for the light grey. In this example, the choice of the driver
is usually close to the expected value but sometimes fall below, as between 43 and 44
seconds. Depending on the value of 3, the system is more or less conservative. In this
example, the threshold is set to 40, similar to the previous work. In the hypothetical
#
case where
is set to one, the active system would start intervention at around 42.5
seconds. The choice of / is explained in the next section and relies on the analysis of
the receiver operating characteristic (ROC) curve.
7060-
c 40
--
-
..-
-
....-
.
50 L
E
-/+2 sigma
-/+ sigma
expected margin
30
-
20[-
-
10
42
42.5
43
43.5
44
-
max margin
human operator
safety threshold
44.5
45
t [s]
Figure 5-11: Intervention law based on expected margin
5.3.3
Human Testing with CarSim
The same data collected with carsim from a pool of human subjects is used in this
section in order to validate the probabilistic model assumption. First, the behavior or
performance of the human drivers is characterized by computing the optimal tuning
parameter a. Then, the performance of the intervention law is tested using standard
evaluation techniques such as the ROC curve.
122
Characterization of Driver Behavior
One of the advantages of the probabilistic approach to the human behavior
modeling is the possibility to refine the prediction by assigning a well suited value to
the parameter a that is specific to every person and characteristic of the smoothness
of the driving and the risk aversion. Indeed, if the value of a associated with one
driver is high, it means the driver is most of the time considering the safest corridor,
i.e.
the one that offers the largest control freedom.
This behavior allows to
anticipate future threat and and to react more easily to unpredicted action from
other vehicles.
On the contrary, if the value of a is low, it is plausible that the
priority of the driver is not safety and that he prefers to take more risks to reach his
destination faster for instance. It could also show that the driver is not completely
focused on the driving task and sometimes reacts with some delay to the threat.
Figure 5-12 displays the relative error between the expected margin and the
actual choice of human subject. The error is averaged over the whole duration of
the simulation. The reference is the human choice and is set to zero (black line).
The green line represents the relative error of the metric associated with the
instantaneous maximal margin.
In the case of this specific driver, the maximal
margin is on average 5% above the real choice. The red curve represent the relative
error between the expected margin and the reference for several values of a. For
a = 1, the metric is below the reference by 10% and the error decreases as a
increases.
The curve eventually crosses the reference line and the error starts to
increase again and eventually converges to the maximal margin, as expected. For
this specific human subject, a value of a between 5 and 6 appears to be a good
estimate. The predicted metric is then very close to the actual choice of the driver
on average with a standard deviation of 8%. This analysis can be done for every
driver and the best a is given to the high level planner for better synergies with the
human operator.
123
...
.
. .....
......
.. ....
.....
..
..
..
. ............
....... . ...
. .....
. .....
....
....
. ...............
...........
........
..................
. .........
.......
0.1
0.05
2-0.054)
-0.1
-0.15
-0
-
-0.2
variance
expected margin
max margin
I-03
-0.25
01
2
3
4
5
6
7
8
9
10
alpha
Figure 5-12: Relative error between the expected margin and the actual choice of the
driver in function of the tuning parameter a
Performance of the Intervention Law
In order to characterize the performance of the intervention law and determine the
optimal pair (a, /) to comply with the requirement of the semi-autonomous system,
a receiver operating characteristic (ROC) curve is plotted for each individual. ROC
curves are popular tools to illustrate the performance of a binary classifier system as
its discrimination threshold is varied. It is created by plotting the sensitivity of the
classifier that is the fraction of true positives out of the total actual positives (TPR
= true positive rate) versus the fraction of false positives out of the total actual
negatives (FPR = false positive rate), at various threshold settings. In our case, the
threshold varies with the parameter 0 of the extended threat metric Q.
Besides, the ROC curve can also be used as a complementary tool to determine
the optimal parameter a of the driver model. Indeed, the further away from the
identity function in the direction of the top left corner, the better the classifier. It
implies a high rate of true positive for a small rate of false positive. More precisely,
one common technique to estimate the performance of the classifier is to compute the
124
area under the ROC curve: the closer the result to one, the better. With respect to
our semi-autonomous system, we can plot the ROC curves that correspond to various
values of alpha and choose the one that offers the largest integral.
Once the optimal a is obtained, the value of 0, which is equivalent to the
threshold of the classifier, can be determined with respect to the requirement one
wants to impose on the active safety system. There are three main approaches. The
first possibility is to fix to a safety threshold, for instance to detect at least 90% of
dangerous situations.
Then # is chosen so as to minimize the false positive rate
while guaranteeing the safety requirement. On the contrary, the second possibility is
to bound the rate of false positive and choose the value of
detection of real intervention.
/
that maximizes the
Finally, the third approach consists in finding a
trade-off between true and false positive by choosing the
/
that maximizes the
distance from the ROC curve to the identity line.
Figure 5-13 illustrates the concept with the example of one human subject (same
as in Figure 5-12). Ten ROC curves are superimposed on the same plot, corresponding
to various values of a. Low and high values of a correspond respectively to the red
and green curves. In this example, intermediate value of alpha (brownish curves) are
the best choices. The black ROC curve (a
=
6) corresponds to the maximal area
under the curve and therefore the best performance. When this value of a is selected,
if the rate of false positives is bounded to 10%, then 80% of real threat situations are
detected. Likewise if we want the rate of real positive to be greater than 90%, then
the rate of false positives exceeds 20%.
125
1
..-.
...-.-.
.-.-.-..-.
.-.
0.9 - . . .--.
.
-.
0.7
-.
--
. . . . .--.
.......-.
.-.
. .. . .
--.
.-.
.-.-.-.
..-.
..-.
....
.
0.8
-...- - /
----------
0.6
.
.......
.....
..........
.......
...
.
..
..-I
..
--.
...
---
L
..
.
.......
-
..
- ..-.
0.5 ---- ..
Zl
0
/ ..... .........
0.4
1 ---
-. -.
0.31 --- .----
/ .............
-
......-.--.-.
....-.-.
.-.-.
.--------------
U 0.95
0.9
/.........../
ass
/-- - -
2
-
0.j
------
3
4
a
5
7
8
9
10-
alpha
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
false positive
Figure 5-13: Typical Receiver Operating Characteristic curve for one human subject
This analysis can be performed for every driver. In real life situation, the high level
planner would first estimate the optimal pair (a, 3) during an initial learning period
and use them once the calibration is over. Figure 5-14 displays the computed optimal
value of the a parameter among the human subjects. It appears that most driver
exhibit a similar behavior with the value of a ranging from 3 to 6.5 with a strong
prevalence between 4 and 4.5. Incidentally, in addition to allowing to more finely tune
the semi-autonomous system, this computation of this parameter could provide car
insurance companies with a more relevant indicator to estimate the driving behavior
126
of their clients and adjust the fees accordingly.
9-
7-
Ca
6-
Q
-
;;Z 5
-
4
E
0
2-
2
4
6
10
12
Human Subject
Figure 5-14: Optimal Alpha tuning parameter among subjects
This tuning parameter is a new concept introduced in this thesis to characterize
the behavior and aggressiveness of the human driver. Current techniques focus on
the sole analysis of the acceleration (average value or variance). Although this type
of analysis partially reflects the behavior of the driver (aggressive driving involve
strong variations of the velocity), it doesn't take into consideration the environment
surrounding the vehicle and can't differentiate accelerations that are required to
avoid collision
(car crash or pedestrian) from "fun and hazardous"
driving.
Figure 5-15 displays side by side the value of alpha with the average value of the
relevant acceleration, i.e. the minimum acceleration required in order to avoid
collision during high threat events. It arises that drivers that present a large alpha
(degree of prudence) tend to anticipate future threat so that the minimum
deceleration to avoid collision remains small. The correlation is not perfect as the
control margin approach to threat assessment doesn't only focus on these simple
avoidance maneuver but consider the whole control freedom associated with more
realistic maneuvers associated with corridor navigation.
127
71
0.
-
Optimal value of alpha
-+
SMinimum acceleration
1
2
3
4
5
6
7
8
9
10
11
12
Human Subject
Figure 5-15: Comparison of optimal alpha and minimum acceleration maneuver
Once the tuning parameter a is identified, the second parameter
#3can
be chosen
so as to abide by the requirements of the system. Figure 5-16 shows the performance
of the semi-autonomous system for all human subjects. On average, the results
range
from satisfactory to very good and augur good progress in this direction if further
research is made. More precisely, the green curve represents the rate of true positive
(TP) intervention in the case the rate of false positive (FP) is bounded to 10%.
Except for three drivers, the rate of TP exceeds 70% and generally evolves around
80% which is rather good for a preliminary result and no refinement. Conversely, the
red curve displays the rate of FP when the safety requirement is that the rate of TP
exceeds 90%. In that case, the rate of FP lies between 10% and 30%, which also
seems reasonable for a first prototype of shared control driving system.
128
-
-
-
. . .. . . . .. . . . . .. . .
-
-
0
-
.
. . . . . .. . . . .. . .
CL
...
of0--R:
false positive for 90% true positive goal
-G
------ ----- Tr.e.P.s.tie
~ --tg
o - - ....
-Rate
of true positive for 10% false positive bound
-
50
ad.t
60
100
2
4
6
8
10
12
human subject
Figure 5-16: Percentage of True Positive and False Positive for given requirements
We can then define some performance metric as the rate of TP for 10% FP over the
rate of FP for 90% TP. The larger the metric, the larger the rate of true detections
and/or the smaller the number of false alarms.
It appears that the margin-based
approach for threat assessment is sensitive to the behavior of the driver. For instance,
if the driver "oscillates" too much within its corridor instead of trying to keep a
straight trajectory, it can alter the prediction of the planner. In Figure 5-17, the
performance metric is displayed alongside with the variance of lateral offset (distance
to the center line) of each driver. Although there is not a strong correlation between
the two, we can see that large lateral offset accounts partially to the bad performance
of the intervention law. However, the data was acquired using a driving simulator
and human drivers were more likely to oscillate than in real life driving situations.
129
6*
subject
no
muman
Figure 5-17: Performance of the prediction and variance of lateral offset
130
5.4
Future Work
Implementation of the Semi-Autonomous
System in Real
Time
The threat and margin based active safety system described in this thesis was
successfully simulated both for fully-autonomous and semi-autonomous scenario in
Matlab environment.
The semi-autonomous mode was tested based on some
reasonable driver model. The hypothesis regarding the behavior of the human driver
(safety threshold and probabilistic intent) were then tested with relative success
using the car simulator Carsim and promise great advances in the improvement of
the synergies between human and automation in the field of driving.
The next step is to implement this active system in real time and synchronize it
with Carsim.
experience,
So far, the computation time is a bit too high to allow a smooth
but converting the code into C and taking
advantage of the
parallelisation of the computation would undoubtedly lead to much faster running
time that guarantees a sufficient operating frequency.
Improvement of the Intent Prediction
In order to improve this active safety system and in particular the prediction of
the intent of the driver, one possible research area is to merge this probabilistic model
with other sources of information that would provide complementary indications (gaze
points, neurological factors). As a matter of fact, the errors result primarily from
two distinct sources. The first one is the fact that the human driver is not always
focused on the driving task (distraction, fatigue) and therefore the hypothesis that
supports the model is not valid in this event. This type of error reduces the rate
of true positive. The second source of error occurs when the system unnecessarily
intervenes. This is usually the case when the variance is high. In this case, the system
forces the vehicle to stay in the safe corridor, which is what the driver would have
done anyway. This phenomena, although not dangerous, increase the rate of false
131
positives and can make the driver feel uncomfortable. If the secondary information
show that the driver is focused and likely to make the good choice, then the variance
can be reduced (increased confidence) for a more refined prediction.
Other Types of Intervention Laws
The intervention laws presented in this thesis are based on the notion of safety
threshold.
In other words, whenever the threat metric falls below the defined
threshold, it triggers the intervention of the onboard controller which takes complete
control of the vehicle temporarily in order to perform the hazardous maneuver. This
approach is binary in the sense at each time step, either the human driver or the
automated system is the only one in control.
Although simple, this approach
ensures that there are no conflict between the human and the machine in the case of
multiple navigation strategies, which is often the case.
However, we could also
implement more subtle intervention laws in which the semi-autonomous system
would only restrict the input range available to the driver instead of completely
overwriting the commands. For instance, the inputs that correspond to a homotopy
class that presents a high threat would be forbidden to the driver. This approach
would also guarantee collision avoidance and would additionally offer more freedom
to the driver. This type of intervention is difficult to simulate in Matlab but could
easily be tested in real time if the computation power is sufficient.
132
Appendix A
4-Wheeled Vehicle Model
The linearized equation of motion of a 4-wheeled Ackermann-steered model are
described here. This model is employed to generate the trajectories from the inputspace lattice, in particular when post-processing the data from CarSim. It is more
realistic than the standard bicycle model introduced in chapter 3 and accounts for
the lateral, longitudinal and yaw dynamics.
Figure A-1: Vehicle Slip Model
Vehicle states include the position of its center of gravity [x, y], the vehicle yaw
angle T, the yaw rate xI and the sideslip angle / as illustrated in Figure A.
133
The equations of motion are:
= V cos(P + 0)
y=V sin(IF + 0)
EFy
=
mi[ sin(T + 0) + V(4' + /)cos(T + /)]
EMz= IZzx
Tire compliance is included in the model by approximating lateral tire force Fy as
the product of each tires cornering stiffness C and sideslip angle a: Fy = Ca.
The discretization time of the prediction horizon is usually small enough (typically
0.1 s) so that we can consider the speed to be constant over the small dt. We also
assume small slip and steering angles in order to linearize the equations that become:
J. = V
3=
y=V(F + 0)
-Cr+C,
+
mV
SCrx,-Cxf
(CrX,-CX
1)
-
MV2
Crx2
Izz
Cfx
IzzV
2
+
C_
mV
Cx
IZZ
where Cf and C, represent the cornering stiffness of the front and rear wheels,
and xf and x, denote the longitudinal distances from the center of gravity of the front
and rear wheels, respectively.
The table below defines and quantifies each model's parameters. These parameters
were chosen to best match the vehicle plant in CarSim Simulator.
Table A.1: Vehicle Parameters
Symbol
m
Description
Total Vehicle Mass [Kg]
Yaw moment of inertia [Kg.m 2]
Value
1670
Wd
Distance to front wheels [im]
Distance to rear wheels [m]
Total width of the vehicle [m]
2550
1.2
1.3
1.7
Cf
Front cornering stiffness [N/deg]
1500
C,
Rear cornering stiffness [N/deg]
1350
Izz
Xf
Xr
134
Appendix B
Eye-tracking Information
This appendix presents some preliminary results regarding the analysis of the eye
tracking data in order to infer the intent of the human driver in terms of corridor
navigation.
The data was recorded in the iLab at USC using Xmotor simulator
synchronized with an eye tracker.
The goal is to determine whether it is possible
to predict the candidate homotopy the driver is about to choose in simple cases (no
lane structure, limited number of options...). Essentially, the method is based on the
comparison of the number of gaze points located on the left and on the right of the
obstacles.
Note that some of the gaze points are filtered out (sky, speedometers).
This simple method works pretty well in the cases of a single obstacle in the center of
the road, either for straight lines or curves. However, in the case of multiple obstacles
or secondary sources of threat on the side (pedestrian, parked car), the predictions
are not as good. This area of research could be further investigated with the addition
of car telemetry for instance.
Experimental Setup
A small pool of human subjects were asked to drive for a short time (5 mn) on
a simple track simulated in Xmotor.
The scenario consisted in an open single road
littered with obstacles in the center and points of interest on the edges (pedestrians
of parked cars). During the driving task, the car telemetry along with the gaze points
of the human driver were recorded simultaneously. The eye-tracker was an ISCAN
135
240Hz video-based machine. The principle is the following: an infrared light source
illuminates the subject's right eye, and a high-speed video camera records a close-up
movie of the eye; two features are tracked in real-time on the video sequence: the
outline of the pupil and the specular reflection of the light source onto the cornea.
After a calibration procedure used to establish a mapping between stimulus referential
and camera referential, this setup allows the controlling computer to compute the
(X, y) coordinates of the point of fixation.
Decomposition of the Camera View
Transposing the gaze points from the driver view to the top view of the track is
not very accurate and suffers from the approximation of the camera model.
Especially around the horizon line, a very small displacement of the gaze point on
the camera view can lead to great displacement on the top view of the track.
Besides, in real driving conditions, the acquisition of the gaze points would be less
accurate than in simulated experiments, amplifying the phenomena. Therefore, we
suggest to work directly on the camera view and transpose the position of the
obstacles in this framework.
A basic approach consists in dividing the image in three main regions: the sky, the
"active region" and the speedometer region. If the driver is looking either at the first
or third region for a long period (greater than Is for example), we can assume that
the driver is distracted from his primary task which is the driving task. The central
region is subdivided into smaller regions, some including obstacles, other being free
of any source of threat. Figure B.1 presents this decomposition in the simplest case
of a single obstacle in the middle of the road (left) and in a slightly more complex
case (right). The blue region contains the obstacle and the two side regions (green)
are free zones.
136
Figure B-1: Decomposition of the image with respect to obstacles
In this preliminary investigation, the prediction of the intent of the driver
regarding the choice of the homotopic region is based on the simple count the
number of gaze points on the left and on the right of the obstacles that occurred
between 0 and 50 meters before reaching the obstacle. Evaluated on a very limited
%
pool of human subjects, this approach allowed to predict the right choice with 75
success. In the case of a single obstacle in the middle of the road or two obstacles
side by side, the results are satisfactory since the algorithm nearly always predict
the intention of the driver. In the presence of another obstacle on the side of the
road (pedestrian, parked car), the results are not as good. Filtering out the gaze
points located around this secondary obstacle improves the prediction but they
remain not reliable enough.
In the future, in order to improve the prediction,
several paths can be explored, such as a better filtering of the gaze points that are
not related to the decision making process, the identification of pattern in the gaze
points or also the refinement brought by the driving telemetry.
Complementary Information for Attention and Threat
In addition to providing a useful tool to predict the intent of the driver, gaze
points can also be employed to estimate the state of attention of the human driver.
As mentioned above, it is possible to determine when the human operate not focused
on the driving task, for instance if he's not looking at the road or if the point of
137
. ..
.. . ..
.
.......
............
....................
......
..
. .......
fixation remains for an abnormally long time at a given spot. If such instants are
identified, the active safety system described in this thesis can intervene even if the
computed threat is below the safety threshold.
Finally, although mapping the gaze points on the 2-D top view is not very accurate,
it is useful to have a first insight of where the driver focuses his attention and see if
there is a correlation with the main sources of threat. Figure B.2 displays the discrete
(left) and continuous (right) accumulation of the gaze points on the top view of the
track. Note that the gaze points located in the sky or around the speedometers are
filtered out. We can see that there is a higher concentration around some of the
obstacles and the inside of the turns (figure B.2). Given this observation, we can
assume it is likely there is some correlation between the gaze points and the sources
of threat. This hypothesis can lead to further investigations. On the one hand, gaze
focus on some specific region of the environment may provide some information on
potential threat the planner hasn't taken into consideration. On the other hand, if
the semi-autonomous system detects hazards the human operator hasn't notice, it
may provide another way to estimate his state of attention.
IIS
0
-M
-4MI
0
o
4
-iM
M
-100
0
100
M
Figure B-2: Correlation between sources of threat and gaze points
138
30
4W
Bibliography
[1] National Highway Traffic Safety Administration. 2012 motor vehicle crashes:
Overview. U.S. Dept. Transp.
[2] James R McBride, Jerome C Ivan, Doug S Rhode, Jeffrey D Rupp, Matthew Y
Rupp, Jeffrey D Higgins, Doug D Turner, and Ryan M Eustice. A perspective
on emerging automotive safety applications, derived from lessons learned
through participation in the darpa grand challenges. Journal of Field Robotics,
25(10):808-840, 2008.
[3] Andreas Eidehall, Jochen Pohl, and Fredrik Gustafsson. Joint road geometry
estimation and vehicle tracking. Control EngineeringPractice, 15(12):1484-1494,
2007.
[4] Ardalan Vahidi and Azim Eskandarian. Research advances in intelligent collision
avoidance and adaptive cruise control. Intelligent TransportationSystems, IEEE
Transactions on, 4(3):143-153, 2003.
[5] Wassim G Najm, Mary D Stearns, Heidi Howarth, Jonathan Koopmann, and
John Hitz. Evaluation of an automotive rear-end collision avoidance system.
Technical report, 2006.
[6] Quoc Huy Do, H.T.N. Nejad, K. Yoneda, S. Ryohei, and S. Mita. Vehicle path
planning with maximizing safe margin for driving using lagrange multipliers. In
Intelligent Vehicles Symposium (IV), 2013 IEEE, pages 171-176, June 2013.
[7] Matthew McNaughton. Parallel algorithms for real-time motion planning. PhD
thesis.
[8] Thomas Sattel and Thorsten Brandt. From robotics to automotive: Lanekeeping and collision avoidance based on elastic bands. Vehicle System Dynamics,
46(7):597-619, 2008.
[9] Sergey Drakunov, Umit Ozguner, Peter Dix, and Behrouz Ashrafi. Abs control
using optimum search via sliding modes. Control Systems Technology, IEEE
Transactions on, 3(1):79-85, 1995.
[10] Anton T Van Zanten. Evolution of electronic control systems for improving the
vehicle dynamic behavior. In Proceedings of the 6th InternationalSymposium on
Advanced Vehicle Control, pages 1-9, 2002.
139
[11] Chi-Ying Liang and Huei Peng. Optimal adaptive cruise control with guaranteed
string stability. Vehicle System Dynamics, 32(4-5):313-330, 1999.
[12] Dae-Jin Kim, Kwang-Hyun Park, and Zeungnam Bien. Hierarchical longitudinal
controller for rear-end collision avoidance.
Industrial Electronics, IEEE
Transactions on, 54(2):805-817, April 2007.
[13] A Polychronopoulos, M Tsogas, A Amditis, U Scheunert, L Andreone, and
F Tango. Dynamic situation and threat assessment for collision warning systems:
The euclide approach. In Intelligent Vehicles Symposium, 2004 IEEE, pages 636641. IEEE, 2004.
[14] Yizhen Zhang, Erik K Antonsson, and Karl Grote. A new threat assessment
measure for collision avoidance systems. In Intelligent TransportationSystems
Conference, 2006. ITSC'06. IEEE, pages 968-975. IEEE, 2006.
[15] Gerald H Engelman, Jonas Ekmark, Levasseur Tellis, M Nabeel Tarabishy,
Gyu Myeong Joh, Roger A Trombley Jr, and Robert E Williams. Threat level
identification and quantifying system, April 25 2006. US Patent 7,034,668.
[16] Mattias Brannstrom, Erik Coelingh, and Jonas Sjoberg. Model-based threat
assessment for avoiding arbitrary vehicle collisions. Intelligent Transportation
Systems, IEEE Transactions on, 11(3):658-669, 2010.
[17] Paolo Falcone, Mohammad Ali, and Jonas Sjoberg. Predictive threat assessment
via reachability analysis and set invariance theory. Intelligent Transportation
Systems, IEEE Transactions on, 12(4):1352-1361, 2011.
[18] Sterling J Anderson, Steven C Peters, Tom E Pilutti, and Karl Iagnemma. An
optimal-control-based framework for trajectory planning, threat assessment, and
semi-autonomous control of passenger vehicles in hazard avoidance scenarios.
InternationalJournal of Vehicle Autonomous Systems, 8(2):190-216, 2010.
[19] Soonkyum Kim, Koushil Sreenath, Subhrajit Bhattacharya, and Vijay Kumar.
Optimal trajectory generation under homology class constraints. In Decision
and Control (CDC), 2012 IEEE 51st Annual Conference on, pages 3157-3164.
IEEE, 2012.
[20] Sterling J Anderson, Sisir B Karumanchi, Karl Iagnemma, and James M Walker.
The intelligent copilot: A constraint-based approach to shared-adaptive control
of ground vehicles. Intelligent TransportationSystems Magazine, IEEE, 5(2):4554, 2013.
[21] John Markoff. Google cars drive themselves, in traffic.
10:A1, 2010.
The New York Times,
[22] Karl Iagnemma, Martin Buehler, and S Singh. Special issue on the 2007 darpa
urban challenge. Journal of Field Robotics, 25(8):9, 2008.
140
[23] Chris Urmson, Joshua Anhalt, Drew Bagnell, Christopher Baker, Robert
Bittner, M. N. Clark, John Dolan, Dave Duggins, Tugrul Galatali, Chris Geyer,
Michele Gittleman, Sam Harbaugh, Martial Hebert, Thomas M. Howard, Sascha
Kolski, Alonzo Kelly, Maxim Likhachev, Matt McNaughton, Nick Miller, Kevin
Peterson, Brian Pilnick, Raj Rajkumar, Paul Rybski, Bryan Salesky, YoungWoo Seo, Sanjiv Singh, Jarrod Snider, Anthony Stentz, William Red Whittaker,
Ziv Wolkowicki, Jason Ziglar, Hong Bae, Thomas Brown, Daniel Demitrish,
Bakhtiar Litkouhi, Jim Nickolaou, Varsha Sadekar, Wende Zhang, Joshua
Struble, Michael Taylor, Michael Darms, and Dave Ferguson. Autonomous
driving in urban environments: Boss and the urban challenge. Journal of Field
Robotics, 25(8):425-466, 2008.
[24] Roland Siegwart, Illah Reza Nourbakhsh, and Davide Scaramuzza. Introduction
to autonomous mobile robots. MIT press, 2011.
[25] Peng Cheng, Zuojun Shen, and S La Valle. Rrt-based trajectory design for
autonomous automobiles and spacecraft. Archives of control sciences, 11:167194, 2001.
[26] Yoshiaki Kuwata, Gaston A Fiore, Justin Teo, Emilio Frazzoli, and Jonathan P
How. Motion planning for urban driving using rrt. In Intelligent Robots and
Systems, 2008. IROS 2008. IEEE/RSJ InternationalConference on, pages 16811686. IEEE, 2008.
[27] Shingo Shimoda, Yoji Kuroda, and Karl Iagnemma. Potential field navigation
of high speed unmanned ground vehicles on uneven terrain. In Robotics and
Automation, 2005. ICRA 2005. Proceedings of the 2005 IEEE International
Conference on, pages 2828-2833. IEEE, 2005.
Lyapunov based performance
[28] Eric J Rossetter and J Christian Gerdes.
system. Journal of
assistance
field
lane-keeping
the
potential
guarantees for
dynamic systems, measurement, and control, 128(3):510-522, 2006.
[29] Arthur Richards and Jonathan P How. Model predictive control of vehicle
maneuvers with guaranteed completion time and robust feasibility. In American
Control Conference, 2003. Proceedings of the 2003, volume 5, pages 4034-4040.
IEEE, 2003.
[30] Wenda Xu, Junqing Wei, John M Dolan, Huijing Zhao, and Hongbin Zha. A
real-time motion planner with trajectory optimization for autonomous vehicles.
In Robotics and Automation (ICRA), 2012 IEEE International Conference on,
pages 2061-2067. IEEE, 2012.
[31] Matthew McNaughton, Chris Urmson, John M Dolan, and Jin-Woo Lee. Motion
planning for autonomous driving with a conformal spatiotemporal lattice. In
Robotics and Automation (ICRA), 2011 IEEE International Conference on,
pages 4889-4895. IEEE, 2011.
141
[32] Martin Rufli and Roland Siegwart. On the design of deformable input-/statelattice graphs. In Robotics and Automation (ICRA), 2010 IEEE International
Conference on, pages 3071-3077. IEEE, 2010.
[33] Thomas M Howard, Colin J Green, Alonzo Kelly, and Dave Ferguson. State
space sampling of feasible motions for high-performance mobile robot navigation
in complex environments. Journal of Field Robotics, 25(6-7):325-345, 2008.
[34] Paolo Falcone, Francesco Borrelli, Jahan Asgari, Hongtei Eric Tseng, and Davor
Hrovat. Predictive active steering control for autonomous vehicle systems.
Control Systems Technology, IEEE Transactions on, 15(3):566-580, 2007.
[35] James J Gibson and Laurence E Crooks.
A theoretical field-analysis of
automobile-driving. The American journal of psychology, pages 453-471, 1938.
[36] Subhrajit Bhattacharya, Vijay Kumar, and Maxim Likhachev. Search-based
path planning with homotopy class constraints. In Third Annual Symposium on
CombinatorialSearch, 2010.
[37] Elisabetta Fabrizi and Alessandro Saffiotti. Extracting topology-based maps
from gridmaps. In Robotics and Automation, 2000. Proceedings. ICRA'00. IEEE
International Conference on, volume 3, pages 2972-2978. IEEE, 2000.
[38] Kevin D Jenkins. The shortest path problem in the plane with obstacles: A
graph modeling approach to producing finite search lists of homotopy classes.
Technical report, DTIC Document, 1991.
[39] E. Hernandez, M. Carreras, J. Antich, P. Ridao, and A. Ortiz. A topologically
guided path planner for an auv using homotopy classes. In Robotics and
Automation (ICRA), 2011 IEEE International Conference on, pages 2337-2343,
May 2011.
[40] Erwin Schmitzberger, Jean-Louis Bouchet, Michel Dufaut, Didier Wolf, and Rene
Husson. Capture of homotopy classes with probabilistic road map. In Intelligent
Robots and Systems, 2002. IEEE/RSJ International Conference on, volume 3,
pages 2317-2322. IEEE, 2002.
[41] Lisanne Bainbridge. Ironies of automation. Automatica, 19(6):775-779, 1983.
[42] Thomas B Sheridan and Raja Parasuraman. Human-automation interaction.
Reviews of human factors and ergonomics, 1(1):89-129, 2005.
[43] Savvas G Loizou and Vijay Kumar. Mixed initiative control of autonomous
vehicles. In Robotics and Automation, 2007 IEEE International Conference on,
pages 1431-1436. IEEE, 2007.
[44] Mariana Netto, J-M Blosseville, Benoit Lusetti, and Said Mammar. A new
robust control system with optimized use of the lane detection data for vehicle
full lateral control under strong curvatures. In Intelligent TransportationSystems
Conference, 2006. ITSC'06. IEEE, pages 1382-1387. IEEE, 2006.
142
[45] Cristina Urdiales,
Manuel Fernindez-Carmona,
Jose Manuel Peula,
R Annicchiaricco, F Sandoval, and Carlo Caltagirone.
Efficiency based
modulation for wheelchair driving collaborative control.
In Robotics and
Automation (ICRA), 2010 IEEE International Conference on, pages 199-204.
IEEE, 2010.
[46] Jihua Huang and Han-Shue Tan. Vehicle future trajectory prediction with a
dgps/ins-based positioning system. In American Control Conference, 2006, pages
6-pp. IEEE, 2006.
[47] Anca D Dragan and Siddhartha S Srinivasa. A policy-blending formalism for
shared control. The International Journal of Robotics Research, 32(7):790-805,
2013.
[48] Dario D Salvucci. Inferring driver intent: A case study in lane-change detection.
In Proceedings of the Human Factors and Ergonomics Society Annual Meeting,
volume 48, pages 2228-2231. SAGE Publications, 2004.
[49] John Krumm. A markov model for driver turn prediction. SAE SP, 2193(1),
2008.
[50] Toru Kumagai and Motoyuki Akamatsu. Prediction of human driving behavior
using dynamic bayesian networks. IEICE TRANSACTIONS on Information and
Systems, 89(2):857-860, 2006.
[51] Anup Doshi and Mohan M Trivedi. On the roles of eye gaze and head dynamics
in predicting driver's intent to change lanes. Intelligent TransportationSystems,
IEEE Transactions on, 10(3):453-462, 2009.
[52] Stefan Haufe, Matthias S Treder, Manfred F Gugler, Max Sagebaum, Gabriel
Curio, and Benjamin Blankertz. Eeg potentials predict upcoming emergency
brakings during simulated driving. Journal of neural engineering, 8(5):056001,
2011.
[53] Jose del R Milln, Ricardo Chavarriaga Lozano, Lucian Andrei Gheorghe, et al.
Steering timing prediction in a driving simulator task. In Proceedings of the
35th Annual InternationalConference of the IEEE Engineering in Medicine and
Biology Society, number EPFL-CONF-188216, 2013.
[54] E. Schmitzberger, J. L. Bouchet, M. Dufaut, D. Wolf, and R. Husson. Capture of
homotopy classes with probabilistic road map. In Intelligent Robots and Systems,
2002. IEEE/RSJ InternationalConference on, volume 3, pages 2317-2322 vol.3,
2002.
[55] Leonard Jaillet and Thierry Sim6on. Path deformation roadmaps: Compact
graphs with useful cycles for motion planning. The International Journal of
Robotics Research, 27(11-12):1175-1188, 2008.
143
[56] Osamu Takahashi and RJ Schilling. Motion planning in a plane using generalized
voronoi diagrams. Robotics and Automation, IEEE Transactions on, 5(2):143150, 1989.
[57] Nils J Nilsson. A mobile automaton: An application of artificial intelligence
techniques. Technical report, DTIC Document, 1969.
[58] Mark de Berg, Marc van Kreveld, Mark Overmars, and Otfried Cheong
Schwarzkopf. Visibility graphs. In Computational Geometry, pages 307-317.
Springer, 2000.
[59] Franz Aurenhammer. Voronoi diagramsa survey of a fundamental geometric data
structure. ACM Computing Surveys (CSUR), 23(3):345-405, 1991.
[60] Kenny Wong and Hausi A M6ler. An efficient implementation of fortune's
plane-sweep algorithm for voronoi diagrams. Citeseer, 1991.
[61] Douglas Demyen and Michael Buro. Efficient triangulation-based pathfinding.
In AAAI, volume 6, pages 942-947, 2006.
[62] SW Sloan. A fast algorithm for generating constrained delaunay triangulations.
Computers & Structures, 47(3):441-450, 1993.
[63] Edsger W Dijkstra. A note on two problems in connexion with graphs.
Numerische mathematik, 1(1):269-271, 1959.
[64] Jin Y Yen. Finding the k shortest loopless paths in a network. management
Science, 17(11):712-716, 1971.
[65] DexterC. Kozen. Depth-first and breadth-first search. In The Design and
Analysis of Algorithms, Texts and Monographs in Computer Science, pages 1924. Springer New York, 1992.
[66] DL Kleinman, S Baron, and WH Levison. An optimal control model of human
response part i: Theory and validation. Automatica, 6(3):357-369, 1970.
[67] Torsten Butz and Oskar Von Stryk. Optimal control based modeling of vehicle
driver properties. 2005.
[68] Zhaoheng Liu. Characterisation of optimal human driver model and stability
of a tractor-semitrailer vehicle system with time delay. Mechanical systems and
signal processing, 21(5):2080-2098, 2007.
[69] Mihail Pivtoraiko, Thomas M Howard, I Nesnas,
experiments in rover navigation via model-based
nonholonomic motion planning in state lattices.
International Symposium on Artificial Intelligence,
in Space, 2008.
144
and Alonzo Kelly. Field
trajectory generation and
In Proceedings of the 9th
Robotics, and Automation
[70] Mihail Pivtoraiko, Ross A Knepper, and Alonzo Kelly. Differentially constrained
mobile robot motion planning in state lattices. Journal of Field Robotics,
26(3):308-333, 2009.
[71] Dave Ferguson, T.M. Howard, and M. Likhachev. Motion planning in urban
environments: Part ii. In Intelligent Robots and Systems, 2008. IROS 2008.
IEEE/RSJ International Conference on, pages 1070-1076, Sept 2008.
[72] Mikhail Pivtoraiko, Ross Alan Knepper, and Alonzo Kelly. Differentially
constrained mobile robot motion planning in state lattices. Journal of Field
Robotics, 26(CMU-RI-TR-):308-333, March 2009.
[73] Alonzo Kelly and Bryan Nagy. Reactive nonholonomic trajectory generation
via parametric optimal control. The InternationalJournal of Robotics Research,
22(7-8):583-601, 2003.
[74] Moritz Werling, Julius Ziegler, S6ren Kammel, and Sebastian Thrun. Optimal
trajectory generation for dynamic street scenarios in a frenet frame. In Robotics
and Automation (ICRA), 2010 IEEE International Conference on, pages 987993. IEEE, 2010.
[75] Bryan Nagy and Alonzo Kelly. Trajectory generation for car-like robots using
cubic curvature polynomials. Field and Service Robots, 11, 2001.
[76] Felix Von Hundelshausen, Michael Himmelsbach, Falk Hecker, Andre Mueller,
and Hans-Joachim Wuensche. Driving with tentacles: Integral structures for
sensing and motion. Journal of Field Robotics, 25(9):640-673, 2008.
[77] Franco P. Preparata and Se June Hong. Convex hulls of finite sets of points in
two and three dimensions. Communications of the ACM, 20(2):87-93, 1977.
[78] ND Botkin and VL Turova-Botkina. An algorithm for finding the Chebyshev
center of a convex polyhedron. Springer, 1994.
[79] Nathan Ratliff, Brian Ziebart, Kevin Peterson, J Andrew Bagnell, Martial
Hebert, Anind K Dey, and Siddhartha Srinivasa. Inverse optimal heuristic control
for imitation learning. AISTATS, 2009.
[80] Brian D Ziebart, Andrew L Maas, J Andrew Bagnell, and Anind K Dey.
Maximum entropy inverse reinforcement learning. In AAAI, pages 1433-1438,
2008.
145