Content uploaded by Jens Schulz
Author content
All content in this area was uploaded by Jens Schulz on Oct 16, 2018
Content may be subject to copyright.
Estimation of Collective Maneuvers through Cooperative Multi-Agent
Planning
Jens Schulz, Kira Hirsenkorn, Julian L¨
ochner, Moritz Werling, and Darius Burschka
Abstract— In order to determine a cooperative driving strat-
egy, it is beneficial for an autonomous vehicle to incorporate
the intended motion of surrounding vehicles within its own
motion planning. However, as intentions cannot be measured
directly and the motion of multiple vehicles often are highly
interdependent, this incorporation has proven challenging. In
this paper, the problem of maneuver estimation is addressed,
focusing on situations with close interaction between traffic
participants. Therefore, we define collective maneuvers based on
trajectory homotopy, describing the relative motion of multiple
vehicles in a scene. Representing maneuvers by sample tra-
jectories, maneuver-dependent prediction models of the vehicle
states can be defined. This allows for a Bayesian estimation of
maneuver probabilities given observations of the real motion.
The approach is evaluated by simulation in overtaking scenarios
with oncoming traffic and merging scenarios at an intersection.
I. INTRODUCTION
Predicting the motion of nearby vehicles is a prerequisite
for safe, foresighted, and cooperative driving of autonomous
vehicles. As human drivers have individual and highly com-
plex behaviors, prediction will always be afflicted with uncer-
tainty. Thus, it is difficult to derive one accurate deterministic
prediction that an autonomous vehicle can rely on for its
own motion planning. One alternative to this problem is
to consider multiple discrete maneuver hypotheses within
the prediction and estimate the probability distribution over
them. This maneuver estimate can in turn be used as a basis
for a continuous motion prediction.
Especially in situations where decisions of traffic partici-
pants are highly coupled, the assumption of independent mo-
tion becomes invalid. This motion interdependency of nearby
vehicles and the ego-vehicle yields additional challenges, as
it is not possible to separate the problems of prediction and
planning anymore. This can be seen in Fig. 1 where the black
and white vehicle closely interact. Without anticipating the
maneuver decision of the other driver, one can hardly decide
for one of the two options. However, the own decision may
influence the decision of the other driver as well.
In this paper, we approach this problem by defining
collective maneuvers on a multi-agent scale, describing the
relative motion of multiple vehicles in a scene. As the
interconnection of decisions often arise from geometric colli-
sion avoidance constraints, the maneuver definition is based
J. Schulz is with BMW Group and the Department of Computer Science,
Technical University of Munich, Germany. jens.schulz@bmw.de
K. Hirsenkorn, J. L¨
ochner, and M. Werling are with BMW Group,
Munich, Germany. {kira.hirsenkorn| julian.loechner
| moritz.werling}@bmw.de
D. Burschka is with the Department of Computer Science, Technical
University of Munich, Germany. burschka@tum.de
AB
C
A
D
E
Fig. 1. Two situations where the black and white vehicles’ motions
are highly coupled. Multi-agent trajectories are shown for two collective
maneuver hypotheses (blue and green). Both vehicles should intend to follow
the same maneuver for collisions and misunderstandings to be avoided.
on the concept of trajectory homotopy. Cooperative multi-
agent trajectories are planned for the given maneuvers to
reflect common human behavior. These sample trajectories,
used to represent the corresponding maneuvers, allow for a
comparison of abstract maneuver classes and observations
of the actual motion. Through Bayesian statistic, using an
interacting multiple model estimator, the probability distri-
bution over the possible maneuvers is derived, serving as a
basis for the decision making of the autonomous vehicle.
As the ego-vehicle as well as other vehicles are included in
the multi-agent planning process, both tasks, prediction and
motion planning, are solved at once. Thus, we overcome the
interdependency of prediction and planning, resulting in a
cooperatively driving autonomous vehicle.
Fig. 2 provides an overview of the maneuver estimation
and planning framework. The single components are ex-
plained in detail within the remaining chapters of this paper:
Sec. II summarizes the related work in the area of motion
prediction and maneuver representation. The collective ma-
neuvers and how they are derived are described in Sec. III.
Sec. IV depicts the multi-agent trajectory planning including
the constraints to enforce a specific maneuver class. In Sec. V
the maneuver probability distribution is derived given the
sample trajectories and the motion observations. Sec. VI
evaluates and Sec. VII concludes the presented work.
II. RE LATED WORK
In the area of autonomous vehicles, motion prediction of
other traffic participants has been widely studied. One com-
ponent is the estimation of currently conducted or upcom-
ing maneuvers. Commonly, the set of possible maneuvers
is defined by hand using expert knowledge. For highway
scenarios, this is often done by using discrete lane matchings
and distinguishing between different states a vehicle can
reside in, as for example free flow,car following,lane change
and overtaking [1]. A more detailed description has been
used by [2], where an overtaking maneuver is compound by
a series of behaviors: acceleration phase,sheer out,overtake,
Determination of
Maneuver Classes
Trajectory Planning
for each Maneuver
Maneuver
Estimation
Driving Strategy
World
u∗,ego
z
z
M={M1,...,M|M|}
u∗
M
PM
J∗
M
Fig. 2. Maneuver estimation and driving strategy framework. Dashed
arcs represent temporal dependencies whereas solid arcs represent causal
dependencies. The variables are defined in the corresponding sections.
sheer in. For intersections, many previous works have tried
to predict the outgoing lane vehicles are going to take [3]–
[8]. For that purpose, maneuvers of type turn right,turn left
or go straight have been defined.
Another way of defining maneuvers is to cluster paths or
trajectories into homotopy classes, which has already been
used in the area of robotics for ego-vehicle motion planning
[9]–[16]. Kuderer et al. [14] present an online framework
to generate homotopically distinct navigation paths with the
help of Voronoi diagrams. By using homotopy classes they
avoid sticking to local minima during the path optimization.
Bender et al. [16] propose to distinguish different maneu-
vers of autonomous vehicles with nearby dynamic obstacles
based on homotopy classes and determine the best trajectory
within those classes without the need of global optimization
techniques. Gu et al. [15] automatically discover tactical
maneuver patterns based on sampled trajectories. Using
pseudo-homology, they can extract distinct maneuver classes
and decide according to cost functions. All these works
focus on ego-vehicle motion planning and assume other
traffic participants to have known motion profiles such as
constant velocity. Thus, these approaches are not suitable
for maneuver prediction and only allow for single-agent
environments, neglecting the fact that vehicles react to each
other and their decisions may be coupled.
For the actual estimation of the intended maneuver given
available observations, there are many different methods,
reaching from discriminative models like SVMs [17] to prob-
abilistic discriminative models like CRFs [8] to probabilistic
generative models like Bayesian networks or HMMs [2]–
[6]. Although some of those works explicitly model inter-
actions between vehicles within their behavior models, their
maneuver representations still lack the descriptive power to
specify the relative motion of vehicles in detail, beneficial
for scenarios with close interaction between vehicles.
III. MANEUVER DET ER MINATIO N
In this section, a collective maneuver representation for
multi-agent systems based on trajectory homotopy is pro-
posed and it is shown how a set of possible maneuvers can
be derived given a traffic scenario with lane-precise map data.
Within the scope of this paper, the desired exit direction of
a vehicle crossing an intersection is assumed to be known.
A. Multi-Agent System
1) Trajectory: Assume a set of vehicles
V={V1,· · · , VN}including the ego-vehicle is considered
within the multi-agent system. The state of vehicle Vn∈ V
is given by xn= [sn, vn
s, dn, vn
d]>∈R4, consisting of the
longitudinal and lateral Fren´
et-coordinates [18] snand
dnof the vehicle’s center-position and the corresponding
velocities vn
s/d. The area occupied by this agent is
approximated with a bounding box with length lnand width
wnoriented along the tangent of the road’s centerline:
A(sn, dn, ln, wn) := An⊂R2. Changes in orientation, e.g.
during lane changes, are neglected. Furthermore, the area
occupied by static obstacles is denoted as O⊂R2. The
multi-agent configuration is defined as
x= [x1,· · · ,xN]>∈X⊂R4N(1)
within the collision-free configuration space
X={x| ∀n, m ∈ {1,· · · , N}, n 6=m:
(An∩O=∅)∧(An∩Am=∅)}.(2)
On the basis of [16] for single agent trajectories, we define
a multi-agent-trajectory to be a mapping from time span to
configuration space: γ: [0, T ]→X.
2) Formation: To describe the relative order of the differ-
ent agents within a multi-agent configuration xin an abstract
way, the formation F(x)is extracted. Any formation F
holds information about the two dimensional relative position
of objects, neglecting the exact distances and lengths. This
allows for a compact description of the maneuver relevant
aspects of the current scene.
For every street section without intersections, a local
Fren´
et coordinate system is defined. The two dimensional
positions of all vehicles on this section are first projected
onto the longitudinal dimension. All vehicles are sorted
in a list in ascending order according to sn, storing the
sequence of vehicles along the section. This list is then
extended to a second dimension using the discrete lane
matching information, resulting in the formation for that
section (see Fig. 3(a)). Each cell within a formation is at
most occupied by one vehicle, two vehicles cannot be on
the same longitudinal column, i.e. laterally adjacent cells of
an occupied cell are always unoccupied. Although in reality
vehicles might overlap, it is not stored in the formation as it is
not needed for the determination of the possible maneuvers.
Using map data, it is possible to extract the different
sections and how they are connected at intersections. The
formations of the single sections are derived independently
and then linked using intersection cells (Fig. 3(b)). If lanes
C
A B
A
C
B
s
d
F
s
A
s
B
s
C
lane
1
lane
2
(a) Formation of a section using a local Fren´
et coordinate system and lanes.
E
F
D
F
E
F
s
F
s
E
D
s
D
sec1
sec
2
sec
3
intersec
(b) Formations of multiple sections are connected using intersection cells.
Fig. 3. Derivation of the formation F(x)of a given configuration x.
Fig. 4. Single-agent path homotopy: All paths of same color are homotopic
relative to the start and end position areas (gray circles).
of an intersection overlap, they share at least one common
cell. If a vehicle is currently driving on an intersection, the
formation will relocate the vehicle to the connecting lane of
the subsequent normal section and queue it behind all other
vehicles, hence, intersection slots are never occupied.
B. Homotopy Classes
1) General Homotopy: In the field of topology, two
functions g1, g2:Q→Rare said to be homotopic (or to be
in the same homotopy class), if it is possible to continuously
deform one into the other [13]. Hence a continuous homotopy
H:Q×[0,1] →Rhas to exist, such that
H(q, 0) = g1(q)∧H(q, 1) = g2(q)∀q∈Q. (3)
Furthermore, g1and g2are called homotopic relative to the
subspace ˜
Q⊂Q, if they are homotopic and
H(˜q, λ) = g1(˜q) = g2(˜q)∀˜q∈˜
Q∧ ∀λ∈[0,1].(4)
2) Single-Agent Path Homotopy: In the case of robot mo-
tion planning, path homotopies are usually defined relative
to the paths’ start and end positions [10], [14]. Typically,
the start position is known and it is desired to distinguish
different end positions and whether obstacles are passed on
the left or right side. To allow for inaccuracy in planning,
this condition is often relaxed such that the start and end
positions only need to be in specific areas. This is illustrated
in Fig. 4. Without keeping the start and end positions in fixed
areas, it would be possible to continuously deform all paths
into each other. Hence, no distinction between passing the
obstacle on the left or right side could be made.
∞
∞
∞
∞
-∞
-∞
-∞
-∞
s
d
t
∞
∞
∞
∞
-∞
-∞
-∞
-∞
s
d
t
Fig. 5. Multi-agent trajectory homotopy: Two non-homotopic multi-agent
trajectories of the scene of the left part of Fig. 1 are shown. On the left, VA
(blue) passes VB(black) first, then VC(green), on the right the other way
around. Considering road boundaries and planning horizon to be obstacles
of infinite size, different homotopy classes can be defined relative to the
start and end formation.
3) Multi-Agent Trajectory Homotopy: Similar to [12] and
[15], we extend the concept of path homotopy to the area of
trajectories by adding the dimension of time and introduc-
ing the following assumptions for structured environments:
First, the constraint of keeping start and end states at fixed
positions or areas is relaxed to keeping them within fixed
formations. Secondly, road boundaries and the planning
horizon are interpreted as obstacles of infinite size, as shown
in Fig. 5. Hence, it is not possible for a trajectory to run
outside the road boundaries or beyond the planning horizon.
For two multi-agent trajectories to be homotopic, the
trajectories of all vehicles need to allow a continuous trans-
formation without colliding with obstacles or each other.
Two mulit-agent trajectories γ0, γ1: [0, T ]→Xwith the
same start and end formation F(γ0(0)) = F(γ1(0)) := F0
and F(γ0(T)) = F(γ1(T)) := FTare said to be homotopic
relative to {F0, FT}, if there exists a continuous homotopy
H: [0, T ]×[0,1] →X, such that
H(t, 0) = γ0(t)∧H(t, 1) = γ1(t)∀t∈[0, T ](5)
and
F(H(0, λ)) = F0∧F(H(T, λ)) = FT∀λ∈[0,1].(6)
In addition to the lateral sides agents pass each other, tra-
jectory homotopy also considers temporal aspects: Consider
the scenario in Fig. 5 with a laterally bounded street such
that at most two vehicles can be next to each other. Given the
start and end formation, both VAand VChave to pass VB. As
at the longitudinal occupancy of VBthere is only space for
one other vehicle, different sequences in which VBis passed
need to be distinguished. As it is impossible to continuously
transform the trajectories of Fig. 5 (left) to those of Fig. 5
(right), they belong to different homotopy classes.
4) Pseudo Multi-Agent Trajectory Homotopy: Except for
the start and end formations, the trajectory homotopy defined
previously is solely based on the multi-agent configuration
including the areas of obstacles and agents, but not on high
level information such as lanes. Consider the same scenario
depicted in Fig. 5, but assume that geometrically, there is
enough space for all three vehicles to be on the same lon-
gitudinal position. As drivers tend to drive on lanes, typical
behavior for VAwould still include a discrete decision to
either overtake before VCor after. With trajectory homotopy,
it is not possible to distinguish these two maneuvers, as it
is possible to continuously deform one into the other. The
same is true for three or more adjacent lanes.
Therefore, we propose a pseudo homotopy based on
formations, that combines the advantages of homotopy with
additional domain knowledge. The following spatial and
temporal relations allow a high-level description of the
relative motion in a comprehensible way that can be used
to distinguish maneuvers. First of all, if two longitudinally
adjacent vehicles pass each other, the side on which they do
is of interest. This is denoted as pairwise lateral relations
and can be derived by the discrete lane matchings. In many
situations, the temporal order in which vehicles pass a
specific area is of importance. This area might either be on
an intersection, where vehicles have to pass sequentially, or
on a normal section where there are conflicts on a lane such
as in the example above. This temporal order is denoted as
sequence of passing of that critical area.
A maneuver Mi∈ M is given by a series of consecutive
formations from F0to FT i, including all pairwise lateral
relations and all sequences of passing of the critical areas,
and represents one pseudo trajectory homotopy class.
C. Formation Tree
As this paper focuses on the prediction of human drivers,
we are not only interested in the one optimal homotopy class,
but in all possible reasonable classes that human drivers
might choose. To determine the set of possible maneuvers
in every time step, first the current formation F0of a given
scene is identified using the positions of all considered
vehicles and the available map data. Building a formation
tree with root node F0and expanding it iteratively, different
possible end formations FT i together with the corresponding
intermediate formations can be found.
1) Expansion: The relative motion between agents is
described on high abstraction level using a set of discrete
actions A={along, aleft , aright}vehicles can choose from.
The actions do not represent continuous motion, but changes
in the relative order. A vehicle-action-pair (V, a)∈ V × A
transforms one formation into another. The vehicle that is
in the longitudinal sequence of vehicles next in driving
direction, independently of the lane, is from now on ab-
breviated as the vehicle ahead. The lateral actions aleft and
aright represent lane changes whereas the longitudinal action
along can represent two things: passing the vehicle ahead,
resulting in a switch of their longitudinal order, or passing
an intersection and queuing into the connecting lane on the
subsequent normal section. Intersection cells only serve as
a connection between regular cells and are never occupied
within a formation.
For any vehicle-action-pair (V, a)∈ V × A and a given
formation Fi, a new formation F0
ican be derived. See
Fig. 6(a) for an example of how the formation changes for the
different possible vehicle-action-pairs. Different tuples can in
fact result in the same new formation, the relative motion is
expressed on a multi-agent scale (e.g. in the formation on top
of Fig. 6(a), (VB, along) results in the same new formation
as (VC, along)). Through exhaustive expansion by checking
all possible actions of all vehicles, the tree can be built up
gradually.
C
AB
A C
B
C
A B
ABC
B C
A
C
BA
C
B
C
A
AB
C
A B
V
A
,a
left
V
A
,a
right
VA,a long
V
B
,a
left
V
B
,a
right
V
B
,a
long
V
C
,a
long
V
C
, a
right
V
C
, a
left
p
2
p
1
p
2
p
2
p
2
p
2
p
5
(a) First iteration of the maneuver expansion tree, showing how vehicle-
action-pairs transform one formation to another. Formations that are pruned
due to the pruning conditions pi∈ P are crossed out.
t
n
A
C
AB
A C
B
C
B
C
A B
A C
B
C
B A
C
B A
C
B A
C
A B
C A
B
C A
B
C
B A
F
T1
F
T2
F
T3
F
0
(b) Three possible maneuvers M1, M2, M3consisting of the sequences from
F0to FT1(VAfollows VB), FT2(VAovertakes VBbefore VCpasses VB)
and FT3(VAovertakes VBafter VChas passed VB).
Fig. 6. Formation tree expansion to identify reasonable maneuvers.
2) Pruning: The pruning conditions Pare used exclude
the occurrence of loops and unfeasible or unlikely behavior:
•p1:Passing on same lane: passing the vehicle ahead if
it is on the same lane
•p2:Unsuitable lane change: changing to the lane of the
vehicle ahead if it is oncoming;
performing a lane change to a non existing lane
•p3:Reverse lateral action: changing to a lane previ-
ously driven on within this maneuver, without having
overtaken another vehicle
•p4:Re-overtaking: two vehicles passing each other that
have previously passed each other within this maneuver
•p5:Existing new formation: a new formation that al-
ready exists having the same parent formation
These heuristics do not consider reasonable vehicle dynamics
as a prerequisite yet, but this will be part of future work to
further reduce complexity.
3) Final formation: A sequence of consecutive formations
is only considered to be a valid maneuver if its final
formation meets the final formation conditions F:
•f1: all vehicles with different driving directions have
passed each other
•f2: all vehicles are on a lane with correct driving
direction
The expansion process is continued even on possible final
formations until no new formations can be found anymore. A
graphical representation of a maneuver search tree including
the possible final formations is shown in Fig. 6(b).
4) Removing Duplicates: It is possible that multiple ma-
neuvers in the tree are similar in a way no distinction
is desired. As an example, consider two vehicles crossing
an intersection using lanes that do not overlap (i.e. using
different intersection cells). Then the temporal order in which
they cross that intersection is not of importance. Similarly
on normal sections, consider four vehicles V1–V4driving
successively. If V1passes V2and V3passes V4, the order in
which these two actions happen is not of interest. Similar
maneuvers like those are united, and the corresponding
constraints are left out for the trajectory planning.
IV. TRAJECTORY PLANNING
To allow for a comparison of abstract maneuver classes
and an observed trajectory, we suggest to represent any
abstract maneuver class by (at least) one multi-agent sample
estimate trajectory. Assuming this trajectory reflects common
driver behavior for that maneuver, it is possible to infer
the probability distribution over the possible maneuvers by
comparing the sample trajectories to the observed motion.
To generate a reasonable sample trajectory for every
maneuver, an optimization problem is formulated that aims
to minimize a collective cost-function while satisfying the
hard constraints given by the specific maneuver class. Sim-
ilar to [19], mixed-integer quadratic programs with logical
constraints are used to enforce specific maneuvers.
A. State-Space Model
Since the purpose of the motion model is to determine a
maneuver estimate rather than to generate smooth trajecto-
ries, it is represented by a simple double integrator, assuming
the longitudinal and lateral acceleration un= [an
s, an
d]>of
vehicle Vncan be controlled directly. For such a separated
motion model, the side slip angle β= arctan(vd/vs)can
be limited to allow the trajectories to be followed by non-
holonomic vehicles [20].
The multi-agent trajectory γcan be derived with
the initial state x0and the sequence of control inputs
u(k)=[u1(k),· · · ,uN(k)]>with k∈ {0,· · · , K −1}.
The motion models of the single vehicles are independent
given their control inputs. However, their control inputs
are coupled through the multi-agent optimization which
accounts for the interdependencies between vehicles.
B. Hard Constraints
Independently of the maneuver, the state and control
variables of all vehicles are bounded to ensure reasonable ve-
hicle dynamics. Furthermore, vehicles are constrained to stay
within the road boundaries. These maneuver-independent
constraints are denoted as C.
To enforce the planned trajectory to lie within the range
of the given maneuver Mi, restricting the planning space
to a specific pseudo-homotopy class, maneuver-dependent
constraints CMiare formulated. As explained in Sec. III,
maneuvers distinguish between spatial conditions (i.e. on
which side two vehicles pass each other) and temporal
conditions (i.e. in which order vehicles pass a critical area).
1) Spatial Collision Avoidance: Consider any pair of two
vehicles VAand VBwithin a multi-agent environment, ap-
proximated by rectangles with length land width woriented
along the centerline of a two-lane road. The spatial collision
avoidance of VAand VB, i.e. interdicting any overlapping but
not stating whether they pass each other or on which side,
can be enforced by:
(sA≤sB−l)∨(sA≥sB+l)
∨(dA≤dB−w)∨(dA≥dB+w)(7)
Additional safety distances can easily be incorporated by
adding or subtracting the desired margin respectively.
Constraints of that nature are called logical constraints,
as they combine linear constraints using logical operators.
With the use of the so-called Big M method [21], [22], those
logical constraints can be reformulated into a set of linear
inequality constraints. By introducing an application-specific
big number Mbig, single conditions can be rendered inactive
by the addition or subtraction of Mbig depending on the value
of a binary variable δi. This binary variable states whether
the condition is active (δi= 1) or not (δi= 0). The logical
disjunction of single binary variables is equivalent to at least
one of the corresponding conditions to be active:
sA≤sB−l+ (1 −δ1)Mbig (8)
sA≥sB+l−(1 −δ2)Mbig (9)
dA≤dB−w+ (1 −δ3)Mbig (10)
dA≥dB+w−(1 −δ4)Mbig (11)
δ1+δ2+δ3+δ4≥1(12)
The pairwise lateral relations specified by a maneuver
can be enforced using these spatial conditions by setting the
corresponding binary variable (either δ3or δ4) to 0.
2) Temporal Collision Avoidance: Now consider the sce-
nario depicted in Fig. 5 with the three vehicles VA,VB,
VC. Additionally to the conditions for the pairwise lateral
relations, a desired sequence of passing (e.g. VAbefore VC)
can be enforced by the temporal condition
(sC≤sB)⇒(sA> sB).(13)
This can be reformulated in Big M notation as
sC> sB−δkMbig (14)
sA> sB−(1 −δk)Mbig,(15)
thus, only one additional binary variable is needed to enforce
the desired sequence.
3) Separation of Longitudinal and Lateral Optimization:
To reduce the number of needed binary variables and hence
reduce complexity, the problem is divided into two subprob-
lems: optimizing the longitudinal and lateral control inputs
separately. In the field of single-agent planning, this has al-
ready been suggested by [23]. The longitudinal optimization
ensures the correct sequences of passing, whereas the lateral
optimization ensures the correct pairwise lateral relations.
By first solving the longitudinal optimization, its results can
be used as an input for the lateral part. Hence, for the lateral
optimization, the longitudinal positions of all vehicles are
known in advance. The spatial collision avoidance constraints
can thus be expressed without the need of binary variables.
For the aforementioned overtaking scene, the number of
binary variables is reduced from ten (three per pairwise
lateral relation and one for the sequence of passing) to one
per time step. Furthermore, as the values of the single δi
are known for k= 0, motion is only allowed within driving
direction and re-overtaking is forbidden, the single binary
variables can change their values at most once during one
maneuver. Hence the number of possible combinations per
binary variable reduces from 2Kto (K+ 1).
l
A
/2
l
A
/2
l
C
/2
l
B
/2
β
α
C
A
B
l
A
/2
l
A
/2
l
C
/2
l
B
/2
β
α
C
A
B
Fig. 7. Longitudinal constraints for maneuver M2to enforce a desired
sequence of passing. On the left, equation (16) is active (δ1= 0) and VC
is not allowed to enter the green area. As soon as VAhas overtaken VB,
this restriction is removed and equation (17) is activated (δ1= 1).
The longitudinal constraints to enforce a desired sequence
of passing are adapted by the typical length needed for a lane
change αand a sufficient safety distance β. These distances
are illustrated exemplarily for the maneuver M2in Fig. 7
(see Fig. 6(b) for maneuver definition). The longitudinal
constraints (14) and (15) of M2change accordingly:
sC> sB+lB/2 + α+lA+β+lC/2−δ1Mbig (16)
sA> sB+lB/2 + α+lA/2−(1 −δ1)Mbig (17)
The constraints of M3are defined analogously, the ones for
M1only contain a safety distance between VAand VB. For
intersection scenarios, the longitudinal constraints are defined
relative to the overlapping areas.
The state and the control inputs as well as the optimization
constraints, the cost function and its weights can be divided
into a longitudinal and a lateral part, which are from now on
denoted by subscript sand d, respectively.
C. Cost-Function
For the trajectories to reflect common human behavior, a
quadratic cost-function is chosen that penalizes acceleration
as well as the deviation from the desired speed and the
desired lateral position. The deviation of state and reference
state xn,ref = [sn,ref, vn,ref
s, dn,ref, vn,ref
d]>is given by
∆xn
k=xn
k−xn,ref
k.(18)
The value of vn,ref
srepresents the desired speed vdes
s,vn,ref
dis
zero, and dn,ref is the center of the current lane. The value
of sn,ref is irrelevant, as the corresponding weight is zero.
The collective cost function of the multi-agent optimiza-
tion problem is defined quadratic w.r.t. the control inputs as
well as the deviation of state and reference state as
J=Js+Jd=
N
X
n=1
Jn
s+
N
X
n=1
Jn
d,(19)
with the components of the single vehicles
Jn
s=
K
X
k=1
(∆xn
s,k)>Qn
s∆xn
s,k +
K−1
X
k=0
un
s,kRsun
s,k,(20)
Jn
d=
K
X
k=1
(∆xn
d,k)>Qn
d∆xn
d,k +
K−1
X
k=0
un
d,kRdun
d,k.(21)
By taking the costs of other vehicles into account, drivers
are assumed to behave cooperatively. The state weighting
matrices Qn
s=diag(0, ω)and Qn
d=diag(ω, ω/2) and the
control input weights Rn
s=ωand Rn
d=ω/2are chosen by
expert knowledge. The weight ω= 1 + (γ−1)ρdepends on
whether a vehicle has right of way (ρ= 1) or not (ρ= 0)
and on how strong drivers weigh these traffic rules (γ). This
encourages traffic rule compliance in a soft manner, still
allowing drivers to slightly deviate if necessary.
The longitudinal optimization for maneuver Mican be
formulated as
[u∗
s,Mi,δ∗]>= arg min
[us,δ]>
Jssubject to Cs∪ CMi,s,(22)
resulting in the optimal longitudinal costs J∗
Mi,s. After de-
termining u∗
s,Mi, the QP for the lateral optimization can be
defined only using linear inequality constraints as
u∗
d,Mi= arg min
ud
Jdsubject to Cd∪ CMi,d,(23)
resulting in the optimal lateral costs J∗
Mi,d. Note that only
the hard constraints depend on the specific maneuver Mi,
whereas the cost-function is maneuver-independent. The
overall optimal costs of Miare determined by the sum of the
optimal costs of both the longitudinal and lateral component:
J∗
Mi=J∗
Mi,s +J∗
Mi,d (24)
The trajectory planning with the corresponding hard con-
straints is exemplarily depicted for an overtaking scenario in
Fig. 8. Within this example, only VAand VCare considered
within the multi-agent planning, VBis considered to be
parked. The cost-optimal trajectory of each of the three
possible maneuver hypotheses are shown (M1:follow,M2:
overtake before oncoming,M3:overtake after oncoming).
V. MANEUVER ESTIMATION
Any driver is assumed to have exactly one intended
maneuver at a given time step. The intended maneuver
can switch between time steps, which satisfies the fact that
drivers continuously re-evaluate situations and may change
their intentions. Thus, the motion of vehicles is modeled as
a stochastic process that consists of |M| different models,
exactly one being active per time step. Following [4], the
maneuver switching probability is set to µ= 0.1and
distributes uniformly among all other maneuvers.
The prediction task is to estimate the active model from
which the drivers choose their actions. This is achieved
by an interacting multiple model (IMM) Kalman filter.
The prediction steps of maneuver Miare performed ac-
cording to the controls u∗
Miof the optimal trajectory.
Through Bayesian statistics, the posterior mode probabilities
PM= [PM1,· · · , PM|M| ]>are obtained in every time step
given the predicted states, the measurement of the positions
z= [s1
z, d1
z,· · · , sN
z, dN
z]>, the last posterior and the switch-
ing probabilities. Initially, a uniform prior is assumed. For
the determination of the single distributions, we refer to [24].
Although drivers may deviate from the sample trajectory
of their intended maneuver, it is still possible to estimate
the maneuver correctly, as long as the deviation is smaller
compared to the sample trajectories of the other maneuvers.
An exemplary probability distribution of a given scene is
depicted in Fig. 8(d).
02468
100
200
300
s[m]
M1
02468
t[s]
M2
02468
M3
(a) Longitudinal part of trajectories with hard constraints.
02468
−2
0
2
d[m]
M1
02468
t[s]
M2
02468
M3
(b) Lateral part of trajectories with hard constraints.
160 170 180 190 200 210 220 230 240
−2
0
2
A B
C
s[m]
d[m]
M1
M2
M3
(c) Optimal trajectories of all considered maneuvers.
M1M2M3
45
1
54
PM[%]
(d) IMM maneuver probabilities.
Fig. 8. Overtaking scenario with oncoming traffic: Multi-agent trajectories for all maneuvers in Mand resulting maneuver probability distribution PM.
The colors of the trajectories (lines) and the hard constraints (areas) correspond to the vehicles’ colors. The hard constraints constrain the trajectories due
to the road boundaries and other vehicles: the trajectories are not allowed to be inside the areas of same color.
To close the loop of the prediction and planning framework
(see Fig. 2), a simple driving strategy is applied, that decides
for the most likely maneuver according to PM. The ego-
vehicle is then controlled according to the accelerations
u∗,ego of the corresponding optimal trajectory.
VI. EX PER IM E NT S
The maneuver estimation capabilities of our IMM ap-
proach is compared to a trajectory-cost-based classification
and a cost-gradient-based classification, inspired by [25]. In
the latter two approaches, the ego-vehicle decides for the
maneuver Miwith the lowest optimal cost J∗
Mi(k)and low-
est optimal cost-gradient ∆J∗
Mi(k) = J∗
Mi(k)−J∗
Mi(k−1),
respectively, whereas our approach for the one with high-
est probability PMi(k). Within the closed-loop simulation
framework the desired maneuver of the non-ego-vehicles can
be set a priori. All vehicles are controlled with receding
horizon control according to the optimal trajectories of the
given maneuver. Gaussian noise with variances according to
Tab. I is added to the transition model and measurements.
The two scenarios in Fig. 1 are statistically evaluated.
For the overtaking scenario, the maneuvers M1,M2and
M3have been determined, with VCbeing the ego-vehicle,
VAbeing predicted and VBhaving constant velocity. For
the merging scenario, two maneuvers are distinguished: VD
merges before VE(M4) and VDmerges after VE(M5). VE
is the ego-vehicle whereas VDis predicted. All parameters
used for the evaluation are chosen by domain knowledge
and are shown in Tab. I. To reflect different situations, the
initial conditions consisting of desired velocities and initial
positions of all vehicles are altered iteratively. Each run
consists of 14 seconds and is repeated five times with the
same initial conditions for each possible maneuver intention,
generating the same number of sample points per maneuver.
The execution of one time step of the overtaking scenario
including maneuver determination (C++), multi-agent tra-
jectory planning and IMM update (MATLAB) takes approx-
TABLE I
PARA ME TE RS U SE D FO R EVALUATI ON
T14 sl5mσ2
s1m2
∆T1sw1.75 mσ2
d0.1m2s−2
as[−9,5] m s−2vdes
s10 m s−1σ2
vs0.25 m2
ad[−2,2] m s−2α2.5mσ2
vd0.01 m2s−2
vs[0,20] m s−1β30 mσ2
zs5m2
vd[−5,5] m s−1γ2σ2
zd5m2
vdes 30 m s−1µ0.1
imately 0.26 s on an Intel Core i7-4910MQ with 2.9GHz
without code optimization or parallelization.
Fig. 9 depicts the IMM maneuver probabilities depending
on the tracking time steps with the maneuver intention set to
follow (i.e. ground truth is M1). Although maneuver follow
and overtake after (M3) have similar optimal trajectories,
they can still be distinguished after about three time steps.
The confusion matrices given in Tab. II show a quantitative
comparison of the actual maneuver and the estimates of the
different estimators for time steps 1−14 for the cost-based
estimator and 2−14 for the other estimators. The results
show for both overtaking (left part) and merging (right part)
that only relying on the costs often yields wrong maneuver
estimates as the simulated vehicles randomly decide for a
maneuver which is not necessarily the one with lowest costs.
However, the costs of the intended maneuver tend to get
smaller the longer the predicted vehicles follow their inten-
tions, slightly improving the accuracy compared to a random
classifier. One advantage of the cost-based estimation is the
instant assessment without having to wait for the subsequent
time step. The cost-gradient estimator achieves better and
faster results, as it uses two consecutive measurements and
therefore is able to analyze the actual behavior instead of
only a static situation. However, the cost-gradient completely
neglects the absolute costs, sporadically resulting in even
worse accuracies. The IMM estimator, using all available
past measurements, outperforms the cost-based as well as
the cost-gradient-based classification.
0 2 4 6 8 10 12 14
0
0.2
0.4
0.6
0.8
1
time steps
PM
M1
M2
M3
Fig. 9. IMM maneuver probabilities for the overtaking scenario with
maneuver intention follow, i.e. ground truth M1. The mean values of all
runs with different initializations are drawn as lines with white boarders.
TABLE II
CONFUSION MATRI CE S FO R EST IM ATORS B AS ED O N J∗,∆J∗AND P
Actual based on J∗Acc Actual based on J∗Acc
M1M2M3M4M5
M1746 811 3393 0.15 M411112 5763 0.66
M2312 3833 805 0.77 M55660 11215 0.66
M3693 836 3421 0.69 0.66
0.54
Actual based on ∆J∗Acc Actual based on ∆J∗Acc
M1M2M3M4M5
M1505 517 3598 0.11 M413690 2060 0.87
M219 4458 143 0.96 M52352 13398 0.85
M3546 526 3548 0.77 0.86
0.61
Actual based on PAcc Actual based on PAcc
M1M2M3M4M5
M13476 60 1084 0.75 M415060 690 0.96
M220 4539 61 0.98 M5530 15220 0.97
M3825 84 3711 0.80 0.96
0.85
VII. CONCLUSION
In this paper, we presented a Bayesian collective maneuver
estimation framework, based on trajectory homotopy and
multi-agent planning. The maneuver representation is capa-
ble of describing the relative motion of multiple vehicles
within a scene and can handle adjacent as well as intersecting
or merging lanes. Our interacting multiple model approach
outperforms the cost-based as well as the cost-gradient-based
classification method in the tested scenarios. The calculated
optimal trajectory of the most likely maneuver can directly
be used as control input resulting in a closed-loop framework
with a cooperatively driving autonomous vehicle.
Future work includes the evaluation on real world data as
well as the adaptation of the trajectory planning parameters
to better reflect real human behavior. It should be tested,
how many sample trajectories per maneuver are sufficient to
reflect the variance within different drivers. Different vehicle
types and driving styles could also be modeled explicitly or
learned from data.
REFERENCES
[1] M. Tsogas, X. Dai, G. Thomaidis, P. Lytrivis, and A. Amditis,
“Detection of maneuvers using evidence theory,” in Intell. Veh. Symp.
(IV), pp. 126–131, IEEE, 2008.
[2] T. Gindele, S. Brechtel, and R. Dillmann, “A probabilistic model
for estimating driver behaviors and vehicle trajectories in traffic
environments,” in Int. Conf. Intell. Transp. Syst. (ITSC), pp. 1625–
1631, IEEE, 2010.
[3] M. Liebner, M. Baumann, F. Klanner, and C. Stiller, “Driver intent
inference at urban intersections using the intelligent driver model,” in
Intell. Veh. Symp. (IV), pp. 1162–1167, IEEE, 2012.
[4] S. Lef`
evre, C. Laugier, and J. Iba˜
nez-Guzm´
an, “Risk assessment at
road intersections: Comparing intention and expectation,” in Intell.
Veh. Symp. (IV), pp. 165–171, IEEE, 2012.
[5] S. Klingelschmitt, M. Platho, H.-M. Gross, V. Willert, and J. Eggert,
“Combining behavior and situation information for reliably estimating
multiple intentions,” in Intell. Veh. Symp. (IV), pp. 388–393, IEEE,
2014.
[6] F. Kuhnt, J. Schulz, T. Schamm, and J. M. Z¨
ollner, “Understanding
interactions between traffic participants based on learned behaviors,”
in Intell. Veh. Symp. (IV), pp. 1271–1278, IEEE, 2016.
[7] D. Petrich, T. Dang, G. Breuel, and C. Stiller, “Assessing map-
based maneuver hypotheses using probabilistic methods and evidence
theory,” in Int. Conf. Intell. Transp. Syst. (ITSC), pp. 995–1002, IEEE,
2014.
[8] Q. Tran and J. Firl, “A probabilistic discriminative approach for
situation recognition in traffic scenarios,” in Intell. Veh. Symp. (IV),
pp. 147–152, IEEE, 2012.
[9] D. J. Demyen and M. Buro, “Efficient triangulation-based pathfind-
ing,” in Assoc. for the Advancement of Artificial Intell. (AAAI), vol. 6,
pp. 942–947, 2006.
[10] S. Bhattacharya, V. Kumar, and M. Likhachev, “Search-based path
planning with homotopy class constraints,” in 3rd Annu. Symp. Com-
binatorial Search, 2010.
[11] B. Banerjee and B. Chandrasekaran, “A framework of voronoi dia-
gram for planning multiple paths in free space,” J. Experimental &
Theoretical Artificial Intell., vol. 25, no. 4, pp. 457–475, 2013.
[12] S. Bhattacharya, M. Likhachev, and V. Kumar, “Identification and
representation of homotopy classes of trajectories for search-based
path planning in 3d,” 2011.
[13] S. Bhattacharya, M. Likhachev, and V. Kumar, “Topological con-
straints in search-based robot path planning,” Auton. Robots, vol. 33,
no. 3, pp. 273–290, 2012.
[14] M. Kuderer, C. Sprunk, H. Kretzschmar, and W. Burgard, “Online
generation of homotopically distinct navigation paths,” in Int. Conf.
Robot. Autom. (ICRA), pp. 6462–6467, IEEE, 2014.
[15] T. Gu, J. M. Dolan, and J.-W. Lee, “Automated tactical maneuver
discovery, reasoning and trajectory planning for autonomous driving,”
in Int. Conf. Intell. Robot. and Syst. (IROS), pp. 5474–5480, IEEE,
2016.
[16] P. Bender, O. S. Tas, J. Ziegler, and C. Stiller, “The combinatorial
aspect of motion planning: Maneuver variants in structured environ-
ments,” in Intell. Veh. Symp. (IV), pp. 1386–1392, IEEE, 2015.
[17] P. Kumar, M. Perrollaz, S. Lefevre, and C. Laugier, “Learning-based
approach for online lane change intention prediction,” in Intell. Veh.
Symp. (IV), pp. 797–802, IEEE, 2013.
[18] M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory
generation for dynamic street scenarios in a frenet frame,” in Int. Conf.
Robot. Autom. (ICRA), pp. 987–993, IEEE, 2010.
[19] X. Qian, F. Altch´
e, P. Bender, C. Stiller, and A. De La Fortelle, “Op-
timal trajectory planning for autonomous driving integrating logical
constraints: A MIQP perspective,” in Int. Conf. Intell. Transp. Syst.
(ITSC), pp. 205–210, IEEE, 2016.
[20] J. Nilsson, M. Br¨
annstr¨
om, J. Fredriksson, and E. Coelingh, “Longi-
tudinal and lateral control for automated yielding maneuvers,” IEEE
Trans. Intell. Transp. Syst., vol. 17, no. 5, pp. 1404–1414, 2016.
[21] J. C. Smith and Z. C. Taskin, “A tutorial guide to mixed-integer pro-
gramming models and solution techniques,” Optimization in Medicine
and Biology, pp. 521–548, 2008.
[22] J. Nocedal and S. Wright, Numerical optimization. Springer Science
& Business Media, 2006.
[23] B. Gutjahr, C. Pek, L. Gr¨
oll, and M. Werling, “Efficient trajectory
optimization for vehicles using quadratic programming,” Automa-
tisierungstechnik, 2016.
[24] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and data fusion.
YBS publishing, 2011.
[25] A. von Eichhorn, M. Werling, P. Zahn, and D. Schramm, “Maneuver
prediction at intersections using cost-to-go gradients,” in Int. Conf.
Intell. Transp. Syst. (ITSC), pp. 112–117, IEEE, 2013.
