Trajectory Planning for Automated Driving
in Dynamic Environments
An Integrated Approach to Spline-based Motion Planning using
Hierarchical Trajectory Optimization
DISSERTATION
submitted in partial fulfillment
of the requirements for the degree
Doktor Ingenieur
(Doctor of Engineering)
in the
Faculty of Electrical Engineering and Information Technology
at Technische Universität Dortmund
by
M.Sc. Christian Lienke, born Götte
Gelsenkirchen, Germany
Date of submission: November 23, 2021
First examiner: Univ.-Prof. Dr.-Ing. Prof. h.c. Dr. h.c. Torsten Bertram
Second examiner: Univ.-Prof. Dr.-Ing. Ulrich Konigorski
Date of approval: June 1, 2022

To my family

Acknowledgement
This thesis was written during my work at the Institute of Control Theory and Systems
Engineering of the Faculty of Electrical Engineering and Information Technology at
the Technical University of Dortmund.
I would like to thank Univ.-Prof. Dr.-Ing. Prof. h.c. Dr. h.c. Torsten Bertram for the
unconditional support, the personal and scientific feedback and for providing me
the necessary resources to successfully accomplish my doctoral studies. I am deeply
grateful for the given possibility, which helped to advance my skills in the field of
science, but also enabled me to grow as a person.
I would also like to thank Univ.-Prof. Dr.-Ing. Ulrich Konigorski for the interest in my
work and for reviewing this thesis as a second examiner. Additionally, I would like to
thank Univ.-Prof. Dr.-Ing. Timm Faulwasser for his involvement as the third examiner
and Univ.-Prof. Dr.-Ing. Martin Pfost for chairing the examination committee.
I sincerely thank all employees and former employees of the institute for their valuable
input and the inspiring atmosphere during my time at the department. I would like
to thank apl. Prof. Dr. rer. nat. Frank Hoffmann for his ingenious suggestions and
Dr.-Ing. Daniel Schauten for the instructive discussions on general control systems
theory topics and for sharing his experience with regard to administrative affairs. Spe-
cial thanks go to Dr.-Ing. Christian Wissing and Andreas Homann, not only for the
trusting collaboration dedicated to advance automated driving functionality, but also
for taking the time to provide helpful feedback and their valuable fellowship. For the
close cooperation and the excellent scientific exchange I thank Manuel Schmidt, Niklas
Stannartz and Martin Krüger. I also thank Christopher Diehl and Philip Dorpmüller
who have supported my work in many different aspects. In addition, I would like to
thank Dr.-Ing. Malte Oeljeklaus, Dr.-Ing. Christoph Termühlen, Dr.-Ing. Javier Oliva,
Jan Braun, Myrel Tiemann, Franz Albers and many others whether for the cooper-
ative work in a wealth of given tutorials, or with regard to the daily business and
organizational tasks. Likewise, I thank Dr.-Ing. Christoph Rösmann for sharing his
immense expertise across many different topics. For the technical support I sincerely
thank Jürgen Limhoff, Rainer Müller-Burtscheid, Sascha Kersting and Halit Cicek. On
the same lines I thank Gabriele Rebbe and Nicole Czerwinski for their assistance in all
administrative and organizational matters.
Furthermore, I would like to express my gratitude to Prof. Dr.-Ing. Martin Keller
for his encouragement and companionship throughout my career. Without his belief
and persuasiveness I would not have taken this path in the first place and I am very
fortunate for his personal support and thankful for the comprehensive guidance with
respect to my research. The results and experiences gained throughout my research
project would not have been possible without the great and universal support of the
colleagues of the ZF Group. I sincerely thank Dr. rer. nat. Karl-Heinz Glander, Dr.-Ing.
Carsten Haß, Dr. rer. nat. Till Nattermann and all other ZF employees involved in the
work to get the developed ideas on the road.
I also thank my parents for their relentless support which brought me into the position
of pursuing my ambitious goals. Moreover, it is important to mention that this thesis
could not have been finished without the substantial support of my wife Magalie.
She has made many sacrifices during this time and deserves more than I could ever
hope to repay. Finally, I want to thank my children for their love and distraction they
provided. Your lightheartedness kept reminding me what is most important in life.
Gütersloh, June 2022 Christian Lienke
Abstract
Considering the last decades, the trend in the automotive industry to continuously
increase the level of automation of vehicles is evident. A lot of research and develop-
ment effort has been invested to improve upon driving safety and comfort in traffic.
Nowadays, advanced driver assistance systems, and the development of automated
driving functions in particular, represent one of the main areas of innovation in auto-
motive engineering. In order to cope with challenges arising from complex dynamic
environments the automated vehicle needs to perform comprehensive cognitive tasks
that come along with the presence of other traffic participants and the necessity to
adhere to prevailing traffic regulations. As a consequence, the automated driving task
is decomposed into several sub problems. In the functional architecture of automated
vehicles, motion planning that addresses the generation of a comfortable and safe
trajectory is a key component that directly affects the overall driving performance.
This thesis is about the development of a trajectory planning approach suitable to deal
with dynamic environments. A two level hierarchical trajectory planning framework
is proposed that unites the capability of optimality and spline interpolation and ex-
plicitly considers the aspect of contradicting planning objectives. The framework is
designed to work in receding horizon fashion by performing cyclic replanning and
hence accounts for the dynamic character of the environment. The hierarchization into
two separate levels of optimization leads to an approach that covers basic driving
functionality on low level, while required high level behavior is still prioritized. The
presented framework relies on a spline-based trajectory representation with an under-
lying optimal interpolation strategy. The optimal trajectory with respect to a certain
situation is found by joint optimization on high and low level. A continuous and a
discrete trajectory optimization variant to generate an optimal trajectory with respect
to high level objectives are presented that basically differ in the definition of possible
solutions in terms of the optimal decision variables. Constraints like drivability incor-
porated by exploiting the flatness property of the applied vehicle model and accurate
collision avoidance checking are considered explicitly to comply to essential require-
ments for automated driving. To evaluate the quality of the trajectory in terms of the
associated driving behavior, several objectives are defined. For dedicated objectives a
curvilinear frame is used, which enables a precise formulation of the desired vehicle
behavior with respect to driving applications in structured environments. Hence, this
measure permits to formulate objectives independent of road curvature, extending
the scope of the applied trajectory planning approach to a wide range of scenarios.
Evaluation works out the distinct characteristic features of the two presented high
level optimization approaches, showing the achieved performance at the example of
typical (highway) traffic scenarios. It is shown that both, the continuous as well as the
discrete approach, are suitable to solve the trajectory generation problem supporting
the idea of creating a generic trajectory planning framework for automated driving.

Contents
Nomenclature iii
1. Introduction to Trajectory Planning in the Context of Automated Driving 1
1.1. Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2. Motion Planning as Constrained Optimization . . . . . . . . . . . . . . . 3
1.3. Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4. Scope and Organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2. Survey on Trajectory Planning Approaches to Automated Driving 10
2.1. Taxonomy of Trajectory Planning Approaches . . . . . . . . . . . . . . . 11
2.2. Related Work for Automated Driving Applications . . . . . . . . . . . . 14
3. A Concept for Trajectory Planning in Dynamic Environments 20
3.1. Incorporation in the Functional Architecture of Automated Vehicles . . 20
3.1.1. Prerequisites and Realization . . . . . . . . . . . . . . . . . . . . . 22
3.1.2. Interfaces for Trajectory Planning . . . . . . . . . . . . . . . . . . 23
3.2. Requirements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.3. Derivation of a Trajectory Planning Concept . . . . . . . . . . . . . . . . 25
3.4. Comparison to Other Trajectory Planning Approaches . . . . . . . . . . 28
4. Environment-aware Maneuver Planning 30
4.1. Extraction of Maneuver Relevant Information . . . . . . . . . . . . . . . 31
4.2. Longitudinal Maneuver Planning . . . . . . . . . . . . . . . . . . . . . . . 35
4.3. Lateral Maneuver Planning . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.4. Maneuver Trajectory Generation . . . . . . . . . . . . . . . . . . . . . . . 40
5. Modeling and Objectives for Trajectory Planning 42
5.1. Modeling of Vehicle Motion . . . . . . . . . . . . . . . . . . . . . . . . . . 42
5.1.1. System Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
5.1.2. Control Input Calculation . . . . . . . . . . . . . . . . . . . . . . . 47
5.2. Modeling of the Environment . . . . . . . . . . . . . . . . . . . . . . . . . 48
5.3. Trajectory Planning Objectives . . . . . . . . . . . . . . . . . . . . . . . . 52
6. Spline-based Trajectory Representation 54
6.1. Variational Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
6.2. Distinct Analytic Spline Interpolation . . . . . . . . . . . . . . . . . . . . 57
6.3. Minimum Kinematics Trajectory Generation . . . . . . . . . . . . . . . . 58
6.3.1. Formulation of the Optimal Interpolation Problem . . . . . . . . 58
6.3.2. Solution of the Optimal Interpolation Problem . . . . . . . . . . . 60
i
Contents
7. Trajectory Planning for Automated Driving 64
7.1. Bilevel Programming . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
7.2. Formulation of the Trajectory Optimization Problem . . . . . . . . . . . 65
7.3. Discrete Trajectory Optimization . . . . . . . . . . . . . . . . . . . . . . . 71
7.3.1. Previous Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
7.3.2. Sampling Strategy and Trajectory Evaluation . . . . . . . . . . . . 72
7.4. Continuous Trajectory Optimization . . . . . . . . . . . . . . . . . . . . . 75
7.4.1. Previous Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
7.4.2. Nonlinear Constrained Optimization . . . . . . . . . . . . . . . . 77
8. Evaluation in a Dynamic Environment 80
8.1. Setup of the Trajectory Planning Approaches . . . . . . . . . . . . . . . . 80
8.2. Analysis and Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
8.3. Discussion of the Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
9. Conclusion and Outlook 97
9.1. Summary of the Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
9.2. Outlook . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
Bibliography 100
A. Appendix 114
A.1. Terminology and Definitions . . . . . . . . . . . . . . . . . . . . . . . . . 114
A.2. Components of the Functional Architecture . . . . . . . . . . . . . . . . . 115
A.3. Supplements to Environment-aware Maneuver Planning . . . . . . . . . 119
A.4. Linear Single Track Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
A.5. Kinematic Vehicle Model with Ackermann Steering . . . . . . . . . . . . 125
A.6. Calculation of the Minimum Curve Radius according to RAS-L . . . . . 125
A.7. Inverse Flat Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
A.8. Adaptive Sampling Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . 127
A.9. Sequential Quadratic Programming Algorithm . . . . . . . . . . . . . . . 128
A.10.Convergence Test and Runtime Assessment for Continuous Approach . 129
A.11.Further Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131
A.12.Settings and Configuration . . . . . . . . . . . . . . . . . . . . . . . . . . 145
ii
Nomenclature
The following list explains abbreviations and symbols used throughout this work. In
general all mathematical symbols are introduced in their respective context. On that
note the definition of symbols conforms to the following pattern:
scalars normal font letters a, b, . . .
vectors bold lower case letters a, b, . . .
matrices bold upper case letters A, B, . . .
Sets are denoted by uppercase blackboard bold letters (A, B, . . . ) and spaces are de-
noted by uppercase Zapf Chancery letters (A , B, . . . ). Coordinate frames are given by
a leading superscript to the symbol and if not stated otherwise running indecies are
uniformly incremented by one. Newton’s notation, also referred to as dot notation ̇
is used to denote the time derivative of a function. Throughout this work vehicles are
generally illustrated as follows:
ego vehicle, obstacle vehicle.
When referring to an obstacle it is denoted by its ID:$ to label them without ambiguity.
Abbreviations and Acronyms
ABS Antilock Braking System
ACC Adaptive Cruise Control
AD* Anytime Dynamic A*
ADMM Alternating Direction Method of Multipliers
ARA* Anytime Repairing A*
BFGS Broyden–Fletcher–Goldfarb–Shanno
CL-RRT Closed-loop Rapidly-exploring Random Tree
CoG Center of Gravity
DAG Directed Acyclic Graph
DARPA Defense Advanced Research Projects Agency
DSC Dynamics Stability Control
ESP Electronic Stability Program
MATLAB MATrix LABoratory
MPC Model Predictive Control
OSQP Operator Splitting Quadratic Program
PID Proportional-integral-derivative
PROMETHEUS PROgraMme for a European Traffic of Highest Efficiency and
Unprecedented Safety
QP Quadratic Programming
RRT Rapidly-exploring Random Tree
iii
Nomenclature
SQP Sequential Quadratic Programming
cmp. compare
e.g. exempli gratia, for example
etc. et cetera, and so forth
i.e. id est, that is
Coordinate Systems
C Curvilinear Coordinate System
E World Coordinate System
F Vehicle Coordinate System
N Natural Coordinate System
R Tire Coordinate System
Running Indices
` Lane marker index ` = 0, 1, 2, 3
ε Minimum kinematics derivative index
ı General running index
i (High level) equality constraints index i = 1, 2, . . . , Λ
(Low level) equality constraints index i = 1, 2, . . . , Ω
ι Coefficient index ι = 0, 1, . . . ,V
 General running index
j (High level) inequality constraints index j = 1, 2, . . . , Γ
(Low level) inequality constraints index j = 1, 2, . . . , Θ
k Ego trajectory index k = 0, 1, . . . , K
κ Spline segment index κ = 0, 1, . . . ,K
κ Collision circles ego index κ = 1, 2, . . . , η̊
ł SQP subproblem constraints index ł = 1, 2, . . . , Ξ
ν Collision circles obstacle index ν = 1, 2, . . . , η̊
v Ego bounding box cornerpoint index v = 1, 2, 3, 4
$ Obstacle index $ = 1, 2, . . . ,z
Symbols and Functions
a Acceleration
amax Maximum vehicle acceleration
ag Acceleration of gravity
acomf Comfort acceleration
ăx, ăy Comfort longitudinal and lateral acceleration limits
A˜ Minimum kinematics augmented constraint matrixA Minimum kinematics constraint matrix
As Constraint matrix with specified constraint values
Au Constraint matrix with unspecified constraint values
Acon` , Acona Continuity constraint matrix at the start of a spline segement
Aval` , Avala Value constraint matrix at the start of a spline segment
 SQP jacobian of constraints
â, â SQP subproblem constraints
āx, āy Comfort longitudinal and lateral acceleration costs
iv
Nomenclature
Aĝ, Aĥ QP linear equality and inequality constraints matrix
acon, acon` a Mapping of spline coefficients to build a continuity constraint
aval, aval` a Mapping of spline coefficients to build a value constraint
α̂ SQP line search step size parameter
α Tire slip angle
b̊ Collision circles distance
B̂ Damped BFGS hessian
b˜ Minimum kinematics augmented specified derivativesb Minimum kinematics specified derivatives
bs Constraint value vector with specified constraint values
bu Constraint value vector with unspecified constraint values
ba, ba Value constraints at the end of a spline segment
b`, b` Value constraints at the start of a spline segment
bĝ, bĥ QP linear equality and inequality constraints vector
ba` Continuity constraint value
β Side slip angle
C Configuration space
cv, ch Cornering stiffness of the front and rear tire
c Spline coefficient
dx, dy Longitudinal and lateral distance
D̂ SQP merit function directional derivative
Cd Cl, dt Actual lead and tail vehicle distance
d̄l, d̄t Lead and tail vehicle distance costs
dmin Minimum distance to lead and tail vehicle
Cd̆l, Cd̆t Reference lead and tail vehicle distance
δr Steering angle
δA Ackermann steering angle
δmax Maximum steering angle
e Cumulative product
η̊ Collision circles number of circles
η̂ SQP line search acceptance parameter
ð Adaptive discretization variable
F Force
Fa,x, Fa,y Aerodynamic drag forces
F Functional
F̂ (High level) objective function
f̂ (Low level) objective function
Fz Normal force
Fy,max Maximum lateral force
F̂c High level objective for comfort driving
F̂d High level objective for distance keeping
F̂p High level objective for reference lateral position
F̂v High level objective for reference velocity
Ĝ (High level) equality constraints
ĝ (Low level) equality constraints
v
Nomenclature
γ̂ Lagrange multipliers for equality constraints
Ĥ (High level) inequality constraints
h̄ Number of nodes on a level of respective sampling structure
ĥ (Low level) inequality constraints
aĤ4, δĤ4, gĤ4 High level inequality constraint for system dynamics
xĤ1, yĤ1 High level inequality constraint for time
$Ĥ2 High level inequality constraint for collision avoidance
lĤ3, rĤ3 High level inequality constraint for road boundaries
ı̂ SQP iteration
Jz Moment of inertia
l Vehicle length
lego Ego vehicle length
la Inter axle distance
lv, lh Distances from the center of gravity to the front and rear axle
lc Distance between center of gravity and center of pressure
L Lagrangian
λ Course angle
λ̂ Lagrange multipliers
m Vehicle mass
f,f1,f2 Adaptive discretization functions
µ̂ SQP merit function parameter
µr Maximum radial adhesion coefficient
µt Maximum tangential adhesion coefficient
nB̂ Dimension of damped BFGS Hessian
npx, npy Number of samples for discrete optimization of the position
ntx, nty Number of samples for discrete optimization of time
np̂ QP decision variable dimension
o Configuration
ωd, ωv, ωp, ωc High level objective weights
ωr Minimum kinematics weight
pcc Center of curvature
pcr Center of rotation
p̊ Ego and obstacle collision circles center
p̌ Ego bounding box corner point
ps Stop position
p̃y Lateral reference position
P Polynomial
p Position
px, py Longitudinal and lateral position
Pego Ego trajectory
p̂ QP decision variables
Φ Flat transform
φ̂ SQP merit function
Ψ Inverse flat transform
ψ Yaw angle
vi
Nomenclature
Q QP cost matrix
q QP cost vector
ru Control input derivative
r̂ Damped BFGS intermediate result
rz Flat output derivative
R Set of real numbers
Rss, Rsu, Rus, Ruu Matrices for unconstrained solution to the optimal interpolation
rc Minimum kinematics demanded continuity derivative order
rf Minimum kinematics demanded cost derivative order
rV Distinct spline interpolation derivative
r Derivative order
ρ̊ Collision circles radius of obstacle vehicle
ρ̊ego Collision circles radius of ego vehicle
ρcc Radius of curvature
ρmin Minimum curve radius
ρcr Radius of rotation
ŝ Damped BFGS intermediate result
S Low level solution set
S Spline
ς Exploitation of the radial adhesion coefficient
σ̂ Lagrange multipliers for inequality constraints
Tm Minimum knot time difference
Tl Constant time gap for lead vehicle
Tt Constant time gap for tail vehicle
Tp Ego planning horizon
Tr Target lane reaching time
Ts Ego trajectory sample time
t Time
tsim Simulation time
τ̂ SQP line search step size parameter factor
θ̂ Damped BFGS parameter
U Input space
u Control input trajectory
vch Characteristic velocity
V Interpolation order
ṽ Reference velocity
v Velocity
w Vehicle width
wego Ego vehicle width
X State space
Xfree Free region
Xgoal Goal region
Xdyn Dynamic environment constraints region
Xenv Environment constraints region
Xstat Static environment constraints region
vii
Nomenclature
x, x` State trajectory, start state
ξ Cross slope
ŷ Damped BFGS intermediate result
Y Output space
y System output trajectory
z Flat output
Z Flat output space
ẑh High level decision variables
ẑl Low level decision variables
z Spline breakpoint, also referred to as spline knot
zx, zy Flat output for longitudinal and lateral motion of the vehicle
ẑ SQP decision variables
viii
1
Introduction to Trajectory Planning in the
Context of Automated Driving
In recent years a lot of effort has been investigated towards the idea of self-driving
vehicles. From the viewpoint of many manufacturers the development of automated
driving functions will have disruptive consequences for society. The concept of mo-
bility for individuals and also in the context of logistics will change fundamentally.
The targeted advantages comprise the aspects of improved safety in traffic, expanded
access to affordable and efficient mobility as well as the ecological aspect by offering
sustainable transportation solutions.
The development of advanced driver assistance functions in the past years highly
contributed to improvements in road safety. The continuing development towards
automated vehicles is not only leading to the improvement of active safety functions,
but also to an advance in driving comfort, which relates to the effort of relieving the
driver from the complex and exhausting task of driving, while enabling entertainment
or an increase of productivity for the passengers.
The consequences of climate change highlight the necessity of considering ecological
aspects in future mobility concepts. In the automotive industry this leads to the goal
of fuel consumption reduction coming along with reduced pollution. For sure this
mainly addresses alternative drivetrains, but also covers predictive driving strategies
that can rigorously be achieved by means of automated vehicles.
With respect to the demographic change accompanied with an increasing average age
of the population, automated driving has the potential to offer unconditional access to
mobility for the elderly. The same holds true for physically handicapped people, for
whom automated driving functions promise a more inclusive society, and for the rural
population that can seamlessly be connected to the urban infrastructure.
The increasing traffic density in the metropolises of this world represents a major
challenge for future mobility concepts. Automated driving functions could remedy
traffic jams by optimizing the traffic flow and by efficient exploitation of the available
traffic area.
Furthermore, the demand on individual and flexible mobility solutions gives raise to
agile transportation services, which should help to provide flexible, fast and efficient
mobility. Shared mobility concepts intend to decrease the number of vehicles in the
urban environment to cover the problems of limited traffic area and parking space.
1
Chapter 1. Introduction to Trajectory Planning in the Context of Automated Driving
The development of automated driving functions will highly impact the progress and
shape of services in this area.
The application of automated driving functions also allows to economize logistics in a
globalized economy. It is expected that the transition of automated driving functions
from research to series production will be in the area of commercial vehicles. This
is because the transportation of goods over a long distance offers attractive terms.
Furthermore, it provides high economic opportunities for logistics companies.
The market players cannot only be found in the same line of business. Tech companies
and mobility service providers are expecting growth opportunities, since it is hardly
surprising that in the presence of automated driving functions the importance of
valuable software development increases rapidly.
With all these aspects in mind innovative concepts are required to efficiently shape the
future mobility. The technological ingenuity and creativity of engineers is not limited
with respect to highly automated driving and the vision of a world with autonomous
vehicles can be advanced immeasurably. Still, today’s vehicles are not entirely capable
to drive autonomously. The levels of driving automation are defined by SAE (2018),
reaching from SAE Level 0 (no automation) to SAE Level 5 (full vehicle autonomy).
The levels apply to the driving automation feature(s) that are engaged during on-road
driving of an automated vehicle.
1.1. Motivation
On the way to automated driving the vehicles are equipped with an increasing number
of driver assistance systems with the ability to accurately sense their environment.
The beginning of safety-oriented driver assistance systems is marked by the antilock
braking system (ABS) as well as the yaw dynamics stability control (DSC) that is
also referred to as electronic stability program (ESP). Statistics attest the decrease of
the number of accidents since safety-oriented driver assistance systems have been
launched, but also reveal the necessity of an active driver support, as 90 % of the
accidents can be traced back to human errors. To reach the goal of reducing the
accidents caused by human misbehavior, assistance systems could take over vehicle
control.
The technical challenges regarding the application of automated driving functions are
manifold. One challenge is that the approaches have to cope with highly complex
urban environments and especially have to take care of collision avoidance. Hence,
constraints arise from other traffic participants, but as well from vehicle dynamics.
Another problem is the perception, which goes along with uncertainty, effects like
occlusion of traffic participants and possibly inaccurate detection of objects. For the
application in dynamic traffic it is furthermore fundamental to achieve real time op-
eration of the developed algorithms, since the automated vehicle should account for
sudden changes in the situational awareness. A vital capability of an automated ve-
hicle is the planning of an appropriate motion with respect to various time frames.
Hence, the planning algorithm should find a collision-free and comfortable trajectory
on short term, but also contribute to mid and long term requirements given by high
2
1.2. Motion Planning as Constrained Optimization
level modules like mission planning and decision making. For reliable and safe driv-
ing in the dynamic environment, trajectory planning is fundamentally important, as
new types of trajectory planning and control approaches are exactly addressing the
aforementioned challenges. With the aim to achieve a further reduction in road-traffic
accidents despite continued growth in traffic volumes, the vehicle has to be capable of
detecting imminent collisions and to perform an adequate reaction. The vehicle envi-
ronment is dynamic and highly uncertain, which necessitates the online generation of
the trajectory.
1.2. Motion Planning as Constrained Optimization
A core problem of automated driving is the task of motion planning. LaValle (2006)
considers motion planning under differential constraints as a variant of the classical
two-point boundary value problem. The difficulty for motion planning lies in the fact
that obstacle avoidance is performed additionally to the task of finding a solution
through a state space that connects initial and goal states while satisfying differential
constraints at the same time. The system dynamics are commonly described by:
ẋ(t) = f (x(t), u(t)) , x(0) = x` , (1.2.1)
y(t) = h(x(t), u(t)) , (1.2.2)
with state x(t) ∈ X , output y(t) ∈ Y and control input u(t) ∈ U, for all t ≥ 0, as well
as start state x` ∈ X . The general motion planning problem for dynamical systems is
composed of:
1. A state transition equation ẋ(t) = f (x(t), u(t)) (cmp. equation 1.2.1), defined for
every x(t) ∈ X and u(t) ∈ U that accounts for system dynamics, which could
arise from any differential model.
2. A set Xenv of constraints on the states arising from the environment, accounting
for dynamic Xdyn and static restrictions Xstat such as lane boundaries and obstacle
avoidance given by Xenv = Xstat ∪ Xdyn. The free space then is Xfree = X \ Xenv.
3. An initial state x` ∈ Xfree.
4. A set Xgoal ⊆ Xfree, which represents the desired goal region for the vehicle.
Then a complete algorithm must find a trajectory x(t), t ∈ [t0, Tp] and associated
control inputs u(t), t ∈ [t0, Tp] such that the motion satisfies x(0) = x` and x(Tp) ∈
Xgoal, x(t) ∈ Xfree and u(t) ∈ U for all t ∈ [0, Tp]. In other words the trajectory planning
approach should find a solution that moves the vehicle from start to a desired goal
without colliding with other obstacles.
To solve the motion planning problem the planning approach has to conduct a search
in the state and/or control input space. In general there exists more than one solution
to the motion planning problem. Then, usually additional objectives to be optimized
are specified to rank found solutions among each other. These objectives account e.g.
for minimum fuel consumption, maximum comfort or minimum travel time. This way
3
Chapter 1. Introduction to Trajectory Planning in the Context of Automated Driving
relevant information about the surrounding environment, vehicle dynamics as well
as the desired driving behavior can seamlessly be merged in a framework that builds
upon performance criteria.
The process of generating a trajectory in a way that it minimizes some measure of
performance, while simultaneously satisfying a set of constraints is referred to as
optimal control. Typically, methods in this field deal with generating the best trajectory
by finding the control inputs of the system as functions of time.
In general an optimal control problem is concerned with minimizing a functional
F (x(t), u(t)) and a function u(t) as decision variable:
min F (x(t), u(t)) (1.2.3a)
u(t)
s.t. ẋ(t) = f (x(t), u(t)) ∀t ∈ [0, Tp], (1.2.3b)
u(t) ∈ U ∀t ∈ [0, Tp], (1.2.3c)
x(t) ∈ Xfree ∀t ∈ [0, Tp], (1.2.3d)
x(0) = x`, (1.2.3e)
x(Tp) ∈ Xgoal . (1.2.3f)
Obviously, the constraints 1.2.3b-1.2.3f of the optimal control problem align with the
definitions of the motion planning problem stated previously.
For differentially flat systems there exists a unique relation between trajectories in the
output space Y and the state space X and control input space U. In this context the
flat output z(t) ∈ Z is introduced, as the output y(t) and the flat output z(t) are not
necessarily the same. For more information regarding differential flatness it is referred
to section 5.1.2. The advantage of differential flatness in the context of trajectory
planning is that the optimization problem can directly be defined with respect to the
flat output and control inputs can analytically be derived as a function of the output
trajectory and its derivatives. Furthermore, the representation in the flat output space
might simplify the specification and optimization of the trajectory in comparison to
a search over the control input space. The goal of motion planning in the flat output
space is to find a flat output trajectory z(t) ∈ Z, such that the corresponding unique
state trajectory x(t) ∈ X and control inputs u(t) ∈ U obey the differential constraints
of the vehicle for all t ∈ [0, Tp], consider constraints imposed by the environment
x(t) ∈ Xfree for all t ∈ [0, Tp] and reach the goal region x(Tp) ∈ Xgoal. Hence for
nonlinear systems that are differentially flat the optimal control problem (1.2.3) can be
mapped to a problem with the flat output z(t) as decision variable.
One of the major challenges with regard to the solution of the planning problem is
the handling of the complex restrictions arising from the environment and the vehicle
dynamics. According to Werling et al. (2010) this prohibits to limit the optimal solution
of the planning problem to a specific function class.
In this context the optimal control problem can be solved by writing it as a nonlinear
program1. This includes the transition from a functional to a function optimization
1A nonlinear program is a constrained parameter optimization problem where either the objective
function or the constraints are nonlinear. For further reading it is referred to e.g. Betts (2010) or Kelly
(2017).
4
1.3. Coordinate Systems
problem, in which the decision variables are considered to be parameters instead
of functions. That way trajectory optimization deals with finding a local solution (a
sequence of control inputs or e.g. flat output variables) to the optimal control problem
in order to obtain an optimal trajectory2.
A general solution to the constrained trajectory optimization problem can ideally
be found by directly accounting for the constraints, whereas another possible way
is to apply an optimization heuristic. In the latter case a search within the set of
optimal solutions to the unconstrained problem is performed and the best solution,
which fulfills the restrictions is chosen in order to derive a closed-form solution that
approximates the constrained problem reasonably well (Werling et al. 2010).
This thesis investigates on trajectory planning approaches that solve the trajectory
optimization problem in the context of automated driving. Instead of solving one
comprehensive trajectory optimization problem, objectives are prioritized and grouped
into two hierarchical levels. However, due to the exploitation of particular properties
it is still possible to reformulate the planning problem, such that the final solution is
once more obtained by solving a nonlinear program. This certainly leads to a trajectory
planning approach that is in general based on constrained optimization. Solutions can
be found within the designated classes of trajectory planning approaches. The class of
sampling-based planning approaches is in this thesis referred to as discrete trajectory
optimization, which relates to the process of constrained numerical optimization over
a discretized parameter space. Optimization-based planning approaches on the other
hand are also referred to as continuous trajectory optimization to account for the
continuous character of the decision variables. Chapter 7 elaborates on a discrete
and continuous trajectory optimization approach to automated driving in dynamic
environments and discusses the characteristics and challenges with respect to the
respective approach in detail.
1.3. Coordinate Systems
Throughout this thesis several coordinate systems are used, which are defined within
this section. Figure 1.1 illustrates the world coordinate system, the vehicle coordinate
system as well as the curvilinear coordinate system, which represent the most relevant
frames within the scope of this thesis.
World Coordinate System
The world coordinate system is a global coordinate system with a fixed origin and
is denoted by a leading superscript E. The world frame is a right-handed coordinate
system. Whereas px and py describe the two-dimensional plane, elevation is considered
to play a neglectable role in the context of trajectory planning for automated driving
and will thus be omitted within this thesis.
2In contrast to an optimal policy (or global solution to the optimal control problem), which provides
the optimal control for every point in the state space.
5
Chapter 1. Introduction to Trajectory Planning in the Context of Automated Driving
F py
F px
Fp = [F p F T$ $ x $ py]
ID:$
F
$ λ
C py E py
C p Ex px
Figure 1.1.: Major coordinate frames applied throughout this thesis. The world coordinate
system is a cartesian frame with fixed origin, whereas the curvilinear coordinate frame cor-
responds to a Frenet-Serret coordinate system that enables a description with respect to the
course of the road. Especially sensor measurements, such as for example position and course
angle relative to the ego vehicle, are naturally given in vehicle coordinates.
Vehicle Coordinate System
The ego coordinate system is aligned with the longitudinal axis of the vehicle pointing
forward. The py-axis is orthogonal to the px-axis and points to the left. Popular choices
for the origin position are the center of the front bumper or the center of gravity of
the vehicle. Most of the data that is processed within the automated vehicle frame-
work is naturally defined in this ego-centered coordinate system. This particularly
encompasses sensor measurements like the position F$ p and course angle F$ λ of ob-
stacle vehicle ID:$ (see Figure 1.1), as well as prediction and planning algorithms. In
virtue of the applied vehicle dynamics model and to the benefit of a more precise
approximation of the vehicle shape in terms of collision avoidance, in this thesis the
center of gravity is chosen as the origin of the vehicle frame. The vehicle coordinate
system is indexed by a leading superscript F.
Curvilinear Coordinate System
The curvilinear coordinate system is defined along an arbitrary reference curve and
does hence not belong to the class of cartesian coordinate systems like the world and
vehicle coordinate system. From mathematical point of view the curvilinear frame
corresponds to the Frenet-Serret coordinate system. The description in curvilinear
frame enables a very intuitive way of coping with various, complex traffic scenarios.
Finally, in on-road driving traffic scenarios the lateral and longitudinal driving tasks
are performed with respect to the lane markers. This accounts for following the course
of the road as well as for distance keeping functionality concerning other objects.
A curvilinear coordinate system with a respective lane marker as reference naturally
handles curved road topologies. All quantities in curvilinear coordinates are described
with respect to the reference curve by a longitudinal coordinate along the reference
6
1.4. Scope and Organization
curve (i.e. the arc length) and a coordinate which is orthogonal to this. To get from
curvilinear to cartesian frame and vice versa a nonlinear transform has to be applied,
which has to find the minimum distance between the query point and the reference
curve. This certainly represents the major drawback of this coordinate system as sensor
data is given in vehicle coordinates and perception results have to be transformed at
least at one stage in the functional architecture of the vehicle to make them accessible
in curvilinear coordinates. As it is aligned with the road course all techniques can
be applied likewise for a curved and straight road geometry. Within this thesis all
curvilinear coordinates are denoted with a leading superscript C.
Natural Coordinate System
The natural coordinate system basically corresponds to the vehicle coordinate frame
that is shifted along the planned trajectory. It hence accounts for the future motion of
the ego vehicle and can be used to express kinematic quantities for planned states from
vehicle perspective. The transform thus corresponds to a two-dimensional rotation by
the heading angle at the respective planned trajectory position. Within this thesis
natural coordinates are denoted with a leading superscript N.
Tire Coordinate System
For the sake of completeness the tire coordinate system is mentioned here as well. This
coordinate system is in particular used to define important quantities with respect to
the lateral vehicle dynamics (e.g. tire forces) in section 5.1. The tire coordinate system
is denoted by a leading superscript R.
1.4. Scope and Organization
In this thesis a suitable concept for trajectory planning for on-road driving in structured
environments should be developed. Derived approaches should be capable to master
dynamic traffic and should seamlessly integrate into the functional architecture of
automated vehicles. The scope of this work should cover a set of motion primitives
which represent the basic functionality that should be provided with respect to on-road
driving. Namely this is lane keeping (and likewise adaptive cruise control (ACC)
capability), lane changes and a target brake maneuver.
The developed approaches include aspects of several state-of-the-art trajectory plan-
ners, which hence build the fundamentals of the work presented in this thesis. The
idea of Werling et al. (2010) for trajectory planning is extended from polynomials to
polynomial splines via an optimal interpolation strategy developed by Bry et al. (2015).
This improves the flexibility of the trajectory and enables the presented two level hier-
archical trajectory optimization approach. The longitudinal and lateral trajectory are
coupled via the defined objectives and constraints, intended to generate a human-like
high level driving behavior and to produce dynamically feasible results for different
kinds of maneuvers. The distinction between constraints and objectives enables to
distinguish between basic driving goals that have to be satisfied (constraints) and
7
Chapter 1. Introduction to Trajectory Planning in the Context of Automated Driving
a desired non-mandatory behavior like reaching set speed, or keeping an extended
safety distance (objectives). To include vehicle dynamics in the trajectory planning
approach this thesis advances the concept introduced by Schlechtriemen et al. (2016).
Instead of using Ackermann steering, the vehicle model utilized to derive the control
inputs via flat transform is enhanced to provide a higher range of validity. The usage
of a curvilinear frame is applied gingerly at different stages and beneficially supports
the precise formulation of objectives and constraints. Still, trajectories are evaluated
in cartesian frame. This guarantees drivability and jerk optimality also in such cases
when the reference for curvilinear transform contains discontinuities.
This thesis is dedicated to develop a generic trajectory planning approach to automated
driving and addresses the problem of conflicting planning objectives3 by proposing an
hierarchical two level trajectory planning approach. The approach separates basic low
level optimality and desired high level behavior and inherently accounts for the fact
that in automated driving the desired high level behavior and hence the associated
low level behavior change with respect to the current situation. The procedure can
be compared to an optimization heuristic of determining the optimal solution of an
unconstrained problem first and to choose the best high level solution that fulfills
the constraints in a second step. The applied interpolation strategy improves on the
efficiency and covers the optimal low level behavior in terms of minimum kinematics
for comfort related aspects. By means of an objective function a suitable trajectory
is chosen in dependence on the desired maneuver as well as on the current traffic
situation. This enables the application in various, different traffic scenarios. This thesis
makes the following core contributions:
• Development of a hierarchical trajectory optimization framework for trajectory
planning in the context of automated driving.
• Time-parameterized spline-based trajectory representation with optimal interpo-
lation strategy suitable for dynamic traffic.
• Seamless integration of ego vehicle dynamics via the property of differential
flatness.
This work is intended to provide insight in the complex world of trajectory planning for
automated vehicles. The development and analysis of trajectory planning approaches
suitable for dynamic environments is reflected by the following outline:
Chapter 2 This chapter gives a comprehensive overview on planning approaches to
automated driving. For reasons of clarity and comprehensibility it is necessary
to group the vast amount of approaches in terms of common features. In the
first section a taxonomy is introduced in order to classify planning approaches
with respect to their characteristics. The second section then reviews relevant
approaches in each respective class, focusing on but not limited to trajectory
planning approaches to on-road automated driving.
3A typical example is the minimization of lateral acceleration for comfort purpose. This objective is in
conflict with each maneuver that demands lateral acceleration such as e.g. lane changes.
8
1.4. Scope and Organization
Chapter 3 Automated driving is a highly complex task. The functional architecture
of automated vehicles hence decomposes the task into several sub problems
and defines the interrelation between modules as well as their collaboration. To
derive a concept for trajectory planning the incorporation into the functional
architecture has to be considered, providing the interfaces and requirements
for the developed trajectory planning approach. For this reason, this chapter
introduces a functional architecture and therein defines the specific task for the
trajectory planner. A holistic concept is derived, leading to the proposed two
level hierarchical trajectory planning framework.
Chapter 4 For a generic design of the trajectory planner it is vital to process each kind
of maneuver decision. As a trajectory planning approach clearly benefits from a
rough future motion plan that results from the maneuver decision, it is assumed
that if not already considered within the decision making process, a maneuver
planning approach is obligatory. Hence, for seamless cooperation the maneuver
decision is compiled to a maneuver trajectory and target region. In this chapter
a maneuver planning approach is presented that generates maneuver related
features, which serve as an input to the trajectory planner.
Chapter 5 For the purpose of trajectory planning in the field of automated driving par-
ticular attention has to be directed to the topic of vehicle dynamics modeling as
well as to the modeling of the environment. Furthermore, the goals of automated
driving have to be mapped to respective objectives in the trajectory planning ap-
proach. This chapter deals with the formulation of trajectory planning objectives,
introduces approaches to static and dynamic environment modeling and gives a
detailed description of the applied vehicle model.
Chapter 6 This chapter proposes a spline-based trajectory representation for trajectory
planning to improve on the overall performance of the developed trajectory
planner. An optimal solution for an unconstrained planning problem is derived
inferring a beneficial interpolation strategy. This approach supports the idea of
the two level hierarchical trajectory planning framework and hence serves as the
basis for the developed trajectory planners.
Chapter 7 This chapter presents the mathematical background for the proposed two
level hierarchical trajectory optimization framework. The trajectory optimiza-
tion problem is formulated and a discrete as well as a continuous trajectory
optimization approach are introduced.
Chapter 8 In this chapter a thorough analysis with respect to the advantages and dis-
advantages of the developed approaches is given. The characteristics of discrete
and continuous trajectory planning approaches are worked out and discussed
in detail. The evaluation should furthermore give an impression of the general
applicability for automated driving in dynamic environments.
Chapter 9 This chapter briefly concludes the thesis including a discussion about the
accomplished results and potential directions for future research.
9
2
Survey on Trajectory Planning
Approaches to Automated Driving
Approaches to trajectory planning arise from many different fields. But even though
each field originally considered different problems the approaches have been broad-
ened in scope to consolidate in the topic of motion planning. Recent developments in
robotics and control theory contribute to more advanced solutions with respect to the
motion planning problem, such that there exists a vast amount of diverse trajectory
planning approaches. The reason for this is that algorithms in this field always come
along with weaknesses that lack general suitability for each planning problem since
requirements largely differ with respect to the application.
Mobile robotics investigated the motion planning problem striving for advances in
service robotics and unmanned vehicles to perform e.g. difficult or even dangerous
tasks in areas which might not be accessible to humans. Hence, a transfer of technology
from mobile robotics to the field of automotive can be identified.
However, with respect to automated driving a lot of innovative research has been done
in the last decades. The team of Ernst Dickmanns at Bundeswehr University Munich
started their pioneering work in the 1980’s, showing the capability of on-road driving
on highways. The concept was basically relying on visual information and interpre-
tation (Dickmanns and Zapp 1987). This work was followed by the PROgraMme for
a European Traffic of Highest Efficiency and Unprecedented Safety (PROMETHEUS),
which intended to enhance efficiency, sustainability and safety in road traffic and sig-
nificantly contributed to the development of fundamentals for automated driving. In
2007 the Defense Advanced Research Projects Agency (DARPA) Urban Challenge at-
tracted attention, in which each team was assigned individual missions. The research
effort of all participants generated many innovations in the field of automated driving.
An excellent overview about the achievements from technical perspective is given in
Buehler et al. (2009) and Campbell et al. (2010).
In general typical fields of application concern driving in highway and urban traffic
scenarios or parking. It is obvious that parking more or less concerns static scenarios
with lower run time restrictions (algorithms are allowed to take more time to find
a solution, as this will certainly not end up in a critical situation), whereas driving
in traffic has to deal with dynamic objects, which complicates the planning problem
with respect to collision avoidance and run time. The summary of related work in
10
2.1. Taxonomy of Trajectory Planning Approaches
this section should provide an overview of trajectory planning approaches focusing
on on-road driving in dynamic environments, complemented by some considerable
methods for other specific applications. In this regard the term on-road1 refers to nav-
igating in structured (driving applications) and semi-structured (parking applications)
environments.
2.1. Taxonomy of Trajectory Planning Approaches
The taxonomy of trajectory planning approaches is not standardized and many con-
tributions in the past have been designed for path instead of trajectory planning.
Nevertheless, without regard to each individual case methods of path planning are
included in the overview as the focus should be on the basic working principle and
it is expected that path planning approaches can in general be adjusted to work for
trajectory planning as well.
Katrakazas et al. (2015) tailor their survey about real-time motion planning methods
to the functional architecture of an automated vehicle, suggesting three levels of plan-
ning. The proposed classification tree separates planning approaches with respect to
their functionality in the hierarchical functional architecture and does not group the
approaches by means of their key properties. Gonzalez et al. (2016) and Nilsson (2016)
divide planning approaches into graph search, sampling based planners, interpolating
curve planners and numerical optimization approaches, whereas Paden et al. (2016)
for example categorizes into geometric methods, variational methods, graph search
and incremental search. Beside different naming at least an evident overlap in the
classification of planning approaches can be noted. In this thesis the terminology is
chosen to reflect and emphasize the core differences between the approaches.
Figure 2.1 shows the consolidated taxonomy that should highlight the differences
between the planning approaches with respect to their distinctive characteristics. Gen-
erally, trajectory planning approaches to automated driving can be divided into two
basic categories: optimization and sampling based approaches. This distinction is em-
phasized by the fact that all sampling based methods can be represented as a graph
generated from the discretized state space, whereas optimization based algorithms
focus on (quasi-) continuous solutions. Optimization based trajectory planning mainly
deals with approaches performing numerical optimization. Sampling based methods
can be subdivided into trajectory rollout, graph search and incremental search. The
class of trajectory rollout is introduced as a special case of graph search and accounts
for the fact that trajectory samples are compared directly to each other, which renders
graph search unnecessary. Hence, trajectory rollout is listed separately as graph search
would be a misleading description for this concept.
Numerical optimization (also referred to as variational methods) is concerned with
formulating the motion planning problem as an optimal control problem. Solutions
in this context are given by optimal control theory. Numerical methods for optimal
control are dynamic programming, indirect methods and direct methods (Diehl et al.
2005). Dynamic programming intends to find an optimal policy, which provides the
1In contrast to off-road, which applies to navigation in unstructured environments.
11
Chapter 2. Survey on Trajectory Planning Approaches to Automated Driving
Optimization-based Sampling-based
Trajectory Planning Trajectory Planning
Numerical Optimization Incremental Search Trajectory Rollout Graph Search
2 3
1
4 5
Curve Interpolation
representative contributions for respective field:
1 Werling and Liccardo (2012), Ziegler et al. (2014)
2 Montemerlo et al. (2008), Werling et al. (2012), Schwesinger et al. (2013)
3 Ziegler and Stiller (2009), McNaughton et al. (2011)
4 LaValle (1998), Karaman and Frazzoli (2011)
5 Kavraki et al. (1996), Karaman and Frazzoli (2011)
Figure 2.1.: Taxonomy of trajectory planning approaches to automated driving.
optimal control for every point in the state space. Dynamic programming is closely
connected to Bellman’s Principle of Optimality (Bellman 1957), which states that an opti-
mal solution is composed of optimal partial solutions. A major drawback of dynamic
programming is the computational burden, which rapidly increases with the order of
the system, also referred to as Bellman’s Curse of Dimensionality. Indirect methods use
the necessary conditions of optimality of the infinite problem to derive a boundary
value problem, which then has to be solved numerically. The class of indirect methods
encompasses the calculus of variations as well as the Pontryagin Maximum Princi-
ple. Direct methods transform the optimal control problem into a finite dimensional
nonlinear programming problem, which can then be solved by methods of nonlinear
optimization. The resulting parameter optimization problem can be solved by conve-
nient numerical solvers. The transformation from the original optimal control problem
into a nonlinear program is also denoted as transcription. For the discretization of the
optimal control problem with direct methods three methods are distinguished: direct
single shooting, direct multiple shooting and direct collocation. Direct methods are
frequently used for Model Predictive Control (MPC) that cyclically solve the optimal
control problem. Starting with the current state observation as initial value the first
part of the calculated optimal plan is applied to the real world system. As this proce-
dure is repeated, feedback is generated that accounts for disturbances as well as for the
12
Randomized Systematic
Sampling Sampling
2.1. Taxonomy of Trajectory Planning Approaches
actual model-plant-mismatch. Further advantages are that MPC algorithms consider
the system dynamics and constraints directly. As disadvantages the computational
burden and non trivial stability proof can be listed.
Sampling-based approaches often make use of simplifications of the motion planning
problem to meet requirements like for example the limited computation time as well
as to facilitate the handling with constraints arising from the environment. The overall
goal is to approximate a continuous space with a finite number of samples. Hence,
sampling based methods tackle the motion planning problem by discretizing the state
space in form of a graph. The graph represents a set of possible trajectory candidates
and is generated by either sampling in action or state space. The best trajectory is then
found by evaluation of each trajectory sample. Typically the trajectory optimization
problem is relaxed to a search of possible (open-loop) solutions, which are checked for
compliance with constraints afterwards. This certainly simplifies the motion planning
problem, which is why sampling based methods are popular in automated driving
applications.
The basic difference between the approaches within the class of sampling based plan-
ning methods lies in the way trajectories are sampled and in the way the best solution
is chosen within the set of generated trajectories. First of all the trajectory set can be
sampled in batch fashion or incrementally, which might also impact the choice of an
appropriate graph type. In addition to this, for the sake of efficiency, the evaluation
is mainly dependent on the graph representation. The term graph search refers to
the fact, that for the purpose of exploring the environment the state space is divided
into graph nodes or grid cells leading to a graph or lattice, which can subsequently be
searched by graph search algorithms to find the minimum cost path through the graph.
In this thesis trajectory rollout is defined as a special case of graph search algorithms.
The main feature concentrates on the fact that a trajectory rollout approach is repre-
sented by a rooted tree. This obviates the use of graph search algorithms and leads
to a pure comparison between complete trajectory samples. The distinctive property
between graph search and incremental search algorithms is that for graph search the
graph is built one step in advance and then performing a search in the full graph,
whereas incremental algorithms explore the state space for the best solution while si-
multaneously expanding the graph by adding new states. Karaman and Frazzoli (2011)
uses the terms batch and incremental to distinguish between graph and incremental
search. Incremental search algorithms usually add new nodes if there exists a valid tra-
jectory that connects the randomly sampled query node to the graph. Advantageously
incremental planners are not required to connect two nodes exactly. Additionally, it is
not necessary to define the number of samples a priori, as because of the incremental
property the algorithm returns a solution as soon as one is found. The remaining time
might be used to refine the solution by adding more samples to the graph.
The major challenge of sampling based methods in general is the task of adequately
approximating the state space. This comprises the generation of expressive trajectory
candidates to avoid analyzing samples with a very low probability of contributing to
the optimal solution, not to mention the effort of approximating the optimal solution
in the best possible way. Obviously, knowledge about the surrounding environment
such as dynamic and static objects, as well as information about the course of the road
13
Chapter 2. Survey on Trajectory Planning Approaches to Automated Driving
can contribute to a better sampling strategy. Moreover, it is easy to ascertain that a
compromise between resolution of the discretized state space and the overall number
of processable candidates has to be found. As the first aspect impacts the quality of
the trajectory the latter is connected to the run time of the algorithm.
In contrast to several references curve interpolation is not listed as a method for
trajectory planning. The reason why curve interpolation is not considered to be an
unmitigated trajectory planning approach is the fact that it does not cover the genera-
tion of reference points, which are elementary in a planning concept. Curve planning
can be utilized as an underlying layer in each of the mentioned planning categories.
Clearly the sampling and the optimization based approach (cf. section 7.3 and sec-
tion 7.4) presented in this thesis make use of an interpolation strategy leading to a
spline based trajectory representation, which is explained in more detail in chapter 6.
2.2. Related Work for Automated Driving Applications
Perhaps the most difficult issue with respect to a survey of a field as diverse as trajec-
tory planning is to restrict the scope of the survey to permit a meaningful discussion.
This thesis will focus on approaches with sufficiently low run time suitable to be
applied in a dynamic environment, ranging from urban to highway driving. Never-
theless, to account for the advantages of each class inferring suitability for a particular
application and in order to sketch a comprehensive view of planning approaches, some
considerable methods for different (parking) applications will also be mentioned in the
corresponding sections. It is assumed that the planning algorithm is only relying on
ego vehicle sensor data and on-board computational power excluding the possibility
of data acquisition via V2X communication and distributed calculation of trajectories.
Numerical Optimization
Motion planning problems can be transformed to nonlinear optimization problems
to make them accessible from the solver’s point of view. The requirements on the
desired trajectory are formulated in terms of an objective function and constraints.
The resulting problem is to find a finite number of decision variables that minimize a
cost function while satisfying equality as well as inequality constraints. The solution
is typically dependent on an initial guess and might not converge to the optimal
solution as the objective function and the set of constraints are generally not convex
(for minimization problems). Especially for dynamic environments the concept of
optimization leads to a more general solution of the planning and control problem.
The works of Hilgert et al. (2003) and Sattel and Brandt (2005) for example show
the capabilities of path optimization for automotive applications based on elastic
band theory. Advancing further from this point Ziegler et al. (2015) describe how in
2013 a Mercedes-Benz S-Class prototype completed the Bertha-Benz-Memorial-Route,
referred to as the Bertha Benz drive. The automated vehicle had to handle different
traffic scenarios reaching from low to high difficulty and variability. Ziegler et al.
(2014) explain the applied trajectory planning approach in detail. The optimization
14
2.2. Related Work for Automated Driving Applications
problem is designed with a quadratic objective function constrained by nonlinear
inequalities. The latter are composed of external (collision avoidance and driving
corridor) and internal constraints (limits of vehicle kinematics and dynamics). To
solve for the optimal trajectory a gradient based method namely Sequential Quadratic
Programming (SQP) is used. Therefore, the constraints have been designed such that
the solution converges to a single optimum, which then is supposed to be equal to
the global optimal solution. A similar approach in the context of emergency situations
is introduced by Keller et al. (2014). The approach differs to the previous mentioned
one in the way that objectives are tailored to emergency situations instead of normal
driving and in the way constraints are handled to formulate the optimization problem.
Hard constraints are transformed and integrated into the objective function by means
of terms that penalize the violation of the constraints. The nonlinear program is hence
transferred to an approximate, non-restricted optimization problem, which then results
in a nonlinear least-squares problem that has to be solved. Coming from the same
root Ulbrich et al. (2017) and Ulbrich et al. (2018) present a similar approach tested on
different scenarios.
For explicit consideration of vehicle stability and feasibility in the process of trajectory
planning, knowledge of system dynamics in form of a vehicle model is needed. The
explicit consideration of a vehicle model to perform trajectory generation is followed
in (nonlinear) model predictive control approaches.
Werling and Liccardo (2012) propose a model predictive approach for combined steer-
ing and braking maneuvers. Nonlinear coupling effects in the vehicle dynamics are
considered by a model that is subject to acceleration constraints, which thus limit the
combined steering and braking force. As per design the approach always performs at
these limits as the analyzed simulations on the scenario of pedestrian collision avoid-
ance show. Further work in this field is provided by Bauer et al. (2012), who use a
single-track model for trajectory generation combined with a subordinated tracking
controller and Frasch et al. (2013) who introduce a nonlinear MPC approach using a
double track model for autonomous vehicle guidance. Publications in this field have
been made for emergency scenarios using a single and double track model, respectively
(Götte et al. 2016d; Götte et al. 2015a).
Because maneuver and trajectory planning both tackle (different parts of) the mo-
tion planning problem these tasks are deeply interleaved. Hence, it is not surprising
that several approaches present a coordinated solution for maneuver and trajectory
planning. Nilsson et al. (2017) present an approach for lane change maneuvers that
select an appropriate inter-vehicle traffic gap and generates a safe and smooth lane
change trajectory by solving a quadratic program. Yi et al. (2019) find different driving
maneuvers in terms of homotopy classes first (combinatorial framework) and then use
model predictive methods to determine the optimal trajectory in each class. The final
global optimal trajectory is then found by comparison among all classes.
Other interesting contributions in this field are presented by e.g. Qian et al. (2016),
Miller et al. (2018), Pek and Althoff (2018), Bergman and Axehill (2018), Nietzschmann
et al. (2018), and Rösmann et al. (2021). In previous work a combination of curve
interpolation and numerical optimization for trajectory planning has been presented
(Götte et al. 2017b; Götte et al. 2017c; Götte et al. 2017d).
15
Chapter 2. Survey on Trajectory Planning Approaches to Automated Driving
The major advantage of optimization based approaches is the ability to find the op-
timal solution for various maneuvers even in complex scenarios. On the other hand
optimization algorithms suffer from high computational burden.
Trajectory Rollout
As already mentioned trajectory rollout can be seen as a special case of graph search
in which the graph, or even more specific a rooted tree (start state of the vehicle is
given) is generated, such that the best solution is found by comparing each candidate
trajectory to each other. In its simplest version of trajectory rollout the expansion is
stopped after one step, leading to a tree of height one.
In the trajectory set generation phase, sampling is typically performed in action space
by uniformly sampling across the range of possible inputs. The trajectory is generated
with the assumption that chosen inputs are held constant over a certain future time
interval. Still, it is also possible to define the set of trajectories via state space sampling.
Either way both sampling strategies finally lead to an exhaustive search, which is
dedicated to find the best solution. This additionally contains the task of checking
each candidate with respect to the given constraints.
Montemerlo et al. (2008) roll out trajectories with lateral shifts with respect to the
smoothed center of the lane. To generate the trajectory an internal vehicle simulation
with different steering parameters is performed. Ferguson et al. (2008b) and Urmson et
al. (2009) generate trajectories in model predictive fashion by connecting the initial state
to a set of desired terminal states (position and orientation), that vary in lateral offset.
The vehicle controls are parameterized with a time based linear velocity function and
an arc-length based curvature function and numerical optimization for the curvature
profile is conducted in terms of an iterative shooting method. Then the best trajectory
is chosen according to an evaluation function.
Werling et al. (2010) and Werling et al. (2012) derive an optimal control based solu-
tion for the trajectory generation problem, that leads to quintic polynomials for an
unconstrained problem minimizing the jerk. The strategy is to first generate lateral
and longitudinal trajectory sets, which are subsequently combined by superposition.
The generation of trajectories as well as the evaluation are dependent on the desired
high level behavior. Another approach that performs trajectory rollout is presented
in Schwesinger et al. (2013), wherein sample states are connected via forward simula-
tion of a vehicle model in combination with a feedback control. This corresponds to the
idea of closed-loop prediction that is also applied in the context of incremental search
(cmp. Closed-loop Rapidly-exploring Random Tree) and returns drivable trajectories in
terms of accounting for vehicle dynamics. Candidate trajectories are hence denoted
as system-compliant. The advantage of closed-loop prediction can be summarized as
good way to easily include system dynamics and such generating only candidates
which are drivable. Generally, the approach of Schwesinger et al. (2013) could be cate-
gorized as a graph search algorithm, but just a single tree level was chosen as edges
within deeper tree levels were marked as an approximation of future costs, but not
contributing to the overall behavior due to replanning.
Instead of sampling in state space another way to generate candidate trajectories is
16
2.2. Related Work for Automated Driving Applications
by sampling in action space, as for example assuming continuous curvature (Hun-
delshausen et al. 2008) leading to the Tentacles approach that is also addressed by
Mouhagir et al. (2020), or by sampling control inputs of a vehicle model (Keller et al.
2015). Schlechtriemen et al. (2016) exploit the differential flatness of a single track
model and generate jerk-optimal trajectory candidates. The tasks of maneuver plan-
ning and trajectory planning are deeply interleaved, because the trajectory set is gen-
erated by sampling in action spaces that are defined from the occupancies detected in
the traffic scenario. For trajectory planning this means that constraints are generated
in terms of checkpoints (breakpoints), which the vehicle has to pass through. Then
jerk optimal interpolation in the flat output space is performed to generate the trajec-
tories. Therefore, the start and goal state are transformed to the flat output space and
interpolated along with each intermediate checkpoint. The evaluation takes then place
in the state and input space.
Some approaches combine the trajectory rollout with subsequent optimization to ben-
efit from the advantages of both methods (see e.g. Xu et al. (2012) and Kunz and
Dietmayer (2016)). In connection with an underlying interpolation strategy an ap-
proach to trajectory rollout has been proposed that features online generation and
evaluation of trajectory candidates (Lienke et al. 2018b).
Graph Search
Graph search methods discretize the state space into a grid to find the optimal solution
to the motion planning problem. The planning problem is thus represented as a search
for a solution in a known finite graph. Given is a graph that represents the problem
and the task is then to find a path from start to the goal region with minimal costs
(also referred to as shortest path). The expansion strategy is chosen to minimize the
number of expansions required to guarantee that the optimal cost path is under the
least-costs path examined. In contrast to uninformed search strategies that expand
states in the order of the costs of the best path found so far from start state to the
query state (e.g. Dijkstra (1959)), a heuristic function can be utilized to perform an
informed search, where an additional estimate of the cost of a least-cost path from the
query state to the goal state is considered (e.g. Hart et al. (1968)).
State-of-the-art graph search algorithms for automated driving applications share the
basic principle and advance the basic ideas in different aspects. In particular ap-
proaches were extended to run in an anytime fashion in order to meet run time re-
quirements or were particularly developed to deal with dynamic environments. Any-
time planning algorithms like the Anytime Repairing A* (ARA*) (Likhachev et al. 2003)
try to find the best plan within the available amount of time. Therefore, they follow a
two-step strategy: Find a fast and possibly highly suboptimal plan first and then im-
prove the initial solution within the remaining time. The ARA* algorithm relies on the
fact, that the environment does not change and hence the graph remains unchanged.
In dynamic environments (or in general for imperfect environment information) the
graph needs to be updated over time and a replanning is necessary. Algorithms like
the Anytime Dynamic A* (AD*) improve the solution over time (anytime property) and
17
Chapter 2. Survey on Trajectory Planning Approaches to Automated Driving
repair its solution in case that parts of the graph are affected by changes (incremental2
property). Likhachev and Ferguson (2008) and Likhachev and Ferguson (2009) show
the application of AD* in several parking and driving scenarios. Dolgov et al. (2010)
propose Hybrid A* for planning in semi-structured environments (mainly navigating
in parking lots) to account for the problem that because of their discrete nature, regular
A* provide solutions that might not be executable for the vehicle. It improves on the
A* algorithm by assigning a continuous vehicle coordinate to each discrete grid cell.
Primary designed for path planning applications, but mentioned because of its popu-
larity is the use of Probabilistic Road Maps introduced by Kavraki et al. (1996). They rely
on a randomized sampling of graph nodes with each edge corresponding to feasible
path between these. In the same spirit, but featuring a systematic sampling strategy is
the application of state lattices, that build a directed graph in the vehicle’s state space
and solve a two boundary value problem to connect each state. Especially the latter
task is closely connected to well-known optimal path planners like Dubins (1957) or
Reeds and Shepp (1990) that provide the solution of the two boundary value problem,
but do not take obstacles into consideration.
The yet mentioned approaches reflect the fact that graph search methods predom-
inantly demonstrate their strength in semi-structured environments i.e. in parking
applications. For on-road driving Ziegler and Stiller (2009) advance the idea of state
lattices to trajectory planning approaches in dynamic environments by explicitly con-
sidering time, leading to the concept of spatiotemporal state lattices. They propose
a lane adapted reparametrization of the space and generate a spatiotemporal state
lattice, which connects each node with quintic polynomials. McNaughton et al. (2011)
enhance the trajectory rollout approach of Ferguson et al. (2008b) to a graph based
search in a spatiotemporal state lattice. A path is defined as a cubic polynomial spiral
and a spatiotemporal lattice is generated to account for the dynamic environment.
The problem of dimensionality resulting from the combinatorial way of sampling is
addressed by proposing an efficient pruning of the search space.
For graph search approaches, which specify a trajectory planning problem that ex-
plicitly contains time information, it will generally result in a Directed Acyclic Graph
(DAG). This is due to the fact that time is strictly monotonically increasing and con-
nections that go back in time do not exist. The basic concept to find the shortest paths
in a DAG is based on the idea to apply topological sorting in advance, such that the
resulting algorithm is linear in time. This means that DAG shortest path algorithms
perform faster than e.g. Dijstra’s algorithm (Cormen et al. 2009). McNaughton et al.
(2011) argue that for automated driving applications an exhaustive search is necessary
anyway, because the computation time should never exceed a certain threshold to
meet real-time requirements. This should also hold for the worst case in which all
nodes have to be expanded in order to find the optimal path. As for lattice planners
in dynamic environments the occurrence of the worst case is very likely (a heuristic
estimate can hardly be found) they conclude to use dynamic programming to search
the entire graph. As in the case of trajectory rollout some contributions apply graph
search with subsequent optimization (Hesse et al. 2010)
2In this context the incremental property is related to the capability of repairing a previous solution
of a graph search and should not be confused with the class of incremental search approaches.
18
2.2. Related Work for Automated Driving Applications
Graph based search algorithms allow for a simplified handling of constraints, but
suffer from the fact that for a fixed resolution, the memory and computation time grow
exponentially with the number of dimensions of the search space. Further drawbacks
go along with the discretization of the state space as the choice of resolution will
certainly lead to suboptimal results.
Incremental Search
Incremental search techniques rely on random or deterministic sampling to incremen-
tally build a graph or tree in the state space. Algorithms in this class generally follow a
three step procedure: Take a random sample from the free state space, find the nearest
state in the current graph and apply a local planner to generate a connection to add
the random sample (the last step can also be replaced by adding a new state in the
direction of the random sample, as it is not required to connect two states exactly). Fi-
nally, this will result in an increasingly finer discretization of the state space, in which
the solution can be found at the time a path through the graph connecting start state
and goal region exists.
The most prominent approach representing incremental search is the Rapidly-exploring
Random Tree (RRT), which has been introduced by LaValle (1998). In the context of
automated driving Ma et al. (2015) apply an RRT based on various rule templates for
different maneuvers and an aggressive expansion strategy. Kuwata et al. (2008) and
Kuwata et al. (2009) introduce the Closed-loop Rapidly-exploring Random Tree (CL-RRT),
which extends the original RRT approach by a low level controller to follow a given
reference by forward simulation using a vehicle model. In a subsequent step the gen-
erated state trajectory is checked against constraints like obstacle avoidance. Another
variant, the CL− RRT#, has been developed and introduced by Arslan et al. (2017).
The approach relies on the closed-loop dynamics to connect nodes and additionally
ensures asymptotic optimality. The main drawback of the RRT approach is that it
certainly is not going to return an optimal solution. Karaman and Frazzoli (2011) in-
troduce a variant named RRT* (optimal RRT) which achieves asymptotic optimality,
by additionally performing a rewiring of the tree after a new state has been added
to the graph. In the field of automated driving several approaches using RRT* have
been presented (e.g. Karaman et al. (2011) and Jeon et al. (2013)). The characteristics of
incremental search support its application in parking scenarios, such that various pub-
lications have been made within this area (see e.g. Banzhaf et al. (2018a) and Banzhaf
et al. (2018b)).
Incremental search algorithms tend to be probabilistically complete, which means that
the algorithm will find a solution, if one exists, with probability approaching one for
increasing computation time. In contrast to graph search methods incremental search
planners do not require to connect two states exactly, which facilitates the construction
of the graph. A major drawback of incremental and graph search methods is the
significant increase of the computational burden with increasing dimension of the state
space, while completeness (see A.1) cannot be guaranteed for real-time applications.
19
3
A Concept for Trajectory Planning in
Dynamic Environments
The complex task of automated driving necessitates a precise design of the overall
system, in which the part of trajectory planning has to be integrated appropriately.
Because of the diversity and difficulty of the driving task the functional architecture
of automated vehicles is mostly structured hierarchically. To define the interfaces for
each module it is important to specify the basic requirements and capabilities with
respect to the functional interaction leading to the desired synergy. Hence, it is crucial
to adjust the capabilities of the planning module based on the quality and integrity
of the provided data, when designing the concept for trajectory planning in dynamic
environments.
3.1. Incorporation in the Functional Architecture of
Automated Vehicles
Since the functional architecture is core to structure the technical development, as well
as to divide the complex problem into several sub-tasks it directly impacts the overall
performance of the system. The functional architecture of automated vehicles is not
standardized, but still it is evident that it is in general composed of several basic com-
ponents. Namely this is perception, planning and action. Because the characteristics
of architectures are as diverse, this thesis waives to pinpoint the exact details of each
component, but proposes a possible design of the system architecture focusing on a
seamless integration of trajectory planning. A prototype of a functional architecture
accounting for several aspects of the driving task is shown in Figure 3.1.
Although not shown in the figure it is worth to mention, that modules in the functional
architecture of automated vehicles do not only work in feed forward fashion, but have
to gainfully feed back their obtained information at characteristic stages. The example
of a functional architecture of automated vehicles, consists of modules for perception,
situation analysis, mission planning, decision making and trajectory planning as well
as for vehicle dynamics control. The perception module generates a description of
the actual traffic situation (environment model). The environment model incorporates
20
3.1. Incorporation in the Functional Architecture of Automated Vehicles
Map Data Mission Planning
Route Planning
Decision Making and
Perception Situation Analysis
Trajectory Planning
Fusion and Object Maneuver
Tracking Maneuver DetectionDetection
Object Maneuver Maneuver Decision
Localization Dynamics
Estimation Maneuver Related
Free Space Feature Generation
Detection Object
Trajectory Trajectory Planning
Prediction
Communication Dynamics Control
Vehicle to Vehicle Lateral Control
Longitudinal
Vehicle to X
Control
Figure 3.1.: Example of a functional architecture of an automated vehicle.
the estimated states of all obstacle vehicles and objects as well as a representation
of the road topology. Map data and communication can rigorously support the data
acquisition by contributing profound information. Situation analysis enhances the
information with intention estimation of observed traffic participants and prediction
of the traffic situation. The predicted trajectories complete the environment model
to a predictive version that can be used by the maneuver and trajectory planning
modules. During mission planning a static route is planned that neglects for example
obstacle vehicles. In addition vehicle dynamics are not taken into account at that
stage. During driving a decision making module manages upcoming events and infers
appropriate maneuvers. At the trajectory planning level system constraints are taken
into consideration, as well as other traffic participants are considered for the purpose
of collision avoidance1. Subsequently a reference trajectory is commanded to the low
level vehicle controller, that takes care of realizing the planned trajectory. Because
1Note that these characteristics also have to be considered in some way during decision making,
which in general necessitates an interleaved design between decision making, situation analysis and
trajectory planning.
21
Stabilization Guidance Navigation
Chapter 3. A Concept for Trajectory Planning in Dynamic Environments
of sudden changes in the environment frequent replanning is essential for safe and
comfortable driving.
The main path of the shown functional architecture follows the principle of perception
(pure acquisition of data), planning (situation analysis, decision making and trajectory
planning) and action (vehicle dynamics control). The planning part has been subdi-
vided to account for other-directed impact factors covered by the situation analysis
on the one hand and to emphasize the self-determined part of decision making and
planning on the other hand. This separation also allows to augment Figure 3.1 with
respect to the hierarchy of driving tasks according to Donges (see e.g. Donges (2016)).
A brief but more detailed description of the components of the functional architecture
can be found in chapter A.2.
3.1.1. Prerequisites and Realization
The interconnection of components within the functional architecture builds up a com-
plex system, that can be designed in many different ways revealing and emphasizing
different characteristics of the automated vehicle performance. For a common under-
standing of the trajectory planning task, it is important to sketch a clear idea of the
functional architecture in that respect.
Especially the complex task of decision making and trajectory planning is deeply
interleaved. The problem is that a comprehensive decision making is dependent on
detailed and precise modeling of all relevant aspects regarding the scenario assessment
leading to the desired high accuracy and performance. This explicitly entails the task
of an accurate trajectory planning with appropriate modeling of the vehicle dynamics.
Although the number of potential maneuver classes in a scenario is finite, the num-
ber of maneuver trajectory candidates is infinite. Having in mind that computation
time is limited for automotive applications, this fact leads to the conclusion that with
nowadays hardware a hierarchical structure seems to be inevitable. In other words,
the decision making is set up with maneuver modeling in a sufficient level of detail
with some simplifications to match calculation time requirements, whereas trajectory
planning as a subsequent module benefits from the global knowledge and implements
the motion problem with a higher accuracy.
The trajectory planning algorithms presented in this thesis are developed under the
following set of assumptions.
• The ego vehicle is equipped with a sensor system providing relevant data, such
as e.g. relative positions and velocities of surrounding traffic participants and
road boundaries.
• The ego vehicle is equipped with a situation analysis module that provides pre-
dicted trajectories of surrounding traffic participants. In this work the prediction
of other traffic participants relies on the approach from Wissing et al. (2018).
• The ego vehicle is equipped with a decision making module that provides high
level demands for the trajectory planner. The maneuver detection and the utility
22
3.1. Incorporation in the Functional Architecture of Automated Vehicles
function based maneuver decision own similar characteristics as the approach
presented by Wissing et al. (2017) in the context of driver modeling.
• The ego vehicle is equipped with a low-level control system, which is capable of
following the planned trajectory.
All these aspects are reflected in the given example of a functional architecture shown
in Figure 3.1. In this work the process of decision making is separated into maneuver
detection and decision. The maneuver detection mainly corresponds to a maneuver
request and categorizes the desired maneuver based on the current traffic situation.
Then the maneuver request is reviewed by means of a gap acceptance strategy to
finally select a suitable maneuver class. Thus, the maneuver decision acts on the basic
level of motion primitives such as lane keeping and lane changing. Because also more
sophisticated maneuvers like overtaking can be decomposed into a set of these ma-
neuver classes this interface is sufficient to manage even complex scenarios. Moreover,
limitations arising from traffic rules, e.g. speed limits, stop signs or traffic lights are
handled in this context.
3.1.2. Interfaces for Trajectory Planning
Based on the prerequisites mentioned before the interfaces between adjacent modules
can be defined. With respect to the given example of a functional architecture illus-
trated in Figure 3.1 the question is how to generate a trajectory from the output of the
maneuver decision and what is needed for the vehicle dynamics controller to work
appropriately.
Interface to the Behavior Layer
The decision making within the functional architecture is designed in a way that it im-
plements a general interface for the trajectory planning module. Beside many variants
there are basically two options how the decision making output can be defined: Ei-
ther the decision is propagated on maneuver class level, or a representative maneuver
trajectory is provided. From trajectory planning perspective the favorable possibility
is the generation of a maneuver trajectory, since this kind of pre-planning appends
valuable information to the maneuver decision. Nevertheless, the developed trajectory
planners should be able to deal with both kinds of input. This necessitates the imple-
mentation of an intermediate step to transform the maneuver decision to a suitable
maneuver trajectory. This is achieved by means of the environment-aware maneuver
planning described in chapter 4.
The input of the trajectory planning module is hence a maneuver trajectory, that func-
tions as a representative prototype trajectory of the selected maneuver class. Fur-
thermore, a spatiotemporal target region can be extracted, which involves topological
knowledge about the environment. This resembles the demand on a desired maneu-
ver providing a target position, orientation, velocity as well as further region-related
information.
23
Chapter 3. A Concept for Trajectory Planning in Dynamic Environments
Interface to the Control Layer
The interface to the control layer is straightforward. It is assumed that a trajectory
following controller is apparent, such that a trajectory is provided from the trajectory
planner to the controller. The trajectory should primarily contain the coupled lateral
and longitudinal position expressed in vehicle coordinates. Together with the inherent
time information of the generated trajectory this allows to derive arbitrary reference
signals (e.g. a reference velocity or reference yaw-rate) facilitating a variable controller
design. For a high control quality a continuous description of the trajectory or at least
a sufficiently high resolution in time and position is preferred. The control strategy
can be designed as a fragment execution approach. The current trajectory is therefore
considered as reference and the controller takes care about the realization of (at least
some parts of) the trajectory. The sequence of trajectories does not necessarily con-
tribute to an overall continuous driving, because deviations between the planned and
realized trajectory are very likely to occur. The direct execution approach on the other
hand could permanently provide updated trajectories to the controller. In this work
the direct execution approach is chosen.
3.2. Requirements
The definition of requirements with respect to trajectory planning is twofold. First the
requirements related to the planning approach itself have to be specified assigning the
desired characteristics to the approach from development perspective. Secondly, de-
mands on the characteristics of the trajectory in the context of automated driving need
to be defined shaping the outlook of the planned trajectory that is directly coupled
with the expected driving experience. The design of the trajectory planning approach
should comply with the following considerations:
Requirements on the Trajectory Planning Approach
1. The requirement of temporal consistency accounts for the fact that the generated
trajectory between two subsequent planning cycles should not change signifi-
cantly, assumed that changes in the surrounding environment are consistent.
2. The approach should be complete, which means that the algorithm terminates
in finite time and returns a valid solution to the trajectory planning problem or
ascertains that no valid solution exists.
3. The approach should be real-time capable to be able to react to sudden changes
in the perceived environment. This means that the computation time should
be bounded from above in a way that it accounts for the challenges arising
from the environment. An approximation of a suitable calculation time for one
process cycle (including perception, situation analysis and decision making) can
be derived from the dynamics of the environment. Kuwata et al. (2008) propose
that the planning interval must be less than 0.1 s.
24
3.3. Derivation of a Trajectory Planning Concept
4. Arbitrary trajectory shapes should be realizable. This requirement is equivalent
to the capability of covering driving tasks in static, unstructured environment as
well as driving in dynamic and structured environment involving use-cases such
as lane changing or lane keeping.
5. The approach should be optimal with respect to predefined criteria. The objec-
tives should be derived from requirements on the desired ego trajectory for the
respective situation. Additionally, the trajectory planning approach should be
hybrid, which is defined as being aware of the global situation, while converg-
ing to a local minimum. This requirement is equivalent to finding the globally
optimal solution.
6. The trajectory planning approach should be adjustable in a reasonable way. This
requirement relates to the estimated application effort that is necessary to tune
the developed approach in order to generate the desired behavior. Effectively,
higher adjustability reflects a reduced effort for applying the proposed approach
in practice.
7. It should be possible to integrate superordinate decisions in the trajectory plan-
ning process.
Requirements on the Trajectory
1. Drivability: The trajectory should consider nonholonomic constraints arising from
the kinematics of the vehicle. This requirement is equivalent with the desire of a
system compliant behavior and can be extended to the consideration of all kind
of vehicle limitations.
2. Collision Avoidance: The trajectory should provably be collision free. This is es-
pecially addressing the capability of coping with dynamic traffic participants.
Furthermore, safety distances should be satisfied in compliance with the passen-
gers sense of security.
3. High Level Behavior: The trajectory should reflect the ability to drive in corre-
spondence to the prevailing traffic rules (speed limits, lane discrete driving, etc.)
and should be capable of reaching arbitrary goals (e.g. demanded maneuver). In
particular two important functionalities should be covered: lane keeping with
respect to lateral guidance as well as adaptive cruise control like longitudinal
behavior.
4. Comfort: The trajectory should provide a comfortable drive for the passengers in
the ego vehicle.
3.3. Derivation of a Trajectory Planning Concept
To derive a consistent concept the stated aspects with respect to the functional archi-
tecture and the requirements have to be merged into one comprehensive trajectory
25
Chapter 3. A Concept for Trajectory Planning in Dynamic Environments
planning framework.
To successfully accomplish the driving task a cyclic replanning is vital. This is first and
foremost due to the dynamic character of the environment, since previous assumptions
about the future development of the traffic situation might become invalid and need
to be updated with the latest sensor data. Moreover, because of practical reasons
the planning horizon is finite, which finally leads to a receding horizon approach
that likewise relies on frequent updates. With permanent replanning there is a risk
that due to e.g. suboptimality or unexpected events a discrepancy in consecutive
planned trajectories arises, which might lead to undesired effects like overshoot or
oscillations (Werling et al. 2012). This issue is addressed by the requirement of temporal
consistency, because if temporal consistency holds then stability is implied and the
resulting trajectory is independent of the replanning frequency. Whereas unexpected
events (because of the uncertain prediction a certain unexpectedness is inherently
expected) or wrong decisions aggravate the adherence of temporal consistency since
they infer inconsistent restrictions, the aspect of optimality is explicitly considered
in the design of the trajectory planning approach. This refers to the choice of an
optimal function class, as well as to the overall optimization scheme. The necessary
fundamentals are explained in chapter 6 that thoroughly investigate the choice of the
trajectory representation in terms of optimality.
Overall, to solve the constrained trajectory optimization problem (see section 1.2) in
accordance with the defined requirements on the trajectory this work relies on an
optimization heuristic as a fundamental basis. The underlying idea is to perform a
search within the set of optimal solutions to an unconstrained subproblem and choose
the best solution of the associated high level problem2, which fulfills the restrictions
in order to derive a solution to the constrained problem. In principle the procedure of
solving an unconstrained problem first and then complementing it by the consideration
of restrictions in a second step is unique to discrete trajectory planning algorithms, but
in consequence of the applied optimal interpolation strategy it is possible to transfer
this idea to continuous optimization techniques. Hence, the proposed interpolation
strategy paves the way to create a common basis for a generic trajectory planning
approach to automated driving.
Finally, this leads to a two level hierarchical trajectory optimization framework in
which the low level behavior is covered by the optimal solution of the unconstrained
minimum kinematics problem, given by a spline-based trajectory representation with
an optimal interpolation strategy. In order to satisfy the constraints like drivability and
collision avoidance and to account for the desired high level behavior a superordinated
optimization is performed. This way low level optimality with respect to basic driving
functionality is assured, while simultaneously high level behavior is still prioritized.
The separation into two different levels moreover addresses the problem of conflicting
planning objectives and is completely compliant with the requirements on the trajec-
tory. In particular, it is very difficult to define optimality in the context of the desired
high level behavior. This is because of subjective assessment of driving maneuvers on
the one hand and diverging interests in terms of maneuver demands on the other hand.
2The connection is established via the high level cost function, which is designed as a function of the
low level solution.
26
3.3. Derivation of a Trajectory Planning Concept
A lane change for example might require a different set of optimality criteria than a
lane keeping maneuver. By the application of the two-level hierarchical framework this
conflict vanishes since an optimal low level behavior in terms of passenger comfort
is always guaranteed by the optimal interpolation strategy, while the optimizer can
solely take care to achieve the desired specifications given by high level maneuver
requirements, covering a universally valid description of the automated driving task.
Figure 3.2 shows the proposed concept of a two level hierarchical trajectory optimiza-
tion framework. As a matter of fact relevant information in terms of the requirements
Two Level Hierarchical Trajectory Planning Ego Vehicle in Dynamic Environment
Constraints and
High Level Behavior fix Update of ego and environment data
variables
Optimization optimize
variables
Trajectory Optimizer
Low Level Behavior
Optimal Interpolation adapt Output of optimal reference trajectory
variables
Figure 3.2.: Sketch of the proposed trajectory planning concept. Cyclic replanning in receding
horizon fashion with the two level hierarchical trajectory optimization framework.
can be processed in three different ways, based on the prior knowledge about the
desired trajectory. If for example some trajectory quantities can be directly inferred
from the current traffic situation it is vain effort to search for the optimal solution
in that respect. For this reason the developed trajectory planning approach allows to
fix certain trajectory quantities (i.e. variables in terms of breakpoint elements) that
are already known in advance. Remaining breakpoint elements can then be used as
optimization parameters to shape the trajectory to comply to the desired behavior.
As mentioned before a further subdivision is proposed to beneficially separate low
and high level behavior. Low level decision variables hence adapt to the result ob-
tained by optimization on high level. In case of the low level behavior a minimum
kinematics trajectory generation approach is presented that relies on a spline-based
trajectory representation in combination with an optimal interpolation. Details are
given in chapter 6. To account for the high level behavior classical trajectory planning
approaches are applied. In this thesis a discrete and a continuous approach are fol-
lowed. A detailed description of the discrete trajectory optimization can be found in
section 7.3 and for the continuous trajectory optimization in section 7.4. A separate
spline for each direction is specified, yielding a lateral and longitudinal trajectory3.
3It is common practice to choose the number of separate splines in correspondence to the number of
flat output variables of the system.
27
Chapter 3. A Concept for Trajectory Planning in Dynamic Environments
However, the evaluation of objectives and constraints is performed on the combined
trajectory accounting for the coupled dynamics of the system.
The two level hierarchical trajectory optimization framework is seamlessly embedded
into the functional architecture and intended to work in receding horizon fashion
implying cyclic replanning. In general, to design the cyclic replanning strategy there
are two options to choose the start state of the ego vehicle for the next planning cycle.
The first option is to proceed from the state obtained from the previous planning
solution. This enables the possibility to account for updates from the environment
or new maneuver demands, however in this case feedback is realized on controller
level. The overall performance (especially in terms of stability) is then mainly based
on the subordinate trajectory following controller. The second option is to consider
the updated measured quantities in the new planning cycle. Hence, at the start state
deviations between the planned and realized trajectory introduced by inaccuracies of
the low level controller will be eliminated as per design. Feedback is then realized on
trajectory planning level likewise model predictive control approaches. In this work a
similar variant of the latter approach is pursued.
The key features of the proposed approach can be summarized in the choice of
the trajectory representation as well as in the design of the optimization scheme.
A spline-based trajectory representation with optimal interpolation strategy allows
for arbitrary shapes of the trajectory and also enhances computational efficiency. The
optimization scheme is set up in order to find the optimal solution to the trajectory
planning problem, leading to the concept of a two level hierarchical trajectory opti-
mization. Beside other advantages the hierarchical framework promotes adjustability,
since it enables the possibility to incorporate objectives such as comfort and high level
decisions on different levels and allows to reduce the number of optimization param-
eters to the relevant parameters for the respective use-case. This leads to a highly
versatile design that is capable to adaptively reshape in correspondence to demanded
development features, resulting in a generic trajectory planning concept for automated
driving.
3.4. Comparison to Other Trajectory Planning Approaches
Looking at the taxonomy of trajectory planning approaches (see section 2.1), there
is numerical optimization, incremental search, trajectory rollout and graph search. In
particular randomized, incremental search algorithms show some undesirable proper-
ties with respect to on-road driving, since randomized approaches do to a large extend
not benefit from exploiting the structure of the environment. Moreover, it cannot be
guaranteed that a solution is found within finite computation time. Since asymptotical
optimality is also not an inherent feature of randomized approaches, the overall result
might in addition be heavily suboptimal. The major disadvantage is the complexity
that comes along with the dimension of time, which is vital for dynamic environments.
This is because of the extension of the sampling space on the one hand that directly
links to an increase of the computational effort, but it also complicates the connection
between a node of the existing tree and the sampled query state on the other hand.
28
3.4. Comparison to Other Trajectory Planning Approaches
In the context of graph search algorithms a well chosen heuristic estimate will have
a positive impact on the overall runtime of the algorithm. But in general such an
heuristic that is valid for various scenarios is hard to find and worst-case assump-
tions should not be ignored as well. Furthermore, graph search algorithms are usually
constructed to reach a fixed goal position. This is not necessarily the case for on-road
driving scenarios, such that as a consequence a realization is complicated and might
lead to undesired behavior. Jerky solutions for example possibly exclude drivability
in terms of the non-holonomic constraints of the vehicle, but in any case decrease
driving comfort. In that respect the application of trajectory rollout and numerical
optimization approaches seems to represent the more promising way. A common ap-
proach to trajectory planning for automated driving in dynamic environments is the
generation of multiple trajectories that fan out to cover the basic driving options of
the vehicle. From this set the optimal trajectory in terms of a cost function is chosen.
The popular work of Werling et al. (2010) for example is inspired by the theory of
optimal control, in which the cost function is tuned to fit the subsequent trajectory se-
lection process. It is shown that by this design desirable characteristics such as stability
and temporal consistency are attained. Moreover, it enables a simplified handling of
constraints. Polynomials are identified to represent the optimal function class in that
respect (also see section 6.1). The usage of polynomials to represent longitudinal and
lateral trajectories coincides with a trajectory rollout that stops expansion after the
first step. In total this limits the general scope of maneuvers that can be performed,
because it restricts the shape of the trajectories (e.g. a complete overtaking maneu-
ver cannot be planned). For these kinds of maneuvers optimality cannot be declared.
For this reason polynomial splines are applied in this work, allowing for arbitrary
trajectory shapes and extending the scope of plannable maneuvers. Nevertheless, op-
timality is not neglected as the presented minimum kinematics trajectory generation
features an optimal interpolation strategy on low level and on high level dedicated
optimization approaches are applied. Therefore, it is assumed that a spline is capable
of matching the optimal function class with respect to the high level problem. The
adaptiveness of a spline suggests this capability, since the high level cost function
merely concentrates on kinematic objectives, while trajectory smoothness induced by
spline continuity conditions moreover relates to the non-holonomic characteristics of
the ego vehicle. The work of Ziegler et al. (2014) in the context of the Bertha Benz drive
represents one milestone in the history of automated vehicles. The approach basically
follows the procedure of numerical optimization techniques. The decision variables
are represented by longitudinal and lateral position and hence directly correspond to
the discretized states of the trajectory. All objectives of the comprehensive objective
function are handled on the same level balanced by manually adjusted weights. In
this thesis the developed trajectory planning framework introduces a hierarchy that
explicitly accounts for conflicting objectives, leading to less tuning effort. Moreover,
as the trajectory is represented by a spline a significant reduction of the number of
decision variables can be achieved. To sum up, from theoretical perspective the devel-
oped approach retains desirable features of well-known approaches, but advances the
underlying ideas in some particular points. Namely this is the spline-based trajectory
representation embedded in the proposed two level hierarchical framework.
29
4
Environment-aware Maneuver Planning
As mentioned before a major objective for trajectory planning is the realization of the
desired vehicle behavior. In this chapter an environment-aware maneuver planning ap-
proach is described. Based on a high level decision a maneuver reference trajectory and
spatial bounds are provided, which are used for trajectory planning in a subsequent
step. For the sake of simplicity the level of detail with respect to the description of
vehicle motion is reduced to the essential characteristics. Complex interdependencies
in terms of the lateral and longitudinal vehicle dynamics are hence neglected. Calcu-
lations are done in curvilinear coordinates and obstacle vehicles are associated in a
lane discrete fashion. The output can be used for initialization in optimization-based
trajectory planning approaches or as a directive to generate the sampling set for dis-
crete trajectory planners. Hence, it is worth to mention that this kind of pre-planning
highly impacts the overall performance of the trajectory planning approach. For this
reason and in order to give each investigated trajectory planning approach a common
base a maneuver planning approach has been implemented in prototype fashion. The
characteristics of the designed maneuver planning approach align with the proposed
functional architecture given in section 3.1.
To show how the maneuver planning works, the approach is explained at the example
of the situation depicted in Figure 4.1. The ego vehicle is surrounded by four obstacle
10
ID:3 ID:45 ID:1
0 ID:2
−5
−30 −20 −10 0 10 20 30 40 50
E px [m]
Figure 4.1.: Example situation to show the general functioning of the maneuver planning
approach.
vehicles that are traveling along the road at different speeds. The start configuration
of the test case is summarized in Table 4.1. In chapter A.3 another use case is shown.
30
E py [m]
4.1. Extraction of Maneuver Relevant Information
Table 4.1.: Test case definition with start conditions of the ego vehicle and obstacle vehicles.
The target lane is defined with respect to the ego vehicle, with 1 for the left lane, 2 for the
center lane and 3 for the right lane. This means that the ego vehicle intends to perform a lane
change to the left, whereas the obstacle vehicles stay on their respective current lane.
ID [ ] F p [m] F p [m] Fv [km/h] Fx y ψ [rad] target lane [ ] Fvdes [km/h]
ego 0 0 100 0 1 130
1 40 2.5 100 0.1 2 not known
2 20 −3.35 90 0.05 3 not known
3 −25 3.75 110 0 1 not known
4 25 4.65 120 0.1 1 not known
Therefore, the test case definition is slightly altered leading to a cut-in and cut-out
maneuver of obstacle vehicle ID:2 in front of the ego vehicle.
At first the surrounding environment has to be analyzed with respect to arising spatial
limitations for the ego vehicle. Then the lateral and longitudinal maneuver trajectory
can be calculated and finally be composed to the maneuver trajectory. As another
result the maneuver planning further provides a target region.
4.1. Extraction of Maneuver Relevant Information
By considering only the relevant information with respect to the intended ego ma-
neuver, the complexity of maneuver planning can significantly be reduced. Therefore,
it is necessary to extract the maneuver relevant vehicles (which will be referred to
as impeding vehicles) as well as the corresponding position bounds. The spatial re-
lation between the ego vehicle and the ego maneuver relevant obstacle vehicles is
summarized in the definition of impeding vehicles.
Impeding vehicles Impeding vehicles are classified with respect to the ego vehicle
position as the closest obstacle vehicle on the respective lane. The closest obstacle
vehicle to the front is denoted as lead, whereas the closest vehicle at the rear is denoted
as tail vehicle. Hence, the definition covers the spatial relations between the ego vehicle
and the other traffic participants in the scenario for each individual lane.
As the behavior decision is based on the current time step, the maneuver planning
assumes that the current impeding vehicle constellation is binding and should also
hold true for the future evolution of the scenario until a lane change of one of the
obstacle vehicles is detected. This means that if a lane change is demanded, the current
lead and tail vehicle on the target lane represent the spatial gap in which the ego vehicle
should drive. The same holds for lane keeping as obviously in this case the lead vehicle
limits the reachable space of the ego vehicle. In a first step other vehicles are predicted,
such that the impeding vehicles, which are relevant for the intended maneuver can be
identified. Then, in dependence on the desired maneuver, the position bounds for the
31
Chapter 4. Environment-aware Maneuver Planning
ego vehicle arising from the surrounding vehicles can be extracted.
Impeding Vehicles Prediction
The prediction of impeding vehicles is based on the lane association of the predicted
obstacle vehicles (see section 3.1.1). The identification of future impeding vehicles is
mandatory for predictive driving and enables the ego vehicle to handle for example
cut-in maneuvers.
Normally, impeding vehicles are detected for the current time step based on the current
constellation. With the aim to extract the impeding vehicles for future time steps the
problem is that the constellation changes over time. This change is mainly caused by
two reasons:
1. difference velocities between the other traffic participants that change their spatial
relations to each other and the ego vehicle respectively,
2. lane changes of obstacle vehicles that lead to a different lane association.
It is obvious that the motion of the ego vehicle highly impacts the correct extraction of
the impeding vehicles, as this metric is defined with respect to the position of the ego
vehicle. The problem is that the future ego vehicle position is unknown at this stage
of the maneuver planning. Based on the behavior plan it is not sufficient to assume
constant velocity for the ego vehicle as this might be in contradiction to the assumption
that the current impeding vehicle constellation is decisive as it reflects the conditions
with respect to the desired maneuver. It could for example be necessary to accelerate
to drive in the desired spatial gap on the target lane, but still the ego vehicle is not
allowed to overtake the lead vehicle on the target lane.
It is important that the impeding vehicles reflect and correspond to the desired ego
vehicle behavior. A consistent evolution of the situation has to be assured, which
implies that the situation has to be evaluated for all behavior hypotheses of the ego
vehicle (i.e. for all ego vehicle target lanes). It is presumed that the identified impeding
vehicles constellation does not change until a lane change of one of the obstacle vehicles
is detected. Until then the ego vehicle velocity is considered to be in the interval of the
lead and tail vehicle velocity on the respective target lane1. This measure contributes
to the desired ego vehicle behavior to take care of realizing the decision to take the
current gap on the target lane. The situation is reassessed when a lane change of one
of the obstacle vehicles is detected. At that moment the process of determining the
impeding vehicles is triggered again and it is decided if the lane changing vehicle
merges in front or behind the ego vehicle and thus represents a potential lead or tail
vehicle. The detection of impeding vehicles at respective lane change times can be
regarded as a new decision that has not been made in advance, but approximates the
development of the situation reasonably well.
For the considered example the predicted impeding vehicles are shown in Figure 4.2
1It is assumed that the ego vehicle velocity at the predicted time step is independent of the past time
steps.
32
4.1. Extraction of Maneuver Relevant Information
lead vehicle left lane lead vehicle center lane lead vehicle right lane
tail vehicle left lane tail vehicle right lane
4
3
2
1
-1
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure 4.2.: Predicted impeding vehicles shown for the considered example test case. In cor-
respondence to the selected test case (obstacle vehicles do not change lanes) the assignment
with respect to lead or tail vehicle for the respective lane does not change. As no tail vehicle
for the right lane exists it is mapped to a vehicle index of -1.
It is assumed that the tail vehicle velocity is less than or equal to the lead vehicle
velocity on the target lane (which is a rather evident assumption as the tail cannot
overtake the lead vehicle). Still, this shows that in general it is preferable to consider
the interaction of obstacle vehicles during prediction. Furthermore, the relation to
another vehicle in the future depends on a rough estimate of the ego vehicle velocity
and the correctness of the impeding vehicles prediction is thus subject to high uncer-
tainty. To cover this kind of problem a more advanced approach is necessary, most
likely requiring a description in probabilistic fashion. Still, for most of the scenarios
encountered in dynamic traffic the presented impeding vehicles prediction provides
accurate results and definitively meets the requirements within the scope of maneuver
trajectory generation.
Extraction of Ego Maneuver Position Bounds
From the detected lead and tail vehicles on each lane the resulting consequences for
the ego vehicle are derived. This includes the consideration of the desired ego vehicle
behavior determined during the stage of decision making.
To break the variety of possible situations down into a generalized description, the
relevant information for the ego vehicle with respect to the current and desired target
lane is merged into one representation. This is achieved by means of a lower position
bound, an upper position bound and a lane change initiating vehicle position. The
latter serves the purpose of providing an indication for the lateral motion of the ego
vehicle maneuver and adds relevant information from the current ego lane, whereas
the lower and upper position bound refer to the target lane. Generally speaking, based
on the desired ego maneuver and the previously determined predicted impeding
vehicles, up to three relevant obstacle vehicles are extracted. In case of a lane change
two vehicles on the target lane, which define the merge-in gap and the lead vehicle on
the ego lane are considered. In lane keeping case the number of considered obstacles
reduces to the lead vehicle on the ego lane. To get the ego maneuver position bounds
the relevant impeding vehicles are selected based on the desired ego vehicle behavior
33
vehicle index
Chapter 4. Environment-aware Maneuver Planning
(i.e. the ego vehicle target lane) and matched to the corresponding position of the
respective obstacle vehicle. This means that for every predicted time step the identified
predicted impeding vehicles position is extracted. Additionally, in lane change state,
the lane change initiating vehicle is of importance as it defines the position in time at
which the ego vehicle should reach the center of the target lane. The latter particularly
addresses the requirement to prevent a collision with the lead vehicle on the initial
ego lane during lane change. For the considered example the ego maneuver position
bounds are shown in Figure 4.3. As the ego vehicle intends to perform a lane change
200
upper position bound
150
lane change initiating vehicle position
100
50
lower position bound
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure 4.3.: Merged relevant information for the ego vehicle. The lower position bound arising
from obstacle vehicle ID:3 on the target lane, the upper position bound arising from obstacle
vehicle ID:4 on the target lane and the lane change initiating vehicle position arising from
obstacle vehicle ID:1 on the ego lane.
to the left the relevant impeding vehicles on the target lane provide the upper (obstacle
vehicle ID:4) and lower position bound (obstacle vehicle ID:3). The lead vehicle on the
ego lane determines the position at which the lane change should be finished (lane
change initiating vehicle position given by obstacle vehicle ID:1).
The maneuver planning approach also covers the case of overtaking by applying the
simplified assumption that an overtaking maneuver has to be performed if oncoming
traffic on the target lane is detected. It should be noted that in this thesis the case
of planning a complete overtaking maneuver as such is considered as a special case,
since in general it demands different requirement specifications2 like e.g. a longer
planning horizon (also cmp. section 3.2). Still, some ideas how to deal with this type
of maneuver are given. This mainly relates to the definition of the maximum pass
time for longitudinal and the target lane time zone for lateral maneuver planning (see
section 4.3).
The maximum pass time is particularly defined for the case of overtaking and affects
the time at which the lower position bound becomes active. It is considered to be
the point in time when the ego lead vehicle position and target lane lead vehicle
position intersect. If no intersection is discovered the maximum pass time is set to
2A change in the requirements with respect to the scope of the trajectory planning approach will also
affect the design of the functional architecture.
34
C px [m]
4.2. Longitudinal Maneuver Planning
be the prediction horizon of the maneuver planning approach. From the variety of
possibilities only one relevant case with respect to overtaking is considered during
maneuver planning. In this case the distance of the approaching target lane lead
vehicle is larger than the distance to the ego lane lead vehicle. Then the lane change
initiating vehicle position is set to be the ego lead vehicle position. The lower position
bound is given by the ego lead vehicle position activated after the maximum pass time.
With the assumption that the overtaking maneuver is completed before the maximum
pass time, an upper position bound is not necessary. Table 4.2 gives a summary of
the relation between the ego maneuver relevant position bounds and the respective
impeding vehicle.
Table 4.2.: Summary of ego position bounds with respect to the intended ego maneuver and
driving direction on the target lane.
oncoming traffic ego vehicle upper lower lane change
on target lane maneuver position bound position bound initiating vehicle
ego lane ego lane
lane keeping -
no lead vehicle tail vehicle
target lane target lane ego lane
lane change
lead vehicle tail vehicle lead vehicle
ego lane ego lane
yes overtaking -
lead vehicle lead vehicle
4.2. Longitudinal Maneuver Planning
After the impeding vehicles and the respective ego maneuver position bounds have
been identified a representative longitudinal prototype trajectory as well as a range
of validity are derived in correspondence to the desired ego maneuver. To plan the
longitudinal maneuver trajectory all states are given in curvilinear coordinates and
thus describe the progress of the ego vehicle and other obstacle vehicles along the
road. For a given behavior it is now of interest to determine the longitudinal target
region (i.e. spatial gap between the lead and tail vehicle on the target lane) and the
desired longitudinal position for the ego vehicle over time.
To determine the longitudinal target region three aspects are taken into account: lim-
its arising from the ego vehicle system dynamics considered in terms of drivability
(drivable region), spatial limitations arising from other surrounding traffic participants
(admissible region) and the consideration of a safety distance to the vehicles on the
proposed target lane (unimpeded region). In a subsequent step the ego demands in
terms of a desired reference velocity are taken into account for longitudinal reference
generation. The process consists of three steps: calculation of a reference position in or-
der to reach the desired velocity (reference states generation), calculation of a reference
position constrained by other traffic participants (bounded reference states generation)
35
Chapter 4. Environment-aware Maneuver Planning
and the calculation of an optimal reference position based on a least squares formula-
tion subject to kinematic constraints (optimal reference states generation).
Drivable Region
The drivable region corresponds to the reachable space, that is identified by assuming
maximum acceleration and deceleration of the ego vehicle, respectively. Furthermore,
the drivable region is bounded by the maximum and minimum velocity. It is intended
to cover a coarse description of the ego vehicle dynamics regardless of constraints
arising from the current surrounding environment. To account for the torque-speed
characteristic (that is proportional to the acceleration-velocity characteristic) of the
vehicle the maximum acceleration is described as a function of the current velocity
(cmp. Figure 7.2). The maximum deceleration remains at constant −9 m/s2 for positive
velocities. For the considered example the drivable region is depicted in Figure 4.4.
The lower region bound corresponds to a full braking maneuver bringing the vehicle
to a standstill, whereas the upper region bound reflects the position that is reached
with maximum acceleration.
Admissible Region
The admissible region includes the constraints arising from the dynamic environment.
It hence limits the space accounting for obstacles and a desired longitudinal stop
position, respectively. Figure 4.4 shows how the admissible region is derived. Therefore,
the drivable region is narrowed by the maneuver relevant lead and tail vehicle position.
For the given example the drivable region is only constrained by the lower position
bound as the upper position bound is larger than the upper bound of the drivable
region.
200
admissible region
150 upper position bound
100
50
drivable region
0 lower position bound
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure 4.4.: The drivable region and the upper (derived from the lead vehicle on the target
lane) and lower position bound (derived from the tail vehicle on the target lane) constitute to
the admissible region. Note that the drivable region fully contains the admissible region.
36
C px [m]
4.2. Longitudinal Maneuver Planning
Unimpeded Region
As it might be desirable to maintain a certain safe distance to other vehicles, the
unimpeded region marks the area in which the ego vehicle can drive freely, without
being disturbed by other traffic participants. Hence, the unimpeded region further
narrows the admissible region as a safety distance to the respective lead and tail
vehicle on the target lane is taken into account. Safety distances are derived in ACC
fashion using a Constant Time Gap Policy (see e.g. Swaroop and Rajagopal (2001)) to
describe the safety distance to the lead and tail vehicle. A lead and tail vehicle safe
position are introduced, which represent the respective position bound for the ego
vehicle arising from the desired safety margin to the lead and tail vehicle, respectively.
For the considered test case the result is depicted in Figure 4.5.
200
admissible region
150
100 lead vehicle safe position
unimpeded region
50
0 tail vehicle safe position
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure 4.5.: The admissible region and the lead and tail vehicle safe position, which account
for the desired safety margin to the lead and tail vehicle, constitute to the unimpeded region.
Reference States Generation
Yet, the available space has been defined and vehicle limits, limits from the dynamic
environment and maneuver demands are already considered. In a first step the desired
ego vehicle behavior is defined without consideration of the other traffic participants,
but only acting in self-interested sense. The reference states are calculated by intending
to reach the set speed. A predefined comfortable acceleration is used and by integration
the corresponding position is obtained. Then the procedure is as follow:
• Identify the desired acceleration by checking if the ego vehicle has to acceler-
ate or break to reach the set speed. Apply a predefined comfortable value for
acceleration ă+x and deceleration ă−x , respectively.
• Integrate the constant desired acceleration to calculate the reference velocity.
Limit the desired acceleration in dependence on the velocity to take the vehicle
limits into account (cmp. Figure 7.2).
37
C px [m]
Chapter 4. Environment-aware Maneuver Planning
• Bound the reference velocity to the desired set speed and the vehicle’s speed
limit.
• Integrate the reference velocity to obtain the corresponding reference position.
The reference states generation hence provides a reference longitudinal position and a
reference velocity.
Bounded Reference States Generation
In a second step limitations from the system dynamics and the environment are
considered. As depicted in Figure 4.6 the reference position is therefore constrained by
the unimpeded region (system limits and safety distance to predicted obstacle vehicles
position) yielding a bounded reference position. Moreover, the bounded reference
states generation provides the bounded reference velocity (see Figure 4.7), which is
derived from the bounded reference position by taking the difference quotient. The
bounded reference velocity is then limited in a way that it prevents the ego vehicle
from driving faster than the reference velocity or driving backwards. It is furthermore
200
reference
150
bounded reference
100
unimpeded region
50
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure 4.6.: The reference position is obtained by integration with the aim to reach the set
speed with a desired comfortable acceleration. The reference position is then bounded by the
unimpeded region yielding the bounded reference position.
assumed that the prevailing speed limit has already been considered in the stage of
set speed determination.
Optimal Reference States Generation
In the third and final step the optimal reference position is calculated by formulating
a constrained least squares problem, in which a least squares fit on the bounded refer-
ence velocity is performed. The solution is obtained by solving a quadratic program-
ming problem subject to linear constraints on the position, velocity and acceleration.
38
C px [m]
4.3. Lateral Maneuver Planning
The problem with respect to the decision variables vector p̂ reads:
min 0.5p̂T Qp̂     (4.2.1a)p̂  vdes   Alsqp A  s.t.  min  ≤  pos  p̂ ≤ 
vdes
pmax  . (4.2.1b)
vmin   A  vel vmax 
amin Aacc amax
The desired velocity vdes is the bounded reference velocity and subject to the least
squares formulation. The maximum and minimum position and velocity bounds are
derived from the admissible region, with the maximum and minimum acceleration
given by configurable parameters and set to a comfortable acceleration ă+x and a
braking limit ă−x . Matrices Q and Alsq constitute the least squares problem, whereas
matrices Apos, Avel and Aacc contain finite difference approximations of the derivatives
to account for the respective kinematic constraint. The detailed structure of the matrices
is given in section A.3.
Figure 4.7 shows the velocity obtained from the bounded reference generation (bounded
reference velocity) as well as the solution from the formulated constrained least squares
problem (the optimal reference velocity is given by differentiation of the optimal ref-
erence position). It can be seen that the ego vehicle accelerates from its initial velocity
of 100 km/h to the desired velocity of 130 km/h. Because of the slower lead vehicle on
the target lane the ego vehicle finally has to adapt to a speed of 120 km/h.
140
120 optimal reference
bounded reference
100
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure 4.7.: The optimal reference position is calculated by a least squares fit on the bounded
reference velocity subject to position constraints (from admissible region), velocity constraints
(from drivable region) and acceleration constraints (comfort acceleration/deceleration).
4.3. Lateral Maneuver Planning
The lateral maneuver planning is performed subsequent to the longitudinal planning
and hence is restricted to the predefined longitudinal motion. The task of lateral
maneuver planning can be seen in the generation of a corresponding lateral motion
that does not interfere with the goal of the maneuver planning. This means that the
only requirement is not to collide with other obstacles, which should be ensured by
the time the ego vehicle reaches the target lane. This could be generalized to a time
39
Cv [km/h]
Chapter 4. Environment-aware Maneuver Planning
zone in which the ego vehicle is supposed to drive on the target lane, denoted as the
target lane time zone.
Target Lane Time Zone
The target lane time zone defines the start and end point in time between which the ego
vehicle should be located on the selected target lane. This definition also generalizes to
the special case of planning a complete overtaking maneuver (cmp. Extraction of Ego
Maneuver Bounds in section 4.1). The start point is inferred from the positions obtained
by the longitudinal maneuver planning. Therefore, the ego vehicle reference position
is compared to the lane change initiating vehicle position to identify the point in time
at which the ego vehicle should be on the target lane. The point of intersection of the
bounded optimal reference position and the lane change initiating vehicle position
then marks the start time of the target lane time zone. For lane changes the end time
of the target lane time zone equals the planning horizon, whereas for the special case
of an overtaking maneuver the end time would be below (set in dependence on the
desired distance to the lane change initiating vehicle when driving back to the origin
lane).
Lateral Reference Trajectory Generation
The application of curvilinear coordinates drastically simplifies the calculation of the
respective lateral boundaries to determine the desired lateral translation. The lateral
motion is divided into three parts, in which the first one moves the ego vehicle from
the actual to the desired lateral offset derived from the target lane. The second and
third part are designed in correspondence to the target lane time zone. Hence, for lane
changes both parts adhere to the lateral position of the target lane center, whereas
for the special case of overtaking the third part moves the ego vehicle back to the
origin lane. The resulting lateral position profile is interpolated linearly in each of the
three cases. In this context it should be noted that the final maneuver decision of the
superordinated decision making module is assumed to be imminent, which excludes
delayed lane changes after an earlier lane keeping phase for target gap alignment.
4.4. Maneuver Trajectory Generation
The maneuver trajectory is finally composed of the longitudinal (corresponds to the
optimal reference position) and lateral reference trajectories, also fusing the informa-
tion about the respective target regions. Figure 4.8 shows the result obtained for the
considered example.
The coupling between lateral and longitudinal planning is accomplished by the lateral
target lane time zone, which permits to consider only the lead and tail vehicle on
the target lane for longitudinal planning. One problem with respect to this might be
that as the longitudinal planning determines the target lane time zone for the lateral
planning and no feedback is implemented it could lead to unfeasible low lane change
times. It is therefore assumed that the decision algorithm already roughly covers the
40
4.4. Maneuver Trajectory Generation
200
100
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
(a) Longitudinal maneuver trajectory and longitudinal target region.
6
4
2
0
−2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
(b) Lateral maneuver trajectory and lateral target region.
Figure 4.8.: Final maneuver trajectory and target region of the maneuver planning approach.
characteristic of a lane change. Finally, the subsequent trajectory planning algorithm
will take care of precise, combined lateral and longitudinal planning with respect to all
obstacles. As already mentioned in section 4.1 especially the prediction of the future
development of a situation is dependent on many impact factors. This aggravates the
deterministic representation and requires a probabilistic description of the evolution of
the situation to improve on the maneuver planning output. A possible alternative that
brings maneuver planning to a wider scope is given by Schmidt et al. (2019b). In the
work at hand the purpose of the maneuver planning approach is the derivation of an
initial trajectory3 as well as the definition of a sampling region. For this purpose it can
be concluded that the developed environment-aware maneuver planning approach is
entirely suitable.
3Especially for the task of initialization it is mostly important to find the correct homotopy class.
41
C py [m] C px [m]
5
Modeling and Objectives for Trajectory
Planning
For trajectory planning in the context of automated driving several aspects have to be
taken into account. Key properties encompass the consideration of system dynamics
and the surrounding environment. In this chapter the modeling of the ego vehicle
motion and the modeling of the environment are introduced in detail. Moreover, this
chapter will elaborate on the trajectory planning objectives.
5.1. Modeling of Vehicle Motion
To describe vehicle dynamics there exist a lot of various options. In previous work
(Götte et al. 2016a; Götte et al. 2016b) the suitability of vehicle models with respect
to emergency steering maneuvers has been analyzed. In this work the focus is on
normal driving situations, which poses different requirements to the modeling of
lateral and longitudinal vehicle dynamics. It is assumed that the complex vehicle
dynamics is addressed by the low level controller. Nevertheless, it is essential that
the trajectory planning approach provides a roughly drivable trajectory, meaning that
vehicle constraints are thoroughly considered.
5.1.1. System Modeling
The driving characteristics of a vehicle are typically split in longitudinal, vertical and
lateral dynamics. In order to derive an adequate theoretic vehicle model for trajec-
tory planning especially the longitudinal and lateral dynamics have to be considered.
Whereas longitudinal dynamics can be captured on kinematic level with sufficient
quality, the lateral dynamics, which play an important role in cornering, can be mod-
eled at different levels of complexity rigorously affecting the overall performance of
the vehicle model. The modeling of lateral dynamics involves the challenging aspect
of capturing the non-holonomic constraints of the vehicle. As the description of lateral
dynamics is crucial they will be introduced in more detail.
42
5.1. Modeling of Vehicle Motion
RF /Ry Fz la
linear lh lv
region E py
Fv δr
c RF α Rv,h x,h Jz, m β v vv
αh ψ
R
RF Rv E p Fx R y,vy,h h Fx,v
α lc
(a) Lateral tire force characteristic. (b) Illustration of the single track model.
Figure 5.1.: Major components for the modeling of vehicle dynamics.
Lateral Tire Forces
The interaction between tire and roadway is an important aspect to be considered
in the modeling of vehicle dynamics, since the contact between vehicle and roadway
represents the primary possibility to actively affect the motion of the vehicle. For the
purpose of vehicle modeling the complexity varies from simple linear to complex
nonlinear models. To generate a lateral force a tire slip angle α is necessary, which
results if the tire’s direction of motion is different to the rolling direction. In general
the tires of the vehicle follow a nonlinear characteristic, but in dependence on the
application simplifying assumption can be made. For normal driving situations it is
reasonable to use a linear tire model that well describes the vehicle behavior in the
linear area of the tire characteristics (see Figure 5.1a).
In the linear model the lateral tire force is proportional to the tire slip angle. With the
assumption of small tire slip angles |α| < 4° (see Meywerk (2015, p. 180)) the linear
tire forces at the front and rear axle read:
RFy,v = cvαv , (5.1.1)
RFy,h = chαh . (5.1.2)
For more insight in the world of tire modeling providing a comprehensive description
of the lateral and longitudinal tire behavior, especially for tire models covering the
nonlinear region, see Pacejka (2007) and Schramm et al. (2018).
Equations of Motion of the Single Track Model
The single track model enables comprehensive analysis of the vehicle motion and
allows to draw elementary conclusions on the lateral vehicle dynamics. Since it offers
an excellent compromise between modeling effort and model accuracy, its use is widely
spread among several application areas.
To derive the equations of motion for the single track model it is basically assumed
that the vehicle’s center of gravity lies in the plane on which the vehicle is traveling
along the trajectory, e.g. road level. By this the wheel load transfer is neglected and
the front and rear wheels are consolidated to one single wheel at the front and rear
43
Chapter 5. Modeling and Objectives for Trajectory Planning
axle respectively. Figure 5.1b shows a schematic sketch of the single track model.
The vehicle mass m and the moment of inertia around the vertical vehicle axis Jz
are combined in the vehicle’s center of gravity. The geometric parameters lv and lh
describe the distances from the center of gravity to the front and rear axle, the inter-axle
distance is given by la. The single track model is modeled with front steering, where
the front wheel can be turned by δr. Furthermore, tire slip angles αv and αh, side slip
angle β and yaw angle ψ are depicted.
Note that beside the translational part the tire and vehicle coordinate system primarily
differ at the front wheel, since this is rotated by the steering angle δr. For the following
considerations the latter permits to omit the notation of coordinate systems for the
sake of readability. In the same line, where appropriate, time arguments are omitted
in this chapter as well. The equations of motion for the single track model read:
v2
m sin(β)−mv̇ cos(β) + Fx,h − Fa,x + Fx,v cos(δr)− Fy,v sin(δr) = 0 , (5.1.3)ρcc
v2
m cos(β) + mv̇ sin(β)− Fy,h + Fa,y − Fx,v sin(δr)− Fy,v cos(δr) = 0 , (5.1.4)ρcc
Jzψ̈− (Fy,v cos(δr) + Fx,v sin(δr))lv + Fy,hlh + Fa,ylc = 0 (5.1.5)
with the radius of curvature ρcc, aerodynamic drag forces Fa,x and Fa,y and the distance
lc between the center of gravity and the center of pressure.
Relation of Angles
It is obvious that the velocity components along the longitudinal vehicle axis have to
be equal, as the vehicle does not elongate:
v cos(β) = vh cos(αh) , (5.1.6)
v cos(β) = vv cos(δr − αv) , (5.1.7)
whereas the lateral velocity components deviate by the yaw rate:
vh sin(αh) = lhψ̇− v sin(β) , (5.1.8)
vv sin(δr − αv) = lvψ̇ + v sin(β) . (5.1.9)
This leads to the following equations:
lhψ̇− v sin(β)tan(αh) = , (5.1.10)v cos(β)
− lvψ̇ + v sin(β)tan(δr αv) = , (5.1.11)v cos(β)
which can further be simplified and rearranged to get the tire slip angles α at the front
and rear tire respectively:
αv ≈ δr −
ψ̇
β− lv , (5.1.12)v
≈ − ψ̇αh β + lh . (5.1.13)v
44
5.1. Modeling of Vehicle Motion
Circle of Curvature
The circle of curvature locally approximates the vehicle trajectory at one point at which
the velocity v is a tangent to the vehicle trajectory. The direction of velocity v changes
with the course rate λ̇ = β̇ + ψ̇. Hence it yields:
v = ρcc(β̇ + ψ̇) . (5.1.14)
The lateral (i.e. centripetal) acceleration for a vehicle that moves with speed v along a
circular path is with the assumption of a small side slip angle β given by:
ay = v2/ρcc . (5.1.15)
It should be noted that in general the vehicle rotates around the instantaneous center
of rotation pcr. The circle of curvature locally approximates the motion of the center of
gravity. For now this description is sufficient as in the relevant case of circular driving
at a constant speed the instantaneous center of rotation pcr and the center of curvature
pcc coincide (Meywerk 2015; Mitschke and Wallentowitz 2014).
Circular Driving at a Constant Speed
Because of the steady state indicating constant yaw rate and side slip it is:
β̇ = 0 , (5.1.16)
ψ̈ = 0 . (5.1.17)
The tire slip angle difference αv − αh can be calculated via equations 5.1.1, 5.1.2 and
l v2
Fy,v =
h m , (5.1.18)
la ρcc
lv v2Fy,h = m , (5.1.19)la ρcc
which finally yields:
2
αv −
chlh − cvlm v vαh = . (5.1.20)cvchla ρcc
In the stationary case (β̇ = 0 in equation 5.1.14) and with equations 5.1.12, 5.1.13 and
equation 5.1.20 the steering angle δr can be expressed by:
l c l − c l v2a v v
δr = + m
h h . (5.1.21)
ρcc cvchla ρcc
At low velocities the so called Ackermann steer angle δA is:
l
δA = lim
a
δr = . (5.1.22)
v→0 ρcc
45
Chapter 5. Modeling and Objectives for Trajectory Planning
The relation between steering angle δr and yaw rate ψ̇ at steady state can be derived
from equation 5.1.14 and equation 5.1.21:
v
ψ̇ = δr (5.1.23)la(1 + (v/v )2ch ))
in which vch is the characteristic velo√city
l2acvcv = hch . (5.1.24)m(chlh − chlv)
For further reading see Mitschke and Wallentowitz (2014) and Schramm et al. (2018).
Vehicle Model with Steady State Yaw Dynamics of a Linear Single Track Model
With regard to the application of trajectory planning for normal driving situations a
vehicle model with steady state yaw dynamics of a linear single track model is chosen
as the best compromise between computational effort and accuracy.
The applied vehicle model reads:
ṗx = v cos(ψ) , (5.1.25a)
ṗy = v sin(ψ) , (5.1.25b)
v
ψ̇ = δ
l 1 v/v 2 r
, (5.1.25c)
a( + ( ch) )
[ ] v̇ = a (5.1.25d)
with states x = px py v
T
ψ and control inputs u = [a δ ]Tr .
Note that because of the steady state assumption the course rate λ̇ equals the yaw rate
ψ̇ and hence can be used interchangeably. Furthermore, as will be shown in the next
subsection, the trajectory planning approach benefits from the chosen vehicle model as
it is differentially flat, facilitating the integration of system dynamics into the trajectory
planning approach.
Limitations Arising from the Choice of Vehicle Model
From the derivation it is obvious that the validity of the chosen vehicle model is
restricted to a certain field of application. Consequently, neither the behavior for larger
steering angles nor for higher lateral accelerations is covered. The scope of application
can be inferred from the way the lateral tire forces are handled, as the tire force
characteristics can in general be linearized up to one third of the maximum lateral tire
force Fy,max. This yields a lateral acceleration of
v2 ≤ 1µt , (5.1.26)
ρccag 3
which is equivalent to the definition of a normal driving situation in chapter A.1. On
a dry road surface a vehicle lateral acceleration of 0.4ag marks the range of validity for
the consideration of the linearized single track model.
46
5.1. Modeling of Vehicle Motion
From the construction design of German roads stated in RAS-L (1995) the minimum
curve radius ρmin is given in dependence on the velocity v, the exploitation of the
maximum radial adhesion coefficient ς and the cross slope ξ (see Table 5.1).
Table 5.1.: Rounded minimum curve radius as defined in RAS-L (1995) for road construction.
The calculations to derive the values can be found in chapter A.6.
v [km/h] 50 60 70 80 90 100 120
for ξ = 7 %, ς = 50 % 80 120 180 250 340 450 720
ρmin [m] for ξ = 2.5 %, ς = 10 % 320 490 700 980 1400 1700 2700
Via equation 5.1.15, it can hence be inferred that from the constructional point of view
the lateral acceleration is below the admissible limit for the validity of the model.
Although it should be noted that the lateral acceleration caused by a lane change has
to be taken into account on top of this considerations, it can be concluded that the
derived model is valid for normal driving situations and thus fulfills the requirement.
5.1.2. Control Input Calculation
To calculate the control input values from the trajectory (i.e. solve the inverse dynamics
problem) the property of flatness can be exploited. Flatness was first defined by Fliess
et al. (1992) and Fliess et al. (1995) and generalizes the basic idea of controllability from
linear systems to nonlinear system dynamics1. Hence, systems of that kind represent an
important subclass of nonlinear control systems denoted as differentially flat systems
or just flat systems, providing a unique relation between trajectories in the flat output
space Z and the state space X and control input space U.
A nonlinear system ẋ = f(x, u) is flat if the states x ∈ X and control inputs u ∈ U can
be expressed by a new variable z ∈ Z and a finite number of its derivatives:
[ ]z = Φ(x, u, u̇, . . . , u(ru)) , ru ≥ 1 , (5.1.27)
x
= Ψ(z, ż, . . . , z(rz)) , rz ≥ 1 , (5.1.28)u
where variable z is denoted as flat output. The property of differential flatness enables
efficient analytic calculation of the control inputs of a trajectory represented in the flat
output space. The advantages of differentially flatness can beneficially be combined
with a representation of the flat output variables expressed as a sequence of differen-
tiable trajectories. This is because states and inputs of a flat system can be mapped
algebraically to the flat output variables and their derivatives and vice versa. To obtain
the control inputs this means that the derivatives of the flat output have to be calcu-
lated, which promotes the choice of a differentiable flat output representation. On that
1E.g. all linear, controllable systems are flat. In fact it can be shown that any system that can be trans-
formed into a linear system by changes of coordinates or static or dynamic feedback transformations
is also flat (Martin et al. 2003).
47
Chapter 5. Modeling and Objectives for Trajectory Planning
account chapter 6 is dedicated to the choice of a spline-based trajectory representation,
giving details with respect to the general concept and the interpolation strategy.
The following calculations to determine the necessary algebraic functions are similar
to Schlechtriemen et al. (2016), but differ in the important aspect of lateral vehicle
dynamics modeling. In particular, the difference lies in the description of the yaw
dynamics as Schlechtriemen et al. (2016) assume Ackermann steering, which is only
valid at very low speeds. For more details in that respect it is referred to chapter A.5 in
which the corresponding equations for the kinematic vehicle model with Ackermann
steering are given.
In this work a vehicle model with steady state yaw dynamics of a linear single track
model as given in equation 5.1.25 is used. This vehicle model is differentially flat with
respect to the outputs px and py. The flat transformation which maps the states x and
control input u to the flat output var[iable]s zx[and z]y is then given by:
x zx
 Φ : 7→ ,u zy        zx,1   px   px   px z x,2
ṗx vx cos(ψ)v 
 z x,3   p̈x   ax   cos(ψ)a− sin(ψ)ψ̇v z 
=  =py    =py  py 

 . (5.1.29)y,1zy,2 ṗy vy sin(ψ)v
zy,3 p̈y ay sin(ψ)a + cos(ψ)ψ̇v
In accordance to the applied vehicle model as given in equation 5.1.25 the inverse
transform to calculate the control in[puts y]ields[: ]
zx x
Ψ : 7→ ,
 zy u   
px
 
zx,1
 py 
   z

y,1 
 v 
 zx,2 
 = 

 ψ  
cos(arctan(zy,2/zx,2))  . (5.1.30)
arctan(zy,2/zx,2)) 
 a   zx,3 + sin(ψ)v zy,3−tan(ψ)zx,3 cos (ψ) cos(ψ) tan(ψ) sin(ψ)v+cos(ψ)v 
zy,3−tan(ψ)zδ x,3 l (1+(v/v )
2)
r a chtan(ψ) sin(ψ)v+cos(ψ)v v
Finally, this means that the control inputs can be derived analytically from the output
trajectory. For further details with respect to the derivation of the equations for the
inverse flat transform it is referred to chapter A.7.
5.2. Modeling of the Environment
For comfortable and safe driving knowledge about the surrounding environment is es-
sential. The transition from perceived sensor data to trajectory planning is performed
48
5.2. Modeling of the Environment
by generating an environment model. The modeling of the environment is hence cru-
cial for the performance of the trajectory planning approach. The chosen environment
model and chosen solution approach to trajectory planning go hand in hand, tuned
to match the requirements from numerical perspective as well as with respect to the
overall performance. In the following an overview on environment modeling for col-
lision avoidance in trajectory planning is given. The choice and performance of the
environment model is dependent on the provided data from perception, as well as
the situation analysis module. In most cases dynamic obstacles like other vehicles, or
moving pedestrians are provided as an dynamic object list. The current as well as the
predicted states of each detected traffic participant are given, mainly focusing on the
relevant quantities like position and velocity. Lane boundaries on the other hand can
be regarded as a kind of static obstacles, which among other static objects contribute
to the derivation of a description of a drivable free space. For trajectory planning in
particular it is important to take a rather accurate environment model into account
that simultaneously does not complicate the overall process of trajectory generation.
In dependence on the respective category of the trajectory planning method, some
approaches even limit the number of usable environment models, since they require
the presence of certain features.
One possibility to represent the environment is the use of line models. Such kind of
description is especially valid for static environments, still there are some approaches
that show some kind of consolidated models. Ziegler et al. (2014) for example choose
to model the environment as polygons. Therefore, the static environment is imposed by
the driving corridor, which is derived as a sequence of lane segments. Static objects are
incorporated by building constraint polygons from sensor data. Likewise the motion
of moving objects is predicted into the future to form a polygon for every discrete time
interval. To incorporate obstacle vehicles into the description of lane boundaries Nils-
son et al. (2015) adjust the forward collision avoidance constraint and the rear collision
avoidance constraint in dependence on the driving lane of the obstacle. A prevalent
holistic representation of the environment is the use of occupancy grids, which are
introduced by Elfes (1989). In general 2D occupancy grids are designed to model the
static environment. Nevertheless, several subsequent ideas deal with the development
of an extension for dynamic objects. Tanzmeister and Wollherr (2017) for example, pro-
pose a grid-based tracking and mapping algorithm that simultaneously estimates the
static and the dynamic features of the environment. With regard to trajectory planning
it should be mentioned that occupancy grids rely on their representation as discrete
cells with fixed sizes, which leads to a discontinuous description that is e.g. not ap-
propriate for continuous optimization approaches. For this reason they can only be
used for sampling based trajectory planning approaches like presented by Mouhagir
et al. (2016). In the same line as occupancy grids, a potential field is another common
way to represent the environment. The basic idea can be summarized to be similar
to a hazard map in which a high potential denotes a high risk and vice versa. Hence,
high and low potentials represent non negotiable and negotiable areas, respectively.
Bauer et al. (2012) introduce a potential field within an approach to collision avoid-
ance, which is composed of a static and a dynamic potential part. Wang et al. (2015)
present a comprehensive version of a potential field, denoted as driving safety field.
49
Chapter 5. Modeling and Objectives for Trajectory Planning
It is divided in a potential field, a kinetic field and a behavior field, intended to take
the static and dynamic environment as well as the (estimated) driver behavior into
account. Brandt et al. (2007) introduce a potential field hazard map for lane keeping
and collision avoidance.
The two essential, if not even the most important parts in the context of environment
modeling for trajectory planning are the aspects of collision checking and distance
calculation. It is obvious that the choice of a representative model has a direct impact
on the choice of a suitable collision detection and distance calculation approach and
vice versa. In particular for the modeling of lane boundaries the shortest distance is of
high interest. Dynamic obstacle modeling on the other hand is mainly concerned with
finding an accurate geometric shape that is moreover suitable for applying fast collision
checks. For some general remarks with respect to the topic of collision detection it
is referred to Jimenez et al. (2006). In the following modeling and collision checking
strategies for the relevant environment components will be discussed in more detail.
Modeling of Lane Boundaries and Static Collision Checking
A very important aspect with respect to automated driving in structured environments
is the choice of a suitable lane boundary representation, which will affect the accuracy
and the computational burden (which explicitly includes memory consumption) with
respect to vital functionality. One major aspect in that regard is the determination of
the shortest distance from the ego vehicle to the lane boundaries, which is e.g. crucial
for lane boundary collision checking as well as for curvilinear transformations.
A polynomial description of lane markers is beneficial in terms of memory consump-
tion and also allows for simple evaluation to derive essential position bounds. On the
other hand the accuracy can suffer from this kind of representation, since the approxi-
mation of the real lane boundaries is committed to the polynomial shape. Obviously,
to attain a certain quality the polynomial order is of importance. An analytic solution
to the shortest distance calculation problem that calculates the shortest distance from
a point to the lane boundary polynomial is non-existent for a polynomial order higher
than two. Nevertheless, numerical calculations can be done to get approximate results.
For further reading see e.g. Willemsen et al. (2003).
Polygonal chains on the other hand are disadvantageously in the point of memory
consumption, but are beneficial in accurately fitting the lane marker and fast shortest
distance calculation. For these reasons the polyline description is chosen in this work.
To account for discontinuities at the intersection of consecutive polyline segments a
pseudo distance transform can be applied. Finally, this allows to derive a fast solution
to the problem of shortest distance calculation. For details with respect to the calcula-
tions see Bender et al. (2014). In this work static collision checking to detect a collision
with road boundaries, is performed by calculating the closest distance between the ego
vehicle’s bounding box and the respective lane boundary. The road model containing
the respective lane markers considers at most three lanes: the ego lane and if present
the left and/or right neighboring lane. Figure 5.2 shows the utilized road model with
lane markers represented by polygonal chains. Lane markers are labeled in vehicle
coordinate frame from the left to the right, starting with lane marker index ` = 0 for
50
5.2. Modeling of the Environment
` = 0
lane 1
` = 1
lane 2
` = 2
lane 3
` = 3
Figure 5.2.: Applied road model for static collision checking.
the left lane marker of the left lane 1, index ` = 1 for the left and index ` = 2 for the
right lane marker of center lane 2, as well as index ` = 3 for the right lane marker of
the right neighboring lane 3.
Modeling of Dynamic Obstacles and Dynamic Collision Checking
In order to check for collision avoidance, an accurate geometric modeling of the vehicle
shape is essential. Therefore two main representations can be identified that build up
on basic geometric shapes. Namely the vehicle shape is modeled with either rectangles
or by circles. The aspect of accuracy with respect to both kinds of vehicle shape mod-
eling is discussed controversially, since a suitable arrangement with multiple circles
will lead to a good coverage of the vehicle shape. However, it seems more intuitive to
model the spatial dimensions of vehicle by means of a rectangular bounding box.
In general, to account for the dynamic character of the environment collision checking
has to be performed in space-time domain. One approach in that regard is to check
each ego candidate trajectory point-wise against all obstacle trajectories and repeatedly
apply a static interference test. This is hence denoted as multiple interference test. It
should be noted that the reliability of the performed collision checks is hence subject
to the resolution of the trajectory, since with a too coarse sampling occurring collisions
might be missed. In general to reduce run time with respect to the elaborate task of
collision checking, strategies like Hierarchical Pruning (Ferguson et al. 2008a) or the
Bounding Volume Hierarchy (Schwesinger et al. 2015) have been applied to improve
upon the efficiency.
Typically, in dependence on the choice of geometric modeling, several characteristics
are exploited to perform collision detection. The decomposition of the vehicle shape
into rotation invariant primitives using multiple circles is for example shown by Ziegler
and Stiller (2010). With this kind of approximation collision detection can be performed
via distance calculations and comparatively simple threshold evaluations. Therefore, at
each time instance the distances between the ego vehicle position and each center of the
approximating circles is checked whether it is below the radius of the respective circle.
The Separating Axis Theorem, which is accurate for convex shapes can be utilized to
check for intersections between two rectangles in time. To detect a collision a projection
onto the aligning axis of the ego vehicle and the respective object in consideration
is performed. The two objects are not colliding, if there exists a line onto which
51
Chapter 5. Modeling and Objectives for Trajectory Planning
the projections of the ego vehicle and the object do not overlap. The Separating Axis
Theorem is a pure collision detection approach and does not provide any information
with respect to the actual distance between two objects. The Gilbert-Johnson-Keerthi
Algorithm (Gilbert et al. 1988) for collision detection on the other hand computes
the distance between two convex shapes. It works with an implicit representation
of convex shapes through support functions, Minkowski sums and simplexes. If the
Minkowski difference contains the origin of the space, the shapes intersect. Otherwise
the minimum distance between the origin and the Minkowski difference represents
the distance between the two objects.
In this work a circular approximation is chosen likewise the one presented by Ziegler
and Stiller (2010). Then the radius ρ̊ of each approximating circle and the distance b̊
between two consecutive circle√centers is given by:√
l2 w2
ρ̊ = 0.5 2 + w
2 , b̊ = 2 ρ̊2 − . (5.2.1)
η̊ 4
Each vehicle (i.e. the ego and obstacle vehicles) is approximated by η̊ circles, as illus-
trated in Figure 5.3. A vehicle can hence be approximated by for example η̊ = 3 circles
l/η̊
1p̊ 2p̊ 3p̊
w . . . ρ̊
b̊
l
Figure 5.3.: Circle approximation of the vehiclel shape.
with circle center positions 1p̊, 2p̊ and 3p̊, placed in relation to the vehicle’s center. As
can be seen this setup adds a certain margin to the vehicle shape. Obviously, a higher
number of approximating circles leads to a more accurate approximation of the vehicle
shape, but increases the calculation effort for collision checking at the same time.
5.3. Trajectory Planning Objectives
When designing the overall cost function each objective has to be chosen thoughtfully.
One reason for this is that the cost function must consider both safety and comfort at
the same time, while the aggregated objectives should comply to the idea of mitigating
the effect of mutual interference. The main challenge is to find a set of objectives that
describe the desired behavior of the ego vehicle in a general way, but simultaneously
promote the optimal behavior for each specific traffic situation as well. It is hence
necessary to carefully balance each objective against each other, since it might some-
times be better to increase or decrease some quantity of interest in dependence on the
current situation. This issue is particularly addressed in the overall trajectory planning
52
5.3. Trajectory Planning Objectives
concept by proposing the two-level hierarchical trajectory optimization framework as
presented in section 3.3. Still, to derive an explicit solution to the trajectory planning
problem distinctive features have to be identified. Unfortunately, the definition of op-
timality in the context of automated driving is not unique and can be expressed in
a range of various instances of a maneuver that would lead to the desired outcome.
However, some general purpose objectives can be defined.
Distance Keeping
A very basic feature that should be captured by the trajectory planning approach is
the objective of distance keeping to other obstacle vehicles. In contrast to collision
avoidance the safety distance that supports the overall sense of security cannot be
defined straightforward. First of all, safety distance is a subjective quantity that varies
with respect to the individual passenger as well as in regard to the current situation.
For a consistent driving style it is moreover essential that the case of driving below
a certain safety distance is permitted for at least some time. The distance keeping
objective is based on the constant time gap assumption similar to adaptive cruise
control functionality. The idea is generalized in a way that lead and tail vehicle are
identified for each trajectory point and the costs are then given with respect to the
calculated distance in relation to the required time gap. Note that on the current ego
lane only the lead vehicle is considered in order to prevent from being inordinately
impacted by the tail vehicle driving to close to the ego vehicle.
Reference Speed Control
The ego vehicle should be able to travel at a desired set speed. This objective is on the
one hand intended to promote progress along the route, but also covers the aspect of
adherence to speed limits.
Reference Path Following
This objective is dedicated to the lateral guidance of the ego vehicle and intended to
provide information about the preferred driving lane. It hence incorporates knowledge
from the high level maneuver decision and is formulated as a penalty on the deviation
to a desired lateral reference path.
Comfort Driving
This objective originates from a soft-constraint formulation for the comfort lateral and
longitudinal acceleration, acting as a penalty in case of exceeding a certain comfort
acceleration threshold. This means that the ego vehicle should obey the desired com-
fortable acceleration as long as it is appropriate with respect to the current situation.
The integration into the objective function accounts for the fact that it is desired, but
not mandatory to perform motions below the comfort acceleration threshold.
53
6
Spline-based Trajectory Representation
The spline-based trajectory representation constitutes the base frame of this work. It
rigorously supports the idea of the two level hierarchical frame work as described
in section 3.3. To catch up the considerations made with respect to the trajectory
planning concept the assumption is made that a spline is capable of matching the
optimal function class. This is not a daring hypothesis, since the flexibility and the
adaptiveness of a spline are evident. Moreover, as shown in this chapter, a polynomial
representation yields the optimal solution to a minimization of kinematics without
constraints.
With regard to the idea of exploiting the property of flatness to incorporate system
dynamics into the trajectory planning approach, it is beneficial to express the flat
output variables as a sequence of differentiable trajectories such as polynomials. This
is perfectly matching to the considerations made in chapter 5.1.2, since derivatives are
needed for the respective calculations. Finally, trajectory optimization over the spline
coefficients results in a lower-dimensional and hence more efficient solution. Especially
for flat systems it is common to specify a separate spline for each of the flat output
variables of the system. Hence, in this work one time parameterized trajectory for each
flat output variable is planned, covering each dimension of the flat output space Z.
In a first step the lateral and longitudinal motion are considered independently and
finally connected via discrete sampling in each direction with fixed sampling time Ts.
For spline-based trajectory planning the trajectory is composed of several segments:
P0(t) t0 ≤ t ≤ t 1P1(t− t1) t1 ≤ t ≤ t2
S(t) = P2(t− t2) t2 ≤ t ≤ t 3 (6.0.1)...PK(t− tK) tK ≤ t ≤ tK+1
with a polynomial Pκ of degree V given as:
Pκ(t) = c 2 V−1 V0,κ + c1,κt + c2,κt + · · ·+ cV−1,κt + cV ,κt . (6.0.2)
The coefficients cι,κ with ι = 0, 1, . . . ,V for each spline segment κ = 0, 1, . . . ,K are
written in a vector cκ, which yields c = [cT, cT0 1 , . . . , c
T T
K] for the coefficients of the
54
6.1. Variational Formulation
entire spline. The link between two segments is denoted as spline breakpoint z, also
referred to as spline knot. Figure 6.1 illustrates an example in which three breakpoints
for longitudinal and four breakpoints for lateral direction have been configured.
longitudinal knot position lateral knot position trajectory
20 4
15 3
10 2
5 1
0 0
0 1 2 3 4 5 6 0 1 2 3 4 5 6
t [s] t [s]
6
4
2
0
−2
−6 −4 −2 0 2 4 6 8 10 12 14 16 18 20 22
F px [m]
Figure 6.1.: Illustration of the spline-based trajectory representation. The trajectory (bottom) is
composed of one spline for longitudinal (top left) and one for lateral direction (top right).
The continuous spline-based formulation allows the choice of resolution in time Ts
independently of the number of segments K and the fixed planning horizon Tp. Thus,
by interpolation and evaluation of the splines for longitudinal and lateral direction
K = Tp/Ts + 1 trajectory points p = [F p , Fk x,k py,k]T are gained yielding the ego vehicle
trajectory:
Pego = [p0, p1, . . . , pk, . . . , p ]TK (6.0.3)
with constant time intervals t ∈ [tk, tk+1] for k = 0, 1, . . . , K.
6.1. Variational Formulation
In cases without any constraints arising i.a. from obstacles or the vehicle dynamics,
the variational problem of trajectory planning can be solved analytically. The goal is to
find the optimal solution for position p(t) by minimizing a function L that interrelates
the unknown function p(t) to∫its derivatives:
∗ Tpp (t) = arg min L(p(t)(r), p(t)(r−1), . . . , ṗ(t), p(t), t)dt . (6.1.1)
p(t) 0
55
F p [m] Fy px [m]
F py [m]
Chapter 6. Spline-based Trajectory Representation
The calculus of variation aims at finding the extrema e.g. the stationary functions of a
functional. The functional ∫F is given as the integral of function L:Tp
F [p(t)] = L(p(t)(r), p(t)(r−1), . . . , ṗ(t), p(t), t)dt . (6.1.2)
0
The optimal solution can now be obtained via the Euler-Lagrange equation1. For an
integrand L with deriv(ative)s up to(orde)r r the Euler-LagranL L 2 L r (
ge equ
L )
ation reads:
∂ d ∂ d ∂ d ∂
︸︷︷︸− ︸ ︷︷ ︸+ 2 ︸ ︷︷ ︸− · · ·+ (−1)r = 0 . (6.1.3)∂p dt ∂ ṗ dt ∂ p̈ dtr ∂p(r)
=0 =0 =0
The solutions of equation 6.1.3 are the functions for which the fu(nctio)nalF is stationary2
and hence indicates optimality. In case of an integrand L = p(r) the first r terms
vanish to zero and it y(ields: )
− r d
r ∂L dr ( ) ( )
( 1) r = (−1)r 2p(r)r = (−1)r 2p(2r) = 0 , (6.1.4)dt ∂p(r) dt
which finally results in the following differential equation that has to be solved:
p(2r) = 0 . (6.1.5)
The solution of this differential equation, with coefficients cι is given as:
p(t) = c0 + c1t + c2t2 + · · ·+ c − t(2r−1)2r 1 . (6.1.6)
This means that for an unconstrained problem, the optimal solution with respect to
the r-th derivative of the vehicle position p(t) is given as a polynomial of (2r− 1)-th
order. Table 6.1 summarizes the relationship between trajectory optimality and the
polynomial order of the corresponding solution.
Table 6.1.: Overview of the relationship between trajectory optimality and polynomial order of
the corresponding solution.
cost functional derivative trajectory optimality polynomial order
r = 1 minimum velocity line with V = 1
r = 2 minimum acceleration cubic polynomial with V = 3
r = 3 minimum jerk quintic polynomial with V = 5
r = 4 minimum snap septic polynomial with V = 7
Note that the derivation has been shown for a one-dimensional case, but can easily
be extended to more dimensions. For example in a two-dimensional case the solution
results in a system of two independent differential equations that have to be solved.
1For the sake of readability arguments indicating time dependence are omitted in the following
derivation.
56
6.2. Distinct Analytic Spline Interpolation
The remaining problem consists of finding the coefficients for the polynomial with
respect to given constraints on the trajectory. To increase the scope of action for the
trajectory planning algorithm in terms of flexibility, instead of finding the optimal
coefficients for one single polynomial a sequence of polynomials is optimized, which
yields the spline-based representation for the trajectory.
6.2. Distinct Analytic Spline Interpolation
For a distinct analytic spline interpolation the coefficients are calculated analytically.
As can be inferred from Table 6.1 the polynomial order to minimize the respective
kinematic quantity is an odd number, which hence means that an even number of
coefficients needs to be determined. It can be found that considering the correlating
derivative start and end conditions for each respective segment can be derived yielding
the required number of equations to set up a square system of linear equations2. Since
this is done for each single spline segment this allows for a formulation in terms of
spline breakpoints.
For segments κ = 0, 1, . . . ,K and the required derivative rV = (V − 1)/2 to allow for
analytic determination of the coefficients cι,κ it is:
c V−10,κ + c1,κ t`,κ + . . . + cV−1,κ t`,κ + cV ,κ tV`,κ = p`,κ ,
c0,κ + c1,κ ta,κ + . . . + cV−1,κ tV−1a,κ + cV ,κ tVa,κ = pa,κ ,
c1,κ + . . . + (V − 1) cV− tV−21,κ `,κ + V cV ,κ tV−1`,κ = ṗ`,κ ,
c V−2 V−11,κ + . . . + (V − 1) cV−1,κ ta,κ + V cV ,κ ta,κ = ṗa,κ , (6.2.1)
V ( ) ...∑ (∏rV (ι− e)) c tι−rV (rV )ι=rV e=0 ι,κ `,κ = p`,κ ,(r )
∑V ∏rV ι−rV Vι=rV e=0(ι− e) cι,κ ta,κ = pa,κ
where start and end conditions are denoted by the subscript ` and a respectively. Note
that the specified start and end conditions coincide for connecting segments. Following
this construction the distinct analytic spline interpolation is inherently accompanied
by the following characteristics:
• The spline passes through the specified breakpoints z, which coincide with the
start and end conditions of the respective spline segments in terms of the desired
values for each derivative,
• the spline is continuous between each spline segment up to the rV -th derivative.
Solving the linear system of equations analytically for the coefficients is comparatively
simple and fast. However, it is not easy to determine the corresponding values for the
breakpoints and its derivatives (i.e. start and end conditions of each polynomial) to
result in an overall optimal behavior of the concatenated spline segments.
2In that context this means a system with an equal number of equations and unknowns.
57
Chapter 6. Spline-based Trajectory Representation
6.3. Minimum Kinematics Trajectory Generation
The term minimum kinematics accounts for the idea of calculating the coefficients with
respect to a cost function with a kinematic derivative of arbitrary order. In dependence
on the desired output it is possible to minimize acceleration as well as to generate a
minimum jerk trajectory.
Bry et al. (2015) introduce a piecewise polynomial joint optimization with the aim of
generating optimal trajectories that should pass through a sequence of given waypoints
(i.e spline breakpoints). This corresponds to an optimal spline interpolation strategy
that will be used as an underlying minimum kinematics trajectory scheme within the
developed trajectory planning concept (see section 3.3). In contrast to the trajectory
generation presented in this work Bry et al. (2015) propose an A* or RRT* algorithm
for waypoint generation to enable quadrotor flights in mostly static environments.
Schmidt et al. (2019b) use this idea and transfers it to the context of automated driving
for the calculation of longitudinal trajectories.
For the sake of flexibility and thus coinciding with optimality of the trajectory with
respect to a given traffic situation, a sequence of multiple polynomial trajectory seg-
ments is optimized. The trajectory between each pair of breakpoints is considered as
an individual polynomial trajectory segment, for which a joint optimization promises
lower costs for the resulting optimized trajectory.
6.3.1. Formulation of the Optimal Interpolation Problem
To find the op∫timal coefficients cost function f̂ for one spline segment is defined as:a
f̂ = ω P (t)2 + ω Ṗ (t)2 (V)κ 0 κ 1 κ + ω2P̈κ(t)2 + · · ·+ ωVP 2κ (t) dt (6.3.1)
t=`
in which ωr are the weights that balance the desired penalty among each derivative.
In order to define the costs for the entire spline with κ = 0, 1, . . . ,K spline segments
this can be rewritten in terms of the spline coefficients c, which yields:
f̂ (c) = cTQc . (6.3.2)
In the following the construction of the cost matrix Q is described. Furthermore, it is
shown how to impose several constraints on the spline.
Generation of the Cost Matrix
The cost matrix Q contains the desired penalty on each derivative of the spline seg-
ments. For the r-th derivative the cost matrix element of one spline segment reads:
 ı+−2r+1
ı, 2(∏r−1(ı− tε)(− ε) a,κQ  ε=0 ı+−2r+1) ı ≥ r ∧  ≥ rr κ = , (6.3.3)0 ı < r ∨  < r
where ta,κ is the spline segment duration and ı = 0, 1, . . . ,V indicates the row and
 = 0, 1, . . . ,V the column of the cost matrix. The complete cost matrix for each spline
58
6.3. Minimum Kinematics Trajectory Generation
segment κ is a weighted sum of Hessian matrices for each derivative of the polynomial:
V
Qκ = ∑ ωr rQκ . (6.3.4)
r=0
Finally, the cost matrix for the entire spline is composed as a block diagonal matrix of
matrices Qκ for each segment:   Q0 Q1Q = . . . 
 . (6.3.5)
QK
Generation of the Constraint Matrix
The constraint matrix A is built to represent the desired behavior in terms of specified
values for the position and its derivatives.
For each spline segment κ one row vector element of the constraint matrix for start
and end cond{itions denoted by the subscript `{and a respectively is constructed via:
r−1 r−1 −r
 ∏ε=0(− ε) r =   (∏ε=0(− ε))t r ≤ 
ra`,κ = , raa,κ =
a . (6.3.6)
0 r 6=  0 r > 
The column index  = 0, 1, . . . ,V thereby denotes the respective coefficient of the spline
segment. The complete row vectors ra`,κ, raa,κ for a spline segment κ correspond to a
constraint on the r-th derivative and basically map the coefficients to the derivatives
of the polynomial.
To build the constraint matrix two different types of constraints are considered. The
first type ensures adherence to specified values for the position p and its derivatives,
which is also referred to as value constraints. The second type cares about continuity
between subsequent spline segments and is henceforth termed continuity constraints.
To constrain the shape of the spline segment κ either a value constraint or a continuity
constraint is specified. For the r-th derivative it is rb`,κ for a start condition, rba,κ for an
end condition and rb 3a`,κ to enforce continuity :
(r) (r)
rb`,κ = P (0) , rba,κ = P (a) , rba`,κ = 0 . (6.3.7)
Then a value constraint on the r-th derivative at the start of the respective spline
segment reads:
raval`,κcκ = rb`,κ (6.3.8)
and an end condition is defined by:
avalr a,κ cκ = rba,κ . (6.3.9)
3Note that Bry et al. (2015) elaborate on the possibility to specify nonzero offsets by setting rba`,κ to a
value other than zero. Since clear continuity should be attained this is no option in this work.
59
Chapter 6. Spline-based Trajectory Representation
To enforce continuity between two consecutive spline segments with respect to the
r-th derivative one row o[f the constraint m]a[trix is g]iven by:
acon con cκr a,κ −ra`,κ+1 = rbc a`,κ . (6.3.10)κ+1
It should be noted that a value constraint at an intermediate segment infers a continuity
constraint by setting up a value constraint for the connecting segment as well. A
continuity constraint on the other hand is designated for segment transitions, which
are not constrained to a certain value. Finally, the overall constraint matrix A, as well
as the constraint value vector b are given by: val  A`,0 val 
 
b`,0
 A a,0 
 b 
 Acon con 
 a,0 
 a,0
−A`,1   b a`,0  Aval  `,1 Aval a,1 
  b`,1 b a,1 
Acona,1 −Acon A `,2 = . .  , b = 
 ba`,1 
..  (6.3.11)

 . 
  . 
 A
val   
`,K−1   b`,K−1 
 Aval K−   ba,K−1  a, 1 Acona,K−1 −Acon  `,K   ba`,K−1 Aval b a,K `,K
Aval ba,Ka,K
in which matrices Aval and Acon are composed in accordance to the desired behavior
of the spline.
6.3.2. Solution of the Optimal Interpolation Problem
The cost matrix Q and constraint matrix A can now be set up in accordance to the
desired interpolation behavior. It is hence determined which kinematic quantities are
minimized in order to perform the optimal interpolation with respect to the requested
reference values.
Quadratic Programming Solution
To solve the optimal interpolation problem in terms of the cost function 6.3.2 subject
to the constraints specified in equation 6.3.11 the following quadratic programming
problem has to be solved:
min cTQc (6.3.12a)
c
s.t. Ac− b = 0 . (6.3.12b)
In this case the problem is solved directly for the optimal coefficients c∗, since the
decision variables are the polynomial coefficients of all spline segments.
60
6.3. Minimum Kinematics Trajectory Generation
Analysis of the Optimal Interpolation Strategy
So far all necessary ingredients for applying the minimum kinematics trajectory gener-
ation in terms of the optimal interpolation have been introduced. To better understand
the process of matrix generation and the differences with respect to the chosen inter-
polation order an analysis is performed.
The general functioning is illustrated by an example, choosing a spline with 2 (K = 1)
segments. The constraint matrix is composed in accordance to the values specified in
Table 6.2. Moreover, continuity should hold up to the second derivative rc = 2 and a
minimum jerk trajectory should be generated, by constructing the cost matrix for the
third derivative rf = 3.
Table 6.2.: Setup for constraint generation.
spline knot z0 z1 z2
position [m] F p 0 1 8
velocity [m/s] Fv 0  
acceleration [m/s2] Fa 0  
time [s] t t0 = 0 t1 = 1 t2 = 3
 : Free value to be determined via optimal interpolation
The construction of the constraints matrix needs special attention. In this work the
automated process of constraints generation is implemented in a way that at maximum
2(rc + 1)(K+ 1) conditions are generated. From the given setup it can be concluded
that six value constraints4 and two continuity constraints for the velocity and the
acceleration at t1 = 1 s can be derived.
The generation of the constraint matrix is shown in detail for interpolation order V = 4.
For the two segments this means that in total 10 coefficients need to be determined.
The constraints read:
  c0,0   1 1t 2 3 4  `,0
1t`,0 1t`,0 1t`,0 0 0 0 0 0  c0,1  0
 0 1 2t 2 3     `,0
3t`,0 4t`,0 0 0 0 0 0  c0,2   
 0 
 0 0 2 6t 12t
2
`,0 `,0 0 0 0 0 0
 
 c0,3   0 
 1 1ta,0 1t
2 3 4
a,0 1ta,0 1ta,0 0 0 0 0 0  c0,4 
 
=  1 
 0 0 0 0 0 1 1t 1t
2
`,1 `,1 1t
3
`,1 1t
4 `,1  c1,0 
  1 
 0 0 0 0 0 1 1t 2 3a,1 1ta,1 1ta,1 1t4a,1  c   1,1   8 0 1 2t 2a,0 3ta,0 4t3a,0 0 −1 −2t −3t2 −4t3 `,1 `,1 `,1  c1,22 − − − 2 
  0 
0 0 2 6ta,0 12ta,0 0 0 2 6t`,1 12t`,1 c1,3  0
c1,4
As each spline interval is defined from t ∈ [0, ta,κ], for the given example it is t`,0 = 0 s,
ta,0 = t1 − t0 = 1 s, t`,1 = 0 s and ta,1 = t2 − t1 = 2 s. With cost matrix Q for rf = 3 in
4The position value constraint at the intermediate point t1 = 1 s infers two separate value constraints
in the constraint matrix to account for the continuity between the first and second segment as well.
61
Chapter 6. Spline-based Trajectory Representation
order to derive a minimal jerk trajectory this finally leads to:
   
 
0 0 0 0 0 0 0 0 0 0
 1 0 0 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0   0 1 0 0 0 0 0 0 0 0   0  0 0 0 0 0 0 0 0 0 0 
    
0 0 2 0 0 0 0 0 0 0   0  0 0 0 72 144 0 0 0 0 0 
 1 1 1 1 1 0 0 0 0 0   1  0 0 0 144 384 0 0 0 0 0A  , b  , Q  

=  = = . 0 0 0 0 0 1 0 0 0 0

 0 0 0 0 0 1 2 4 8 16−  
1  0 0 0 0 0 0 0 0 0 0 8   0 0 0 0 0 0 0 0 0 0 0 1 2 3 4 0 1 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 2 6 12 0 0 − 2 0 0 0 0 0 0 0 0 0 0 0 144 576 
0 0 0 0 0 0 0 0 576 3072
Then the optimal coefficients can be obtained by solving the quadratic program 6.3.12.
The resulting coefficients are shown in Table 6.3.
Table 6.3.: Resulting coefficients of the considered example with interpolation order V = 4.
c0,0 c0,1 c0,2 c0,3 c0,4 c1,0 c1,1 c1,2 c1,3 c1,4
0 0 0 1.6429 -0.6429 1 2.3571 1.0714 -0.3571 0.0536
Obviously, the starting conditions can directly be extracted from coefficients c0,0, c0,1
and 2c0,2. The same holds for coefficient c1,0 that reflects the position constraint at
the intermediate breakpoint at t1 = 1 s. The corresponding optimal trajectory with
interpolation order V = 4 is visualized in Figure 6.2a.
In a subsequent experiment the interpolation order is varied. Since the setup as given in
Table 6.4.: Resulting costs with respect to a different interpolation order. Since the number of
segments is fix, the number of variables exclusively corresponds to the interpolation order.
interpolation order number of variables optimal costs []
V = 3 8 344.25
V = 4 10 54.00
V = 5 12 50.55
V = 6 14 50.55
V = 7 16 50.55
Table 6.2 is still holding, the number of constraints remains unchanged. However, with
the varying interpolation order the number of variables (i.e. the number of coefficients)
changes.
Moreover, the results of Table 6.4 confirm the findings of section 6.1 that a fifth order
polynomial is the optimal choice with respect to generating a minimum jerk trajectory.
In particular, it is shown that this aspect can be transferred to a spline that is composed
of several segments as an interpolation order beyond V = 5 does not improve up on
the obtained solution. Figure 6.2 shows the suboptimal solutions for interpolation
62
6.3. Minimum Kinematics Trajectory Generation
order V = 3 and V = 4 in comparison to the optimal one. The deviation, as depicted
in Figure 6.2b, shows that an interpolation with interpolation order V = 4 already
yields comparable accuracy in terms of the position, whereas an interpolation order
of V = 3 leads to deviations of up to 1.79 m.
V = 5 V = 4 V = 3
10
8
6
4
2
0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6 2.8 3
t [s]
(a) Resulting trajectories shown for interpolation order V = 3, V = 4 and V = 5.
2
1.5
1
0.5
0
−0.5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6 2.8 3
t [s]
(b) Deviation of the trajectory with interpolation order V = 3 and V = 4 with respect to the optimal
result generated with interpolation order V = 5.
Figure 6.2.: Result of the optimal interpolation for the considered example.
To sum up it can be concluded that the optimal interpolation strategy supports the
idea of the two level hierarchical frame work. Based on the considerations made with
respect to the variational problem of trajectory planning without constraints, the theory
of minimum kinematics trajectory generation expands the scope to a spline-based
trajectory representation. This promises a higher flexibility and better results in terms
of convenient trajectory generation. Nevertheless, the analysis shows that the setup
has to be chosen carefully to obtain a meaningful result.
63
∆F p [m] F p [m]
7
Trajectory Planning for Automated Driving
This chapter deals with the transition from the trajectory planning concept as derived
in section 3.3 to a mathematical problem formulation, that allows to determine a suit-
able trajectory for automated driving applications. The proposed two level hierarchical
trajectory optimization framework is therefore mapped to a bilevel programming prob-
lem, with the low level minimum kinematics trajectory generation. Finally, the optimal
trajectory is found by applying a discrete or a continuous trajectory optimization
approach to the high level optimization problem.
7.1. Bilevel Programming
Bilevel programming refers to a class of mathematical problems, in which one op-
timization problem is nested within another. The inner and outer optimization task
are connected by the fact that the constraints of the outer optimization problem are
defined in part by a nested inner parametric optimization problem. Hence, only those
solutions are valid that are optimal in terms of the inner optimization problem, sat-
isfying the constraints of the outer optimization problem at the same time (Sinha
et al. 2018). In general such hierarchical optimization problems are represented by
a two-level hierarchical system, in which the two interconnected entities make their
decisions with respect to their own profit and constraints, but on different levels. As
the two levels have their own objectives and constraints, the set of decision variables
is partitioned accordingly. Although in literature related to bilevel programming the
different levels are commonly referenced as upper and lower level, in compliance to
the terminology used in section 3.3 they will coherently be referred to as high and low
level, respectively. Alternatively, an illustrative description is given in terms of a lead-
er-follower policy. For a high level decision vector ẑh chosen by the leader, the follower
reacts optimally by selecting the low level decision vector ẑl in order to minimize the
low level objective function f̂ (ẑh, ẑl) subject to the constraints ĝ(ẑh, ẑl). Let now S(ẑh)
denote the solution set of the lower level problem for a given high level decision vector
ẑh and ẑl(ẑh) be an element of S(ẑh). Then, assuming that the lower level solution has
at most one optimal solution1, the leader chooses ẑh such that the high level objective
1For more details on how to find a solution to the problem in case of a non-unique lower level optimal
solution see e.g. Sinha et al. (2018).
64
7.2. Formulation of the Trajectory Optimization Problem
function F̂(ẑh, S(ẑh)) is minimized subject to the constraints Ĥ(ẑh, S(ẑh)) (Ye and Zhu
2010). Formally the described bilevel programming problem reads2:
min F̂(ẑh, ẑl) (7.1.1a)
ẑh, ẑl
s.t. ẑl ∈ S(ẑh), (7.1.1b)
Ĥj(ẑh, ẑl) ≥ 0 , j = 1, 2, . . . , Γ (7.1.1c)
with the corresponding lower level problem
arg min f̂ (ẑh, ẑl) (7.1.2a)
ẑl
s.t. ĝi(ẑh, ẑl) = 0 , i = 1, 2, . . . , Ω . (7.1.2b)
The theory of bilevel programming constitutes the mathematical background to the
proposed hierarchical trajectory optimization framework from section 3.3. It is hence
represented by the bilevel programming problem as stated in equation 7.1.1 in which
the lower level problem 7.1.2 basically corresponds to the minimum kinematics for-
mulation of the optimal interpolation as given in equation 6.3.12.
As already mentioned the hierarchical trajectory optimization framework allows for
the separation of breakpoint elements into three different types. For already known
values prior to the trajectory generation there is no need to optimize, such that as a
consequence these values are no part of the decision variables. This can for example
be the case in a stopping scenario with a fix target position, in which the velocity and
successive derivatives have to be zero. The set of decision variables to generate the
trajectory is partitioned between low level decision variables ẑl and high level deci-
sion variables ẑh, which are determined to serve optimally on their level of hierarchy.
The high level decision variables represent the main source for optimization, essen-
tially determining the shape of the ego vehicle trajectory. The low level optimization
problem is then solved with respect to the low level decision vector, while the high
level decision variables act as parameters. An advantage of the hierarchical trajectory
optimization framework in terms of bilevel programming is that in contrast to bicri-
teria optimization, no compromise between objectives has to be found. Instead the
adjustability increases since the clear separation in hierarchical levels obviates the joint
consideration of high and low objectives that might (in part) be contradictory.
7.2. Formulation of the Trajectory Optimization Problem
The overall goal is to set up and solve a trajectory optimization problem in a way
that it provides the best trajectory in terms of the desired functionality with respect to
automated driving applications.
As shown in the previous section the idea of the proposed two level hierarchical tra-
jectory optimization framework that involves the separation in high and low level
2The notation of the constraints of the bilevel programming problem has been tailored to the consid-
ered use case. Generally, equality and inequality constraints can be considered for the high level, as
well as for the low level optimization problem.
65
Chapter 7. Trajectory Planning for Automated Driving
problems disembogues in a bilevel programming problem. In this work a spline-based
trajectory representation is chosen and the trajectory is then generated by optimal
interpolation. The low level problem 7.1.2 hence corresponds to the minimum kine-
matics formulation as described in section 6.3, providing a unique optimal solution to
the interpolation problem. According to Dempe (2002) in case the lower level problem
has a unique optimal solution for all parameter values, the bilevel programming prob-
lem 7.1.1 is equivalent to a single level optimization problem that has an implicitly
defined objective function. The bilevel programming problem can then be seen as the
task to find the best solution in the set of the optimal solutions to the lower level
problem with respect to the high level objective function. Since the optimal low level
decision variables ẑl can basically be considered as a function of the high level decision
variables ẑh, a single level problem can be constructed omitting ẑl completely (Sinha
et al. 2018):
min F̂(ẑh) (7.2.1a)
ẑh
s.t. Ĥj(ẑh) ≥ 0 , j = 1, 2, . . . , Γ . (7.2.1b)
From the considerations made in section 5.1.2 system dynamics can be expressed in
the flat output space. This enables to formulate the trajectory optimization problem
with respect to the flat output variable z. As stated in chapter 6 it is beneficial to apply
a spline-based formulation of the trajectory, since the required derivatives to obtain
the control inputs via equation 5.1.30 can be derived straight forward. The high level
decision variables ẑh hence correspond to the respective spline breakpoint elements.
For the two level hierarchical trajectory planning a separate spline for longitudinal and
lateral direction is chosen, covering a two-dimensional plane for vehicle motion. The
trajectory is then generated by optimal interpolation and combining the longitudinal
and lateral result (see chapter 6).
Looking at the remaining problem 7.2.1 to solve in that respect the objective function
F̂ is composed of the objectives mentioned in section 5.3. The combined costs read:
F̂(ẑh) = ωdF̂d(ẑh) + ωvF̂v(ẑh) + ωpF̂p(ẑh) + ωcF̂c(ẑh) (7.2.2)
with costs for distance keeping F̂d, reference speed control F̂v, reference path following
F̂p and comfort driving F̂c and their respective weights ω to balance the importance
between the objectives. To ensure a certain safety distance to other vehicles, an objective
is introduced that produces ACC-like behavior. Therefore, the distance to the respective
predicted impeding vehicles (cmp. section 4.1) for the planned ego lane are taken.
The minimum reference distance that should be obtained to the lead vehicle Cd̆l and
to the tail vehicle Cd̆t is given by:
Cd̆ Cl,k(ẑh) = dmin + Tl vx,k(ẑh) , (7.2.3)
Cd̆t,k(ẑh) = −(d Cmin + Tt vx,k(ẑh)) (7.2.4)
with the respective constant time gaps Tl and Tt and minimum safety distance dmin.
Note that the ego velocity v is expressed in curvilinear coordinates to merely account
for the velocity that is directed along the course of the road, which has been identified
66
7.2. Formulation of the Trajectory Optimization Problem
to be the most appropriate measure for the relevant distance in the considered case.
The ego velocity is dependent on the high level decision variables ẑh, since these
effect the general shape of the trajectory and hence the planned velocity of the ego
vehicle. The objective of distance keeping is then set up by comparing the actual
distances to the lead and tail vehicle Cdl and Cdt to the reference distances calculated
in equation{7.(2.3 and equation 7.2.)4:
Cd̆l,k(ẑh)− Cd (ẑ ) /Cd̆ C Cd̄ ẑ l,k h l,k(ẑh) d̆l,k(ẑh)− dl,k(ẑh) > 0l,k( h) = , (7.2.5)
{0( ) otherwise
Cd (ẑ )− Cd̆ C C C
d̄ ẑ t,k h t,k
(ẑh) / d̆t,k(ẑh) dt,k(ẑh)− d̆t,k(ẑh) > 0
t,k( h) = , (7.2.6)
0 otherwise
also taking into account that only costs need to be considered, which violate the
reference safety distance. Because both, the actual and the reference distance to the
lead and tail vehicle are based on the planned trajectory derived via the high level
decision variables ẑh the objective of distance keeping introduces a predictive behavior
of the ego vehicle. The accumulated costs for the distance keeping objective are:
K K
F̂ 2d(ẑh) = ∑ d̄l,k(ẑh) + ∑ d̄ 2t,k(ẑh) . (7.2.7)
k=0 k=0
To attain the desired reference speed the objective reads:
K ( )2
F̂v(ẑ Ch) = ∑ vx,k(ẑh)− Cṽx,k , (7.2.8)
k=0
which penalizes the deviation of the planned longitudinal ego velocity vx from ref-
erence speed profile ṽx provided by the environment-aware maneuver planning (see
chapter 4). The difference to penalizing a deviation of absolute velocities is hence that
by taking the curvilinear longitudinal velocity into account, the motion of the ego
vehicle is directed along the road. That way a desired progress along the road should
be ensured, inferring that the vehicle orientation with respect to the lanes is minimized
as well.
The objective to reach a lateral reference position encompasses the determination of a
reasonable time at which the desired lateral position should be attained. This allows
for an uncommitted transition, considering that the maximum requested lateral offset
p̃y,max cannot be reached instantaneously. Then, with comfort acceleration acomf and
floor function b·c the target lane reaching time Tr and the corresponding trajectory
index kr are: √
2| p̃y,max|Tr = , kr = b(Ta r/Ts + 1) + 0.5c . (7.2.9)comf
The objective to reach the desired lateral position is intended to incorporate high
level maneuver decisions in terms of the preferred driving lane. In the particular case
considered in this thesis the reference lateral path is deduced from the maneuver
67
Chapter 7. Trajectory Planning for Automated Driving
trajectory provided by the environment-aware maneuver planning (see chapter 4). The
objective reads:
K K ( )2
F̂p(ẑh) = ∑ Cp p2 Cy,k(ẑh) = ∑ py,k(ẑ )− Ch p̃y(C px,k(ẑh)) . (7.2.10)
k=kr k=kr
Here, Cp py is the path-trajectory deviation in terms of the lateral offset of the ego trajec-
tory in curvilinear coordinates with the lateral reference path serving as the curvilinear
basis. This way it is ensured that the shortest distance is taken into consideration ex-
clusively. An alternative formulation is given by performing a comparison of lateral
ego vehicle position C py and reference position C p̃y in curvilinear coordinates with
respect to the same curvilinear basis (i.e. the course of the road), retaining the property
of only considering the shortest lateral distance between the reference path and the
ego trajectory. To calculate the path-trajectory deviation the effective lateral reference
position of the reference path in the corresponding curvilinear frame C p̃y is given in
dependence of the curvilinear longitudinal position of the ego vehicle C px.
The objective of com{fort driving is given in terms of acceleration and makes use of:
(|Na Nx,k(ẑh)| − ăx)/ăx | ax,k(ẑh)| − ăx > 0āx,k(ẑh) = , (7.2.11)
{0 otherwise
(|Nay,k(ẑh)| − ăy)/ăy |Naā (ẑ ) = y,k(ẑh)| − ăy > 0y,k h (7.2.12)
0 otherwise
to incorporate a penalty in case the comfort longitudinal acceleration ăx or the comfort
lateral acceleration ăy is exceeded, respectively. The longitudinal acceleration Nax and
lateral acceleration Nay are derived from the planned ego trajectory by a transformation
into natural coordinates. This basically corresponds to a two-dimensional rotation
around the planned heading angle of the ego vehicle The objective then reads:
K K
F̂c(ẑh) = ∑ ā 2 2x,k(ẑh) + ∑ āy,k(ẑh) . (7.2.13)
k=0 k=0
The calculation of objectives involves a curvilinear transformation of the ego trajectory
with respect to a reference lane marker. This is necessary without any alternative to
formulate the desired behavior with accurate precision. Particularly the determination
of distances profits from curvilinear representation, as relevant measures naturally
coincide with the traffic flow.
Inequality constraints arise from the vehicle model as derived in section 5.1 and the
environment as given in section 5.2. Because of the fact that the breakpoint time might
be part of the decision variables it is essential to require monotonically increasing time.
For breakpoint time tκ with spline segment index κ = 0, 1, . . . ,K and the minimum
desired time difference Tm this is covered by:
xĤ1,κ(ẑh) = tx,κ+1 − tx,κ − Tm , (7.2.14)
yĤ1,κ(ẑh) = ty,κ+1 − ty,κ − Tm (7.2.15)
68
7.2. Formulation of the Trajectory Optimization Problem
for the lateral and longitudinal spline, respectively.
For obstacles $ = 1, 2, . . . ,z the inequality accounting for collision avoidance reads:
$Ĥ2,k(ẑh) = $dk(ẑh) , k = 0, 1, . . . , K (7.2.16)
with distance function dk(ẑh) that calculates the shortest distance to each obstacle
vehicle $. As described in section 5.2 there are several possibilities to approximate the
vehicle shape in combination with an efficient distance calculation.
The circle approximation has been chosen, because it provides an efficient and ac-
curate way to perform collision checking. Relevant calculations are therefore given
in equation 5.2.1. For η̊ approximating circles collision checking hence disembogues
into a distance calculation between each approximating circle center F p̊ = [F p̊ F Tκ κ x κ p̊y]
with κ = 1, 2, . . . , η̊ for the ego vehicle and obstacle approximating circle centers
F
ν,$ p̊ = [ F F Tν,$ p̊x ν,$ p̊y] with ν = 1, 2, . . . , η̊ for each obstacle vehicle $:
κ,ν,$d F Fx,k(ẑh) = κ p̊x,k(ẑh)− ν,$ p̊x,k , (7.2.17)
κ,ν,$dy,k(ẑh) = F Fκ p̊y,k(ẑh)− ν,$ p̊y,k . (7.2.18)
Because of the geometric properties of a circle the final inequality condition for collision
avoidance is given by: √
$dk(ẑh) = min d2 (ẑ ) + d2 (ẑ )− (ρ̊κ, κ,ν,$ x,k h κ,ν,$ y,k h ego + ρ̊$) . (7.2.19)ν
Note that because of the decomposition of the vehicle shape into multiple circles,
the inequality constraint $Ĥ2,k(ẑh) thus becomes dependent on each approximating
circle κ and ν for ego and obstacle vehicles, respectively. In this work each vehicle is
represented by η̊ = 3 circles, which leads to 3 · 3 = 9 distance calculations that have to
be performed for each trajectory time step k.
The formulation of the road boundary constraints corresponds to the definition of a
driving corridor that denotes the negotiable space i.e. the available lanes for the ego
vehicle considering the intended driving direction.
It relies on the road model as given in section 5.2, in which all lane markers are
represented by polygonal chains. The shortest distance between the restricting lane
marker and a bounding box corner point of the ego vehicle Fvp̌ with v = 1, 2, 3, 4
is hence given by the lateral component in curvilinear coordinates calculated with
respect to the corresponding lane marker3. It is:
C
l p̌
C
y,k(ẑh) = max l,v p̌y,k(ẑh) , (7.2.20)v
C C
r p̌y,k(ẑh) = min r,v p̌y,k(ẑh) , (7.2.21)v
which finally leads to a constraint for the left lane and the right lane boundary:
lĤ C3,k(ẑh) = −l p̌y,k(ẑh) , (7.2.22)
rĤ3,k(ẑh) = Cr p̌y,k(ẑh) . (7.2.23)
3Note the relation to the definition of curvilinear coordinates, since the orthogonal component in
curvilinear frame corresponds to the shortest distance to the reference lane marker.
69
Chapter 7. Trajectory Planning for Automated Driving
The minus sign for the left boundary accounts for the direction dependency as cal-
culated orthogonal distances are counted positive, if they are on the left side of the
polygonal chain. Figure 7.1 illustrates the road boundary constraint calculation.
F
4 p̌
F
1 p̌
F F
3 p̌ 2 p̌
Figure 7.1.: Road boundary constraints are defined via a shortest distance calculation between
the polygonal chain lane marker and the corner points of the ego vehicle bounding box.
For the system dynamics inequality constraints the control inputs are calculated as
described in section 5.1.2. This enables to consider system input bounds that can be
inferred from the vehicle system. Constraints accounting for the limited longitudinal
acceleration aĤ4 and limited steering angle δĤ4 read:
aĤ F N4,k(ẑh) = amax( vk)− ax,k(ẑh) , (7.2.24)
δĤ4,k(ẑh) = δmax − |δr,k(ẑh)| . (7.2.25)
The potential longitudinal acceleration that is available for the ego vehicle is given
by amax and dependent on the current ego velocity Fv. It is designed to follow the
speed-torque characteristic of the vehicle engine and hence limits the ability to fur-
ther accelerate at higher speeds. Figure 7.2 shows the chosen characteristic. Lateral
6
4
2
0
0 20 40 60 80 100 120 140 160 180 200
Fv [km/h]
Figure 7.2.: Approximation of maximum longitudinal acceleration over the vehicle speed.
motion is limited by the maximum steering angle δmax. To account for the combined
longitudinal and lateral restriction in terms of acceleration, the limit ag is based on
considerations with respect to the circle of forces (see e.g. Pacejka (2007)) leading to:
Ĥ Ng 4,k(ẑh) = ag − | ak(ẑh)| . (7.2.26)
Especially the property of an unambiguous optimal interpolation solution allows to
set up a single level optimization problem 7.2.1 that is left as the remaining challenge
to generate the optimal trajectory. At this point the similarity between a nonlinear
program that can be deduced from the corresponding optimal control problem and
the final bilevel programming problem becomes obvious.
Since the final bilevel problem 7.2.1 matches the ordinary formulation of an trajectory
optimization problem, classical approaches as stated in chapter 2 can be applied for
trajectory generation. In this thesis a discrete as well as a continuous approach are
presented.
70
a 2max [m/s ]
7.3. Discrete Trajectory Optimization
7.3. Discrete Trajectory Optimization
The class of sampling-based planning approaches is referred to as discrete trajectory
optimization, which relates to the process of constrained numerical optimization over
a discretized parameter space. The challenge is to find a suitable set of trajectory
candidates by applying a sampling strategy in order to discretize the state space in
form of a graph. As the sampling of longitudinal and lateral trajectory breakpoints
explicitly considers time information the resulting graph naturally becomes a Directed
Acyclic Graph (DAG). This is because subsequent breakpoints can only be connected
given that the time is increasing. Since for trajectory generation in the presence of
the two level hierarchical trajectory optimization framework a separate spline for
longitudinal and lateral direction is defined, the sampling strategy follows this policy
accordingly. Longitudinal and lateral trajectories are sampled independently and then
combined at a later step. Finally, trajectory candidates need to be evaluated and the
optimal trajectory with respect to given high level objectives and constraints has to be
found within the set of sampled trajectory candidates. Hence, the chosen approach
can be labeled as trajectory rollout (see section 2.1).
7.3.1. Previous Work
In previous work a sample-based planner for on-road automated driving has been
presented that performs online generation and evaluation of suitable trajectory can-
didates (Lienke et al. 2018b). The idea is to generate a meaningful set of trajectory
candidates starting from the current vehicle state and using a distinct analytic inter-
polation strategy and curvilinear transform of breakpoints to increase performance in
terms of efficiency and generalizability for different scenarios.
The chosen architecture gives more responsibility to the trajectory planning layer
intended to avoid inconsistencies between behavior and trajectory planning. The ma-
neuver planning is performed in terms of a target state generation that represents the
desired high level behavior. The chosen trajectory sampling strategy further features
the generation of maneuver hypotheses defined with respect to the planned lateral
motion of the ego vehicle. This way the sampling set contains several lane keeping
and lane changing maneuvers. Selecting the best trajectory in terms of the objective
function on trajectory level then goes along with a maneuver decision, as each sampled
trajectory is associated to a dedicated maneuver class. To enable a reliable decision of
the automated vehicle it is hence important to design the trajectory objective function
in a comprehensive manner.
For trajectory set generation a mixed sampling strategy is applied, sampling from
action as well as from state space. By sampling the longitudinal acceleration input
a better sense of feasibility is established on the one hand, whereas lateral sampling
with respect to the detected lanes on the other hand permits to exploit the structure of
the environment. The idea behind the latter aspect is to determine breakpoints in close
relation to the course of the road, which helps to generate expressive trajectory can-
didates. The use of a curvilinear coordinate system is ideally suited for this purpose.
Sampling is hence performed in curvilinear coordinates and polynomial lane markers
71
Chapter 7. Trajectory Planning for Automated Driving
then serve as reference for the transformation in the vehicle frame. To keep the number
of curvilinear transformations as low as possible, candidate trajectories are interpo-
lated and evaluated in vehicle frame. Instead of transforming every trajectory point
of each sampled trajectory this allows to transform only sampled breakpoints and ad-
ditionally obviates time consuming point wise transformation of obstacle trajectories.
The curvilinear transformation establishes a basic universality, since the planned tra-
jectories are independent of road curvature and work in straight as well as in curved
lane scenarios.
The design of the architecture is intended to cover maneuver and trajectory planning
in one integrated step by purposely generating trajectory samples for different lateral
maneuver classes. Although not followed in this thesis the architecture as shown in
section 3.1 would also allow to plan for different maneuver hypothesis4 to account for
disadvantageous high level decisions. However, the focus of the presented trajectory
planning approach in this thesis lies on determining the optimal trajectory with respect
to the desired behavior, which is inter alia derived from long-term considerations.
Another salient difference can be found in the proposed sampling strategy. Since in
the previous work a distinct interpolation strategy is applied, it is important that all
kinematic quantities are sampled in consistency to each other in order to generate a
meaningful trajectory sample. Otherwise this will likely lead to uncomfortable or un-
drivable trajectories. This restricts the approach to a constant acceleration assumption
that accounts for the fact that the choice of a varying acceleration over time increases
the number of possible solutions, while complicating the process of determining an
appropriate sampling strategy for each scenario at the same time. The application of
optimal interpolation as described in section 6.3 significantly improves on the applied
sampling strategy. The major advantage can be seen in the fact that it enables the
possibility to harmonize all kinematic quantities at each knot, because they are able to
automatically adapt in accordance to the minimum kinematics formulation. This leads
to a more flexible design of sampling, allowing e.g. varying accelerations, without
taking the risk of producing uncomfortable or even undrivable trajectories. All in all
this leads to a larger set of expressive trajectory candidates and a more intuitive design
of trajectory sampling.
7.3.2. Sampling Strategy and Trajectory Evaluation
An adequate sampling strategy is vital in order to tackle the arising single level prob-
lem that finally represents the mathematical framework of the two level hierarchical
trajectory optimization. The task is hence to find the optimal high level decision vari-
ables that minimize the high level objective function subject to the high level constraints.
With respect to the discrete trajectory optimization approach, the challenge is to find
a suitable discretization that leads to expressive trajectory candidates.
In terms of the two level hierarchy the sampling strategy needs to consider the separa-
tion in high and low level decision variables, which is relevant to appropriately account
for the performed interpolation. The spline-based trajectory representation moreover
4This would mean to include feedback from the trajectory planning module to the high level decision
making in some way.
72
7.3. Discrete Trajectory Optimization
offers the possibility to perform the sampling in terms of breakpoints. Obviously, as
sampling is done before the interpolation, it is vital to sample coherent breakpoints.
The connection of kinematic values of one breakpoint should hence account for this,
which means that position, velocity and acceleration should be chosen in order to
reflect the desired driving behavior subject to the restrictions of the vehicle dynamics.
To enforce the independence of the sampling strategy with respect to road curvature,
breakpoints are sampled in curvilinear coordinates. Lane markers then serve as refer-
ence for transformation in the vehicle frame. There are several possibilities of handling
the chronology of optimal interpolation, transformation and evaluation. However, the
most promising one is to transform the breakpoints and interpolate and evaluate the
trajectories in vehicle frame afterwards, since it prevents from several time consuming
point wise transformations (i.e. obstacle and/or ego trajectories).
The sampling strategy makes use of an adaptive discretization which has been proven
to work well in several applications with respect to automated driving (Lienke et al.
2018b; Homann et al. 2018; Homann et al. 2019). For more details it is referred to
section A.8.
Longitudinal Sampling Strategy
A deterministic sampling is performed in order to build a tree structure that represents
the set of longitudinal trajectories. The tree structure accounts for the fact that a
preceding node of a preceding tree level influences the driving behavior, since due to
the applied interpolation strategy it affects the shape of the trajectory in the respective
segment. This linkage forbids to arbitrarily combine each and every node, because
this will likely lead to undesired driving behavior or undrivable trajectories and hence
increase the number of inexpressive candidates.
To properly map a sampling strategy to the task of high level optimization of spline
breakpoint elements without changing the number of segments of the spline, a tree
level is built for each spline breakpoint. Each breakpoint hence corresponds to a tree
level, which itself is subdivided into several time layers. This is important because
only nodes of successive tree levels are allowed to be connected in order to ensure an
increasing breakpoint time yielding a DAG, while maintaining the total number of
spline segments at the same time. Since time takes on a special role the focus lies on
kinematic quantities like position, velocity and acceleration as meaningful sampling
quantities. One basic guideline to build the tree is that each node should represent
a variation in only one of these kinematic quantities. This measure improves on the
tracking of variations and simplifies the generation of suitable node sequences. The
strategy for node generation is now that each high level decision variable is varied by
sampling, leading to one node for each sample with the kinematic quantities chosen to
yield consistent node values. Connections are established exclusively between nodes
of the respective kinematic quantity, but the applied strategy could also be extended
to work for all nodes (generated by the samples of each kinematic quantity) of the
previous tree level in general. For node generation and connection time takes on a
special role in the sampling strategy. This is because it obviously has a major impact
on the shape of the trajectory and it is hence necessary to thoroughly consider this
73
Chapter 7. Trajectory Planning for Automated Driving
spline
tree
time layer
level
Figure 7.3.: Longitudinal Kinematic Sampling Tree.
relation with respect to the choice of the kinematic sampling quantities. To account
for this aspect the sampling process is defined in a way that the sampled time in-
stance influences the choice of the kinematic quantity values. Generally, the concept
of adaptive discretization is used to calculate the sampling values. For sampling of
the kinematic quantities the start values given by a node of the preceding tree level
should be considered for the same reason as for the choice of tree structure, since
the intention of both is to ensure coherent node sequences that generate appropriate
trajectory segments. The determination of a sampling range for a kinematic quantity
is supported by the use of a quasi-reachability analysis5 that approximates the states
that can be realized from the current start values considering a given acceleration
input range. This way the aspect of drivability is included in the process of trajectory
candidates generation. When sampling a kinematic quantity the corresponding values
of the remaining kinematic quantities of a node are found by integration or derivation,
respectively. However, these values can be regarded as some kind of initial values and
will not be considered (since they are configured to belong to the low level decision
variables they will change with respect to the optimal interpolation), but used as start
values for the subsequent tree level.
Figure 7.3 illustrates the longitudinal kinematic sampling tree. In dependence on the
number of spline breakpoints a predefined number of samples is generated for each
associated tree level. The set of longitudinal trajectory candidates is finally composed
of all trajectory samples that are extracted as all paths that go from the root to a leaf
of the longitudinal kinematic sampling tree.
Lateral Sampling Strategy
The use of curvilinear coordinates rigorously simplifies the sampling for the lateral
quantities, because the respective values can be set independently from the course of
the road. This measure hence generalizes the applied lateral sampling strategy to a
5In contrast to a reachability analysis as defined by Althoff (2010), the intention is not to determine a
set of all possible states that the system (i.e. ego vehicle) can reach, but rather to provide a certain
range of meaningful kinematic values in order to obtain an appropriate sequence of nodes through
the tree.
74
7.4. Continuous Trajectory Optimization
spline
graph
time layer
level
Figure 7.4.: Lateral Kinematic Sampling Graph.
wide range of various scenarios. For the lateral sampling strategy the assumption that
the vehicle dynamics restrict the ego vehicle’s motion is assumed to play a subordi-
nated role. As a consequence, the consideration of quasi-reachability can be neglected
and nodes of different levels can be combined arbitrarily. This leads to the character-
istic of fully connected levels, such that finally a lateral kinematic sampling graph is
generated. The discretization of kinematic values is done analogous to the longitudinal
sampling strategy. The kinematic quantities such as position, velocity and accelera-
tion are adaptively discretized around the actual breakpoint values. The difference
to the generation of longitudinal sampling nodes is that lateral nodes are generated
as a combination of all sampled kinematic quantity values. Figure 7.4 illustrates the
resulting lateral kinematic sampling graph.
Trajectory Evaluation
Although lateral and longitudinal trajectories are constructed as a tree and a graph, no
dedicated approach is applied for evaluation purposes. The major reason can be seen
in the combinatorial problem as each longitudinal trajectory is combined with each
lateral trajectory and not evaluated independently of each other. This finally results
in an exhaustive search that has to be performed for each generated sample trajectory.
Trajectories, which violate the constraints are eliminated from the trajectory sampling
set, whereas remaining trajectories are ranked in accordance to the objective function
costs. The trajectory with the lowest costs is then regarded as the optimal solution to
the specified trajectory optimization problem 7.2.1.
7.4. Continuous Trajectory Optimization
Optimization-based planning approaches are within this thesis also referred to as con-
tinuous trajectory optimization to account for the continuous character of the optimal
parameters. Hence, the solution space for trajectories tends towards infinity and accu-
racy is not restricted by the quantization error coming along with discretization. This
chapter elaborates on a continuous trajectory optimization approach to automated
75
Chapter 7. Trajectory Planning for Automated Driving
driving in dynamic environments, giving some details about techniques for nonlinear
constrained optimization.
7.4.1. Previous Work
In previous work related to continuous trajectory optimization a major research effort
has been dedicated to trajectory planning with spline-based trajectory representation
using a distinct interpolation strategy (Götte et al. 2017c; Götte et al. 2017d). The
trajectory optimization problem is solved by integrating the constraints into the ob-
jective function. This way the nonlinear program is transferred to an approximate,
non-restricted optimization problem. The resulting nonlinear least-squares problem
is solved with the Levenberg-Marquardt algorithm (Levenberg 1944; Marquardt 1963).
Especially the comparison of trajectory optimization with interpolation strategy and
without shows that it is possible to improve on the convergence. This means that while
reducing the computational burden due to the consideration of less decision variables,
faster convergence is achieved at the same time.
As continuous optimization approaches require certain characteristics with respect
to the problem formulation, some effort has been expended to develop a suitable
environment model (Lienke et al. 2018c; Lienke et al. 2019b). A potential field is
introduced, which is composed of a static and a dynamic part. The latter exploits
the results of the situation analysis as it incorporates the predicted trajectories of the
current situation, affecting the safety distance that has to be satisfied. It is shown
that without any regard to the developing situation a predetermined safety distance
(oversized or undersized) will lead to uncomfortable or risky behavior, respectively.
The predictive manner of the developed environment potential field hence significantly
improves safety and comfort in complex traffic scenarios.
This thesis improves on several aspects of previous research. The definition of con-
straints and objectives in their entirety is vital to achieve the desired outcome on the
one hand, but aggravates the task of parameter tuning on the other. Although this is
a general problem, taken measures reveal the advantages of the proposed two level
hierarchical trajectory planning framework. Therein, an optimal interpolation strat-
egy replaces the formerly applied distinct interpolation. The two level hierarchical
trajectory planning framework hence addresses the issue of conflicting objectives and
facilitates the overall adjustment of the approach. In contrast to previous work the
chosen nonlinear optimization algorithm refrains from using a penalty method to
integrate constraints into the objective function and considers specified limitations
as hard constraints instead. A major aspect in the context of trajectory planning for
automated driving can be seen in the modeling of specific requirements leading to
dedicated objectives and constraints. The potential field for environment modeling has
proven to be beneficial for safe driving, but has not been designed to allow for exact
collision checking. It is moreover relying on physical considerations and not defined
with respect to the course of the road, such that it can only approximate ACC-like be-
havior. As a consequence this work adapts the definition of the widely accepted ACC
functionality as an objective, paired with a constraint for accurate collision checking.
Furthermore, the inclusion of a vehicle model by exploiting differential flatness leads
76
7.4. Continuous Trajectory Optimization
to a more precise modeling of the vehicle motion since it enables the possibility to
account for constraints on the control input variables.
7.4.2. Nonlinear Constrained Optimization
Algorithms for nonlinear constrained optimization problems cover a broad range of
applications in e.g engineering, science and economics. The solution of complex non-
linear problems appears to be a difficult task, since the existence of equality and
inequality constraints generally complicates the solution procedure. In the last years a
lot of research has been done to develop numerical approaches to solve such kind of
constrained optimization problems.
According to Nocedal and Wright (2006) there is no standard taxonomy for constrained
nonlinear optimization algorithms. Nevertheless, it appears that especially active set
and interior point methods are largely present among popular solution approaches.
The former approach considers inequalities, which represent one of the main chal-
lenges with respect to nonlinear programming, by making a distinction between active
and inactive constraints. As a consequence active constraints are consolidated in a set
of constraints that are satisfied as equalities at a solution. This leads to the so-called
active set that consists of the equality constraints and inequalities for which Ĥ = 0
holds. An inequality constraint is hence said to be active if it is Ĥ = 0 and labeled as
inactive if inequality Ĥ ≥ 0 is strictly satisfied. Active set methods estimate the active
set in order to find a solution of the reduced problem, in which active set constraints
are treated as equalities. Interior point methods are also referred to as barrier methods
since the constraints are modeled as barrier functions. In an iterative procedure inter-
mediate solutions are generated that stay within the boundaries of the feasible region
defined by the inequality constraints. The influence of the barrier term is controlled
by the barrier parameter. In that respect barrier function parameters are varied and
as soon as the solution is approached, barrier effects are mitigated to increase the
accuracy of the solution until the optimum is reached.
In the following approaches to quadratic and nonlinear programming as used in this
thesis are presented. A Sequential Quadratic Programming (SQP) approach is chosen in
order to tackle the high level problem of the proposed two level hierarchical trajectory
planning framework. Quadratic programming techniques are essential to solve the
emerging SQP subproblems as well as for the low level problem solution represented
by the minimum kinematics trajectory generation (see section 6.3.2). For deeper insight
into the topic of nonlinear optimization in general it is referred to Nocedal and Wright
(2006).
Quadratic Programming
Quadratic programming is concerned with solving a linearly constrained quadratic
optimization problem, also denoted as a quadratic program. The quadratic objective
function f̂ (p̂) = 0.5p̂TQp̂ + qTp̂ with decision variables p̂ ∈ Rnp̂ is given by cost
matrix Q ∈ Rnp̂×np̂ and cost vector q ∈ Rnp̂ . The linear equality constraints ĝi(p̂) for
i = 1, 2, . . . , Ω are expressed by constraint matrix Aĝ ∈ RΩ×np̂ and constraint vector
77
Chapter 7. Trajectory Planning for Automated Driving
b ∈ RΩĝ and inequality constraints ĥj(p̂) for j = 1, 2, . . . , Θ are given by inequality
constraint matrix Aĥ ∈ RΘ×np̂ and vector bĥ ∈ RΘ. The quadratic programming
problem reads:
min 0.5p̂TQp̂ + qTp̂ (7.4.1a)
p̂
s.t. Aĝp̂− bĝ = 0 , (7.4.1b)
Aĥp̂− bĥ ≥ 0 . (7.4.1c)
Quadratic programming problems often arise as subproblems within methods that
deal with finding a solution for general constrained optimization problems. As in-
dicated by the name this is especially true for sequential quadratic programming
techniques, which are designed to cope with nonlinear programming problems.
There are several approaches to quadratic programming. Betts (2010) for example
outlines an active set method for solving a quadratic program. Stellato et al. (2020)
propose an Operator Splitting Quadratic Program (OSQP) solver that is based on the
Alternating Direction Method of Multipliers (ADMM). According to Stellato et al. (2020)
the most computationally expensive part in the algorithm is the step of solving the
linear system that originates from iteratively solving the optimization problem with
ADMM. The linear system is solved by either a direct or indirect method. The latter
relies on an iterative procedure, whereas the direct method involves the factorization
of the coefficient matrix followed by forward and backward substitution.
Since benchmark results show that OSQP outperforms state of the art quadratic pro-
gramming solvers in terms of timing and failure rate, OSQP becomes the algorithm of
choice for quadratic programming in this thesis.
Sequential Quadratic Programming
A general constrained optimization problem that has nonlinear terms in either the
objective or its constraint function is given by:
min F̂(ẑ) (7.4.2a)
ẑ
s.t. Ĝi(ẑ) = 0 , i = 1, 2, . . . , Λ , (7.4.2b)
Ĥj(ẑ) ≥ 0 , j = 1, 2, . . . , Γ . (7.4.2c)
Approaches like active set sequential quadratic programming (SQP) and interior point
methods that deal with such optimization problems belong to the field of nonlinear
programming. In this thesis an SQP algorithm is applied and the following explanatory
notes will hence focus on this approach.
Briefly sketched, an SQP approach models problem 7.4.2 by a quadratic programming
subproblem whose solution defines the search direction to determine the next iterate.
The quadratic programming subproblem at iterate ẑı̂ reads:
min F̂(ẑı̂) +∇F̂(ẑ )Tı̂ p̂ + 0.5p̂T∇2ẑẑL(ẑı̂, γ̂ı̂, σ̂ı̂)p̂ (7.4.3a)p̂
s.t. ∇Ĝ Ti(ẑı̂) p̂ + Ĝi(ẑı̂) = 0 i = 1, 2, . . . , Ω , (7.4.3b)
∇Ĥj(ẑı̂)Tp̂ + Ĥj(ẑı̂) ≥ 0 j = 1, 2, . . . , Θ (7.4.3c)
78
7.4. Continuous Trajectory Optimization
with the lagrangian of the nonlinear program given by:
Λ Γ
L(ẑ, γ̂, σ̂) = F̂(ẑ)−∑ γ̂iĜi(ẑ)−∑ σ̂jĤj(ẑ) . (7.4.4)
i=1 j=1
The hessian of lagrangian∇2ẑẑL(ẑ, γ̂, σ̂) contains the second derivatives of the objective
function and constraints. For complicated problems it might be useful to replace the
hessian matrix by a quasi-Newton approximation.
Inspired by the Broyden–Fletcher–Goldfarb–Shanno (BFGS) algorithm a damped BFGS
update is applied to iteratively approximate the hessian. Therefore, in a first step
vectors ŝı̂ and ŷı̂ are defined as follows:
ŝı̂ = ẑı̂+1 − ẑı̂ , ŷı̂ = ∇ẑL(ẑı̂+1, γ̂ı̂+1, σ̂ ı̂+1)−∇ẑL(ẑı̂, γ̂ı̂+1, σ̂ ı̂+1) . (7.4.5)
Then it is:
r̂ı̂ = θ̂ı̂ŷı̂ + (1− θ̂ı̂)B̂ı̂ŝı̂ (7.4.6)
with scalar θ̂ for ba{lancing the two terms of r̂ı̂
1 ŝTı̂ ŷı̂ ≥ 0.2ŝTı̂ B̂ı̂ŝı̂θ̂ = . (7.4.7)
(0.8ŝTı̂ B̂ı̂ŝı̂)/(ŝ
T
ı̂ B̂ı̂ŝ
T T T
ı̂ − ŝı̂ ŷı̂) ŝı̂ ŷı̂ < 0.2ŝı̂ B̂ı̂ŝı̂
The BFGS update formula is finally given by:
T
− B̂ı̂ŝı̂ŝı̂ B̂ı̂ r̂
T
B̂ B̂ ı̂
r̂
ı̂+1 = +
ı̂
ı̂ ŝT
. (7.4.8)
ı̂ B̂ı̂ŝı̂ ŝ
T
ı̂ r̂ı̂
With respect to the step computation and evaluation of a merit function, inequality
constraints are often converted to equalities using slack variables. For the following
considerations it is hence assumed that all constraints are in the form of equalities
expressed by vector â. Likewise the lagrange multipliers for equality and inequality
constraints are consolidated in λ̂. In the applied line search method an `1 merit func-
tion controls the size of the step, helping to decide whether a step is accepted or not.
With penalty parameter µ̂ > 0 the `1 merit function φ̂ is defined by:
φ̂(ẑ, µ̂) = F̂(ẑ) + µ̂ ‖â(ẑ)‖1 . (7.4.9)
A step α̂ı̂p̂ı̂ with step length parameter α̂ is accepted if the following condition holds:
φ̂(ẑı̂ + α̂ı̂p̂ı̂, µ̂ı̂) ≤ φ̂(ẑı̂, µ̂ı̂) + η̂α̂ı̂D̂(φ̂(ẑı̂, µ̂ı̂), p̂ı̂) , η̂ ∈ [0, 1] (7.4.10)
with the directional derivative of merit function φ̂ in the direction of p̂ı̂ given by:
D̂(φ̂(ẑı̂, µ̂ı̂), p̂ı̂) = ∇F̂(ẑı̂)Tp̂ı̂ − µ̂ı̂ ‖â(ẑ)‖1 . (7.4.11)
The utilized SQP algorithm is outlined in chapter A.9. To solve subproblem 7.4.3 an
approach to quadratic programming as described above is applied.
A major benefit of SQP algorithms for nonlinear programming is that the problem
is typically solved in very few iterations. This is a desirable feature, since in depen-
dence on the complexity of the specified problem function evaluations (i.e. collision
checking) are presumably computational expensive. An SQP algorithm was for exam-
ple successfully applied for trajectory planning on the Bertha Benz drive (Ziegler et al.
2015), showing the capabilities with respect to the application in the field of automated
driving.
79
8
Evaluation in a Dynamic Environment
This chapter provides details with respect to the general applicability of the developed
approaches to automated driving in dynamic environments. The discrete trajectory
optimization (see section 7.4) and the continuous trajectory optimization approach
(see section 7.3) are evaluated in expressive test cases representing typical on-road
traffic scenarios. Finally, the obtained results are summarized and discussed to draw
a comprehensive picture.
8.1. Setup of the Trajectory Planning Approaches
The developed algorithms feature a large variety of setting options. This enables
precise tuning for custom application, but since performance is strongly connected to
the chosen settings on the other hand a well-conceived setup is vital.
Since the trajectory representation is chosen to be a spline, relevant parameters are
given in Table 8.1. The number of segments (and thus the number of breakpoints) and
the chosen interpolation order affect the flexibility and adaptiveness of the spline. The
higher the number of segments, the higher is the expected flexibility. Optimality in
terms of minimum kinematics trajectory generation for interpolation accounts for the
desired low level behavior and is reflected in the choice of the cost and continuity
derivative. For longitudinal direction a 7th order spline composed of two segments is
set up. To account for challenges with respect to lateral positioning due to road curva-
ture and potential lateral maneuvers (lane change, overtaking, etc.) the lateral spline
is composed of three segments. For both directions continuity in jerk is stipulated. In
contrast, with penalty weight ωr chosen as ωr = 1 for r = rf and ωr = 0 otherwise,
Table 8.1.: Spline definition.
number of interpolation cost continuity
segments order derivative derivative
K V rf rc
longitudinal 2 7 2 3
lateral 3 7 3 3
80
8.1. Setup of the Trajectory Planning Approaches
the interpolated longitudinal trajectory is set up to result in a minimum acceleration
solution on low level, whereas in lateral direction the generated minimum kinematics
trajectory is jerk optimal. The ego vehicle trajectory Pego is then characterized by a
chosen resolution of Ts = 0.1 s with a planning horizon of Tp = 5 s. The choice of the
trajectory sample time accounts for the aspect of accuracy in terms of related distance
calculations and has proven to work well in previous work (see e.g. Götte et al. (2017c)
and Lienke et al. (2018b)). The chosen planning horizon is in the range of the duration
of single lane changes (see e.g. Toledo and Zohar (2007) and Mullakkal-Babu et al.
(2020)). However, with respect to the capabilities of the onboard perception system,
the planning horizon can be considered to be quite long as it needs to cope with issues
arising from the limited sensor view range.
The decisive setting with respect to the two level hierarchical framework is the choice
of low level ẑl and high level decision variables ẑh. In the same line fix values for
known reference quantities need to be set. The hierarchical framework configuration
strongly relates to performance and run time of the trajectory optimization approaches.
Moreover, a use case dependent configuration is implemented to account for the in-
dividual demands of a certain maneuver. It is distinguished between a configuration
for driving (see Table 8.2) and for target braking (i.e. stopping, see Table 8.3). The
Table 8.2.: Hierarchical framework configuration Co-D for driving.
spline knot z0 z1 z2 z0 z1 z2 z3
position [m] F px 0   F py 0   
velocity [m/s] Fvx −   Fvy −   
acceleration [m/s2] Fa Fx −   ay −   0
jerk [m/s3] F ȧx −  0 F ȧy −   0
time [s] tx 0  Tp ty 0   Tp
− : Current value  : Low level optimization  : High level optimization
Table 8.3.: Hierarchical framework configuration Co-S for stop maneuver.
spline knot z0 z1 z2 z0 z1 z2 z3
position [m] F px 0  p Fx,s py 0   py,s
velocity [m/s] Fv −  0 Fx vy −   0
acceleration [m/s2] Fa −  0 Fx ay −   0
jerk [m/s3] F ȧx −  0 F ȧy −   0
time [s] tx 0  Tp ty 0   Tp
− : Current value  : Low level optimization  : High level optimization
basic difference between both configurations is that for stop configuration Co-S the
end knot is configured to be fixed with stop position Fps = [p p ]Tx,s y,s . Due to the fact
that a continuous decrease in velocity is desired, an intermediate intervention is not
necessary as well. Hence, the longitudinal position of breakpoint z1 is not optimized
on high level. For configuration Co-D the end knot of the longitudinal spline z2 is
81
Chapter 8. Evaluation in a Dynamic Environment
configured to induce a constant acceleration continuation in order to account for the
limited planning horizon, whereas at the end knot z3 of the lateral spline constant
velocity is assumed for the further course.
Due to their characteristic both trajectory optimization approaches own unique, dis-
tinctive features that need to be addressed. For the continuous trajectory optimization
approach the setup mainly reduces to the maximum number of SQP solver iterations.
In chapter A.10 the convergence behavior of the continuous approach is shown for
a situation in which the initial and final desired trajectory differ to a large extent. It
is hence expected that a higher number of SQP iterations is needed to converge to
the optimal solution. As can be seen, after ı̂ = 10 iterations the result is already very
close to the optimal one, leading to the conclusion that for an adequate initializa-
tion (which is explicitly addressed by the introduced environment-aware maneuver
planning that provides a suitable, situation dependent maneuver trajectory) fewer it-
erations are needed to achieve convergence. The chosen setup can hence be regarded
as a rather conservative setup, intended to focus more on the aspect of trajectory opti-
mality than on the overall run time. The result of the discrete trajectory optimization
approach is dependent on the quantization of high level decision variables. This is
achieved via the number of samples in addition to the provided range extracted from
the target region of the environment-aware maneuver planning. The corresponding
setup for the high level optimization approaches is given in Table 8.4. From the given
Table 8.4.: Setup of trajectory optimization approaches.
description variable value
Continuous Trajectory Optimization
Maximum number of optimization iterations ı̂ 10
Discrete Trajectory Optimization
Number of samples for high level decision variable px npx 5
Number of samples for high level decision variable tx ntx 3
Number of samples for high level decision variable py npy 3
Number of samples for high level decision variable ty nty 3
configurations it is now possible to deduce the number of nodes, edges and paths of
the Longitudinal Kinematic Sampling Tree and the Lateral Kinematic Sampling Graph.
The number of nodes on each level is given by vector h̄ and can be derived from the
respective number of samples (see Table 8.4) in combination with the information if
a quantity is configured as a high level decision variable (see Table 8.2 and Table 8.3,
respectively). The vector of the cumulative product e is given by eı = ∏ı=1 h̄ with
1 ≤ ı ≤ K + 1. Table 8.5 summarizes the characteristics of the sampling structures
for discrete trajectory optimization in dependence on the chosen configuration of the
hierarchical framework.
In order to obtain the desired vehicle behavior the high level objective function weights
are chosen as follows: ωd = 5000, ωv = 10, ωp = 500, ωc = 5000. This reflects the
distinction between preferable behavior (low weights) and penalization of intolerable
82
8.2. Analysis and Evaluation
Table 8.5.: Sampling structures for driving configuration Co-D and stop configuration Co-S.
Longitudinal Kinematic Sampling Tree Lateral Kinematic Sampling Graph
nodes edges paths nodes edges paths
K+1 K+1 K+1 K K+1
h̄ e ∑ eı ∑ eı eK+1 h̄ ∑ h̄ı ∑ h̄ıh̄ı+1 ∏ h̄
ı=1 ı=2 ı=1 ı=1 =1
Co-D [1 15 5] [1 15 75] 91 90 75 [1 9 9 3] 22 117 243
Co-S [1 3 1] [1 3 3] 7 6 3 [1 9 9 1] 20 99 81
behavior (high weights) and is also designed to represent a hierarchical order in respect
to priority (the higher the weight the more priority is given to the respective objective).
To complete this section the vehicle model is set up in accordance to data of an Audi
A3 Sportback 2009 and solver settings for constrained optimization are set to default.
For a holistic overview with respect to the chosen parameters of the vehicle model, the
high level optimization parameters and solver settings it is referred to chapter A.12.
8.2. Analysis and Evaluation
For evaluation in dynamic environment a profound simulation and/or vehicle equip-
ment is necessary to appropriately record relevant data. Especially the testing in real
world environments is challenging, since it represents the highest level of difficulty
with respect to the performance of the overall system. The strength of simulation-based
testing can evidently be seen in the repeatability and reproducibility of test cases.
In general all fused and tracked objects are given by an object list. The road topology
is majorly represented by lane markers given as third order polynomials. Moreover,
information about the lane marker type is provided, which allows to account for
additional traffic relevant aspects. With regard to the road model some preprocessing
is done to generate the required information (see section 5.2). The preprocessing of
the road model corresponds to a transfer from polynomial representation to polygonal
chains, enhanced by a circular extrapolation to increase the overall view range.
Situation-based testing
To focus on the capabilities of the developed trajectory planning approach situation
based testing for one single time step is performed, assuming for example no uncer-
tainty and exact representation of the static and the predicted dynamic environment.
Results can hence be analyzed independent of e.g. perception and controller perfor-
mance. To test the basic maneuver capabilities of the developed approaches a stop, a
passing and a merge test case are chosen for detailed analysis.
The stop test case Si-S-1 is defined in Table 8.6 demanding a target break at stop
position Fps = [40 0]T m. The target lane is set to two which means that the ego vehicle
should follow the center of the current ego lane. Figure 8.1 shows the results for test
case Si-S-1 obtained with configuration Co-S.
83
Chapter 8. Evaluation in a Dynamic Environment
Table 8.6.: Straight road test case definition Si-S-1 for a stop maneuver.
ID [ ] F px [m] F py [m] Fv [km/h] Fψ [rad] target lane [ ] F p Fx,s [m] py,s [m]
ego 0 0 50 0 2 40 0
6
3
0
−3
−20 −15 −10 −5 0 5 10 15 20 25 30 35 40 45 50 55 60
E px [m]
(a) Result of the continuous approach.
6
3
0
−3
−20 −15 −10 −5 0 5 10 15 20 25 30 35 40 45 50 55 60
E px [m]
(b) Result of the discrete approach.
60 60
40 40
20 20
0 0
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(c) Velocity of the continuous approach. (d) Velocity of the discrete approach.
10 10
5 longitudinal lateral 5 longitudinal lateral
0 0
−5 −5
−10 −10
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(e) Acceleration of the continuous approach. (f) Acceleration of the discrete approach.
Figure 8.1.: Results in stop test case Si-S-1.
The results show only small differences between the solution of the continuous and the
discrete approach. Both continuously decrease the initial ego velocity of 50 km/h to
come to a stop at the desired stop position. The maximum longitudinal deceleration is
therefore at around −5 m/s2. As the ego vehicle should follow the current lane center
to stop at the desired stop position the lateral acceleration is zero.
The comparison as depicted in Figure 8.2 reveals that the continuous approach deviates
by at maximum 4 cm from the ideal straight solution provided by the discrete approach.
84
Fa [m/s2] Fv [km/h] E p [m] Ey py [m]
Fa [m/s2] Fv [km/h]
8.2. Analysis and Evaluation
This is because the continuous approach tries to further reduce the costs caused by
the required longitudinal braking acceleration exceeding the chosen comfort threshold
of 3.5 m/s2 as given in equation 7.2.11. In comparison to the discrete approach, the
continuous approach is thus still better in terms of the defined objective function costs
as it is shown in Table 8.9. Obviously, since start and end knot are fixed, the deviation
in lateral and longitudinal position for t = 0 s and t = 5 s is zero. Because configuration
start/end knot position lateral knot position longitudinal knot position
0.2
continuous discrete
0.1
0
−0.1
0 5 10 15 20 25 30 35 40
F px [m]
(a) Comparison of the lateral and longitudinal positions.
−2
0.1 4 ·10
0
2
−0.1
−0.2 0
−0.3 −2
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(b) Deviation in longitudinal position. (c) Deviation in lateral position.
Figure 8.2.: Comparison of the continuous and the discrete approach in stop test case Si-S-1.
Co-S allows for low level optimization of the longitudinal position of the intermediate
knot, the longitudinal behavior is basically determined by one single spline segment.
In both solutions the longitudinal position of the intermediate knot z1 is hence located
at the end of the trajectory. For the continuous approach the longitudinal knot position
is 39.90 m at t = 4.40 s and 38.17 m at t = 3.50 s for the discrete approach.
To illustrate the comprehensive capabilities of the developed approach, a passing
test case is analyzed that requires an adapted, intuitive behavior of the ego vehicle.
This is because the ego vehicle needs to deviate from the ideal center line of the ego
lane to prevent from colliding with the parking vehicle. Although this is contrary to
the intentionally defined objectives, it emphasizes the compliance with respect to the
requirement of collision avoidance incorporated as an inequality constraint. Table 8.7
shows the corresponding test case definition Si-P-1. As depicted in Figure 8.3 the
continuous as well as the discrete approach master the passing maneuver and avoid a
collision with parking obstacle vehicle ID:1, which is illustrated by showing the ego
vehicle bounding box for all trajectory points k = 0, 1, . . . , K. In comparison to the
discrete approach the continuous approach deviates less from the desired lane center
with the maximum deviation in close proximity to the parking obstacle ID:1. Thus, the
discrete approach causes higher costs in terms of the objective function (see Table 8.9).
85
F
con px − F Fdis px [m] py [m]
F F
con py − dis py [m]
Chapter 8. Evaluation in a Dynamic Environment
Table 8.7.: Straight road test case definition Si-P-1 for a passing maneuver.
ID [ ] F p [m] Fx py [m] Fv [km/h] Fψ [rad] target lane [ ] Fvdes [km/h]
ego 0 0 50 0 2 50
1 35 −1.875 0 0 3 0
6
3
0
−3 ID:1
−5 0 5 10 15 20 25 30 35 40 45 50 55 60 65 70 75
E px [m]
(a) Result of the continuous approach.
6
3
0
−3 ID:1
−5 0 5 10 15 20 25 30 35 40 45 50 55 60 65 70 75
E px [m]
(b) Result of the discrete approach.
Figure 8.3.: Results in passing test case Si-P-1.
The test case definition for merge maneuver Si-M-1 in a right curve is given in Table 8.8.
It is designed to show the capabilities of the developed approaches with respect to
realizing a lane change into a gap between two obstacle vehicles (ID:1 and ID:2) on
the neighboring lane in order to overtake obstacle vehicle ID:3. This setup is reflected
in the respective target lane, which is defined with respect to the current ego lane.
Here, the ego vehicle target lane of one indicates a desired lane change to the left.
Figure 8.4 shows the corresponding results for the continuous approach and the dis-
crete approach using configuration Co-D. The general characteristic of the planning
result is similar for the continuous as well as the discrete approach. The continuous
approach realizes the merge-in maneuver with a lower maximum lateral acceleration,
meaning that the discrete approach reaches the target lane faster on the other hand.
For longitudinal planning no significant difference between the continuous and the
discrete approach can be observed. The mentioned differences are also highlighted in
Figure 8.5.
With results for test cases Si-S-2 and Si-M-2 shown in chapter A.11 altered variations
of test cases Si-S-1 and Si-M-1 are analyzed. Comparing the absolute objective function
costs as depicted in Table 8.9 it can clearly be seen that the continuous approach con-
sistently performs better in that respect. It emphasizes the suboptimal character of the
discrete approach due to the quantization of high level decision variables, indicating
an insufficient resolution.
86
E py [m] E py [m]
8.2. Analysis and Evaluation
Table 8.8.: Right curve test case definition Si-M-1 for a merge maneuver.
ID [ ] F p Fx [m] py [m] Fv [km/h] Fψ [rad] target lane [ ] Fvdes [km/h]
ego 0 0 100 0 1 130
1 30 3.10 110 −0.05 1 110
2 −15 3.50 110 0 1 110
3 45 −0.40 85 −0.05 2 85
6
3 ID:2 ID:1
0 ID:3
−3
−6
−20 0 20 40 60 80 100 120 140 160
E px [m]
(a) Result of the continuous approach.
6
3 ID:2 ID:1
0 ID:3
−3
−6
−20 0 20 40 60 80 100 120 140 160
E px [m]
(b) Result of the discrete approach.
120 120
110 110
100 100
90 90
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(c) Velocity of the continuous approach. (d) Velocity of the discrete approach.
10 10
5 longitudinal lateral 5 longitudinal lateral
0 0
−5 −5
−10 −10
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(e) Acceleration of the continuous approach. (f) Acceleration of the discrete approach.
Figure 8.4.: Results in merge test case Si-M-1.
87
Fa [m/s2] Fv [km/h] E py [m] E py [m]
Fa [m/s2] Fv [km/h]
Chapter 8. Evaluation in a Dynamic Environment
start knot position lateral knot position longitudinal knot position
3
continuous discrete
2
1
0
−1
0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160
F px [m]
(a) Comparison of the lateral and longitudinal positions.
2 0.2
1.5 0
−0.2
1 −0.4
0.5 −0.6
0 −0.8
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(b) Deviation in longitudinal position. (c) Deviation in lateral position.
Figure 8.5.: Comparison of the continuous and the discrete approach in merge test case Si-M-1.
Table 8.9.: Comparison of final objective function costs.
Si-S-1 Si-S-2 Si-M-1 Si-M-2 Si-P-1
continuous approach 58 088.02 59 562.76 28 508.14 19 404.07 2067.44
discrete approach 63 205.97 72 415.27 32 759.96 19 681.79 5491.10
Scenario-based testing
A dynamic environment simulation (Wissing et al. 2016) is used to evaluate the de-
veloped algorithms on scenario basis (in closed-loop). To prevent an overestimation
of the trajectory planning capabilities driven by results gained from a simulation en-
vironment it is vital to model certain effects of the perception module. Therefore, the
simulated ego vehicle is equipped with a front camera and radar sensors to the front,
rear and sides. The environment model contains a holistic description of the static
world and dynamic objects. However, the final model is not considered to be ideal,
since the detection of the prevalent lane structure mainly relies on the camera data and
is hence prone to inaccurate measurements and low view ranges. Analogous effects
are considered for dynamic object detection providing realistic results with respect to
the modeling of the dynamic environment. At first, to evaluate the performance for
lateral and longitudinal vehicle motion separately, desired high level behavior in terms
of distance keeping and lane centering is tested.
To test the distance keeping performance defined via a constant time gap, test case
Sc-A-1 as given in Table 8.10 is defined. A slower lead vehicle ID:1 is placed 70 m
88
F p − F Fcon x dis px [m] py [m]
F F
con py − dis py [m]
8.2. Analysis and Evaluation
Table 8.10.: Straight road test case definition Sc-A-1.
ID [ ] F px [m] F py [m] Fv [km/h] Fψ [rad] target lane [ ] Fvdes [km/h]
ego 0 0 120 0 2 120
1 70 0 72 0 2 85
120 120
ego vehicle ego vehicle
100 lead vehicle 100 lead vehicle
85 85
70 70
0 10 20 30 0 10 20 30
tsim [s] tsim [s]
(a) Velocity of the continuous approach. (b) Velocity of the discrete approach.
0.4 0.4
continuous continuous
0.3 discrete 0.3 discrete
0.2 0.2
0.1 0.1
0 0
0 5 10 15 20 25 30 0 0.5 1 1.5 2
Cdl − Cd̆ C Cl dl − d̆l
(c) Distribution for time interval tsim = [0 15] s. (d) Distribution for time interval tsim = ]15 30] s.
Figure 8.6.: Results of the continuous approach and the discrete approach in test case Sc-A-1.
ahead of the ego vehicle. The lead vehicle accelerates to reach 85 km/h, whereas the
ego vehicle drives at a desired speed of 120 km/h. As target lanes indicate the ego
vehicle as well as the obstacle vehicle are supposed to follow the current ego lane.
Figure 8.6 shows the results of test case Sc-A-1 for the continuous as well as the dis-
crete approach. Although the desired speed of the ego vehicle is Fvdes = 120 km/h, the
ego vehicle decelerates to adapt to the speed of the lead vehicle (see Figure 8.6a and
Figure 8.6b). In this test a difference between the continuous and the discrete approach
becomes obvious, as the velocity of the discrete approach contains undesired oscilla-
tions. The prevalent reason for this can be seen in the suboptimality of the discrete
approach, which leads to a volatile convergence towards the ideal lead vehicle distance.
Because of the adaptive discretization strategy that features a stronger dependence on
the realized trajectory of the previous planning cycle, the reciprocal action between the
planning and control part also contributes to the observed effect. To account for the
different phases during the ego maneuver the scenario is divided in two parts. Hence,
Figure 8.6c and Figure 8.6d show the relative probability of the difference between the
actual lead vehicle distance Cdl and the minimum desired reference distance Cd̆l for
89
relative probability Fv [km/h]
relative probability Fv [km/h]
Chapter 8. Evaluation in a Dynamic Environment
two different time intervals. The first part within time interval tsim = [0 15] s repre-
sents the transition phase, in which the ego vehicle approaches the slower lead vehicle.
The discrete approach decelerates less in the beginning, such that the distance to the
lead vehicle decreases faster. The second part for time interval tsim = ]15 30] s then
shows the accuracy with respect to the distance keeping objective. In the latter case the
discrete approach has a mean and standard deviation of (1.375± 0.146) m, whereas
for the continuous approach a mean and standard deviation of (0.310± 0.086) m are
identified. As a matter of fact the result shows that the difference between the actual
lead vehicle distance Cdl and the minimum desired reference distance Cd̆l is never
below zero, which shows that the minimum desired safety distance is always satisfied.
Although the velocity objective is conflicting with the distance keeping objective, the
ego vehicle performs reasonable reflecting the priority in terms of the chosen weights.
The lane centering performance is evaluated in test case Sc-L-1 (see Table 8.11) with
an initial deviation to the lane center of ∆F py = −1.75 m. The ego vehicle target lane
is two, which means that the ego vehicle should center in its current lane.
Table 8.11.: Straight road test case definition Sc-L-1.
ID [ ] ∆F py [m] Fv [km/h] Fψ [rad] target lane [ ] Fvdes [km/h]
ego −1.75 100 0 2 100
0.5 0.5
0 0
−0.5 −0.5
−1 −1
−1.5 −1.5
−2 −2
0 10 20 30 0 10 20 30
tsim [s] tsim [s]
(a) Lane center deviation for continuous approach. (b) Deviation to lane center for discrete approach.
Figure 8.7.: Result of continuous approach and discrete approach in test case Sc-L-1.
The results shown in Figure 8.7 reveal only slight differences between the continuous
and the discrete approach. Interestingly, the depicted results share the same character-
istic with respect to the transition phase, in which convergence of the lateral position
slows down after reaching ∆F py = −0.2 m. One explanation for this behavior is that
based on equation 7.2.9 the objective for lateral positioning is dependent on the relative
position of the ego vehicle to the desired lateral position. Especially in the beginning
high deviations to the desired lateral position contribute to higher lateral velocities
to decrease these costs. Taking into account the results of Sc-A-1 as well (cmp. 8.6a),
it seems that the observed characteristic is hence caused by a power shift between
effective cost terms of the high level objective function.
90
∆F py [m]
∆F py [m]
8.2. Analysis and Evaluation
Finally, a typical highway scenario is investigated in simulation. Test case Sc-H-1,
representing a part of the german highway A2 from Gelsenkirchen-Buer to Herten
is considered. The course of the road highway is modeled starting with E px = 0 m
connected by an arbitrary highway entry. The desired ego vehicle velocity is 100 km/h
for the highway entry part, valid until E px = 100 m. Then, for highway driving the
desired velocity is 120 km/h, changing to 140 km/h at the moment the ego vehicle
passes E px = 300 m. To follow the driven path of the ego vehicle the ego vehicle trace
is visualized as a black dotted line. Figure 8.8 shows the result for the continuous
approach. As can be seen, the velocity adapts to the dynamic traffic, but by overtaking
slower vehicles in front of the ego vehicle the desired target speed is finally reached.
Figure 8.8 also shows some characteristic snapshots of the ego vehicle drive for selected
time instances. At tsim = 5.7 s the ego vehicle performs a lane change to merge on the
highway leaving the ending entrance lane. To reach the modified set speed of 120 km/h,
at tsim = 14.3 s the ego vehicle does a left lane change to overtake obstacle vehicle
ID:10. Since obstacle vehicle ID:9 is driving slower than the second time updated
desired set speed of 140 km/h, after some time at around tsim = 23.1 s the ego vehicle
safely merges in the gap between obstacle vehicle ID:8 and obstacle vehicle ID:11.
The current lead vehicle ID:11 at that time also prevents the ego vehicle to realize the
desired target speed. When obstacle vehicle ID:11 changes its lane to the right the ego
vehicle accelerates and at the moment the ego vehicle passes obstacle vehicle ID:11 a
lane change to the right is performed (tsim = 50.1 s). At tsim = 66.8 s the ego vehicle
continues to overtake obstacle vehicle ID:5 and performs a single lane change to use
the free lane to the right of the current ego lane. For a comprehensive understanding
of the development and a more detailed description of the particular characteristics
that can be observed, corresponding sequences to each of the five mentioned situations
are shown in chapter A.11 (see Figure A.14, Figure A.15, Figure A.16, Figure A.17 and
Figure A.18).
As depicted in Figure 8.9 the discrete approach shows a quite similar result with
respect to highway test case Sc-H-1. Since the trajectory planning solution is not exactly
the same as for the continuous approach, also simulated surrounding obstacle vehicles
react accordingly. However, the basic evolution of the scenario is the same. The velocity
plot reveals that acceleration of the discrete approach is lower, but the desired target
speeds of 100 km/h, 120 km/h and 140 km/h are generally reached. Figure 8.9 also
shows the result of the discrete approach for selected time instances. At tsim = 5.4 s
the ego vehicle plans to merge on the highway lane in front of obstacle vehicle ID:7.
To accelerate up to the desired 120 km/h, at tsim = 14.0 s the ego vehicle does a lane
change to the left, intending to overtake obstacle vehicle ID:10. At tsim = 23.9 s the ego
vehicle accelerates to merge behind obstacle vehicle ID:11 and in front of approaching
vehicle ID:8 to overtake obstacle vehicle ID:9, trying to reach the increased set speed
of 140 km/h. With modified set speed of 140 km/h at tsim = 50.5 s the ego vehicle
then overtakes obstacle vehicle ID:11. Finally, at tsim = 66.8 s to finish the overtaking
maneuver of obstacle ID:5 the ego vehicle performs a single lane change to the right.
Analogous to the results shown for the continuous approach, details and sequences
illustrating the temporal context are given in chapter A.11 (see Figure A.19. Figure A.20,
Figure A.21, Figure A.22 and Figure A.23).
91
Chapter 8. Evaluation in a Dynamic Environment
160 5
140 −5
120 −15
100 −25
80 −35
60 −45
0 15 30 45 60 75 0 500 1000 1500 2000 2500
tsim [s] E px [m]
5 ID:10 ID:9
ID:8 ID:11 ID:5
0 ID:7 ID:0
− ID:65 ID:1ID:8
− tsim = 5.7 s10 ID:4
−180 −160 −140 −120 −100 −80 −60 −40 −20 0 20 40 60 80
−5 ID:11
ID:7
−10
−15
ID:10 ID:9
−20 tsim = 14.3 s ID:6 ID:5
60 80 100 120 140 160 180 200 220 240 260 280 300 320
−20 ID:4
ID:7 ID:8− ID:1125
−30 ID:10
− ID:6 ID:9 ID:1 IIDD::535 4
tsim = 23.1 s
360 380 400 420 440 460 480 500 520 540 560 580 600 620
−30 tsim = 50.1 s
−35
ID:5
ID:9 −40 ID:4 ID:11
ID:7 ID:10 ID:6 ID:1 −45
1380 1400 1420 1440 1460 1480 1500 1520 1540 1560 1580 1600 1620 1640
−5 tsim = 66.8 s
−10 ID:8
−15
−20 ID:5
ID:9 ID:4 2040 2060 2080 2100 2120 2140 2160 2180 2200 2220 2240 2260 2280 2300
ID:1 E px [m]
ID:6
ID:10 Figure 8.8.: Result of continuous approach in test case Sc-H-1.
ID:7
92
E p E E E Ey [m] py [m] py [m] py [m] p [m] Fy v [km/h]
E py [m]
8.2. Analysis and Evaluation
160 5
140 −5
120 −15
100 −25
80 −35
60 −45
0 15 30 45 60 75 0 500 1000 1500 2000 2500
tsim [s] E px [m]
10
ID:11
5 ID:10
ID:9
ID:5
ID:8
0 ID:7
−5 ID:6ID:8 ID:1
t
− sim
= 5.4 s
10 ID:4
−180 −160 −140 −120 −100 −80 −60 −40 −20 0 20 40 60 80
−5 ID:7 ID:11
−10
−15 ID:9
ID:10
−20 tsim = 14.0 s ID:6 ID:5
60 80 100 120 140 160 180 200 220 240 260 280 300 320
ID:4
ID:7 −25 ID:8 ID:11
−30
ID:10 ID:9
−35 ID:6 ID:1 IDID:4:5
− tsim = 23.9 s40
380 400 420 440 460 480 500 520 540 560 580 600 620 640
− tsim = 50.5 s30
−35
ID:8
−40 ID:5ID:9 ID:11
ID:7 ID:10 ID:6 ID:1 −45 ID:4
1400 1420 1440 1460 1480 1500 1520 1540 1560 1580 1600 1620 1640 1660
−5 tsim = 66.8 s
−10 ID:8
−15
−20 ID:5
ID:9 ID:4 2040 2060 2080 2100 2120 2140 2160 2180 2200 2220 2240 2260 2280 2300
E p
ID:6 x
[m]
ID:10 Figure 8.9.: Result of discrete approach in test case Sc-H-1.
ID:7
93
E p [m] E p [m] E p [m] E p [m] E p [m] Fy y y y y v [km/h]
E py [m]
Chapter 8. Evaluation in a Dynamic Environment
8.3. Discussion of the Results
The fundamental goal that is targeted during development is the application in a
vehicle in real world traffic. Test drives related to previous work (see section 7.4.1)
attested the general functioning of the continuous trajectory optimization approach
with distinct spline interpolation for highway driving. However, experience has shown
that especially the correct balance for general purpose is hard to find. Tuning of
lane changes affected the lane keeping performance possibly accompanied by other
unwanted effects like e.g. curve cutting, complicating the application in real world
traffic. Therefore, in this thesis special attention has inter alia been directed to the
requirement of adjustability that is particularly reflected in the design of the developed
planning approach.
In order to assess the results the achievements with respect to the requirements defined
in section 3.2 are collated. Figure 8.10 summarizes the achievements with respect to
the requirements on the trajectory as well as on the trajectory approach. As shown in
× √ optimality
decision
drivability inclusion
adjustability
collision avoidance shape
arbitrariness
temporal
comfort consistency
real-time
capability
completeness
high level behavior
continuous approach discrete approach
(a) Requirements on the trajectory. (b) Requirements on the trajectory approach.
Figure 8.10.: Evaluation with respect to the requirements.
Figure 8.10a the requirements on the trajectory are universally met, irrespective of the
chosen high level optimization approach. The drivability requirement is considered
by the integration of the vehicle dynamics model introduced in section 5.1. Collision
avoidance is covered by the integration of a comprehensive environment model that
accounts for static as well as dynamic collision checking as described in section 5.2.
Since comfort is uniquely related to the low level problem of the proposed two level
hierarchical framework, the minimum kinematic trajectory generation in section 6.3
ensures the fulfillment of this requirement. The high level behavior as the counterpart
within the proposed framework is considered via section 7.2.
The fulfillment of requirements with respect to the trajectory planning approach is
rated by means of a spider web chart depicted in Figure 8.10b. Since in this context
the continuous trajectory optimization and discrete trajectory optimization approach
own different characteristics the estimated score is shown for both approaches. High
values correspond to a better realization of the respective characteristic. This means
94
8.3. Discussion of the Results
that the more area is covered by an approach the more suitable it is for the purpose of
trajectory planning in dynamic environments. As can also be inferred from the chart,
the aspects of adjustability, shape arbitrariness and decision inclusion are determined
by the basic design of the framework, whereas optimality, real time capability, temporal
consistency and completeness are ultimately affected by the chosen approach to the
high level optimization problem.
The requirement of adjustability relates to the estimated application effort that is nec-
essary to tune the developed approach with respect to the desired behavior. This in
particular includes high and low level objective function and constraints parameters
(cmp. Table A.7). To put adjustability in concrete terms it is defined in a way that a
higher adjustability is reached with less parameters, moreover demanding a certain
expressiveness of the parameters to minimize interrelated dependencies. This means
that for good compliance it is important to reduce the number of conflicting objec-
tives as well as the number of parameters itself. In comparison to previous work a
noteworthy improvement has been achieved in that respect. Especially the elimination
of conflicting objectives due to the hierarchical structure of the developed framework
characterizes a beneficial effect. However, the developed approach still relies on a few
parameters that need to be tuned. For this reason the characteristic is not rated with
the highest score. Since adjustability is in some way contrary to the aspect of cus-
tomization (which is not particularly listed as a requirement) it is worth to mention
that the latter is beneficially addressed via the existing parameters. It can further be
concluded that a good compromise between adjustability and customization has been
found.
Within the proposed two level hierarchical framework the requirement of shape ar-
bitrariness is mainly connected to the chosen spline representation. Since prevailing
non-holonomic constraints inherently limit the representable trajectory shapes the fo-
cus of this requirement is directed to the aggregate of all drivable trajectories. This
means that chosen representation should be valid for all possible traffic scenarios. The
flexibility and adaptiveness of a spline are evident and because moreover the num-
ber of breakpoints and spline continuity conditions can be configured there are no
essential restrictions apparent. Still, to take the interpolating behavior into account the
assigned score is not matched to the best value.
The requirement of decision inclusion is fulfilled to its full extent. Not only that
superordinated decisions are covered in terms of a reference, decisions can also be
reflected in the setup of decision variables within the hierarchical framework that
allows to distinguish between fixed, optimized and adapted quantities.
By convention an algorithm is denoted as complete if it is guaranteed that a solution
is found if one exists and failure is returned otherwise. With respect to the devel-
oped continuous and discrete approach a proof is not given as the number of possible
scenarios is too high to prove completeness by trial or any other approach. On the
contrary continuous approaches can end up in local minima, which may cause the
algorithm to terminate without finding a trajectory when one exists. Discrete algo-
rithms on the other hand sacrifice completeness since a solution may be hidden in
the interstice. But there are also some points to mention that have a positive effect
with respect to the requirement of completeness. First of all, the amount of scenarios in
95
Chapter 8. Evaluation in a Dynamic Environment
which a valid solution can be found is expected to be much higher than the ones where
no solution exists. In addition, for most of the cases it is even presumed that several
solutions exist that only differ in their costs with respect to the defined objectives. This
aspect rigorously simplifies the search for a valid solution in general. The problem of
local minima for the continuous approach is mitigated by the inclusion of the result of
the higher level maneuver decision. For the discrete approach it is assumed that the
solution is rather suboptimal than that the applied sampling strategy with its deter-
ministic pattern prevents to find a valid solution in general. As completeness is not
referring to the degree of optimality of the solution, in comparison to the continuous
approach the discrete approach is ranked slightly better.
Results show that with respect to the requirement of optimality the discrete approach
lacks performance, since suboptimality is inherently expected due to the chosen res-
olution in the sampling process. Although the continuous approach is prone to end
up in a local minimum, because of the continuous character of the decision variables
a larger scope for action to realize optimality demands can be inferred. Finally, in
comparison to the discrete approach this leads to a higher score. Unfortunately, there
is no universally valid guarantee of convergence for real-time application. However, in
chapter A.10 convergence behavior of the continuous trajectory optimization approach
is shown for a dedicated scenario. It can hence be concluded that a fundamental
characteristic is represented by the trade off between optimality and run time, as for
the continuous approach optimization iterations are limited to a certain number and
for the discrete planning approach the number of sampled trajectories is ultimately
restricted as well.
Both the continuous and the discrete approach do yet not meet the requirement of
real-time capability to its full extent, which is also reflected in the respective score.
In comparison to the discrete approach the run time of the continuous approach
of around 300 ms to 400 ms is considerably closer to the target value of less than
100 ms. In fact in chapter A.10 it is shown that for the presented approach mean
run times of 75 ms are achievable. Still, improvements with respect to the run time
per iteration are vital. In this context a native C/C++ implementation will certainly
enhance run time performance in a significant manner. To estimate the run time
capabilities of the discrete approach a related approach introduced by Schlechtriemen
et al. (2016) can be adduced. Therein run time is stated to be approximately 50 ms for
3000 sequentially processed trajectories. As for the discrete approach it is moreover
possible to parallelize the computation task, switching to graphics processing units
as target hardware will support significant speed-up in computation time. Hence,
with the contrivable improvement in run time the overall potential of the developed
continuous as well as of the discrete approach is estimated to be quite high.
The requirement of temporal consistency is strongly related to the aspect of optimality,
since suboptimality might induce undesired effects between consecutive trajectory
planning cycles. From that perspective temporal consistency is rated analogous to the
optimality score particularly accounting for this connection.
96
9
Conclusion and Outlook
This thesis is dedicated to the development of a trajectory planning approach to au-
tomated driving. Hereinafter the key features and insights gained through thorough
investigations will be summarized. Moreover, some thoughts are shared on how to
tackle remaining challenges with respect to trajectory planning for automated vehicles.
9.1. Summary of the Work
The development of automated driving functions in order to increase traffic safety
and comfort inter alia relies on high performance approaches to trajectory planning.
Because of the broad variety of approaches to trajectory planning, which is mainly
driven by the requirements originating from diverse applications, a taxonomy is pre-
sented that groups existing approaches to provide a conclusive overview. However,
in general it is vital to define the role of trajectory planning within the functional
architecture of the automated vehicle. To narrow the scope of the trajectory planning
approach, in a first step requirements and interfaces need to be defined. Especially the
interface to the superordinated modules is important, because it has a considerable
impact on the performance of the trajectory planner. For this reason a generic interface
to the maneuver decision is defined that is able to act on maneuver decision as well
as on maneuver planning level. To harmonize the direct interface between trajectory
planning and the high level decision making an approach to maneuver planning is
sketched that enables trajectory planning in accordance to the scope of this work.
Furthermore, the environment-aware maneuver planning is important to provide an
appropriate initialization and starting point for the presented discrete and continuous
trajectory planning approach. Obviously, a discrete approach can deal better with no
initialization. However, for the sake of comparability this aspect is not focused in this
thesis, such that both developed approaches have to solve the same planning problem.
In this thesis an efficient and scaleable, general approach to trajectory planning is
presented. The core of the developed approach is represented by the proposed two
level hierarchical trajectory planning framework. The separation in high level and low
level behavior accounts for the fact that the desired high level behavior changes in
dependence on the current situation, whereas the low level behavior should basically
adjust optimally without totally enforcing, but still pursuing its own objectives (i.e.
97
Chapter 9. Conclusion and Outlook
driving comfort). As a result a generic trajectory planning concept is introduced that
explicitly considers the problem of conflicting planning objectives via its hierarchical
character. The idea of the hierarchical trajectory planning framework can be mapped to
the theory of bilevel programming. For the particular case that the lower level problem
has a unique optimal solution, the bilevel programming problem is equivalent to a
single level optimization problem. As a consequence common approaches to trajectory
optimization can be applied in order to tackle the trajectory planning problem on
high level. To solve the high level problem two distinct approaches are introduced.
Referring to the characteristic of the optimal parameters a discrete and a continu-
ous trajectory optimization approach are developed. Evidently, results show that the
presented approaches share the advantages and disadvantages of optimization and
sampling-based trajectory planning approaches, respectively. Whereas the continuous
approach relies on numerical optimization, the developed discrete trajectory planning
approach belongs to the class of trajectory rollout. One key feature of the presented
trajectory planning framework is the spline-based trajectory representation. On low
level an advanced approach to optimal interpolation for spline-based trajectory plan-
ning is used, denoted as minimum kinematics trajectory generation. The formulated
low level problem can efficiently be solved in an analytical way or by using quadratic
programming techniques.
In general a spline can solve the constrained trajectory planning problem in an opti-
mal fashion, since it is able to adapt to the objectives of the high level cost function.
Desired behavior can hence be reproduced if the corresponding reference is chosen
appropriately. The underlying low level problem, which supports the idea of mini-
mum kinematics trajectory generation for spline interpolation is inherently designed
to be optimal. The spline representation offers planning versatility, while reducing the
number of decision variables at the same time. As an analytic solution is provided
continuous evaluation of each spline is enabled as well, facilitating the coupling of
longitudinal and lateral dynamics. Moreover, interpolation permits to prolong the pre-
diction horizon without increasing the number of decision variables and can hence be
regarded as an efficient way of formulating the trajectory planning problem.
The spline representation and the choice of the vehicle model synthesize to a beneficial
approach as the integration of system dynamics is accomplished by means of accurate
and fast analytic calculations. Whereas longitudinal motion is captured on kinematic
level, the modeling of lateral vehicle dynamics is thoughtfully chosen to find a com-
promise between an accurate solution for the intended use cases and a reasonable
computational effort. A vehicle model with steady state yaw dynamics of a linear
single track model is chosen that provides the relation between steering angle and yaw
rate at steady state. Analogous to the choice of a suitable vehicle model, the applied
approaches to static and dynamic collision checking need to balance the aspects of
accuracy and computational effort. Since collision checking is directly connected to the
aspect of safety, accuracy is prioritized to a large extent. For static collision checking
this means that the ego vehicle shape is modeled by its bounding box and the road
model is built from perceived lane markers represented by polygonal chains. Then a
pseudo distance transform is used that allows for a fast shortest distance calculation.
For dynamic collision checking vehicles are approximated by a predefined number
98
9.2. Outlook
of circles, accounting for the uncertainty in object detection, since a certain margin is
added to the vehicle shape. Dynamic collision checking reduces to simple distance
calculations and threshold evaluations, which ensures a low computational burden.
All taken measures and features of the developed trajectory planning framework en-
gage with each other, since the spline-based representation allows for optimal mini-
mum kinematics trajectory generation and enables the analytic calculation of kinemat-
ics, such that the property of flatness can be exploited to integrate vehicle dynamics. A
seamlessly embedded solution is presented covering key aspects of trajectory planning
for on-road driving in dynamic environments, such as accurate collision checking and
appropriate vehicle dynamics modeling. This leads to a generic trajectory planning
concept that addresses applicability for a broad range of scenarios, since it provides an
optimal solution in terms of the formulated hierarchical trajectory planning problem.
9.2. Outlook
The search for a suitable trajectory planning approach is restricted by real-time re-
quirements on the processing time. In particular, a high replanning frequency leads
to a reactive layer that is capable of dealing with changes in the dynamic environ-
ment. As results and previous discussion have shown the latter aspect leaves some
room for improvement. The spline-based design enables a versatile usage in terms
of the number of breakpoints and could dynamically be adjusted to account for the
current situation. In order to increase the accuracy for lane centering for example, the
number of breakpoints for the lateral spline could be adjusted with respect to the road
curvature. From the general considerations made, the proposed framework should
be readily adaptable to enhance the scope to urban scenarios and parking applica-
tions. Still, some work will be necessary to establish an associated proof of concept.
The dependency of trajectory planning on higher level modules like perception and
decision making is evident. Clearly, the inclusion of map data for example would
further enhance the quality of the overall results. In order to find the best trajectory
among several maneuvers, within maneuver decision the space might be segmented
into regions that will most likely contribute to the best solution. The procedure can
be extended by evaluating different maneuver hypothesis in parallel, planning the
optimal trajectory for each considered possibility and finally choosing the overall op-
timal solution. To broaden the scope of maneuver decision and maneuver planning as
presented in this thesis a more advanced solution is necessary. However, the develop-
ment of an approach related to these kinds of problems represents a separate research
topic. Another interesting research direction is the development of cooperative motion
planning approaches that own a certain kind of interaction awareness. Although hard
to capture this is a very desirable feature, since it reflects the impact of the ego vehi-
cle on the surrounding obstacle vehicles and vice versa. Moreover, it could be worth
considering a probabilistic trajectory representation. This is left for further work.
To finally launch highly automated driving functions it will require combined re-
sources and substantial progress in all related high technology fields. Hence, industry
and research need to show creativity and ingenuity to advance the state of the art.
99
Bibliography
Althoff, M. (2010): “Reachability Analysis and its Application to the Safety Assessment
of Autonomous Cars”. Dissertation. Munich: Technical University of Munich.
Ardelt, M., C. Coester, and N. Kaempchen (2012): “Highly Automated Driving on
Freeways in Real Traffic Using a Probabilistic Framework”. In: IEEE Transactions on
Intelligent Transportation Systems 13.4, pp. 1576–1585.
Arslan, O., K. Berntorp, and P. Tsiotras (2017): “Sampling-based algorithms for opti-
mal motion planning using closed-loop prediction”. In: IEEE International Conference
on Robotics and Automation (ICRA), pp. 4991–4996.
Banzhaf, H., N. Berinpanathan, D. Nienhüser, and J. Zöllner (2018a): “From G 2 to
G 3 Continuity: Continuous Curvature Rate Steering Functions for Sampling-Based
Nonholonomic Motion Planning”. In: IEEE Intelligent Vehicles Symposium (IV), pp. 326–
333.
Banzhaf, H., M. Dolgov, J. Stellet, and J. M. Zöllner (2018b): “From Footprints to
Beliefprints: Motion Planning under Uncertainty for Maneuvering Automated Vehi-
cles in Dense Scenarios”. In: IEEE International Conference on Intelligent Transportation
Systems (ITSC), pp. 1680–1687.
Bauer, E., F. Lotz, M. Pfromm, M. Schreier, S. Cieler, A. Eckert, A. Hohm, S. Lüke,
P. Rieth, B. Abendroth, V. Willert, J. Adamy, R. Bruder, U. Konigorski, and H.
Winner (2012): “PRORETA 3: An Integrated Approach to Collision Avoidance and
Vehicle Automation”. In: at - Automatisierungstechnik 60.12, pp. 755–765.
Bellman, R. (1957): Dynamic Programming. 1st ed. Princeton, NJ, USA: Princeton Uni-
versity Press.
Bender, P., J. Ziegler, and C. Stiller (2014): “Lanelets: Efficient map representation for
autonomous driving”. In: IEEE Intelligent Vehicles Symposium (IV), pp. 420–425.
Bergman, K. and D. Axehill (2018): “Combining Homotopy Methods and Numerical
Optimal Control to Solve Motion Planning Problems”. In: IEEE Intelligent Vehicles
Symposium (IV), pp. 347–354.
Betts, J. T. (2010): Practical Methods for Optimal Control and Estimation Using Nonlinear
Programming. 2nd ed. Society for Industrial and Applied Mathematics.
Brandt, T., T. Sattel, and M. Böhm (2007): “Combining haptic human-machine in-
teraction with predictive path planning for lane-keeping and collision avoidance
systems”. In: IEEE Intelligent Vehicles Symposium (IV), pp. 582–587.
100
Bibliography
Bry, A., C. Richter, A. Bachrach, and N. Roy (2015): “Aggressive Flight of Fixed-Wing
and Quadrotor Aircraft in Dense Indoor Environments”. In: The International Journal
of Robotics Research 34.7, pp. 969–1002.
Buehler, M., K. Iagnemma, and S. Singh (2009): The DARPA Urban Challenge: Au-
tonomous Vehicles in City Traffic, George Air Force Base, Victorville, California, USA.
Vol. 56. Springer.
Campbell, M., M. Egerstedt, J. P. How, and R. M. Murray (2010): “Autonomous
driving in urban environments: approaches, lessons and challenges”. In: Philo-
sophical transactions. Series A, Mathematical, physical, and engineering sciences 368.1928,
pp. 4649–4672.
Cormen, T. H., C. E. Leiserson, R. L. Rivest, and C. Stein (2009): Introduction to Algo-
rithms. 3rd ed. The MIT Press.
De Luca Alessandro and Oriolo, G. and C. Samson (1998): “Feedback control of
a nonholonomic car-like robot”. In: Robot Motion Planning and Control. Ed. by J.-P.
Laumond. Berlin, Heidelberg: Springer Berlin Heidelberg, pp. 171–253.
Dempe, S. (2002): Foundations of Bilevel Programming. 1st ed. Vol. 61. Nonconvex Opti-
mization and Its Applications. Boston MA: Springer.
Dickmanns, E. D. and A. Zapp (1987): “Autonomous High Speed Road Vehicle Guid-
ance by Computer Vision”. In: IFAC Proceedings Volumes 20.5, Part 4. 10th Triennial
IFAC Congress on Automatic Control - 1987 Volume IV, Munich, Germany, 27-31
July, pp. 221–226.
Diehl, M., H. G. Bock, H. Diedam, and P.-B. Wieber (2005): “Fast Direct Multiple
Shooting Algorithms for Optimal Robot Control”. In: Fast Motions in Biomechanics
and Robotics. Vol. 340. Heidelberg: Springer Berlin Heidelberg, pp. 65–93.
Dijkstra, E. W. (1959): “A Note on Two Problems in Connexion with Graphs”. In:
Numerische Mathematik 1.1, pp. 269–271.
Dolgov, D., S. Thrun, M. Montemerlo, and J. Diebel (2010): “Path Planning for Au-
tonomous Vehicles in Unknown Semi-structured Environments”. In: The Interna-
tional Journal of Robotics Research 29.5, pp. 485–501.
Donges, E. (2016): “Driver Behavior Models”. In: Handbook of Driver Assistance Systems:
Basic Information, Components and Systems for Active Safety and Comfort. Ed. by H.
Winner, S. Hakuli, F. Lotz, and C. Singer. Springer International Publishing, pp. 19–
33.
Dubins, L. E. (1957): “On Curves of Minimal Length with a Constraint on Average
Curvature, and with Prescribed Initial and Terminal Positions and Tangents”. In:
American Journal of Mathematics 79.3, pp. 497–516.
Elfes, A. (1989): “Using occupancy grids for mobile robot perception and navigation”.
In: Computer 22.6, pp. 46–57.
101
Bibliography
Ferguson, D., M. Darms, C. Urmson, and S. Kolski (2008a): “Detection, prediction,
and avoidance of dynamic obstacles in urban environments”. In: IEEE Intelligent
Vehicles Symposium (IV), pp. 1149–1154.
Ferguson, D., T. M. Howard, and M. Likhachev (2008b): “Motion planning in urban
environments”. In: Journal of Field Robotics 25.11-12, pp. 939–960.
Fliess, M., J. Lévine, P. Martin, and P. Rouchon (1992): “Sur les systèmes non linéaires
différentiellement plats”. In: C.R. Acad. Sci. Paris 315, pp. 619–624.
Fliess, M., J. Lévine, P. Martin, and P. Rouchon (1995): “Flatness and defect of non-
linear systems: introductory theory and examples”. In: International Journal of Control
61.6, pp. 1327–1361.
Frasch, J. V., A. Gray, M. Zanon, H. J. Ferreau, S. Sager, F. Borrelli, and M. Diehl
(2013): “An auto-generated nonlinear MPC algorithm for real-time obstacle avoid-
ance of ground vehicles”. In: European Control Conference (ECC), pp. 4136–4141.
Gilbert, E. G., D. W. Johnson, and S. S. Keerthi (1988): “A fast procedure for com-
puting the distance between complex objects in three-dimensional space”. In: IEEE
Journal on Robotics and Automation 4.2, pp. 193–203.
Gonzalez, D., J. Perez, V. Milanes, and F. Nashashibi (2016): “A Review of Motion
Planning Techniques for Automated Vehicles”. In: IEEE Transactions on Intelligent
Transportation Systems 17.4, pp. 1135–1145.
Götte, C., M. Keller, C. Hass, and T. Bertram (2016a): “Model Predictive Planning
and Control Applied to Critical Traffic Situations”. In: ATZ worldwide eMagazines
2016-09, pp. 64–68.
Götte, C., M. Keller, C. Hass, and T. Bertram (2016b): “Modellprädiktive Planung
und Regelung im Anwendungsfall kritischer Verkehrssituationen”. In: ATZ - Auto-
mobiltechnische Zeitschrift 9, pp. 68–73.
Götte, C., M. Keller, C. Haß, K.-H. Glander, A. Seewald, and T. Bertram (2015a): “A
model predictive combined planning and control approach for guidance of auto-
mated vehicles”. In: IEEE International Conference on Vehicular Electronics and Safety
(ICVES), pp. 69–74.
Götte, C., M. Keller, T. Nattermann, C. Haß, K.-H. Glander, and T. Bertram (2017b):
“Ein polynombasierter Planungsansatz zur Generierung einer optimalen Trajektorie:
A Polynomial based Planning Approach to the Generation of an Optimal Trajectory”.
In: VDI/VDE Mechatroniktagung. Dresden, pp. 134–139.
Götte, C., M. Keller, T. Nattermann, C. Haß, K.-H. Glander, and T. Bertram (2017c):
“Spline-based Motion Planning for Automated Driving”. In: Proceedings of the 20th
IFAC World Congress, pp. 9444–9449.
Götte, C., M. Keller, C. Rösmann, T. Nattermann, C. Haß, K.-H. Glander, A. See-
wald, and T. Bertram (2016d): “A Real-Time Capable Model Predictive Approach to
102
Bibliography
Lateral Vehicle Guidance”. In: IEEE International Conference on Intelligent Transporta-
tion Systems (ITSC), pp. 1908–1913.
Götte, C., C. Wissing, M. Keller, T. Nattermann, C. Haß, K.-H. Glander, and T.
Bertram (2017d): “Simulationsbasierte Entwicklung eines Trajektorienplanungsver-
fahrens für automatisiertes Fahren: Simulation based Development of a Trajectory
Planning Approach for Automated Driving”. In: AUTOREG 2017, Automatisiertes
Fahren und vernetzte Mobilität. 8. VDI/VDE-Fachtagung. VDI Berichte 2292. Berlin,
pp. 247–259.
Hart, P. E., N. J. Nilsson, and B. Raphael (1968): “A Formal Basis for the Heuristic
Determination of Minimum Cost Paths”. In: IEEE Transactions on Systems Science and
Cybernetics 4.2, pp. 100–107.
Hesse, T., D. Heß, and T. Sattel (2010): “Motion planning for passenger vehicles -
Force field trajectory optimization for automated driving”. In: Proceedings of the
IASTED International Conference on Robotics and Applications.
Hilgert, J., K. Hirsch, T. Bertram, and M. Hiller (2003): “Emergency path planning
for autonomous vehicles using elastic band theory”. In: IEEE/ASME International
Conference on Advanced Intelligent Mechatronics (AIM), pp. 1390–1395.
Homann, A., M. Buss, M. Keller, K.-H. Glander, and T. Bertram (2018): “Multi Stage
Model Predictive Trajectory Set Approach for Collision Avoidance”. In: IEEE Intelli-
gent Transportation Systems Conference (ITSC), pp. 945–950.
Homann, A., C. Lienke, M. Keller, M. Buss, M. Mohamed, and T. Bertram (2019):
“Sampling-Based Trajectory Planning and Control for a Collision Avoidance System”.
In: IEEE Intelligent Transportation Systems Conference (ITSC), pp. 2956–2962.
Hundelshausen, F. v., M. Himmelsbach, F. Hecker, A. Mueller, and H.-J. Wünsche
(2008): “Driving with tentacles: Integral structures for sensing and motion”. In:
Journal of Field Robotics 25.9, pp. 640–673.
Jeon, J. h., R. V. Cowlagi, S. C. Peters, S. Karaman, E. Frazzoli, P. Tsiotras, and K.
Iagnemma (2013): “Optimal motion planning with the half-car dynamical model for
autonomous high-speed driving”. In: American Control Conference (ACC), pp. 188–
193.
Jimenez, P., F. Thomas, and C. Torras (2006): “Collision detection algorithms for mo-
tion planning”. In: Robot Motion Planning and Control. Ed. by J.-P. Laumond. Berlin,
Heidelberg: Springer Berlin Heidelberg, pp. 305–343.
Karaman, S. and E. Frazzoli (2011): “Sampling-based algorithms for optimal motion
planning”. In: The International Journal of Robotics Research 30.7, pp. 846–894.
Karaman, S., M. Walter, A. Perez, E. Frazzoli, and S. Teller (2011): “Anytime Motion
Planning using the RRT*”. In: IEEE International Conference on Robotics and Automa-
tion (ICRA), pp. 1478–1483.
103
Bibliography
Katrakazas, C., M. Quddus, W.-H. Chen, and L. Deka (2015): “Real-time motion plan-
ning methods for autonomous on-road driving: State-of-the-art and future research
directions”. In: Transportation Research Part C: Emerging Technologies 60, pp. 416–442.
Kavraki, L. E., P. Svestka, J.-C. Latombe, and M. H. Overmars (1996): “Probabilistic
Roadmaps for Path Planning in High-Dimensional Configuration Spaces”. In: IEEE
Transactions on Robotics and Automation 12, pp. 566–580.
Keller, M., C. Hass, A. Seewald, and T. Bertram (2015): “A Model Predictive Ap-
proach to Emergency Maneuvers in Critical Traffic Situations”. In: IEEE International
Conference on Intelligent Transportation Systems (ITSC), pp. 369–374.
Keller, M., F. Hoffmann, T. Bertram, C. Hass, and A. Seewald (2014): “Planning of
Optimal Collision Avoidance Trajectories with Timed Elastic Bands”. In: Proceedings
of the 19th IFAC World Congress, pp. 9822–9827.
Kelly, M. (2017): “An Introduction to Trajectory Optimization: How to Do Your Own
Direct Collocation”. In: SIAM Review 59.4, pp. 849–904.
Klomp, M., M. Jonasson, L. Laine, L. Henderson, E. Regolin, and S. Schumi (2019):
“Trends in vehicle motion control for automated driving on public roads”. In: Vehicle
System Dynamics 57.7, pp. 1028–1061.
Kunz, F. and K. Dietmayer (2016): “Hybrid Discrete-Parametric Optimization for Tra-
jectory Planning in On-Road Driving Scenarios”. In: IEEE International Conference on
Intelligent Transportation Systems (ITSC), pp. 802–807.
Kuwata, Y., J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How (2009): “Real-
Time Motion Planning With Applications to Autonomous Urban Driving”. In: IEEE
Transactions on Control Systems Technology 17.5, pp. 1105–1118.
Kuwata, Y., J. Teo, S. Karaman, G. Fiore, E. Frazzoli, and J. How (2008): “Motion
Planning in Complex Environments Using Closed-loop Prediction”. In: AIAA Guid-
ance, Navigation and Control Conference and Exhibit.
LaValle, S. M. (1998): “Rapidly-exploring random trees: A new tool for path planning”.
PhD thesis. Iowa State University.
LaValle, S. M. (2006): Planning Algorithms. Cambridge University Press.
Lefèvre, S., D. Vasquez, and C. Laugier (2014): “A survey on motion prediction and
risk assessment for intelligent vehicles”. In: Robomech Journal 1.
Levenberg, K. (1944): “A Method for the Solution of Certain Non-Linear Problems in
Least Squares”. In: Quarterly of Applied Mathematics 2.2, pp. 164–168.
Lienke, C., M. Keller, K.-H. Glander, and T. Bertram (2018b): “An Ad-hoc Sam-
pling-based Planner for On-road Automated Driving”. In: IEEE Intelligent Trans-
portation Systems Conference (ITSC), pp. 2371–2376.
104
Bibliography
Lienke, C., M. Keller, K.-H. Glander, and T. Bertram (2018c): “Environment Modeling
for the Application in Optimization-based Trajectory Planning”. In: IEEE Intelligent
Vehicles Symposium (IV), pp. 498–503.
Lienke, C., M. Schmidt, C. Wissing, M. Keller, C. Manna, T. Nattermann, and T.
Bertram (2019a): “Core components of automated driving – algorithms for situation
analysis, decision-making, and trajectory planning”. In: ATZ live – Automatisiertes
Fahren.
Lienke, C., C. Wissing, M. Keller, T. Nattermann, and T. Bertram (2019b): “Predictive
Driving: Fusing Prediction and Planning for Automated Highway Driving”. In: IEEE
Transactions on Intelligent Vehicles 4.3, pp. 456–467.
Likhachev, M. and D. Ferguson (2008): “Planning Long Dynamically-Feasible Ma-
neuvers For Autonomous Vehicles”. In: Proceedings of Robotics: Science and Systems
(RSS).
Likhachev, M. and D. Ferguson (2009): “Planning Long Dynamically-Feasible Ma-
neuvers For Autonomous Vehicles”. In: The International Journal of Robotics Research
28 (8), pp. 933–945.
Likhachev, M., G. Gordon, and S. Thrun (2003): “ARA*: Anytime A* with Prov-
able Bounds on Sub-Optimality”. In: Proceedings of Advances in Neural Information
Processing Systems (NIPS). Vol. 16.
Lynch, K. M. and F. C. Park (2017): Modern Robotics: Mechanics, Planning, and Control.
1st ed. Cambridge University Press.
Ma, L., J. Xue, K. Kawabata, J. Zhu, C. Ma, and N. Zheng (2015): “Efficient Sam-
pling-Based Motion Planning for On-Road Autonomous Driving”. In: IEEE Transac-
tions on Intelligent Transportation Systems 16.4, pp. 1961–1976.
Marquardt, D. W. (1963): “An Algorithm for Least-Squares Estimation of Nonlin-
ear Parameters”. In: Journal of the Society for Industrial and Applied Mathematics 11.2,
pp. 431–441.
Marti, E., M. A. de Miguel, F. Garcia, and J. Perez (2019): “A Review of Sensor Tech-
nologies for Perception in Automated Driving”. In: IEEE Intelligent Transportation
Systems Magazine 11.4, pp. 94–108.
Martin, P., R. Murray, and P. Rouchon (2003): “Flat Systems, Equivalence and Trajec-
tory Generation”. In: CDS Technical Report, pp. 1–79.
McNaughton, M., C. Urmson, J. M. Dolan, and J.-W. Lee (2011): “Motion planning for
autonomous driving with a conformal spatiotemporal lattice”. In: IEEE International
Conference on Robotics and Automation (ICRA), pp. 4889–4895.
Meywerk, M. (2015): Vehicle Dynamics. Chichester: John Wiley & Sons Ltd.
Miller, C., C. Pek, and M. Althoff (2018): “Efficient Mixed-Integer Programming for
Longitudinal and Lateral Motion Planning of Autonomous Vehicles”. In: IEEE Intel-
ligent Vehicles Symposium (IV), pp. 1954–1961.
105
Bibliography
Mitschke, M. and H. Wallentowitz (2014): Dynamik der Kraftfahrzeuge. Wiesbaden:
Springer Vieweg.
Montemerlo, M., J. Becker, S. Bhat, H. Dahlkamp, D. Dolgov, S. Ettinger, D. Haehnel,
T. Hilden, G. Hoffmann, B. Huhnke, D. Johnston, S. Klumpp, D. Langer, A.
Levandowski, J. Levinson, J. Marcil, D. Orenstein, J. Paefgen, I. Penny, and S.
Thrun (2008): “Junior: The Stanford Entry in the Urban Challenge”. In: Journal of
Field Robotics 25, pp. 569–597.
Mouhagir, H., R. Talj, V. Cherfaoui, F. Aioun, and F. Guillemard (2016): “Integrat-
ing safety distances with trajectory planning by modifying the occupancy grid for
autonomous vehicle navigation”. In: IEEE International Conference on Intelligent Trans-
portation Systems (ITSC), pp. 1114–1119.
Mouhagir, H., R. Talj, V. Cherfaoui, F. Aioun, and F. Guillemard (2020): “Eviden-
tial-Based Approach for Trajectory Planning With Tentacles, for Autonomous Vehi-
cles”. In: IEEE Transactions on Intelligent Transportation Systems 21.8, pp. 3485–3496.
Mullakkal-Babu, F. A., M. Wang, B. van Arem, and R. Happee (2020): “Empirics and
Models of Fragmented Lane Changes”. In: IEEE Open Journal of Intelligent Trans-
portation Systems 1, pp. 187–200.
Nietzschmann, C., S. Klaudt, C. Klas, D. Will, and L. Eckstein (2018): “Trajectory op-
timization for Car-Like Vehicles in Structured and Semi-Structured Environments”.
In: IEEE Intelligent Vehicles Symposium (IV), pp. 504–510.
Nilsson, J. (2016): “Automated Driving Maneuvers - Trajectory Planning via Convex
Optimization in the Model Predictive Control Framework”. PhD thesis. Chalmers
University of Technology.
Nilsson, J., M. Brännström, E. Coelingh, and J. Fredriksson (2017): “Lane Change
Maneuvers for Automated Vehicles”. In: IEEE Transactions on Intelligent Transporta-
tion Systems 18.5, pp. 1087–1096.
Nilsson, J., P. Falcone, M. Ali, and J. Sjöberg (2015): “Receding horizon maneu-
ver generation for automated highway driving”. In: Control Engineering Practice 41,
pp. 124–133.
Nocedal, J. and S. J. Wright (2006): Numerical Optimization. 2nd ed. Springer Series in
Operation Research and Financial Engineering. New York, NY, USA: Springer.
Pacejka, H. B. (2007): Tyre and vehicle dynamics. 2nd ed. Oxford: Butterworth-Heine-
mann.
Paden, B., M. Čáp, S. Z. Yong, D. Yershov, and E. Frazzoli (2016): “A Survey of
Motion Planning and Control Techniques for Self-Driving Urban Vehicles”. In: IEEE
Transactions on Intelligent Vehicles 1.1, pp. 33–55.
Pek, C. and M. Althoff (2018): “Computationally Efficient Fail-safe Trajectory Plan-
ning for Self-driving Vehicles Using Convex Optimization”. In: IEEE International
Conference on Intelligent Transportation Systems (ITSC), pp. 1447–1454.
106
Bibliography
Qian, X., F. Altché, P. Bender, C. Stiller, and A. de La Fortelle (2016): “Optimal Trajec-
tory Planning for Autonomous Driving Integrating Logical Constraints: An MIQP
Perspective”. In: IEEE International Conference on Intelligent Transportation Systems
(ITSC), pp. 205–210.
Rajamani, R. (2012): Vehicle Dynamics and Control. 2nd ed. New York, Dordrecht, Hei-
delberg, London: Springer Verlag.
RAS-L (1995): Richtlinien für die Anlage von Straßen RAS, Teil: Linienführung RAS-L.
Forschungsgesellschaft für Straßen- und Verkehrswesen, Arbeitsgruppe Straßenen-
twurf.
Reeds, J. A. and L. A. Shepp (1990): “Optimal paths for a car that goes both forwards
and backwards”. In: vol. 145. 2, pp. 367–393.
Rösmann, C., A. Makarow, and T. Bertram (2021): “Online Motion Planning Based
on Nonlinear Model Predictive Control with Non-Euclidean Rotation Groups”. In:
European Control Conference (ECC), pp. 1583–1590.
SAE (2018): Taxonomy and Definitions for Terms Related to Driving Automation Systems
for On-Road Motor Vehicles. J3016.
Sattel, T. and T. Brandt (2005): “Ground vehicle guidance along collision-free trajec-
tories using elastic bands”. In: Proceedings of the 2005 American Control Conference,
pp. 4991–4996.
Schlechtriemen, J., K. P. Wabersich, and K.-D. Kuhnert (2016): “Wiggling through
complex traffic: Planning trajectories constrained by predictions”. In: IEEE Intelligent
Vehicles Symposium (IV), pp. 1293–1300.
Schmidt, M., M. Krüger, C. Lienke, M. Oeljeklaus, T. Nattermann, M. Mohamed,
F. Hoffmann, and T. Bertram (2019a): “Lane detection for automated driving using
Deep Learning”. In: at – Automatisierungstechnik 67.10, pp. 866–878.
Schmidt, M., C. Lienke, M. Oeljeklaus, M. Krüger, T. Nattermann, M. Mohamed,
F. Hoffmann, and T. Bertram (2018): “Fahrspurerkennung mit Deep Learning für
automatisierte Fahrfunktionen”. In: 28. Workshop Computational Intelligence.
Schmidt, M., C. Manna, J. H. Braun, C. Wissing, M. Mohamed, and T. Bertram
(2019b): “An Interaction-Aware Lane Change Behavior Planner for Automated Ve-
hicles on Highways Based on Polygon Clipping”. In: IEEE Robotics and Automation
Letters 4.2, pp. 1876–1883.
Schramm, D., M. Hiller, and R. Bardini (2018): Vehicle Dynamics. Berlin Heidelberg:
Springer Verlag.
Schwesinger, U., M. Rufli, P. Furgale, and R. Siegwart (2013): “A sampling-based par-
tial motion planning framework for system-compliant navigation along a reference
path”. In: IEEE Intelligent Vehicles Symposium (IV), pp. 391–396.
Schwesinger, U., R. Siegwart, and P. Furgale (2015): “Fast collision detection through
bounding volume hierarchies in workspace-time space for sampling-based motion
107
Bibliography
planners”. In: IEEE International Conference on Robotics and Automation (ICRA), pp. 63–
68.
Sinha, A., P. Malo, and K. Deb (2018): “A Review on Bilevel Optimization: From
Classical to Evolutionary Approaches and Applications”. In: IEEE Transactions on
Evolutionary Computation 22.2, pp. 276–295.
Stellato, B., G. Banjac, P. Goulart, A. Bemporad, and S. Boyd (2020): “OSQP: an op-
erator splitting solver for quadratic programs”. In: Mathematical Programming Com-
putation 12.4, pp. 637–672.
Swaroop, D. and K. R. Rajagopal (2001): “A review of constant time headway policy
for automatic vehicle following”. In: IEEE Intelligent Transportation Systems Conference
(ITSC), pp. 65–69.
Tanzmeister, G. and D. Wollherr (2017): “Evidential Grid-Based Tracking and Map-
ping”. In: IEEE Transactions on Intelligent Transportation Systems 18.6, pp. 1454–1467.
Toledo, T. and D. Zohar (2007): “Modeling Duration of Lane Changes”. In: Transporta-
tion Research Record 1999.1, pp. 71–78.
Ulbrich, F., D. Goehring, T. Langner, Z. Boroujeni, and R. Rojas (2017): “Stable
Timed Elastic Band with Loose Ends”. In: IEEE Intelligent Vehicles Symposium (IV),
pp. 186–192.
Ulbrich, F., S. Sundermann, T. Langner, D. Goehring, and R. Rojas (2018): “Follow-
ing Cars With Elastic Bands”. In: IEEE Intelligent Vehicles Symposium (IV), pp. 1529–
1536.
Ulbrich, S. M. (2018): “Towards Tactical Lane Change Behavior Planning for Auto-
mated Vehicles”. PhD thesis.
Urmson, C., J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. Clark, J. Dolan, D. Dug-
gins, T. Galatali, C. Geyer, M. Gittleman, S. Harbaugh, M. Hebert, T. Howard,
S. Kolski, A. Kelly, M. Likhachev, M. McNaughton, N. Miller, and D. Ferguson
(2009): “Autonomous Driving in Urban Environments: Boss and the Urban Chal-
lenge”. In: Journal of Field Robotics 25, pp. 1–59.
Wang, J., J. Wu, and Y. Li (2015): “The Driving Safety Field Based on Driver–Vehi-
cle–Road Interactions”. In: IEEE Transactions on Intelligent Transportation Systems 16.4,
pp. 2203–2214.
Werling, M. (2011): “Ein neues Konzept für die Trajektoriengenerierung und -stabil-
isierung in zeitkritischen Verkehrsszenarien”. German. PhD thesis. 141 pp.
Werling, M., S. Kammel, J. Ziegler, and L. Groell (2012): “Optimal trajectories for
time-critical street scenarios using discretized terminal manifolds”. In: The Interna-
tional Journal of Robotics Research 31, pp. 346–359.
Werling, M. and D. Liccardo (2012): “Automatic collision avoidance using model-pre-
dictive online optimization”. In: IEEE 51st Conference on Decision and Control (CDC),
pp. 6309–6314.
108
Bibliography
Werling, M., J. Ziegler, S. Kammel, and S. Thrun (2010): “Optimal trajectory genera-
tion for dynamic street scenarios in a Frenét Frame”. In: IEEE International Conference
on Robotics and Automation (ICRA), pp. 987–993.
Willemsen, P., J. K. Kearney, and H. Wang (2003): “Ribbon networks for modeling
navigable paths of autonomous agents in virtual urban environments”. In: IEEE
Virtual Reality, pp. 79–86.
Wissing, C., K.-H. Glander, C. Haß, T. Nattermann, and T. Bertram (2017): “Devel-
opment and test of a lane change prediction algorithm for automated driving”. In:
Fahrerassistenzsysteme 2017. Ed. by R. Isermann. Wiesbaden: Springer Fachmedien
Wiesbaden, pp. 385–402.
Wissing, C., T. Nattermann, K.-H. Glander, and T. Bertram (2018): “Trajectory Pre-
diction for Safety Critical Maneuvers in Automated Highway Driving”. In: IEEE
Intelligent Transportation Systems Conference (ITSC), pp. 131–136.
Wissing, C., T. Nattermann, K.-H. Glander, A. Seewald, and T. Bertram (2016): “En-
vironment Simulation for the Development, Evaluation and Verification of Underly-
ing Algorithms for Automated Driving”. In: AmE 2016 - Automotive meets Electronics;
7th GMM-Symposium, pp. 9–14.
Xu, W., J. Wei, J. M. Dolan, H. Zhao, and H. Zha (2012): “A Real-Time Motion Plan-
ner with Trajectory Optimization for Autonomous Vehicles”. In: IEEE International
Conference on Robotics and Automation, pp. 2061–2067.
Ye, J. J. and D. Zhu (2010): “New necessary optimality conditions for bilevel pro-
grams by combining MPEC and the value function approach”. In: SIAM Journal on
Optimization 20.4, pp. 1885–1905.
Yi, B., P. Bender, F. Bonarens, and C. Stiller (2019): “Model Predictive Trajectory
Planning for Automated Driving”. In: IEEE Transactions on Intelligent Vehicles 4.1,
pp. 24–38.
Ziegler, J., P. Bender, T. Dang, and C. Stiller (2014): “Trajectory planning for Bertha
— A local, continuous method”. In: IEEE Intelligent Vehicles Symposium (IV), pp. 450–
457.
Ziegler, J., P. Bender, M. Schreiber, H. Lategahn, T. Strauß, C. Stiller, T. Dang, U.
Franke, N. Appenrodt, C. Keller, E. Kaus, R. Herrtwich, C. Rabe, D. Pfeiffer, F.
Lindner, F. Stein, F. Erbs, M. Enzweiler, C. Knoeppel, and E. Zeeb (2015): “Making
Bertha Drive — An Autonomous Journey on a Historic Route”. In: IEEE Intelligent
Transportation Systems Magazine 6, pp. 8–20.
Ziegler, J. and C. Stiller (2009): “Spatiotemporal state lattices for fast trajectory plan-
ning in dynamic on-road driving scenarios”. In: IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS), pp. 1879–1884.
Ziegler, J. and C. Stiller (2010): “Fast collision checking for intelligent vehicle motion
planning”. In: IEEE Intelligent Vehicles Symposium (IV), pp. 518–522.
109
Bibliography
Peer-Reviewed Publications
With first authorship
Lienke, C., M. Schmidt, C. Wissing, M. Keller, C. Manna, T. Nattermann, and T.
Bertram (2019a): “Core components of automated driving – algorithms for situation
analysis, decision-making, and trajectory planning”. In: ATZ live – Automatisiertes
Fahren.
Lienke, C., C. Wissing, M. Keller, T. Nattermann, and T. Bertram (2019b): “Predictive
Driving: Fusing Prediction and Planning for Automated Highway Driving”. In: IEEE
Transactions on Intelligent Vehicles 4.3, pp. 456–467.
Lienke, C., M. Keller, K.-H. Glander, and T. Bertram (2018b): “An Ad-hoc Sam-
pling-based Planner for On-road Automated Driving”. In: IEEE Intelligent Trans-
portation Systems Conference (ITSC), pp. 2371–2376.
Lienke, C., M. Keller, K.-H. Glander, and T. Bertram (2018c): “Environment Modeling
for the Application in Optimization-based Trajectory Planning”. In: IEEE Intelligent
Vehicles Symposium (IV), pp. 498–503.
Götte, C., M. Keller, T. Nattermann, C. Haß, K.-H. Glander, and T. Bertram (2017b):
“Ein polynombasierter Planungsansatz zur Generierung einer optimalen Trajektorie:
A Polynomial based Planning Approach to the Generation of an Optimal Trajectory”.
In: VDI/VDE Mechatroniktagung. Dresden, pp. 134–139.
Götte, C., M. Keller, T. Nattermann, C. Haß, K.-H. Glander, and T. Bertram (2017c):
“Spline-based Motion Planning for Automated Driving”. In: Proceedings of the 20th
IFAC World Congress, pp. 9444–9449.
Götte, C., C. Wissing, M. Keller, T. Nattermann, C. Haß, K.-H. Glander, and T.
Bertram (2017d): “Simulationsbasierte Entwicklung eines Trajektorienplanungsver-
fahrens für automatisiertes Fahren: Simulation based Development of a Trajectory
Planning Approach for Automated Driving”. In: AUTOREG 2017, Automatisiertes
Fahren und vernetzte Mobilität. 8. VDI/VDE-Fachtagung. VDI Berichte 2292. Berlin,
pp. 247–259.
Götte, C., M. Keller, C. Hass, and T. Bertram (2016a): “Model Predictive Planning
and Control Applied to Critical Traffic Situations”. In: ATZ worldwide eMagazines
2016-09, pp. 64–68.
Götte, C., M. Keller, C. Hass, and T. Bertram (2016b): “Modellprädiktive Planung
und Regelung im Anwendungsfall kritischer Verkehrssituationen”. In: ATZ - Auto-
mobiltechnische Zeitschrift 9, pp. 68–73.
Götte, C., M. Keller, C. Rösmann, T. Nattermann, C. Haß, K.-H. Glander, A. See-
wald, and T. Bertram (2016d): “A Real-Time Capable Model Predictive Approach to
Lateral Vehicle Guidance”. In: IEEE International Conference on Intelligent Transporta-
tion Systems (ITSC), pp. 1908–1913.
110
Bibliography
Götte, C., M. Keller, C. Haß, K.-H. Glander, A. Seewald, and T. Bertram (2015a): “A
model predictive combined planning and control approach for guidance of auto-
mated vehicles”. In: IEEE International Conference on Vehicular Electronics and Safety
(ICVES), pp. 69–74.
With co-authorship
Homann, A., C. Lienke, M. Keller, M. Buss, M. Mohamed, and T. Bertram (2019):
“Sampling-Based Trajectory Planning and Control for a Collision Avoidance System”.
In: IEEE Intelligent Transportation Systems Conference (ITSC), pp. 2956–2962.
Schmidt, M., M. Krüger, C. Lienke, M. Oeljeklaus, T. Nattermann, M. Mohamed,
F. Hoffmann, and T. Bertram (2019a): “Lane detection for automated driving using
Deep Learning”. In: at – Automatisierungstechnik 67.10, pp. 866–878.
Schmidt, M., C. Lienke, M. Oeljeklaus, M. Krüger, T. Nattermann, M. Mohamed,
F. Hoffmann, and T. Bertram (2018): “Fahrspurerkennung mit Deep Learning für
automatisierte Fahrfunktionen”. In: 28. Workshop Computational Intelligence.
Additional Publications and Poster Presentations
Lienke, C., M. Keller, and T. Bertram (2018a): “Umfeldmodellierung für Trajektorien-
planung in dynamischer Umgebung. Environment Modeling for Trajectory Planning
in Dynamic Environments”. In: 13. DortmunderAutoTag (DAT).
Götte, C., M. Keller, C. Haß, K.-H. Glander, and T. Bertram (2017a): “Online-Trajek-
torienoptimierung mit Timed Elastic Splines. Online Trajectory Optimization with
Timed Elastic Splines”. In: 12. DortmunderAutoTag (DAT).
Götte, C., M. Keller, C. Rösmann, T. Nattermann, C. Haß, K.-H. Glander, and T.
Bertram (2016c): “Ein modellprädiktiver Ansatz zur Querführung automatisierter
Fahrzeuge. A Model Predictive Approach to Lateral Guidance of Automated Vehi-
cles”. In: 11. DortmunderAutoTag (DAT).
Götte, C., M. Keller, C. Haß, K.-H. Glander, A. Seewald, and T. Bertram (2015b):
“Ein modellprädiktiver Ansatz zur Trajektorienlanung und Regelung von automa-
tisierten Manövern mit Lenk- und radselektivem Bremseingriff. A Model Predic-
tive Approach to Trajectory Planning and Control for Automated Maneuvers using
Steering Intervention and Wheel Independent Braking”. In: 10. DortmunderAutoTag
(DAT).
Patent Applications
Keller, M., C. Wissing, C. Lienke, M. Schmidt, A. Homann, N. Stannartz, and T.
Bertram (2021): “Steuerungssystem und Steuerungsverfahren für einen hierarchis-
chen Ansatz zum Ermitteln einer Trajektorie für ein Kraftfahrzeug (DE)”. Patent DE
10 2019 128 787 A1.
111
Bibliography
Bertram, T., K.-H. Glander, A. Homann, M. Keller, C. Lienke, T. Nattermann, M.
Schmidt, and C. Wissing (2020a): “Steuerungssystem und Steuerungsverfahren für
einen hybriden Ansatz zum Ermitteln einer möglichen Trajektorie für ein Kraft-
fahrzeug (DE), Control System and Control Method for a Hybrid Approach to De-
termining a Possible Trajectory for a Motor Vehicle (EN)”. Patent DE 10 2018 009
927 A1, WO 002020127011 A1.
Bertram, T., K.-H. Glander, A. Homann, M. Keller, C. Lienke, M. Schmidt, and
C. Wissing (2020b): “Verfahren zum Betreiben eines wenigstens teilweise automa-
tisierten Fahrzeugs (DE), Method for Operating an at least Partially Automated
Vehicle (EN)”. Patent DE 10 2018 123 896 A1, WO 002020002100 A1.
Buß, M., M. Keller, K.-H. Glander, C. Lienke, C. Wissing, M. Schmidt, A. Homann,
and T. Bertram (2020): “Steuerungssystem und Steuerungsverfahren zum sampling-
basierten Planen möglicher Trajektorien für Kraftfahrzeuge (DE), Control System
and Control Method for Sampling-Based Planning of Possible Trajectories for Motor
Vehicles (EN)”. Patent DE 10 2018 008 624 A1, WO 002020089311 A1.
Supervised Theses
Dorpmüller, P. (2019): “Entwicklung eines optimierungsbasierten Trajektorienpla-
nungsverfahrens für automatisierte Fahrzeuge”. Master’s thesis.
Sarmiento, D. (2019): “Development of an Incremental Search Trajectory Planner for
Automated Driving in Structured Environments”. Master’s thesis.
Vetter, S. (2018): “Kombination einer Sample-Based Trajektorienplanung mit einer
modellprädikativen Trajektorienfolgeregelung zur Kollisionvermeidung automa-
tisierter Fahrzeuge”. Bachelor’s thesis.
Dorpmüller, P. (2017): “Analyse und Vergleich von numerischen Optimierungsalgo-
rithmen für die Trajektorienplanung automatisierter Fahrzeuge”. Bachelor’s thesis.
Parsiegel, F. (2017): “Vergleich von Fahrdynamikmodellen zur Anwendung von mod-
ellprädiktiven Verfahren in automatisierten Fahrfunktionen”. Bachelor’s thesis.
Schmidt, M. (2017): “Online-Kalibrierung eines Kamerasystems in Straßenverkehrsszenar-
ien zur Validierung automatisierter Fahrfunktionen”. Master’s thesis.
Wunsch, C. (2017): “Linearisierung eines Einspurmodells zur kombinierten Längs-
und Querregelung eines Fahrzeuges”. Bachelor’s thesis.
Diehl, C. (2016): “Anwendung eines Trajektorienscharverfahrens für die Planungsauf-
gabe automatisierter Fahrzeuge”. Bachelor’s thesis.
Spitzlei, D. (2016): “Entwicklung, Analyse und Vergleich von Fahrdynamikregelungen
zur Trajektorienfolge”. Master’s thesis.
Homann, A. (2015): “Ein Verfahren zur Fahrzeugführung mittels echtzeitfähiger Tra-
jektiorienoptimierung und Fahrdynamikregelung”. Master’s thesis.
112
Bibliography
Supervised Student Project Groups
Bartsch, K., Q. T. Bui, C. Diehl, A. Dikarew, A. Hugenroth, T. Kiratli, and Y. Sun
(2017): “Entwicklung automatisierter Fahrerassistenzsysteme”.
Overmann, A. S., B. Möllenbeck, E. Mares, F. Albers, J. Bruchhaus, M. A. Ali, M.
Hamza, M. S. Irshad, and W. Schulz (2016): “Development of a multistep test
environment for advanced driver assistance systems”.
113
A
Appendix
A.1. Terminology and Definitions
Configuration Space A vehicle configuration o completely specifies the position rel-
ative to a fixed coordinate system. The configuration space is the set of all possible
configurations and its dimension matches the number of degrees of freedom of the
vehicle. Thus, it can also be denoted as the space of the system’s degrees of freedom.
The link between the configuration and the state space is given by o = κ(x) with
function κ : X → C , which returns the configuration o ∈ C associated with state
x ∈ X . In dynamic environments, the vehicle’s configuration space needs to include a
notion of time (LaValle 2006; Lynch and Park 2017).
Holonomic and Non-Holonomic Systems Holonomic constraints are differential con-
straints that are integrable, which means that holonomic constraints restrict the vehi-
cle’s configuration space. Non-holonomic constraints on the other hand are differential
constraints, that cannot be integrated. This means they cannot be converted into con-
straints that involve no derivatives and thus cannot be expressed as constraints on
the vehicle’s pose. Accordingly they do not restrict the reachable configuration space,
but the space of differential motions. A non-holonomic constraint for vehicles arises
from the rolling-without-slipping property, that prevents from instantaneous lateral
motion (LaValle 2006, p. 791).
Normal Driving Situation A normal driving situation is defined in terms of acting tire
forces within the linearized area of tire characteristics, which approximately is one
third of the maximum lateral tire force. For dry roads this corresponds to circa 4 m/s2.
Trajectory versus Path Planning In literature a very confusing aspect is that the terms
path planning and trajectory planning are often used interchangeably. At this point
it should be clarified that there is a strict separation between these two terms as
a trajectory considers time information explicitly, while path planning relates to a
purely geometric problem in the configuration space.
Maneuver The term maneuver is used to summarize a sequence of actions that drive
the vehicle from one steady state to another (Ulbrich 2018, p. 19). A maneuver is
defined with respect to the given road topology, which basically includes lateral vehicle
114
A.2. Components of the Functional Architecture
motion like lane keeping and lane changing, but can also refer to the longitudinal
motion component and the spatial relation to other vehicles (e.g. overtaking).
Scene, Situation and Scenario The traffic scene covers all directly measurable charac-
teristics of the surrounding environment e.g. scenery and dynamic elements. A scene
describes the environment at one point in time and inter alia suffers from incom-
pleteness and uncertainty. A situation on the other hand supplements a scene with
some kind of interpretation to reveal relevant additional information, such that a more
comprehensive description of the environment can be derived. A scenario describes
the temporal development between several scenes. Definitions are based on the work
of Ulbrich (2018).
Motion Planning The term motion planning is used with respect to problems that
involve obstacle avoidance and potentially other constraints. Hence, motion planning
represents the generic term for path and trajectory planning.
Behavior In accordance to Ulbrich (2018) the term behavior is used to group actions
or activities that result in observable outcomes. In this context it mainly relates to
the question of how something is performed (i.e. formulating the driving style in a
descriptive manner), covering aspects that may relate to different hierarchical levels of
the driving task.
Completeness A planning algorithm is denoted as complete, if it terminates in finite
time and returns a valid solution to the trajectory planning problem or ascertains
that no valid solution exists. In the context of sampling-based trajectory planning
approaches it is often referred to the term resolution completeness, which accounts
for the discretized space.
Trajectory Optimization and Optimal Control In literature the terms trajectory optimiza-
tion and optimal control are used interchangeably (see e.g. Kelly (2017)). In this thesis
the term trajectory optimization is predominantly connected, but not necessarily re-
stricted to the description of solving a nonlinear program.
On-road and off-road driving In this thesis the term off-road is used to denote navi-
gation in unstructured environments. On-road driving on the other hand refers to
navigating in structured (driving applications) and semi-structured (parking applica-
tions) environments.
A.2. Components of the Functional Architecture
Taking a closer look at the functional architecture of automated vehicles, in this section
all modules related to the ego vehicle driving task are described in more detail. For
an approach that shows an exemplary solution with respect to the interplay between
selected components it is referred to Lienke et al. (2019a). In the following each com-
ponent of the functional architecture is outlined to provide further information with
respect to the general purpose and underlying concepts.
115
Appendix A. Appendix
Perception
The part of perception deals with the important task to generate a preferably high
quality reflection of the real world, using supplementary sensor concepts. Since there
is no standardized hardware configuration for automated vehicles, the sensor setup is
subject to thorough assessment, taking for example performance or cost aspects into
consideration. Especially the choice and arrangement of exteroceptive1 sensors largely
contribute to the overall perception quality. As subsequent modules in the functional
architecture rely on reliable and accurate environment data, the task of perception
becomes even more important. Common sensors used in this context comprise cam-
eras, radars as well as lidars and ultrasound sensors. A major challenge represents the
fact, that the sensors have to cope with a wide range of external conditions, which is
why pros and cons of the respective technology have to be balanced carefully against
each other. Marti et al. (2019) give a summary on sensor technologies for perception
in automated driving. An approach to lane detection using deep learning techniques
has been described by Schmidt et al. (2018) and Schmidt et al. (2019a).
To consolidate the sensor measurements objects are fused to one common representa-
tion and tracked over a number of cycles to trace the development over time. In addi-
tion a representation of the road topology is deduced from detected lane boundaries
inferring the available free space. The description of the surrounding environment can
be enriched by map data and information gained from vehicle-to-x communication.
Both types provide data with high accuracy and also extend the range of validity, since
the distribution of cloud-based information and communication services represent a
counterpart to measurements conducted by on-board sensors. Still, this requires the
ability of precise localization within the current setting. Finally, with all the informa-
tion obtained, an environment model is generated.
Situation Analysis
The environment model obtained from the perception module represents the basis for
a further analysis of the traffic scene. The topics of intention prediction and estimation
of future trajectories with respect to other traffic participants need to be addressed to
extract meaningful knowledge about the future development of the current situation.
Therefore, the task of situation analysis can be subdivided into maneuver detection,
maneuver dynamics estimation and trajectory prediction. Especially the latter part is
vital for the subsequent maneuver and trajectory planning, as it enhances the envi-
ronment model with assumptions on the future evolution of the situation (predictive
environment model) enabling anticipatory driving. A comprehensive review on mo-
tion prediction techniques is given by Lefèvre et al. (2014).
Mission Planning
At the beginning a mission goal connected to a specific task or event has to be pro-
vided inferring a desired destination for the automated vehicle. Based on that mission
1In contrast to proprioceptive sensors, which relate to the ego vehicle state, measuring e.g. the velocity
and accelerations.
116
A.2. Components of the Functional Architecture
several waypoints can be derived, which are connected via the route planning module.
A route on a road network is planned neglecting dynamic events (e.g. obstacles, traffic
lights) and vehicle constraints. Among others possible objectives for route planning
involve travel time, distance or energy consumption. Route planning could also con-
sider predicted or live data indicating the actual traffic flow comprising as well traffic
congestions or blockages (macroscopic point of view).
Decision Making
The decision making module is a high level maneuver planner designed for the task
of tactical reasoning with topological awareness. Therefore, distinct maneuver classes
have to be identified during the maneuver detection process and to distinguish be-
tween different classes the spatiotemporal topology of the surrounding environment
has to be analyzed. This allows the maneuver planner to make discrete decisions,
whether for example to pass an obstacle vehicle to the right or left. Beside lane change
and lane keeping commands, possible maneuvers are for example parking or stopping.
The process of decision making starts with a maneuver request, which is followed by a
maneuver analysis and decision. A maneuver request can be initiated by an automated
process (e.g. as a static event triggered by the mission planning or as a reaction on
the current traffic situation) or by the human driver (e.g. by activating the indicator).
Furthermore, a request can be categorized as mandatory or discretionary. Whereas the
former is due to infrastructural and mission related aspects the latter corresponds to
expected improvements regarding the actual situation. For maneuver analysis there
are two prevalent approaches in which the complexity of the detection part varies
significantly. The first approach simplifies the possible set of maneuver classes to lane
keeping and lane change maneuvers and deals with the detection of available gaps
to perform a lane change. A more sophisticated solution is the application of ma-
neuver pattern analysis that is often connected with sampling of maneuver trajectory
prototypes. In this approach time augmented topological information is gathered to
identify spatiotemporal distinct maneuver variants. According to Ardelt et al. (2012),
there are two main approaches to decision making in the context of automated driving
that can be summarized as rule or utility function based decision making. In the sense
of gap acceptance models the detected available gaps are compared with respect to
the smallest acceptable gap, also denoted as critical gap. This critical gap is in general
dependent on kinematic quantities such as the relative speed to the relevant obstacle
vehicles. A key feature of decision making is the spatiotemporal topological analysis,
which does not necessarily include the question of reachability. The trajectory plan-
ning benefits, as the property of topological awareness allows for example the efficient
reduction of the search space for sampling-based methods by the formulation of sam-
pling ranges as well as it significantly helps to provide a meaningful initialization for
optimization-based methods.
Trajectory Planning
Based on the future driving strategy provided by the decision making a motion plan
has to be generated accordingly, translating the desired behavior into a trajectory that
117
Appendix A. Appendix
can be tracked by a low-level vehicle dynamics controller. In the context of automated
driving the trajectory planner has to cope with imperfect and incomplete knowledge of
the surrounding environment or rather the highly uncertain motion of other predicted
traffic participants obtained from situation analysis. The resulting trajectory must be
dynamically feasible and consider other obstacle vehicles and traffic participants to
avoid collisions. Especially at high velocities (e.g. highway driving) this is not a trivial
task, since the reachability of a safe state must be guaranteed at all times. Moreover,
to allow for a pleasant drive, the aspect of passenger comfort needs to be addressed.
To cover dynamic scenarios a time parameterized representation of the future motion
of the ego vehicle is preferred over a purely spatial description in terms of a path.
According to Werling (2011, p. 5) static traffic scenarios like parking without dynamic
traffic participants can be seen as a special case of dynamic scenarios. It can hence be
concluded that with a suitable trajectory planning and stabilization approach all traffic
maneuvers are formally manageable. It is moreover important to consider the coupled
lateral and longitudinal characteristics of the vehicle, since particularly overtaking
and merging maneuvers result in a coupled lateral and longitudinal motion planning
problem. Differential constraints arising from the vehicle dynamics complicate the
process of trajectory planning. According to LaValle (2006) one approach is to ignore
the differential constraints in the planning phase and assume that the subsequent
controller will handle them appropriately. A better alternative can be seen in the
direct inclusion of vehicle dynamics within the planning process, resulting in a system
compliant motion plan.
Vehicle Dynamics Control
Since the exact modeling of vehicle dynamics cannot be considered within the planning
phase at reasonable expense, control algorithms account for the remaining modeling
errors of the trajectory planner. An introduction of control related topics in automated
driving is e.g. given by Klomp et al. (2019).
Approaches to trajectory following control for normal driving situations are often de-
composed into separate lateral and longitudinal control concepts. Still, in applications
in which the coupled lateral and longitudinal dynamics come into effect, a compre-
hensive controller design is necessary. From the vast number of control approaches
to trajectory and path tracking three main categories stand out: Proportional-inte-
gral-derivative (PID) controller, state feedback controller and model predictive control
concepts. PID controllers and related concepts are used as cascaded or parallel ver-
sion to control position and yaw rate. Because parameter tuning works quite intuitive
they are widely distributed among lateral and longitudinal control approaches. An-
other established control concept can be seen in state feedback control, which includes
e.g. the use of dynamic feedback linearization, and differentially flat outputs design.
De Luca and Samson (1998) give an overview with respect to feedback control of a
nonholonomic vehicle. Model predictive control uses an internal model to predict the
future motion of the vehicle. By this means the optimal control inputs can be derived
and system constraints can be handled explicitly. The major drawback with respect to
this kind of control certainly is the intense use of computational resources.
118
A.3. Supplements to Environment-aware Maneuver Planning
A.3. Supplements to Environment-aware Maneuver Planning
Further Maneuver Planning Example
To give another example with respect to the functioning of the environment-aware
maneuver planning the test case shown in chapter 4 is changed. This is leading to a
cut-in and cut-out maneuver in front of the ego vehicle, which is now in lane keeping
mode instead of performing a lane change maneuver. The obstacle vehicle ID:2 is
configured to make a double lane change, all other obstacle vehicles stay in their
respective lanes. The test case definition as shown in Table A.1 is similar to the one
given in Table 4.1. Note that solely target lanes for the ego vehicle and obstacle vehicle
ID:2 have changed. This means that the ego vehicle and obstacle vehicles ID:1, ID:3
Table A.1.: Test case definition with start conditions of the ego vehicle and obstacle vehicles.
ID [ ] F px [m] F py [m] Fv [km/h] Fψ [rad] target lane [ ] Fvdes [km/h]
ego 0 0 100 0 2 130
1 40 2.5 100 0.1 2 not known
2 20 −3.35 90 0.05 1 not known
3 −25 3.75 110 0 1 not known
4 25 4.65 120 0.1 1 not known
and ID:4 intend to stay on their current lane, whereas obstacle vehicle ID:2 performs a
double lane change from the right to the left lane given with respect to the ego vehicle.
The impeding vehicles prediction as shown in Figure A.1 predicts obstacle vehicle ID:2
to become the lead vehicle at 1.9 s and to become the tail vehicle on the left at 4.3 s.
This means that obstacle vehicle ID:2 cuts in in front of the ego vehicle and merges
into the gap between obstacle vehicles ID:3 and ID:4 on the left lane. On the other
hand, since obstacle vehicle ID:2 is driving at a slower speed than the ego vehicle, in a
first step it is preliminaryly assumed that the ego vehicle passes obstacle vehicle ID:2.
lead vehicle left lane lead vehicle center lane lead vehicle right lane
tail vehicle left lane tail vehicle right lane
4
3
2
1
-1
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure A.1.: Predicted impeding vehicles shown for the considered example test case. As
obstacle vehicle ID:2 is changing lanes respective lead and tail vehicles change accordingly. In
case that no tail vehicle for the right or left lane exists it is mapped to a vehicle index of -1.
119
vehicle index
Appendix A. Appendix
For lane keeping the only relevant position bound is constituted by the lead vehicle
position on the current ego lane. The corresponding upper position bound for the
considered example is shown in Figure A.2. Clearly, the cut-in and cut-out of obstacle
200
150
upper position bound
100
50
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure A.2.: Merged relevant information for the ego vehicle. Since the ego vehicle is perform-
ing a lane keeping maneuver there is no lower position bound as no tail vehicle for the center
lane is recorded. The upper position bound arises from obstacle vehicle ID:1 on the center lane
and obstacle vehicle ID:2 cutting in and out in front of the ego vehicle at t = 1.9 s and t = 4.3 s,
respectively.
vehicle ID:2 is visible, as during the time of t = 1.9 s and t = 4.3 s the upper position
bound is reduced.
Analogous to the procedure described in chapter 4 the drivable, the admissible and
the unimpeded region are identified to label areas that fulfill certain characteristics
with respect to the ego vehicle position. The drivable region refers to the capabilities
of the ego vehicle in terms of the vehicle dynamics. The admissible region and the
unimpeded region take surrounding obstacle vehicles into account in order to integrate
collision avoidance into the maneuver plan and to consider desired safety margins.
The composition of the admissible region is shown in Figure A.3. With respect to
the possible ego vehicle motion the impact of the limiting upper position bound is
relatively small, as only a small part of the drivable region is excluded.
The unimpeded region depicted in Figure A.4 maintains the characteristics of the
admissible region and adds a desired safety margin to other relevant vehicles. For the
considered test case this is achieved in terms of the lead vehicle safe position. This
time, in comparison to the impact of the upper position bound, the impact of the
lead vehicle safe position adds more difficult constraints on the desired motion of the
ego vehicle. Especially at around t = 1.9 s the range of corresponding longitudinal
positions, which adhere to the desired safety margin is quite small.
The generation of bounded reference states is solely following a predetermined policy
to restrict the longitudinal ego vehicle position, paying less regard to the actual driv-
ability. The resulting reference that in the considered case is in fact bounded by the
lead vehicle safe position is shown in Figure A.5. As already mentioned, the aspect of
drivability is not considered in a first step, but taken into account via the generation
120
C px [m]
A.3. Supplements to Environment-aware Maneuver Planning
200
150
upper position bound drivable region
100
50
0 admissible region
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure A.3.: The admissible region is constituted by limiting the drivable region by the upper
position bound. Note that in general the lower position bound might also be a limiting factor,
hence representing a potential border of the admissible region.
200
admissible region
150
lead vehicle safe position
100
50
unimpeded region
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure A.4.: The admissible region and the lead vehicle vehicle safe position constitute to the
unimpeded region.
of an optimal reference position. The least squares fit on the velocity maintains the
desired characteristic of the ego demands, while accelerations comply to drivability
by explicitly constraining the solution of the quadratic program in that respect. The
optimal reference position for the considered example is given in Figure A.6. As can
be seen, the application of a least-squares fit to generate the optimal reference velocity
rigorously smoothens the extreme characteristic of the bounded reference velocity. The
fact that in the first part of the maneuver the optimal reference velocity is mainly
attracted by the desired ego vehicle set speed of 130 km/h explains the increase of the
ego velocity, which accelerates from 100 km/h to approximately 107 km/h. To account
for slower obstacle vehicle ID:2 the ego vehicle speed is then reduced to 90 km/h.
Finally, after the cut-out of obstacle vehicle ID:2 the ego vehicle adapts its speed to
100 km/h in accordance to the new lead vehicle ID:1. Instead of directly optimizing
the longitudinal position as such, the chosen procedure to take the bounded velocity
121
C p Cx [m] px [m]
Appendix A. Appendix
200
150 bounded reference
100
reference
50
unimpeded region
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure A.5.: The reference position is obtained by integration with the aim to reach the set
speed with a desired comfortable acceleration. The reference position is then bounded by the
unimpeded region yielding the bounded reference position.
125 bounded reference
100
75 optimal reference
50
25
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
Figure A.6.: The optimal reference position is derived from the optimal reference velocity that
is calculated by a least squares fit on the bounded reference velocity subject to position con-
straints (from admissible region), velocity constraints (from drivable region) and acceleration
constraints (comfort acceleration/deceleration).
as reference facilitates to find a valid solution on the one hand and enables to find a
final solution as close as possible to the initially desired ego velocity (i.e. ego vehicle
set speed) on the other. As a consequence the ego vehicle smoothly adapts its speed
with respect to the cut-in maneuver of obstacle vehicle ID:2, accepting to violate the
bounds given by the lead vehicle safe position. The latter can be regarded as a desired
feature, since strong reactions to the behavior of other traffic participants in terms of
keeping a safety margin even in case of a cut-in maneuver of an obstacle vehicle could
be considered as unnecessary. Because the admissible region is taken to constrain
the longitudinal position of the ego vehicle, collision avoidance is still guaranteed by
means of the chosen definition of the quadratic programming problem 4.2.1.
Finally, the longitudinal maneuver trajectory and target region as well as the lateral
maneuver trajectory and target region are generated. The overall result is shown in
Figure A.7. In the fist part of the ego vehicle lane keeping maneuver the longitudinal
ego vehicle motion is predominantly limited by the aspect of drivability (imposed in
terms of the drivable region). In the further course the ego vehicle then manages to
122
Cv [km/h] C px [m]
A.3. Supplements to Environment-aware Maneuver Planning
200
100
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
(a) Longitudinal maneuver trajectory and longitudinal target region.
4
2
0
−2
−4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
t [s]
(b) Lateral maneuver trajectory and lateral target region.
Figure A.7.: Final maneuver trajectory and target region of the maneuver planning approach.
retain at least a small safety distance. Obviously, since the ego vehicle performs lane
keeping the lateral maneuver trajectory in curvilinear coordinates is constantly zero.
Details on Optimal Reference States Calculation
For optimal reference calculation the quadratic programming problem 4.2.1 is set up.
The elements of respective matrices are chosen to describe a least squares fit with
respect to the bounded reference velocity, subject to linear constraints on position,
velocity and acceleration The decision variable vector p̂ can be subdivided into two
parts. The first part is represented by the longitudinal position vector px, whereas the
second part represents the actual variables used to realize an optimal least squares fit
at each trajectory point k = 0, 1[, . . . , K: ]
p̂ = pTx , r1, r2, . . . , rk, . . . , r
T
K, . (A.3.1)
The cost matrix is given by:   0 0 . . . 0 0 0 . . . 0 0 0 . . . 0 0 0 . . . 0 

 .. .. . . . . .  .
. . . .. .. .. . . . .. 
 0 0 . . . 0 0 0 . . . 0 Q =   , (A.3.2) 0 0 . . . 0 1 0 . . . 0  0 0 . . . 0 0 1 . . . 0 .. .. . . . .. .. . . . . . . .. . . . .. 
0 0 . . . 0 0 0 . . . 1
123
C py [m] C px [m]
Appendix A. Appendix
whereas the cost vector q is zero vector, such that the linear cost term can be omitted.
The matrix dedicated to the least squares formulation is:
 − 1 1 ∆t ∆t 0 0 0 −1 0 0 0 0 0 −1 1  ∆t ∆t
0 0 0 −1 0 0 0 
A  0 0 . . . . . . .. . . . .. .. lsq =  . 0 0 . .  . (A.3.3).. .. −1 1 .. .. . . 0 ∆t ∆t . . 0 −1 0 
0 0 0 0 0 0 0 0 0 −1
The matrix composition clearly reveals the least squares fit in terms of the velocity,
since the first derivative of the position is calculated with respect to time by applying
the difference quotient. Moreover, the two separate parts of the matrix are visible,
highlighting again the subdivision of the decision vector on account of the chosen
problem formulation. Finally, the constraint matrices for the position, velocity and
acceleration are given as follows:
  1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 A = 

0 0 . . . .
. .. .. . .. 0 . . . .
. .. 
pos  . .  , (A.3.4).. .. .. .0 1 0 .. .. . 0 0 0 
0 0 0 0 1 0 0 0 0 0
 − 1 1 ∆t −∆t
0 0 0 0 0 0 0 0
 0 1 1  ∆t ∆t
0 0 0 0 0 0 0 
A .=  0 0 . . . . . ... 0 0 . . .. .. vel  . . .  , (A.3.5).. ... . 0 −1 1 .. . ∆t ∆t . .. 0 0 0 
0 0 0 0 0 0 0 0 0 0
 1 −2 1  ∆t ∆t −∆t
0 0 0 0 0 0 0 0 0
 0 1 2 1  ∆t ∆t ∆t
0 0 0 0 0 0 0 0  . . . .A  0 0
. . . . . . . . . .. 0 0 . . . .. .. .. 
acc =


. . . . . (A.3.6).. .. 0 1 −2 1 .. ..

∆t ∆t ∆t 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 0 0 0 0
It can be seen that the matrices represent the prerequisite mapping of the position to
the respective derivative. Numerical differentiation is applied to approximate the first
and second derivative of the position via the difference quotient.
124
A.4. Linear Single Track Model
A.4. Linear Single Track Model
To obtain the linear single track model equations the equations 5.1.1 and 5.1.2 are
substituted and the resulting equations are further linearized, which yields:
mv̇ = Fx,v + Fx,h − Fa,x , (A.4.1)
ψ̇ ψ̇
m(β̇ + ψ̇) + mv̇β = cv(−β + δr − lv ) + ch(−β + lh )− Fa,y , (A.4.2)v v
ψ̇ ψ̇
Jzψ̈ = cvlv(−β + δr − lv )− chlh(−β + lh )− Fv v a,ylc . (A.4.3)
In the case of driving at a constant speed v̇ = 0 and furthermore neglecting the
aerodynamic forces the model can be written as:
− cv + ch mv
2 − (chlh − cvlv) cvβ̇ = β + ψ̇− δ , (A.4.4)
mv mv2 mv r
c 2 2− hlh − cvlv − chlh + cvlv cvlvψ̈ = β ψ̇ + δr . (A.4.5)Jz Jzv Jz
A.5. Kinematic Vehicle Model with Ackermann Steering
A kinematic vehicle model describes the motion without the consideration of present
forces (Rajamani 2012, p. 20). In fact this is the case at small speeds, when the lateral
forces disappear. Then the tire slip angles at both wheels are zero and the following
assumption holds:
l
tan a(δA) ≈ . (A.5.1)
ρcc
This equation is related to equation 5.1.22 in which additionally a small angle assump-
tion has been made. With steady state assumption (β̇ = 0 in equation 5.1.14) this leads
to:
v
ψ̇ = tan(δA) (A.5.2)la
for the relation between yaw rate ψ̇ and the Ackermann steer angle δA.
A.6. Calculation of the Minimum Curve Radius according to
RAS-L
The minimum cross slope is defined as ξmin = 2.5 %, whereas the maximum cross slope
is ξmax = 8 % in general, but limited to ξlim = 7 % when calculating the minimum
curve radius. In the design principles the exploitation of the maximum radial adhesion
coefficient is defined as ςmax = 50 % in case of ξmax and ςmin = 10 % in case of ξmin:
v2
ρmin =
3.62
, (A.6.1)
ag(µrς + ξ)
125
Appendix A. Appendix
µt = 0.241(0.01 v)2 − 0.721(0.01 v) + 0.708 , (A.6.2)
µr = 0.925 µt . (A.6.3)
A description of the utilized variables is given in Table A.2.
Table A.2.: Variable description.
variable unit description
ρmin [m] minimum curve radius
v [km/h] velocity
ag [m/s2] gravitation
µt [ ] maximum tangential adhesion coefficient
µr [ ] maximum radial adhesion coefficient
ς [ ] exploitation of the maximum radial adhesion coefficient
ξ [ ] cross slope
A.7. Inverse Flat Transform
The derivation of the longitudinal position px and lateral position py is straightforward
since the flat output exactly corresponds to these quantities:
px = zx,1 , (A.7.1)
py = zy,1 . (A.7.2)
Starting from equation 5.1.29 to derive the velocity and yaw rate, it is:
zx,2 = cos(ψ)v , (A.7.3)
zy,2 = sin(ψ)v , (A.7.4)
which can be rearranged to
z
v = x,2
zy,2
= . (A.7.5)
cos(ψ) sin(ψ)
When solving this for yaw angle ψ it yields: ( )
z
ψ = arctan y,2 (A.7.6)
zx,2
and the velocity can be determined via equation A.7.5 to be:
z
v = ( x,2 ( ))z . (A.7.7)
cos arctan y,2zx,2
126
A.8. Adaptive Sampling Strategy
To derive the control inputs the starting point is once again given by equation 5.1.29:
zx,3 = cos(ψ)a− sin(ψ)ψ̇v , (A.7.8)
zy,3 = sin(ψ)a + cos(ψ)ψ̇v , (A.7.9)
From equation A.7.8
zx,3 + sin(ψ)ψ̇va = (A.7.10)
cos(ψ)
can directly be derived. Inserting (this result into eq)uation A.7.9 yields:
zx,3 + sin(ψ)ψ̇vzy,3 = sin(ψ) + cos(ψ)ψ̇v (A.7.11)cos(ψ)
and after some algebraic transformations it finally is:
zy,3 − tan(ψ)zx,3
ψ̇ = . (A.7.12)
tan(ψ) sin(ψ)v + cos(ψ)v
Now, by substituting equation A.7.12 into equation A.7.10 the acceleration is given by:
zx,3 sin(ψ)v zy,3 − tan(ψ)za x,3= + . (A.7.13)
cos(ψ) cos(ψ) tan(ψ) sin(ψ)v + cos(ψ)v
With equation A.7.12 and the help of equation 5.1.25c from the system dynamics of
the applied vehicle model the steering angle is then given by:
zy,3 − tan(ψ)zx,3 la(1 + (v/v )2ch )δr = . (A.7.14)tan(ψ) sin(ψ)v + cos(ψ)v v
A.8. Adaptive Sampling Strategy
The adaptive sampling strategy has successfully been applied in various applications
related to automated driving (Lienke et al. 2018b; Homann et al. 2018; Homann et
al. 2019). Assuming that the optimal sampling variable for the current planning step
is close to the one of the last planning cycle a finer resolution around the actual
value is desired. Nevertheless, minimum and maximum values should always be
considered. Two parabolas f1 and f2 are utilized to transform the linearly discretized
values ð online and in dependence on actual value ðact. The respective coefficients are
determined from conditions:
f1(ðmin) = ðmin , f1(ð ) = ð , f′mid act 1(ðmid) = 0 , (A.8.1)
f2(ðmax) = ðmax , f2(ð ′mid) = ðact , f2(ðmid) = 0 , (A.8.2)
with ðmid = (ðmax − ðmin)/2. T{hen the adaptive value f(ð) is:
f1(ð) ð < (ðf ð max
− ð
( ) = mid
)
. (A.8.3)
f2(ð) ð >= (ðmax − ðmid)
The result of the proposed transformation yielding the adaptive discretization is shown
in Figure A.8.
127
Appendix A. Appendix
ðmax
f(ðmid)
ðmin ðmid ðact ðmax
linear discretization
Figure A.8.: Nonlinear mapping from linear to adaptive discretization for actual value ðact. The
minor grid lines show the desired higher resolution around f(ðmid) = ðact for the adaptive
discretization. Note, f(ð) is changing with respect to ðact, which yields the adaptive character.
A.9. Sequential Quadratic Programming Algorithm
The utilized approach given by Algorithm A.1 basically follows the Line Search SQP
Algorithm as described in Nocedal and Wright (2006).
Algorithm A.1.: Sequential Quadratic Programming
Constraints and corresponding lagrange multipliers: â, λ̂
Jacobian of the lagrangian: Â(ẑ)T = [∇â1(ẑ),∇â2(ẑ), . . . ,∇âł(ẑ), . . . ,∇âΞ(ẑ)]
1: function Line Search SQP Algorithm
2: Choose parameters η̂ ∈ [0, 0.5], τ̂ ∈ [0, 1] and initial values p̂0, λ̂0
3: Evaluate F̂(ẑ0), ∇F̂(ẑ0), â(ẑ0), Â(ẑ0)
4: Choose initial symmetric positive definite hessian approximation B̂0 ∈ RnB̂×nB̂
5: repeat
6: Compute p̂ı̂ by solving th∥∥e qua∥∥dratic programming problem 7.4.3
7: Choose µ̂ı̂ to satisfy µ̂ > ∥λ̂ı̂+1∥ so that p̂ı̂ is descent direction for φ̂
∞
8: α̂ı̂ ← 1
9: while φ̂(ẑı̂ + α̂ı̂p̂ı̂, µ̂ı̂) > φ̂(ẑı̂, µ̂ı̂) + η̂α̂ı̂D̂(φ̂(ẑı̂, µ̂ı̂), p̂ı̂) do
10: Reset α̂ı̂ ← τ̂α̂α̂ı̂ for τ̂α̂ ∈ [0, τ̂]
11: end while
12: Set ẑı̂+1 ← ẑı̂ + α̂ı̂p̂ı̂
13: Evaluate F̂(ẑı̂+1), ∇F̂(ẑı̂+1), â(ẑı̂+1), Â(ẑı̂+1)
14: Set ŝı̂ ← ẑı̂+1 − ẑı̂ and ŷı̂ ← ∇ẑL(ẑı̂+1, λ̂ı̂+1)−∇ẑL(ẑı̂, λ̂ı̂+1)
15: Calculate B̂ı̂+1 by updating B̂ı̂ with the BFGS update formula 7.4.8
16: until convergence test is satisfied or maximum number of iterations reached
17: end function
The algorithm addresses the aspects of step calculation by quadratic programming,
backtracking line search using an `1 merit function and iterative updating of the
hessian matrix as discussed in section 7.4.2.
128
adaptive discretization
A.10. Convergence Test and Runtime Assessment for Continuous Approach
A.10. Convergence Test and Runtime Assessment for
Continuous Approach
To get an impression of the general functioning the convergence of the continuous
trajectory optimization approach is tested by way of experiment. Since the optimal
trajectory is generally not known as it depends on subjective assessment the trajectory
optimization problem is reversed for the simple case of driving straight ahead without
any dynamic obstacles. For this example the optimal solution is known beforehand.
A test case Si-C-1 as shown in Table A.3 is created, in which the initial trajectory is a
lane change to the left. Figure A.9 shows the convergence behavior of the continuous
Table A.3.: Test case definition Si-C-1.
ID [ ] F px [m] F py [m] Fv [km/h] Fψ [rad] target lane [ ] Fvdes [km/h]
ego 0 0 72 0 1 72
approach. The largest decrease in the objective function value can be observed in
the first optimization iteration ı̂ = 1. At round ı̂ = 6 iterations the result is already
very close to the final solution. The final result after ı̂ = 15 iterations is depicted in
start knot position lateral knot position longitudinal knot position
4 ı̂ = 0
3
2
1 ı̂ = 1
0 ı̂ = 10
−1
0 10 20 30 40 50 60 70 80 90 100
E px [m]
(a) Trajectory evolution with respect to optimization iterations exemplarily shown for initial trajectory
ı̂ = 0 and optimization iterations ı̂ = 1 and ı̂ = 10.
·1053 106
2 104
1 102
0 100
0 3 6 9 12 15 0 3 6 9 12 15
optimization iteration ı̂ [ ] optimization iteration ı̂ [ ]
(b) Objective function value. (c) Semi-log plot with costs on a logarithmic scale.
Figure A.9.: Convergence of the continuous approach in test case Si-C-1.
129
costs F̂ [ ] E py [m]
costs F̂ [ ]
Appendix A. Appendix
Figure A.10. As can be seen, the optimal trajectory converges to the expected straight
line with constant velocity. This is no general proof of convergence, but at least an
indication towards validity of the continuous variant of the two level hierarchical
trajectory optimization framework.
6
ID:1 3
0
−3
−10 0 10 20 30 40 50 60 70 80 90 100 110
E px [m]
(a) Final result after ı̂ = 15 optimization iterations.
80
75
70
65
60
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
t [s]
(b) Velocity of final result.
Figure A.10.: Final result of the continuous approach in test case Si-C-1.
A question that raises in this regard is related to the compromise between the quality of
the solution and the run time taken to find the optimal solution. For the results shown
in chapter 8 the chosen number of maximum iterations is set to 10, which is a rather
conservative setup considering that the results of test case Si-C-1 represent an artificial
case. This allows to implement a more optimistic setup by reducing the maximum
number of SQP optimization iterations to ı̂ = 5 without suffering significant loss of
performance. Additionally, with time_limit = 0.0001 an OSQP parameter is added
that permits to limit the run time of the underlying quadratic programming solver.
Table A.4 shows the measurements taken on a single core of an Intel Core i5@3.30GHz
with 6MB cache in a representative scenario including lead vehicle following and an
overtaking sequence. It has to be noted that measurements are not taken in a real time
environment, which is why given run times need to be rated carefully.
mean run time [ms] standard deviation run time [ms] mean SQP iterations [ ]
75.197 43.757 2.8646
Table A.4.: Run time assessment.
With the chosen setup it can be seen that on average the optimizer converges even faster
than the permitted number of five SQP iterations. However, the run time standard
deviation shows significant potential for improvement as the run time of one single
iteration should be decreased, instead of relying on a low number of iterations. On the
positive side it can be concluded that mean run times of 75 ms are achievable.
130
Fv [km/h] E py [m]
A.11. Further Results
A.11. Further Results
In this section supplemental results of the developed trajectory planning approaches
are presented. Analogous to section 8.2 experiments are subdivided into situation-based
and scenario-based tests in order to enable a thorough analysis with respect to diverse
characteristics.
For situation-based testing a variant of the stop and a variant of the merge maneuver
are shown. Moreover, for scenario-based testing sequences augmented with details
on the in- and output of the trajectory planning approach are depicted, illustrating
the temporal evolution and allowing an extended analysis of the trajectory planning
results.
Situation-based testing
First, a variant of the stop test case is analyzed. Therefore, test case Si-S-1 is changed
in a way that a lateral motion of the ego vehicle is required to reach the target position.
The definition for test case Si-S-2 is given in Table A.5. In coherence to the defined
Table A.5.: Straight road test case definition Si-S-2 for a stop maneuver.
ID [ ] F p Fx [m] py [m] Fv [km/h] Fψ [rad] target lane [ ] F px,s [m] F py,s [m]
ego 0 0 50 0 1 40 3.75
target lane the lateral stop position is set to the center of the left ego lane. The challenge
is hence to find a valid solution, especially with regard to lateral vehicle dynamics
at lower speeds in order to maintain the non-holonomic characteristics of the vehicle
motion.
Figure A.11 shows the results for the continuous and the discrete approach in test case
Si-S-2 using configuration Co-S. Both, the continuous and the discrete approach suc-
ceed in realizing the demanded stop maneuver. The difference is mainly visible in the
lateral motion as the continuous approach behaves smoothly and reaches the target
lane earlier in time. The discrete approach realizes the stop maneuver at the expense
of higher maximum lateral accelerations. In fact, the maximum absolute value of the
lateral acceleration reached by the discrete approach is 3.54 m/s2 at t = 1.2 s, whereas
for the continuous approach it is 2.71 m/s2 at t = 0.6 s. In comparison to the results
of test case Si-S-1 shown in Figure 8.1, the velocities of the continuous approach and
discrete approach are basically the same. This is not surprising, as the longitudinal
stop distance is the same and stop configuration Co-S leaves no significant freedom to
attain further improvements with respect to the overall result.
In the variant of the merge maneuver test case Si-M-1 the initial velocity of the ego
vehicle is increased to 120 km/h instead of driving at an initial speed of 100 km/h. The
resulting test case definition for test case Si-M-2 is summarized in Table A.6. Since the
initial velocity is higher than the velocity of obstacle vehicles ID:1 and ID:2 the ego
vehicle needs to decelerate to position itself in the gap on the left lane.
131
Appendix A. Appendix
6
3
0
−3
−20 −15 −10 −5 0 5 10 15 20 25 30 35 40 45 50 55 60
E px [m]
(a) Result of the continuous approach.
6
3
0
−3
−20 −15 −10 −5 0 5 10 15 20 25 30 35 40 45 50 55 60
E px [m]
(b) Result of the discrete approach.
60 60
40 40
20 20
0 0
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(c) Velocity of the continuous approach. (d) Velocity of the discrete approach.
10 10
5 longitudinal lateral 5 longitudinal lateral
0 0
−5 −5
−10 −10
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(e) Acceleration of the continuous approach. (f) Acceleration of the discrete approach.
Figure A.11.: Results in stop test case Si-S-2.
The corresponding results for test case Si-M-2 using configuration Co-D are shown in
Figure A.12. The results show that the continuous as well as the discrete approach suc-
ceed in realizing a lane change to the left in a right curve, while maintaining proper
safety distances to lead vehicle ID:1 and tail vehicle ID:2. The latter is indicated by the
decrease of the ego velocity, showing that the ego vehicle adapts to the speed of ob-
stacle vehicles ID:1 and ID:2 that form the gap for the merge-in maneuver. In contrast
to the results of test case Si-M-1 the lateral acceleration shows that the planned lateral
motion of the continuous and the discrete approach is nearly identical. The continuous
approach decelerates faster to adapt to the speed of 110 km/h of obstacle vehicles ID:1
and ID:2 and in comparison the longitudinal velocity of the discrete approach be-
tween t = 4 s and t = 5 s is higher than the one of the continuous approach. However,
with respect to the overall result the effective difference in longitudinal position is
neglectable.
132
Fa [m/s2] Fv [km/h] E py [m] E py [m]
Fa [m/s2] Fv [km/h]
A.11. Further Results
Table A.6.: Right curve test case definition Si-M-2 for a merge maneuver.
ID [ ] F p F F F Fx [m] py [m] v [km/h] ψ [rad] target lane [ ] vdes [km/h]
ego 0 0 120 0 1 130
1 30 3.10 110 −0.05 1 110
2 −15 3.50 110 0 1 110
3 45 −0.40 85 −0.05 2 85
6
3 ID:2 ID:1
0 ID:3
−3
−6
−20 0 20 40 60 80 100 120 140 160
E px [m]
(a) Result of the continuous approach.
6
3 ID:2 ID:1
0 ID:3
−3
−6
−20 0 20 40 60 80 100 120 140 160
E px [m]
(b) Result of the discrete approach.
120 120
110 110
100 100
90 90
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(c) Velocity of the continuous approach. (d) Velocity of the discrete approach.
10 10
5 longitudinal lateral 5 longitudinal lateral
0 0
−5 −5
−10 −10
0 1 2 3 4 5 0 1 2 3 4 5
t [s] t [s]
(e) Acceleration of the continuous approach. (f) Acceleration of the discrete approach.
Figure A.12.: Results in merge test case Si-M-2.
133
Fa [m/s2] Fv [km/h] E p [m] Ey py [m]
Fa [m/s2] Fv [km/h]
Appendix A. Appendix
Scenario-based testing
For scenario-based testing some more figures are shown with respect to highway test
case Sc-H-1 to give a comprehensive understanding of the temporal progress and a
more detailed analysis of the particular characteristics that can be observed.
Especially the impact of perception with respect to accuracy and completeness of the
generated environment model should be investigated. Since sensor data is prone to
various effects it is expected that imperfect perception results mitigate the overall per-
formance of the trajectory planning approach. The quality of lane marker detection by
the camera is directly connected to the generation of the road model used to plan the
trajectory. Limited view ranges and occlusion are the basic reasons why inaccuracies
in that respect might occur. These inaccuracies are very likely to increase with higher
distances. In combination with the relatively high planning horizon of Tp = 5 s the
issue with respect to the necessity of lane marker extrapolation is evident. For this
reason some measures are taken to extend the view range of detected lane boundaries,
by assuming for example parallelism of respective lane boundaries and performing a
circular extrapolation based on the detected curvature. In the same sense the object
detection results will largely impact the planned trajectory with respect to desired
distance behavior and collision avoidance. Figure A.13 shows the legend that accounts
for the visualization of aforementioned aspects related to the environment. The object
Environment
object track position
camera - road boundary camera - dashed lane marker
road model - road boundary road model - dashed lane marker
Ego Vehicle
ego vehicle trace
lateral knot position longitudinal knot position
Figure A.13.: Legend for highway scenario analysis.
track position originates from fused radar and camera data. It is important to mention
that the accuracy of each track improves over time, such that especially in the initial
phase of detection larger deviations to the ground truth position of the object might
occur. In addition, the object track position is dominated by the radar reflection point
and lacks camera support for vehicles driving in the rear (as already mentioned in
section 8.2 the simulated ego vehicle is equipped with a front facing camera only).
However, all relevant lane data can be extracted for the purpose of trajectory plan-
ning. It is primarily distinguished between road boundaries and dashed lane markers.
Whereas the former represents all lane marker types, which do not allow the ego
vehicle to pass, the latter denotes all lane marker types that allow the ego vehicle
to cross. For a deeper understanding of the trajectory planning result it is moreover
vital to investigate the processed input related to the static environment. Hence, the
road model that is composed based on the camera data is visualized. This enables an
informative comparison between camera and road model data and contributes to a
better understanding of the planning results. In the closed-loop simulation environ-
ment the realized path that corresponds to the ego vehicle position of all previous
134
A.11. Further Results
simulation cycles is given in terms of the ego vehicle trace. The characteristics of the
planned trajectory in terms of knot positions of the lateral and the longitudinal spline
are visualized as well. In the following results of highway test case Sc-H-1 will first
be discussed for the continuous approach. Afterwards, the results for the discrete
approach will be shown.
The highway entering is one of the most challenging maneuvers with respect to high-
way driving, because the ending lane forces the ego vehicle to make a lane change.
As depicted in Figure A.14 the ego vehicle accelerates to merge in front of obstacle
vehicle ID:7. In addition the ego vehicle should keep track of obstacle vehicle ID:10,
which performs a lane change to the ego target lane and thus becomes the new lead
vehicle.
5 ID:10 ID:9 ID:5
ID:8 ID:11 ID:4
0 ID:7 ID:6 ID:1
−5
t = 1.7 s
− sim10
−280 −260 −240 −220 −200 −180 −160 −140 −120 −100 −80 −60 −40 −20
5 ID:10 ID:9
ID:8 ID:11 ID:5
0 ID:7
− ID:65 ID:1
− tsim = 5.7 s10 ID:4
−180 −160 −140 −120 −100 −80 −60 −40 −20 0 20 40 60 80
ID:11
ID:8 5 ID:7 ID:10 ID:9
0
−5 ID:6 ID:5
− tsim = 7.5 s10 ID:1ID:11
ID:8 −120 −100 −80 −60 −40 −20 0 20 40 60 80 100 120 140
ID:7 ID:4
0 ID:10
−5 ID:9
− ID:610
t
− sim
= 10.2 s ID:1 ID:5
15
−60 −40 −20 0 20 40 60 80 100 120 140 160 180 200 220
E px [m] ID:4
Figure A.14.: Result of the continuous approach for selected time instances, showing the
entering of the highway.
The problem of perception data is in particular revealed in this part of the highway ma-
neuver as changing curvature and a limited view range contribute to a large mismatch
135
E p [m] E p [m] E Ey y py [m] py [m]
Appendix A. Appendix
of the road model with respect to the ground truth lane markers. As a consequence
spline knots that might seem to be placed inaccurately with respect to the lateral po-
sition, show to be accurately positioned with respect to the applicable road model. A
misdetection of an object can be observed at simulation time tsim = 2.1 s leading to a
ghost object at position Fp = [−69.60 − 1.82]T m. The reason for this is an inaccurate
object track of obstacle vehicle ID:5. While first the accuracy improves over time, the
related object track completely vanishes after a few more simulation cycles. Thus, from
planning perspective the observed issue has no substantial effect.
Once the ego vehicle has entered the highway the increased set speed of 120 km/h
after passing E px = 100 m is leading to an overtaking maneuver. The corresponding
sequence that shows the lane change to the left in order to overtake obstacle vehicle
ID:10 is depicted in Figure A.15.
ID:11
ID:8
0
ID:7
−5
− ID:910 ID:10
ID:8
−15 tsim = 11.1 s ID:6
−20 0 20 40 60 80 100 120 140 160 180 200 220 240
−5 ID:11
ID:7
−10
−15
ID:10 ID:9
−20ID:t8sim = 14.3 s ID:6 ID:5
60 80 100 120 140 160 180 200 220 240 260 280 300 320
−10 ID:4
ID:7 ID:11
−15
−20 ID:9ID:10
−25 t ID:6 ID:5sim = 15.7 s ID:1
100 120 140 160 180 200 220 240 260 280 300 320 340 360 ID:4
−10 ID:8
−15 ID:11
ID:7
−20
ID:9
−25 tsim = 17.5 s ID:10 ID:6 ID:5
160 180 200 220 240 260 280 300 320 340 360 380 400 420 ID:4
E px [m]
Figure A.15.: Result of the continuous approach for selected time instances, showing the lane
change to start overtaking of obstacle vehicle ID:10.
136
E p [m] Ey p [m] Ey py [m] E py [m]
A.11. Further Results
After passing E px = 300 m the desired set speed is updated to 140 km/h. Although
obstacle vehicle ID:9 in the ego lane is driving at a slower speed, the ego vehicle is
not changing its lane immediately. This is because vehicle ID:8 is approaching from
behind, narrowing the gap between obstacle vehicle ID:8 and ID:11 on the left ego lane.
Only at the time, when obstacle vehicle ID:8 has adapted its velocity to approximately
130 km/h while keeping a constant distance to obstacle vehicle ID:11, the ego vehicle
starts to perform a single lane change to the left. The merge-in maneuver of the ego
vehicle is depicted in Figure A.16.
ID−:720 ID:8
−25 ID:11 ID:9
−30 ID:10 ID:5
ID:6
− ID:1 ID:435 tsim = 22.0 s
320 340 360 380 400 420 440 460 480 500 520 540 560 580
−20
ID:7 ID:8− ID:1125
−30 ID:10
− ID:6 ID:935 ID:1 IIDD::45
tsim = 23.1 s
360 380 400 420 440 460 480 500 520 540 560 580 600 620
−20
ID:7 −25 ID:8 ID:11
−30
ID:10 ID:9 ID:5
−35 ID:6 ID:1 ID:4
tsim = 24.2 s
400 420 440 460 480 500 520 540 560 580 600 620 640 660
−25 ID:8 ID:11
ID:7 −30
− ID:10 ID:535 ID:6 ID:9 ID:1
−40 ID:4tsim = 26.8 s
500 520 540 560 580 600 620 640 660 680 700 720 740 760
E px [m]
Figure A.16.: Result of the continuous approach for selected time instances, showing the lane
change to start overtaking of obstacle vehicle ID:9.
The ego vehicle then follows obstacle vehicle ID:11, maintaining a constant time gap.
As soon as obstacle vehicle ID:11 does a lane change to the right, the ego vehicle can
now accelerate to the desired set speed. Restrictions imposed by distance keeping with
137
E py [m] E py [m] E p [m] Ey py [m]
Appendix A. Appendix
respect to preceding vehicles are no longer applicable as there is no impeding lead
vehicle. This finally leads to an overtaking maneuver, which is shown in Figure A.17.
During the maneuver, obstacle vehicle ID:11 performs a second lane change to the right
in order to drive onto the rightmost lane. Subsequently, to complete the overtaking of
obstacle vehicle ID:11 the ego vehicle performs a single lane change to the right.
−30
tsim = 44.8 s
−35
ID:7 ID:9−40 ID:8
ID:10 ID:6 ID:1 −45 ID:11
ID:5
ID:4
1180 1200 1220 1240 1260 1280 1300 1320 1340 1360 1380 1400 1420 1440
−30 tsim = 50.1 s
−35 ID:8
ID:5
ID:9 −40 ID:4 ID:11
ID:7 ID:10 ID:6 ID:1 −45
1380 1400 1420 1440 1460 1480 1500 1520 1540 1560 1580 1600 1620 1640
−25
tsim = 51.2 s
−30
−35 ID:8
ID:5
ID:9 −40 ID:11
ID:7 ID:6 ID:4ID:10 ID:1
1440 1460 1480 1500 1520 1540 1560 1580 1600 1620 1640 1660 1680 1700
− tsim = 55.0 s25
−30 ID:8
ID:9 ID:5ID:4 −35 ID:11
ID:1 −40
ID:7 ID:10 ID:6 1580 1600 1620 1640 1660 1680 1700 1720 1740 1760 1780 1800 1820 1840
E px [m]
Figure A.17.: Result of the continuous approach for selected time instances, showing the
overtaking of obstacle vehicle ID:11.
In the last part of the highway scenario, shown in Figure A.18, the ego vehicle overtakes
obstacle vehicle ID:5. After the ego vehicle passes obstacle vehicle ID:5, the ego vehicle
performs a single lane change to the right. Since lanes are more or less straight and
view ranges are comparably high the road model is accurately matching the course of
the road.
138
E py [m] E py [m] E py [m] E py [m]
A.11. Further Results
−15 tsim = 60.1 s
−20 ID:8
−25
ID:5
ID:9 −30 ID:11ID:4
ID:1 1780 1800 1820 1840 1860 1880 1900 1920 1940 1960 1980 2000 2020 2040
ID:6
ID:10
ID:7 −5 tsim = 66.8 s
−10 ID:8
−15
−20 ID:5
ID:9 ID:4 2040 2060 2080 2100 2120 2140 2160 2180 2200 2220 2240 2260 2280 2300
ID:1 0
ID:6 tsim = 70.9 s
ID:10 −5 ID:8
ID:7
−10
ID:11 ID:5−15
ID:9 ID:4 2200 2220 2240 2260 2280 2300 2320 2340 2360 2380 2400 2420 2440 2460
ID:1
ID:6
ID:10 5 tsim = 75.0 s
ID:7 0 ID:8
−5
ID:5
ID:11 −10
ID:9
ID:4 2360 2380 2400 2420 2440 2460 2480 2500 2520 2540 2560 2580 2600 2620
ID:1 E px [m]
ID:6
ID:10
ID:7 Figure A.18.: Result of the continuous approach for selected time instances, showing the
overtaking of obstacle vehicle ID:5.
139
E p E E Ey [m] py [m] py [m] py [m]
Appendix A. Appendix
For the discrete approach observed characteristics in terms of the perception results
apply analogously to the continuous approach. It should be mentioned that because of
the different planning solutions regarding the continuous and the discrete trajectory
planning approach, the two simulation runs are similar, but not exactly equal to each
other.
In general, as will be shown hereinafter, the applied sampling approach proves as a
convenient strategy for driving on curved roads. The curvilinear transform of sampled
breakpoints into vehicle coordinates with subsequent interpolation enables a suitable
approach to lateral positioning with respect to the road model, being independent of
road curvature.
The highway entry as a result of the discrete approach is shown in Figure A.19.
10
5 ID:10 ID:9ID:11 ID:5
ID:8 ID:4
0 ID:7 ID:6 ID:1
−5
tsim = 2.1 s−10
−280 −260 −240 −220 −200 −180 −160 −140 −120 −100 −80 −60 −40 −20
10
ID:11 ID:10 ID:95 ID:5
ID:8
0 ID:7
−5 ID:6 ID:1
tsim = 5.4 s−10 ID:4
−180 −160 −140 −120 −100 −80 −60 −40 −20 0 20 40 60 80
10
5 ID:11ID:8 ID:10
0 ID:7 ID:9
−5 ID:6
tsim = 8.6 s ID:5−10
−100 −80 −60 −ID4:011−20 0 20 40 60 80 100 120 140 160
ID:8
ID:7
0 ID:10 ID:4
−5
ID:6 ID:9
−10
tsim = 10.1 s ID:1 ID:5−15
−60 −40 −20 0 20 40 60 80 100 120 140 160 180 200
E px [m] ID:4
Figure A.19.: Result of the discrete approach for selected time instances, showing the highway
entry of the ego vehicle.
140
E p [m] E p [m] Ey y py [m] E py [m]
A.11. Further Results
At the time the ego vehicle intends to enter the highway obstacle vehicle ID:10 needs
to be considered that likewise performs a lane change to the ego target lane, becoming
the lead vehicle at around tsim = 8.6 s.
In the next sequence depicted in Figure A.20 the ego vehicle overtakes obstacle vehicle
ID:10. Therefore, the ego vehicle accelerates and performs a single lane change to the
left. This is because of the increased set speed of 120 km/h that is applicable after
E px = 100 m has been passed. At that time the ego vehicle set speed is higher than the
current velocity of obstacle vehicle ID:10 that drives at a speed of 100 km/h. As can
be seen at tsim = 14.0 s the view range prevents from detecting the correct curvature
(especially in terms of the correct sign, i.e. direction), leading to a deviation in the road
model and the real course of the road. However, in this case the impact is quite low as
the model is still quite accurate in the range of the planned trajectory.
ID:8 ID:11
0
− ID:75
−10 ID:10 ID:9
ID:8
−15 tsim = 11.4 s ID:6
−20 0 20 40 60 80 100 120 140 160 180 200 220 240
−5 ID:4ID:7 ID:11
−10
−15 ID:9
ID:10
−20 IDt:s8im = 14.0 s ID:6 ID:5
60 80 100 120 140 160 180 200 220 240 260 280 300 320
ID:4
−10 ID:11
ID:7
−15
ID:9
−20 ID:10
−25 tsim = 15.7 s ID:6 ID:5
100 120 140 160 180 200 220 240 260 280 300 320 340 360 ID:4
−10 ID:8
−15 ID:7 ID:11
−20
ID:10 ID:9
−25 tsim = 17.5 s ID:6 ID:5
160 180 200 220 240 260 280 300 320 340 360 380 400 420 ID:4
E px [m]
Figure A.20.: Result of the discrete approach for selected time instances, showing the lane
change to start overtaking of obstacle vehicle ID:10.
141
E py [m] E py [m] E p Ey [m] py [m]
Appendix A. Appendix
In Figure A.21 the ego vehicle is surrounded by several obstacle vehicles. With desired
set speed of 140 km/h and slower lead vehicle ID:9 the ego vehicle has to perform a
lane change to the left to start the overtaking maneuver. In the considered case this
means that the ego vehicle needs to merge into the gap between obstacle vehicle ID:8
and ID:11. After obstacle vehicle ID:9 has been passed, the ego vehicle follows obstacle
vehicle ID:11 on the leftmost lane.
−20 ID:8
ID:7
− ID:1125 ID:9
−30 ID:10 ID:5
ID:6
− ID:435 tsim = 22.5 s ID:1
320 340 360 380 400 420 440 460 480 500 520 540 560 580
ID:7 −25 ID:8 ID:11
−30
ID:10
−35 ID:6
ID:9
ID:1 IDID:4:5
− tsim = 23.9 s40
380 400 420 440 460 480 500 520 540 560 580 600 620 640
ID:8
−25 ID:11
ID:7 −30
ID:10 ID:9−35 ID:6 ID:1
−40 tsim = 26.0 s
460 480 500 520 540 560 580 600 620 640 660 680 700 720
−25 ID:8
ID:11
ID:7 −30
− ID:10 ID:935 ID:6 ID:5ID:1
−40 tsim = 27.5 s ID:4
520 540 560 580 600 620 640 660 680 700 720 740 760 780
E px [m]
Figure A.21.: Result of the discrete approach for selected time instances, showing the lane
change to start overtaking of obstacle vehicle ID:9.
Figure A.22 shows the performed overtaking maneuver of the ego vehicle, once that
obstacle vehicle ID:11 has left the current ego lane. Since at this time there is no relevant
lead vehicle anymore, the ego vehicle can pursue the aim to reach the set speed
of 140 km/h. Obstacle vehicle ID:8 follows the ego vehicle and intends to overtake
obstacle vehicle ID:11 as well.
142
E py [m] E py [m] E py [m] E py [m]
A.11. Further Results
−30
tsim = 47.2 s
−35
−40 ID:8
ID:7 ID:9ID:10 ID:6 ID:1 ID:5−45 ID:4 ID:11
1280 1300 1320 1340 1360 1380 1400 1420 1440 1460 1480 1500 1520 1540
− tsim = 50.5 s30
−35
ID:8
ID:9 −40 ID:5ID:11
ID:7 ID:10 ID:6 ID:1 −45 ID:4
1400 1420 1440 1460 1480 1500 1520 1540 1560 1580 1600 1620 1640 1660
−25 tsim = 52.0 s
−30
−35 ID:8
ID:5
ID:9 −40 ID:11
ID:7 ID:10 ID:6 ID:1
ID:4
1460 1480 1500 1520 1540 1560 1580 1600 1620 1640 1660 1680 1700 1720
−20 tsim = 57.8 s
−25
ID:8
−30 ID:11 ID:5
ID:9 ID:4 −35
ID:1
ID:6 1680 1700 1720 1740 1760 1780 1800 1820 1840 1860 1880 1900 1920 1940
ID:10 E
ID:7 px [m]
Figure A.22.: Result of the discrete approach for selected time instances, showing the single
lane change to finish the overtaking of obstacle vehicle ID:11.
In the final sequence, depicted in Figure A.23, the ego vehicle overtakes obstacle ve-
hicle ID:5 and drives to the rightmost lane by doing a lane change to the right. From
the results it can be concluded that the continuous as well as the discrete approach
are suitable for highway driving. The chosen simulation environment permits to draw
a conclusive picture of the expected performance in real world applications. This ad-
dresses in particular the observed impact of inaccurate perception results. Due to the
long planning horizon the trajectory goes beyond the view range of the perceived
lane markers in almost every case. Still, it can be shown that the extrapolated road
model mitigates the impact on the trajectory planning result, taking into account that
especially in the close proximity of the ego vehicle perception results are adequately
accurate. Having a closer look on the object tracking result, it can be found that espe-
cially in the back of the ego vehicle inaccuracies appear to a larger extend. However, for
143
E p E E Ey [m] py [m] py [m] py [m]
Appendix A. Appendix
−10 tsim = 62.6 s
−15
−20 ID:8
−25 ID:11 ID:5
ID:9
ID:4 1880 1900 1920 1940 1960 1980 2000 2020 2040 2060 2080 2100 2120 2140
ID:6 ID:1
ID:10 −5 tsim = 66.8 sID:7
−10 ID:8
−15
−20 ID:5
ID:9 ID:4 2040 2060 2080 2100 2120 2140 2160 2180 2200 2220 2240 2260 2280 2300
ID:6
ID:10 tsim = 69.2 s−5
ID:7 ID:8
−10
ID−:1115 ID:5
−20
ID:9 ID:4 2140 2160 2180 2200 2220 2240 2260 2280 2300 2320 2340 2360 2380 2400
ID:1
ID:6
ID:10 0 tsim = 72.0 s
ID:7 − ID:85
−10 ID:5
ID:11 −15
ID:9 ID:4 2240 2260 2280 2300 2320 2340 2360 2380 2400 2420 2440 2460 2480 2500
ID:1 E px [m]
ID:6
ID:10 Figure A.23.: Result of the discrete approach for selected time instances, showing the lane
ID:7 change to finish the overtaking of obstacle vehicle ID:5.
ego trajectory planning the distance keeping objective reflects the desired behavior in
terms of safe driving. It ensures to prevent dangerous situations in advance, effectively
compensating for minor perception deviations. The receding horizon characteristic
certainly contributes to react appropriately on unforeseen and even incorrectly pre-
dicted events. As can be seen from the scenario-based tests, when it comes to driving in
dynamic environments combined with an imperfect perception the latter is a valuable
characteristic of the developed approach.
144
E py [m] E p E Ey [m] py [m] py [m]
A.12. Settings and Configuration
A.12. Settings and Configuration
The high level optimization parameters as shown in Table A.7 summarize all rele-
vant setting options for high level optimization, i.e. objectives and inequalities. They
hence account for the setup of weights and constraint values manipulating the desired
behavior of the vehicle.
Table A.7.: High level optimization parameters.
description variable value unit
distance keeping weight ωd 5000 [ ]
reference velocity weight ωv 10 [ ]
reference lateral position weight ωp 500 [ ]
maximum acceleration weight ωc 5000 [ ]
constant time gap to lead vehicle Tl 1.0 [s]
constant time gap to tail vehicle Tt 0.5 [s]
minimum safety distance dmin 3 [m]
comfort acceleration a 2comf 1.5 [m/s ]
comfort longitudinal acceleration limit ăx 3.5 [m/s2]
comfort lateral acceleration limit ăy 2.5 [m/s2]
minimum breakpoint time difference Tm 0.5 [s]
maximum longitudinal acceleration amax see Figure 7.2 [m/s2]
maximum steering angle δmax 0.64 [rad]
maximum total acceleration ag 9.0 [m/s2]
In order to obtain an accurate description of the vehicle motion, vehicle model param-
eters need to be chosen appropriately. The parameter values as given in Table A.8 are
based on the Audi A3 Sportback 2009.
Table A.8.: Vehicle parameters.
description variable value unit
ego vehicle length lego 4.292 [m]
ego vehicle width wego 1.995 [m]
inter-axle distance la 2.5780 [m]
distance of CoG to front axle lv 1.057 [m]
distance of CoG to rear axle lh 1.521 [m]
vehicle mass m 1400 [kg]
cornering stiffness front cv 117 800 [N/rad]
cornering stiffness rear ch 127 900 [N/rad]
characteristic velocity vch 31.9604 [m/s]
The solver settings relate to the utilized underlying nonlinear constrained optimiza-
tion approaches. Since the optimal interpolation relies on the result of the quadratic
145
Appendix A. Appendix
programming solver it is vital for the continuous as well as for the discrete trajec-
tory optimization approach. On top of that, for the continuous trajectory optimization
approach, the parameters of the SQP algorithm need to be set. Because of the ba-
sic relevance of the constrained optimization algorithms for the developed trajectory
optimization approaches, these parameters have a decisive impact on the overall result.
The OSQP solver settings as shown in Table A.9 are taken from Stellato et al. (2020)
and hence represent the default values for the solver.
Table A.9.: OSQP solver settings.
description parameter name value
ADMM overrelaxation parameter alpha 1.6
ADMM rho step rho 0.1
ADMM sigma step sigma 1 · 10−6
primal infeasibility tolerance eps_prim_inf 1 · 10−4
dual infeasibility tolerance eps_dual_inf 1 · 10−4
relative tolerance eps_rel 1 · 10−3
absolute tolerance eps_abs 1 · 10−3
maximum number of iterations max_iter 4000
The SQP solver setting is given in Table A.10 providing corresponding line search
parameters as well as tolerance values for the definition of reasonable termination
criteria.
Table A.10.: SQP solver settings.
description variable value
relative tolerance 1.49 · 10−8
absolute tolerance 1.49 · 10−8
line search acceptance factor η̂ 0.25
line search step size factor maximum τ̂ 0.5
line search step size factor τ̂α̂ 0.45
146