Trajectory Planning for Automated Merging on Highways
Johannes Potzy
1
, Nadja Goerigk
2
, Thomas Heil
2
, Dennis Fassbender
3
and Karl-Heinz Siedersberger
3
1
Lehrstuhl f
¨
ur Ergonomie, Technische Universit
¨
at M
¨
unchen, Munich, Germany
2
Elektronische Fahrwerksysteme GmbH, Gaimersheim, Germany
3
AUDI AG, Ingolstadt, Germany
dennis.fassbender@audi.de, karl-heinz.siedersberger@audi.de
Keywords:
Automated Driving, Maneuver Decision, Lane Change, Merging Maneuver, Trajectory Planing.
Abstract:
This article introduces a new approach for trajectory planning for merging on highways. The aim of the al-
gorithm, is to find a comfortable driving strategy to merge in a gap on the target lane. Therfore, the proposed
algorithm determines a bunch of trajectories to reach surrounding gaps. The trajectory with the lowest costs
for each gap is chosen. To obtain the longitudinal component of the trajectory, a ve-part section-wise defined
polynomial in Frenet space is used to generate comfortable driving behaviour, with as few changes in the accel-
eration profile as possible. Based on the prediction of surrounding traffic, different variations of deceleration
and acceleration are combined. For each longitudinal part, a lateral component to perform a lane change into
the target gap is evaluated. The concept allows to evaluate the influence of the longitudinal driving strategy
on the dynamics required to change lanes. The algorithm is evaluated in a MATLAB simulation including a
runtime estimation.
1 INTRODUCTION
To integrate automated vehicles into disordered man-
ual traffic, they have to solve complex driving tasks.
For example, when driving on highways, automated
vehicles have to change lanes in high density traffic.
The wrong assessment of distances in the longitudinal
direction or wrong expectations of other traffic par-
ticipants’ behaviour are the main reasons for traffic
accidents (Gr
¨
undl, 2005). Therefore, the automated
vehicle needs a distinct interpretable strategy to in-
teract with manual traffic, respecting safety distances
as well as driving comfort. One popular method of
motion planning in structured environments is to gen-
erate a set of alternative trajectories to different target
positions and select the best trajectory based on a cost
function. For instance, Werling et al. (Werling et al.,
2010) use quintic polynomials for the lateral and the
longitudinal components. Based on a set of trajecto-
ries they formulate an optimization problem in order
to obtain a trajectory that satisfies the comfort crite-
ria of the car occupants, is collision free and mini-
mizes the deviation from to the reference path. Mc-
Naughton et al. (McNaughton et al., 2011) introduce
a lattice structure to obtain various lateral and longi-
tudinal offsets to the lane’s centre. They connect lat-
eral poses with different longitudinal positions using
cubic curvature polynomials, determining the optimal
trajectory based on a cost function. Wei et al. (Wei
et al., 2014) also use a lattice structure to generate
feasible candidate strategies. Thereafter, they are tak-
ing static and dynamic obstacles into account, due to
an interaction aware predicting of surrounding traffic.
The optimal strategy is found based on a cost func-
tion. In contrast, Schwesinger et al. (Schwesinger
et al., 2013) use a closed-loop vehicle model to ob-
tain a set of feasible sample trajectories to certain
goals considering environmental constraints. To plan
the lateral trajectory Lee and Litkouhi (Lee and Litk-
ouhi, 2012) propose quintic polynomials. The algo-
rithm adapts the lateral trajectory in order to not ex-
ceed a defined maximum lateral acceleration. Heil
et al. (Heil et al., 2016) use an asymmetrical lat-
eral trajectory to achieve a human-like lane change
behaviour according to Sporrer et al. (Sporrer et al.,
1998), where a human driver uses a higher lateral ac-
celeration for steering out of the initial lane than for
steering back into the target lane. Ulbrich and Mau-
rer (Ulbrich and Maurer, 2015) propose a decision al-
gorithm for tactical lane changes based on Bayesian
networks to decide if it is possible or beneficial to ex-
ecute a lane change. Hansen et al. (Hansen et al.,
2016) divide driving on highways in a lane keep and a
merging maneuver. The lane keep maneuver is based
Potzy, J., Goerigk, N., Heil, T., Fassbender, D. and Siedersberger, K.
Trajectory Planning for Automated Merging on Highways.
DOI: 10.5220/0007585602830290
In Proceedings of the 5th International Conference on Vehicle Technology and Intelligent Transport Systems (VEHITS 2019), pages 283-290
ISBN: 978-989-758-374-2
Copyright
c
2019 by SCITEPRESS Science and Technology Publications, Lda. All rights reserved
283
on a common ACC vehicle controller. If the decision
is made that a lane change is necessary or beneficial
to the automated vehicle, they use two quintic poly-
nomials to synchronize to a gap. They define the op-
timum intersection of the automated vehicles with the
border of the safety area of the first vehicle building
the gap and the entrance in the safety area of the target
vehicle on the current lane by solving an optimisation
problem minimizing the average velocity during both
parts, as well as the velocity of the automated vehi-
cles at the end of the trajectory. Based on those inter-
sections the longitudinal and lateral component of the
trajectory is defined.
The aim of our planning algorithm is to achieve
comfortable driving behaviour with as few jerk min-
imal changes in the longitudinal acceleration profile
as possible respecting the lateral dynamics and safety
distances to interacting traffic. Therefore, we propose
a planning algorithm that generates a set of trajecto-
ries for reasonable gaps and finds the trajectory with
the lowest cost per gap. The longitudinal component
consists of a new planning approach presenting an an-
alytical solution using a piece-wise defined function.
The approach allows to respect the maximum longi-
tudinal and lateral dynamics of the trajectory as well
as to check for possible collision analytically.
The structure of this article is as follows. Section
2 proposes a way to integrate the algorithm into a sys-
tem architecture. In section 3 the planning algorithm
is presented, where the sector to change lanes, the
longitudinal and lateral components of the trajectory,
cost functions and the algorithm are introduced. The
concept is evaluated using a MATLAB simulation in
section 4. A conclusion is given in section 5.
2 SYSTEM ARCHITECTURE
We propose the system architecture illustrated in fig-
ure 1. To enable an automated vehicle (Ego) to merge
on highways in dense traffic, we describe the free
space in the form of gaps between other cars. A pos-
sible scenario is illustrated in figure 2. The first layer
of the system architecture is divided into the parts
perception and navigation. The second layer is the
behaviour decision and the third layer is the vehicle
controller. The perception extracts the environment
information about other road users and the road. The
navigation estimates the route and labels the lanes as
regular lanes on-ramps or off-ramps. The behaviour
decision builds the scene based on perception infor-
mation and the route. The scene comprises all in-
formation relevant to the driving task (Ulbrich et al.,
2015). Depending on the scene all reasonable gaps
PerceptionNavigation
Vehicle Control
Behaviour
Decision
Scene
Extract Reasonable Gaps
Gap Prediction
Generate Trajectories
Calculate Costs
Select Optimal
Trajectory per Gap
g
1
,g
2
,...
Evaluating
Global Costs
z
1
,z
2
,...
Select Optimal Trajectory
min(z
1
g
1
,z
2
g
2
,...)
Figure 1: The figure shows a system architecture determin-
ing the best trajectory based on the surrounding gaps ex-
tracted from a scene representation. The parts of the pro-
posed planning algorithm are the blocks in grey.
s
GF,1
(t)s
GB,1
(t)s
GF,2
(t)
s
GB,2(t)
s
GF,0
(t)
s
max
s
y
s
x
Figure 2: The reference coordinate system is placed in the
middle of the Ego (red) on the left lane. The proposed plan-
ning algorithm determines the dynamics required to change
lanes for example in gap 1, built by s
GF,1
(t) and s
GB,1
(t), or
gap 2, built by s
GF,2
(t) and s
GB,2
(t), on the right lane. The
algorithm also takes the front vehicle s
GF,0
(t) and possible
lane ends s
max
into account. The black dashed area is an
area that shall be avoided by the other vehicles, including
the Ego.
are extracted. For example, due to the desired route,
gaps on the target lane are selected. The module needs
to evaluate if it is feasible to reach the gap based on
the front vehicle of the Ego, possible lane ends, the
approximated gap velocity and predicted gap sizes.
All selected gaps are given to the proposed planner.
The displacement is predicted for every gap using a
constant velocity model for the cars limiting the gap.
Based on this prediction, it is possible to calculate the
time span available to merge into the target gap. In
a second step, a number of acceleration profiles are
VEHITS 2019 - 5th International Conference on Vehicle Technology and Intelligent Transport Systems
284
evaluated to reach this space. For each longitudinal
part a lateral planning to merge into the gap is made,
considering safety margins in Frenet space (Werling
et al., 2010). The optimal trajectory per gap accord-
ing to comfort and safety margins is considered. Par-
allel global costs evaluate criteria like the velocity of
each gap or strategic decision, for example going to
the right lane as early as possible to increase the time
reserve to reach an exit or the desired lane at an inter-
change.
The dynamic costs and the global strategic costs
are evaluated together, the optimal trajectory is trans-
formed from Frenet into global coordinates and is sent
to the vehicle controller. The transformation is given
in (Werling et al., 2010). The vehicle controller exe-
cutes the desired plan.
3 PLANNING ALGORITHM
The proposed planning approach is based on Hansen
et al. (Hansen et al., 2016). Our goal was to de-
velop an algorithm to determine the most beneficial
driving strategy by finding the optimal trajectory in
a set of candidate trajectories, like in (Lee and Litk-
ouhi, 2012) or (Werling et al., 2010), for several gaps
on the target lane. The concept allows to start a lane
change at a point in the future and to combine an ac-
celerating and decelerating longitudinal driving strat-
egy to reach the gap by taking the front vehicle or
a possible lane end into account. To obtain a com-
fortable driving behaviour with as few changes in the
longitudinal acceleration profile as possible, we use
a section-wise defined longitudinal component with
no more than five parts. For every longitudinal com-
ponent, a lateral component is planned. Therefore, it
is possible to model the coupling of longitudinal and
lateral dynamics when changing lanes. The optimal
trajectory is found based on a cost function. Thus, the
longitudinal and lateral jerk, the deviation from the
target position to the end position of the longitudinal
trajectory and the number of parts of the section-wise
defined function is taken into account. Based on the
gap prediction built by surrounding vehicles, the plan-
ning is made.
The prediction model for the extracted reasonable
gaps is introduced in section 3.1. Second, the lon-
gitudinal component in section 3.2 and thereafter the
lateral component in section 3.3 of the trajectory is
explained. Then, the cost function to determine the
optimal trajectory per gap is introduced in section 3.4.
The algorithm is given in section 3.5.
s
max
s
GF,1
s
GB,1
s
GF,0
s
GF,2
s
GB,2
0
t
Figure 3: The figure illustrates as grey dashed area the dis-
placement of the Ego gap and as blue dashed area, where a
lane change to gap two is possible.
3.1 Gap Prediction
The prediction estimates the space where following
the Ego lane or changing into a gap on the target lane
is possible. An interaction-aware prediction and un-
certainties in perception are neglected. The prediction
is not able to anticipate the reactions of other traf-
fic participants to the action of the Ego and therefore
does not depict interactions between road users. Since
the planning is constantly repeated, only the first time
step is executed until a new plan is available, the plan-
ning reacts to changing behaviour of surrounding traf-
fic participants. Surrounding vehicles are predicted
with constant velocity as given in equation (1), for
each gap n:
s
GF,n
(t) = s
GF,n
(t = 0) + v
GF,n
(t = 0) ·t,
s
GB,n
(t) = s
GB,n
(t = 0) + v
GB,n
(t = 0) ·t,
(1)
where s
GF,0
(t = 0) and s
GB,n
(t = 0) are the starting
position and v
GF,n
(t = 0) and v
GB,n
(t = 0) are the
starting velocity of the gap’s front and back vehicle.
Moreover, the longitudinal distance to traffic partici-
pant should not fall below a minimum. In this case
the maximum velocity of vehicle is the velocity of
its vehicle in front. Figure 3 illustrates the percep-
tion model. The grey dashed area illustrates the dis-
placement of the Ego’s front vehicle s
GF,0
(t). The
dashed blue area shows the predicted areas, where
a lane change to gap n = 2, built by the gap’s front
s
GF,2
(t) and back vehicle s
GB,2
(t) is possible. In addi-
tion, the concept allows to restrict the maximum dis-
placement s
max
allowed for the maneuver due to a lane
end. The prediction can be extended by interaction
aware prediction (Kesting et al., 2010), (Treiber and
Kesting, 2009) or by considering uncertainties in pre-
diction and perception (Suh and Yi, 2017).
Trajectory Planning for Automated Merging on Highways
285
a
t1
a
t2
0
a
1
t
1
a
2
τ
2
a
3
τ
3
a
4
τ
4
a
5
τ
5
t
2
t
a
x
(t)
Figure 4: The parts one, three and five use cubic acceler-
ation equations. The parts two and four use constant ac-
celeration equations. After part five zero acceleration is as-
sumed. The target accelerations a
t,1
and a
t,2
as well as the
time span t
2
are varied in the algorithm.
3.2 Longitudinal Component
The longitudinal component of the trajectory evalu-
ates different variations of acceleration and decelera-
tion to reach the target gap. To ensure collision free
driving, all acceleration profiles that do not reach the
blue or are outside the grey dashed area are discarded,
as illustrated in figure 3. To generate a comfortable
driving behaviour with as few changes in the acceler-
ation profile as possible, a five-part section-wise de-
fined function is used, as given in equation (2):
S
x
(t) =
S
x,1
(t), t t
1
S
x,2
(t t
1
), t
1
< t t
2
= t
1
+ τ
2
S
x,3
(t t
2
), t
2
< t t
3
= t
2
+ τ
3
S
x,4
(t t
3
), t
3
< t t
4
= t
3
+ τ
4
S
x,5
(t t
4
), t
4
< t t
5
= t
4
+ τ
5
(2)
The time span for each polynomial is described by τ
i
.
The section-wise defined acceleration profile is illus-
trated in figure 4. The algorithm varies the first a
t1
and
second a
t2
target acceleration, as well as the time t
2
.
The time t
1
is maximum τ
1
. To determine the polyno-
mials of all five parts, the matrix A(t) is used, as given
in equation (3):
A(t) =
1 t t
2
t
3
t
4
t
5
0 1 2t 3t
2
4t
3
5t
4
0 0 2 6t 12t
2
20t
3
0 0 0 6 24t 60t
2
0 0 0 0 24 120t
. (3)
Cubic acceleration leads to quintic position polyno-
mials. Those can be described by the following equa-
tion (4), where i stands for the part of the section-wise
defined function:
c
x,i
=
c
0,i
,c
1,i
,c
2,i
,c
3,i
,c
4,i
,c
5,i
T
,
S
x,i
(t) =
s
x,i
(t), v
x,i
(t), a
x,i
(t), j
x,i
(t),
˙
j
x,i
(t)
T
= A(t)c
x,i
.
(4)
At first the time for each part of the section-wise de-
fined function τ
i
and the constants c
2,i
, c
3,i
, c
4,i
and
j
ext,k
j
ext,i
0
c
41,i
τ
+,i
c
42,i
τ
-,i
c
41,k
τ
-,k
c
42,k
τ
+,k
t
j
x
(t)
Figure 5: In case of a jerk maximum j
ext,i
, the time span τ
+,i
before the extreme and the time span τ
-,i
after the extreme
is used. In case of a jerk minimum j
ext,k
it is vice versa. For
both cases we use the constants c
41,i
before and c
42,i
after
the extreme point.
c
5,i
are determined. Therefore, it is differentiated be-
tween cubic and constant acceleration equations. The
first, third and fifth part of the five-part section-wise
defined solution are cubic acceleration parts. The sec-
ond and fourth part use constant acceleration equa-
tions. Thus, the time span to reach a certain target
velocity is determined for the fourth part. Thereafter,
the constants c
0,i
, c
1,i
for each part are determined by
solving the trivial solutions by using the starting posi-
tion s
s,i
and velocity v
s,i
of each part.
3.2.1 Cubic Acceleration
Solving the trivial solutions leads to c
2,i
and c
3,i
by
using the starting acceleration a
s,i
and jerk j
s,i
of
each part. The time to reach the target accelera-
tion τ
+/-,i
and the constants c
4,i
and c
5,i
are extin-
guished by solving the following boundary condi-
tions: a
x,i
(τ
i
) = a
t,i
, j
x,i
(τ
i
) = 0,
˙
j
x,i
(τ
jext,i
) = 0 and
j
x,i
(τ
jext,i
) = j
ext,i
. At the time τ
jext,i
the jerk extreme
point j
ext,i
is reached. The time to reach the target
acceleration a
t,i
is given in equation (5):
τ
+/-,i
=
3(a
t,i
a
s,i
)
j
ext,i
+ j
s,i
±
q
j
2
ext,i
j
ext,i
j
s,i
. (5)
The time τ
i
differentiates between a positive case τ
+,i
and a negative one τ
-,i
as illustrated in figure 5. In case
of a positive jerk extreme the positive case is used,
after the extreme point we use the negative one. In
case of a negative extreme point it is vice versa.
For c
4,i
between two constants is distinguished. The
constants are given in equation (6):
c
41,i
=
s
j
ext,i
j
s,i
j
ext,i
+ 1
!
j
s,i
j
ext,i
12τ
i
c
42,i
=
s
j
ext,i
j
s,i
j
ext,i
1
!
j
s,i
j
ext,i
12τ
i
.
(6)
VEHITS 2019 - 5th International Conference on Vehicle Technology and Intelligent Transport Systems
286
The constant c
41,i
is used before and the constant c
42,i
after the jerk extreme. The solution for c
5,i
is given in
the following equation (7):
c
5,i
=
12c
2
4,i
5( j
s,i
j
ext,i
)
.
(7)
If j
s,i
is equal to j
ext,i
, the constant c
4,i
is equal to zero
and the constant c
5,i
= j
s,i
/(60τ
i
).
We determine if the jerk profile is before or after its
extreme point, based on the last plan S
x,t-1
, the cur-
rent acceleration a
e
and the current jerk j
e
. Based on
the last plan S
x,t-1
it is solved for the times where the
jerk component is equal to the current jerk j
e
. At the
smaller time t
1
and greater time t
2
the accelerations
a
e,t-1,1
and a
e,t-1,2
are evaluated based on the last plan,
respectively. Those accelerations are compared with
the current acceleration a
e
.
If the current acceleration a
e
is equal to the acceler-
ation a
e,t-1,1
, the current jerk is before the extreme
point. If the current acceleration a
e
is equal to the
acceleration a
e,t-1,2
, the current jerk is after the ex-
treme point. If the current acceleration a
e
is not equal
to both accelerations a
e,t-1,1
and a
e,t-1,2
the Ego stayed
not on the last plan. In such cases the jerk is always
before the extreme point.
3.2.2 Constant Acceleration
Solving the trivial solutions leads to c
2,i
and c
3,i
by
using the starting acceleration a
s,i
and jerk j
s,i
. While
in part two the algorithm varies the time length to hold
the target acceleration a
t,1
in part four also the time
span to reach the target velocity v
GF,n
with a certain
target acceleration a
t,2
is determined. The solution is
given in equation (8):
τ
4
=
v
GF,n
(v
3
+ v
5
)
a
t,2
. (8)
The velocity v
3
is the Ego velocity at the end of the
third part of the section-wise defined function and the
velocity v
5
is caused by the acceleration of the fifth
part of the section-wise defined function.
3.3 Lateral Component
It is possible to start the lane change if the Ego has
reached the height of the target gap. Therefore, the in-
tersection point of the back of the Ego with the safety
area of the back vehicle t
y,s1
of the target gap is deter-
mined. Moreover, the intersection point of the front of
the Ego with the safety area of the front vehicle t
y,s2
of the target gap is evaluated. The earliest intersection
point is considered as starting time given in equation
(9):
t
y,s
= min(t
y,s1
,t
y,s2
) (9)
Moreover, the lateral planning needs to be executed
if the distance to vehicle in front t
y,e1
of the Ego or
to a possible lane end t
y,e2
falls below the safety dis-
tance or the lateral planning time does exceed a max-
imum time t
y,e3
to change lane, defined by Lange et
al. (Lange et al., 2013) to reach a comfortable driving
behaviour, given in equation (10):
t
y,e
= min(t
y,e1
,t
y,e2
,t
y,e3
) (10)
To determine the intersection, it is evaluated if each
part of the section-wise defined longitudinal compo-
nent, considering the length of the Ego, is in front or
in back of the safety area of the considered vehicle.
If the intersection is in the constant acceleration part
of the component, it is possible to determine the time
analytically. Otherwise, to avoid a numerical solution,
the end time of a part is considered for the times t
y,s1
and t
y,s2
. Moreover, the start time of a part is consid-
ered for the times t
y,e1
and t
y,e2
. The time to change
lane t
TTLC
is given in equation (11):
t
TTLC
= t
y,e
t
y,s
. (11)
To calculate the lateral component quintic polynomi-
als, introduced in (Werling et al., 2010), are used.
3.4 Cost Function
The cost function evaluates the driving comfort
caused by maximum longitudinal and lateral jerk, the
parts of the section-wise defined longitudinal compo-
nent and the distance to the target point at the end of
the fifth part of the longitudinal component. The tra-
jectory with the lowest costs per gap is chosen.
The cost of the longitudinal jerk j
x
(t) is given in equa-
tion (12), where a
tmax,1
and a
tmax,2
are the maximum
acceleration of the first a
t,1
and second a
t,2
target ac-
celeration:
g
1
=
Z
t
5
0
| j
x
(t)|dt
|a
tmax,1
| + |a
tmax,2
|
2
.
(12)
The cost of the lateral jerk j
y
(t) is given in equation
(13), where a
ymax
is the maximum allowed lateral ac-
celeration:
g
2
=
Z
t
5
0
| j
y
(t)|dt
a
ymax
2
.
(13)
To calculate the jerk j
y
(t) to evaluate the cost func-
tion the time to change lane is not constraint by t
y,e3
,
to reward solutions that already changed lane with a
greater time distances to the lane end or possible ve-
hicles in front of the Ego.
Trajectory Planning for Automated Merging on Highways
287
In the first planing step of the longitudinal component
at most five section-wise defined parts are possible to
reach the target velocity. With cyclical replanning,
in every time step at most five section-wise defined
parts can be added to the initial plan and therefore the
possible number of parts of the section-wise defined
longitudinal component with respect to the initial plan
increases. Moreover, to accelerate or decelerate to a
target acceleration in several steps implies the same
total jerk as accelerating at once to the target acceler-
ation. To keep the optimal trajectory at the initial plan
and to gain as few changes in the acceleration profile
as possible, the number of parts p of the section-wise
defined function, are evaluated as well, as given in
equation (14):
g
3
=
p
5
2
.
(14)
Furthermore, the distance at the end of the longitu-
dinal part of the trajectory s
x
(t
5
) to the position to
the gap’s front vehicle or a possible lane end s
t
=
max(s
GF
(t
5
),s
max
) is taken into account. The cost due
to the target point are given in equation (15), where t
s
is the safety time for car following:
g
4
=
1
s
t
s
x
(t
5
)
v
GF,n
·t
s
2
.
(15)
The overall cost per trajectory is the weighted sum of
all introduced partial costs.
3.5 Algorithm
The algorithm samples the first and the second tar-
get acceleration a
t1
and a
t2
, as well as the time span
t
2
to generate the set of longitudinal components.
Moreover the algorithm allows combinations of non-
negative values for a
t1
and non-positive values for a
t2
as target acceleration for target gaps with a smaller
target velocity than the Ego. If the target gap has a
higher velocity it is vice-versa.
At first the algorithm samples a
t1
. Thereafter, we de-
termine if the Ego acceleration a
e
is equal to the target
acceleration a
t1
and if the Ego jerk j
e
is equal to zero.
In this case also τ
1
is equal to zero and the starting
states of the second part are equal to the Ego states.
Otherwise we determine τ
1
and c
x1
using the cubic
acceleration equations. Second, the time span t
2
is
sampled by the algorithm. Since we want to obtain
one to five part-section wise defined trajectories, the
time span t
2
starts from zero. The resulting times t
1
and τ
2
are calculated according to equation (16):
t
1
=
0, t
2
= 0
τ
1
, t
2
> 0
τ
2
= max(0,t
2
t
1
).
(16)
The second part is a constant acceleration part. There-
fore, c
x2
is determined by constant acceleration equa-
tions and τ
2
by sampling the time t
2
. Third, the second
target acceleration a
t2
is sampled. The second target
acceleration is used to accelerate the Ego to the target
velocity of each gap. Here, the target vehicle is the
front vehicle of each gap. If t
2
is equal to zero we try
if the acceleration of the fifth part leads directly to the
target velocity considering the end states of the sec-
ond part as the starting states. If this does not occur
the third part is similar determined as the first part.
Next, the acceleration profile for the fifth part is ex-
tinguished. Therefore, the time span τ
5
and the con-
stants c
25
, c
35
, c
45
and c
55
are determined using cu-
bic acceleration equations. Thereafter, it is possible
to determine the fourth part τ
4
and c
x4
using constant
acceleration equations. By then, it is possible to de-
termine the missing constants c
05
and c
15
of the fifth
part.
All solutions that do not reach the blue-dashed area or
are outside the grey-dashed area are neglected. There-
after, for every longitudinal planning a lateral planing
to change lane is made. All lateral plans where the
acceleration exceeds a defined maximum are also ne-
glected. Then, the costs for every trajectory are cal-
culated. The algorithm returns the trajectory with the
lowest costs per gap.
4 EVALUATION
The proposed planning algorithm is evaluated in a
MATLAB simulation. Therefore, surrounding traf-
fic is simulated with constant velocity and the Ego
is assumed to follow the planned trajectory. Figure 6
shows the generated set of trajectories for the initial
planning, for a lane change into a gap with a smaller
velocity. Due to the vehicle in front of the Ego it is
not possible to reach gap 1. Therefore, the algorithm
only determines trajectories reaching gap 2. The thick
black trajectory is the optimal one. To solve the sit-
uation, the car first accelerates to reach the gap and
to gain enough time to change lane with small lateral
dynamics. Second the car decelerates to target speed.
Since the number of parts of the section-wise defined
longitudinal component are considered, the Ego tra-
jectory has as few changes in the acceleration profile
as possible. Without the vehicle in front of the Ego
or a lane end in a greater distance the Ego would only
decelerate. To evaluate the cost function it is evalu-
ated if the Ego stays on the initial plan over the whole
maneuver. Therefore, the Ego is located cyclically on
the next time step t
step
= 0.2 s of the plan with the low-
est costs, as illustrated in figure 7. At every time step
VEHITS 2019 - 5th International Conference on Vehicle Technology and Intelligent Transport Systems
288
0
5
10
15
0
100
200
300
t
s
x
0
5
10
15
25
30
35
40
t
v
x
0
5
10
15
4
2
0
2
t
a
x
0
5
10
15
0
1
2
3
t
s
y
0
5
10
15
0
1
2
3
t
v
y
0
5
10
15
2
0
2
t
a
y
Figure 6: Longitudinal and lateral component of the set of trajectories in Frenet coordinates: one planning step is executed
with a
t1min
= 2 m/s
2
, a
t1max
= 2 m/s
2
, a
t2min
= 3 m/s
2
and a
t2max
= 3 m/s
2
. The discretization of the target acceleration
is a
t1step
= 0.2 m/s
2
and a
t2step
= 0.1 m/s
2
. The maximum time is t
2max
= 10 s and is sampled with t
2step
= 0.2 s.
0 2 4
6
8 10
2
0
2
t [s]
[m/s
2
]
a
x,opt
a
y,opt
j
x,opt
4
2
0
2
4
[m/s
3
]
Figure 7: The figure illustrates the devolution of the Ego
states a
x,opt
, a
y,opt
and j
x,opt
by cyclic planning of the pro-
posed algorithm. The dots visualize the Ego states at the
different planning steps. The solid lines represent the initial
planning.
the proposed algorithm is executed. The Ego stays on
the initial plan over the whole maneuver.
In addition, to evaluate the computational costs,
we evaluated different scenarios. Since, the calcula-
tion time depends not only on the discretization of the
target velocities a
t1
, a
t2
and the time to hold the first
target acceleration t
2
, but as well on the grey and blue
dashed area. The simulation was running on an HP
Elite Book with an Intel Core i-7 v Pro CPU at 2.60
GHz with 16.0 GB RAM. The time measurement is
made over one scenario until the lane change was per-
formed and the Ego is at target speed. In all evaluated
Table 1: The sampling is executed similar as illustrated in
figure 6. The planning is performed due to one gap only.
s
max
[m] t
G
[s] max [s] mean [s] std [s]
400 1.8 0.2215 0.0805 0.0306
400 1.2 0.2143 0.0718 0.0364
400 0.6 0,1944 0.0626 0.0412
600 1.8 0.6996 0.2026 0.1783
600 1.2 0.3787 0.1071 0.0937
600 0.6 0.2463 0.0684 0.0566
scenarios, the initial distance is s
GF
= 100m and the
initial velocity of the front vehicle of the target gap
is v
GF
= 25 m s
1
. The front and back vehicle of the
target gap drives with constant speed. The initial Ego
speed is v
e
= 33.3ms
1
. The size of the target gap,
as well as the possible displacement on the starting
lane influences the calculation time. Thus, the time
distance from the front and the back vehicle of the
target gap t
G
and the distance to the lane end s
max
is
varied. A vehicle in front of the Ego is not taken into
account. The mean, the maximum (max) and stan-
dard deviation (std) of the computation time is listed
in table 1.
5 CONCLUSION
This article introduces a new approach for trajectory
planning of automated vehicles for merging maneu-
vers on highways. To determine the dynamics neces-
sary to reach several gaps a possible system architec-
Trajectory Planning for Automated Merging on Highways
289
ture is proposed. To evaluate those dynamics, a set
of trajectories is evaluated based on a cost function in
each time step. The longitudinal component is com-
posed of a five-part section-wise defined function, to
accelerate to the target velocity of each gap. All so-
lutions outside the drivable area are discarded. The
lateral planning for potentially changing lane is exe-
cuted, dependent on the situation, at a point in the fu-
ture, considering safety distances. The cost function
evaluates the longitudinal and lateral jerk, the num-
ber of parts and the distance to the target point at the
end of the longitudinal component of the trajectory.
Evaluations in MATLAB simulations show that the
Ego stays on the same plan over the whole maneu-
ver, executing the same planning algorithm in every
time step, assuming constant behaviour of surround-
ing traffic. Moreover, a first evaluation of the compu-
tational costs of the algorithm is made.
To reach a higher discretization of one planning
step, the proposed algorithm can easily be paral-
lelised. Also, the gap prediction model can be ex-
tended to consider interactions between road users.
Moreover, for use in real traffic, the model could be
extended to deal with uncertainties in prediction or
perception, to reward driving in less uncertain spaces.
REFERENCES
Gr
¨
undl, M. (2005). Fehler und Fehlverhalten als Ur-
sache von Verkehrsunf
¨
allen und Konsequenzen f
¨
ur
das Unfallvermeidungspotenzial und die Gestaltung
von Fahrerassistenzsystemen. Universit
¨
at Regens-
burg.
Hansen, T., Schulz, M., Knoop, M., and Konigorski,
U. (2016). Trajectory planning for automated lane
changes. In ATZ worldwide, 118(7):60–65.
Heil, T., Lange, A., and Cramer, S. (2016). Adaptive and
efficient lane change path planning for automated ve-
hicles. In 2016 IEEE 19th International Conference
on Intelligent Transportation Systems (ITSC), pages
479–484.
Kesting, A., Treiber, M., and Helbing, D. (2010). En-
hanced intelligent driver model to access the impact
of driving strategies on traffic capacity. In Philo-
sophical Transactions of the Royal Society of London
A: Mathematical, Physical and Engineering Sciences,
368(1928):4585–4605.
Lange, A., Maas, M., Albert, M. Siedersberger, K. H., Ben-
gler, K. (2013). Automatisches Fahren - So komfort-
abel wie m
¨
oglich, so dynamisch wie n
¨
otig. In VDI
Wissensforum.
Lee, J. W. and Litkouhi, B. (2012). A unified framework
of the automated lane centering/changing control for
motion smoothness adaptation. In 2012 15th Interna-
tional IEEE Conference on Intelligent Transportation
Systems, pages 282–287.
McNaughton, M., Urmson, C., Dolan, J. M., and Lee, J. W.
(2011). Motion planning for autonomous driving with
a conformal spatiotemporal lattice. In 2011 IEEE In-
ternational Conference on Robotics and Automation,
pages 4889–4895.
Schwesinger, U., Rufli, M., Furgale, P., and Siegwart, R.
(2013). A sampling-based partial motion planning
framework for system-compliant navigation along a
reference path. In 2013 IEEE Intelligent Vehicles Sym-
posium (IV), pages 391–396.
Sporrer, A., Prell, G., Buck, J., and Schaible, S. (1998). Re-
alsimulation von Spurwechselvorg
¨
angen im Straßen-
verkehr. In Verkehrsunfall und Fahrzeugtechnik,
(36):69–76.
Suh, J. and Yi, K. (2017). A new adaptive uncertainty
propagation method based stochastic model predictive
control for automated driving vehicles. In 2017 Amer-
ican Control Conference (ACC), pages 5660–5665.
Treiber, M. and Kesting, A. (2009). Modeling lane-
changing decisions with mobil. In Traffic and Gran-
ular Flow ’07, pages 211–221, Berlin, Heidelberg.
Springer Berlin Heidelberg.
Ulbrich, S. and Maurer, M. Towards tactical lane change be-
havior planning for automated vehicles. In 2015 IEEE
18th International Conference on Intelligent Trans-
portation Systems, pages 989–995.
Ulbrich, S., Menzel, T., Reschka, A., Schuldt, F., and Mau-
rer, M. (2015). Defining and substantiating the terms
scene, situation, and scenario for automated driving.
In 2015 IEEE 18th International Conference on Intel-
ligent Transportation Systems, pages 982–988.
Wei, J., Snider, J. M., Gu, T., Dolan, J. M., and Litkouhi,
B. (2014). A behavioral planning framework for au-
tonomous driving. In 2014 IEEE Intelligent Vehicles
Symposium Proceedings, pages 458–464.
Werling, M., Ziegler, J., Kammel, S., and Thrun, S. (2010).
Optimal trajectory generation for dynamic street sce-
narios in a frenet frame. In 2010 IEEE International
Conference on Robotics and Automation, pages 987–
993.
VEHITS 2019 - 5th International Conference on Vehicle Technology and Intelligent Transport Systems
290