HAL Id: hal-00881100
https://hal.archives-ouvertes.fr/hal-00881100
Submitted on 12 Nov 2013
HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entic research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.
L’archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diusion de documents
scientiques de niveau recherche, publiés ou non,
émanant des établissements d’enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.
Vehicle Trajectory Prediction based on Motion Model
and Maneuver Recognition
Adam Houenou, Philippe Bonnifait, Véronique Cherfaoui, Yao Wen
To cite this version:
Adam Houenou, Philippe Bonnifait, Véronique Cherfaoui, Yao Wen. Vehicle Trajectory Prediction
based on Motion Model and Maneuver Recognition. 2013 IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS 2013), Nov 2013, Tokyo, Japan. pp.4363-4369. �hal-00881100�
Vehicle Trajectory Prediction based on Motion
Model and Maneuver Recognition
Adam Houenou
∗
, Philippe Bonnifait
∗
, Véronique Cherfaoui
∗
, Wen Yao
+
Abstract—Predicting other traffic participants trajectories is
a crucial task for an autonomous vehicle, in order to avoid
collisions on its planned trajectory. It is also necessary for
many Advanced Driver Assistance Systems, where the ego-
vehicle’s trajectory has to be predicted too. Even if trajectory
prediction is not a deterministic task, it is possible to point out
the most likely trajectory. This paper presents a new trajectory
prediction method which combines a trajectory prediction based
on Constant Yaw Rate and Acceleration motion model and
a trajectory prediction based on maneuver recognition. It
takes benefit on the accuracy of both predictions respectively a
short-term and long-term. The defined Maneuver Recognition
Module selects the current maneuver from a predefined set
by comparing the center lines of the road’s lanes to a local
curvilinear model of the path of the vehicle. The overall
approach was tested on prerecorded human real driving data
and results show that the Maneuver Recognition Module has a
high success rate and that the final trajectory prediction has a
better accuracy.
I. INTRODUCTION
Active safety systems and self-driving cars are a promising
solution to reduce the number of traffic accidents ([1], [2]).
Some Advanced Driver Assistance Systems (ADAS) such
as Adaptive Cruise Control, Collision Warning System and
Emergency Braking System, that already exist in series
vehicles, are able to warn the driver and even to intervene
on the state of the vehicle when a hazardous traffic situation
is being developed. A Collision Avoidance System (CAS)
needs to continuously make a prediction of the evolution of
the scene, in order to detect any possible future collision with
the ego-vehicle. This means that it is necessary to predict
the trajectory of detected vehicles in the surroundings of the
ego-vehicle and its own trajectory in the case of ADAS.
Predicting the trajectory of a vehicle is not a deterministic
task since it depends on each driver’s intention and driving
habits. However, certain considerations about vehicle dynam-
ics can provide partial or fuzzy knowledge on the future. For
instance it is known that a vehicle moving at a given speed
will need a certain time to fully stop and that the curvature
of its trajectory has to be under a certain value in order to
keep stability. On the other hand, even if each driver has its
own habits, it is possible to point out some common driving
∗Heudiasyc UMR CNRS 7253, Univ. of Techn. of Compiègne, France
+
State Key Lab of Machine Perception (MOE), Peking Univ., China
This work is partially supported by the NSFC-ANR Grant
(No.61161130528)
adam.houenou@hds.utc.fr
(a) Lane changing (b) Entering a bend
Figure 1: Bad trajectory predictions.
maneuvers based on traffic rules for instance, or on some
common behaviors assuming that every driver keeps some
comfort while driving.
In many cases, trajectory prediction is made by assuming
a certain motion model. A comparison of different motion
models for target tracking was made in [3]. It appears that
the model assuming Constant Yaw Rate and Acceleration
(CYRA) gives the best results. This model was used in
[4] and [5] for vehicle trajectory prediction. So predicted
trajectories are very accurate if the vehicle has a monotonic
movement that perfectly fits the motion model. For a varying
movement, the accuracy is good only for a short time ahead
due to inertia, but the prediction can be pretty wrong for
longer term (see Fig.1). A CAS needs to predict trajectories
for at least a few seconds ahead and according to [6], 50% of
accidents occur at intersections or during specific maneuvers
where the dynamic of the vehicle could rapidly change.
In several works, a Maneuver Recognition Module (MRM)
is used in order to have a better prediction considering
the whole prediction. In [7] and [8], a set of trajectories
corresponding to different maneuvers is predefined. Then,
a Hidden Markov Model is used to select the most likely
trajectory of an object, based on its current measurements
sequence. In [9] Probabilistic Finite-State Machines are used
to model complex driving maneuvers as sequences of basic
elements that are specified by a set of rules in a fuzzy logic
system. The rules are obtained from a training data set
including signals such as velocity, acceleration and steering
angle. A Bayes filter approach is employed to recognize a
driving maneuver by computing the probability of each basic
element in the context of the maneuver model. In [10] the ego
vehicle trajectory is predicted by defining a driving context
which is a vector containing relevant signals from sensors of
the testbed such as light indicator or driver’s gaze direction.
The driving context is continuously recorded over a sliding
time frame of 2 seconds and a trained classifier discriminates
between lane change and lane keeping a few seconds before
Figure 2: System overview
the maneuver starts.
This paper proposes a trajectory prediction method that
combines a trajectory predicted by CYRA motion model
and a trajectory predicted by maneuver recognition. The
combination rule takes benefit on the short-term accuracy
of the first trajectory and the better accuracy of the second
trajectory at longer term. The defined Maneuver Recognition
Module (MRM) uses no training data. It is based on the
modeling and the comparison of the instantaneous path of
the vehicle and the shape of the road. The proposed method
is applicable as well for the target vehicles as for the ego-
vehicle. In Section II, an overview of the trajectory prediction
system is shown. Section III presents the MRM. In section
IV, the proposed method is explained in detail. Some
experimental results and analyzes are given in section V.
Finally, section VI presents our conclusions and future work.
II. SYSTEM OVERVIEW
It is assumed that a target tracking system hosted by the
ego-vehicle provides for each target vehicle the state vector
ζ
(target)
and its covariance matrix in a local Cartesian
coordinate system:
ζ
(target)
= [x , y, θ, v, a, ω]
T
(1)
where x and y are the Cartesian coordinates, θ the yaw angle,
v and a the longitudinal velocity and acceleration and ω the
yaw rate.
For one prediction operation, the working frame is static and
corresponds to the current measurement frame. With the
same parametrization, the ego-vehicle’s state is thus defined
as:
ζ
(eg o )
= [0, 0, 0, v, a, ω]
T
(2)
where v, a and ω are provided by proprioceptive sensors.
It is also assumed that a camera based system detects road-
markings and provides a local parabolic model of their center
lines [11] in the same Cartesian frame :
y (x) = c
2
x
2
+ c
1
x
1
+ c
0
(3)
where c
2
, c
1
and c
0
are coefficients. The width of the lanes is
also measured. The tracker and the road-markings detection
system are called the Perception System in the following.
For each vehicle, including the ego-vehicle, the MRM detects
the current maneuver. Then, a first trajectory prediction is
made, only based on the recognized maneuver. A second
prediction is made by using CYRA motion model. The
final predicted trajectory is obtained by combining those two
predictions with a weighting-function (Fig 2).
III. MANEUVER RECOGNITION MODULE (MRM)
In a normal driving context (e.g. no control loss), the path
of a vehicle depends on the maneuvers decided by the driver.
These can roughly be limited to these canonical cases:
• Keep lane
• Change lane (to the right or the left side lane) on the
same road
• Turn (at an intersection).
A u-turn maneuver is excluded here. An overtaking ma-
neuver can be decomposed as: change lane - keep lane -
change lane. As it can be noticed, the maneuver is strongly
relative to the lanes of the road and so is the path of the
vehicle. The MRM is based on an early detection of the
lane where the driver is intending to go (or stay). The lane
intention detection exploits the current similarity between the
path of the vehicle under consideration and the lanes’ center
lines (the one of its current lane and the ones of the adjacent
lanes).
To evaluate the current similarity between the path of the
vehicle and a given lane’s center line, the path of the vehicle
is locally modeled as an arc of circle [12] defined by the
state vector X
(p)
= [d
l
, d
r
, θ, γ]
T
. d
l
and d
r
are respectively
the current distances of the the vehicle to the left and right
boundaries of the considered lane. Their values are calculated
from the lane’s center line equation, the current position of
the vehicle and the known width of the lane. Appendix-A
shows how to compute the distance between a point and a
line. θ is the current heading angle of the vehicle (known
from state ζ) and γ the current curvature of its path. The
estimated value of γ is obtained from Eq. 4, where v and ω
are the velocity and the yaw rate of the vehicle.
γ =
ω
v
(4)
The variances of d
l
, d
r
and θ are retrieved from the covari-
ance matrix of ζ. According to [13], if v and ω are Gaussian
distributed then the variance of γ can be approximated by Eq.
5, where σ denotes the standard deviation of the subscript
parameter.
σ
2
γ
=
ω.v
v
2
− σ
2
v
2
−
ω
2
− σ
2
ω
v
2
− σ
2
v
(5)
The center line of the considered lane is also represented
with the same state parameters: X
(l)
= [d
l
, d
r
, θ, γ]
T
, with
d
l
and d
r
both equaling half the lane’s width. θ and γ are
the lane center line’s heading angle and its curvature in its
closest point to the vehicle’s position. Appendix-A shows
how to define this point. In a given abscissa x, θ and γ
are calculated with Eqs. 6 and 7 where y (x) is the known
equation of the line (Eq. 3).
θ (x) = arctan (y
′
(x)) (6)
γ (x) =
y
′′
(x)
(1 + y
′2
(x))
3
/2
(7)
where y
′
(x) denotes the derivative of y (x). Then, the square
of the statistical distance between a given lane’s center line
and a given vehicle path is calculated with Eq.8 where P
(•)
denotes the covariance matrix of state X
(•)
.
D
2
=
X
(l)
− X
(p)
T
·
P
(l)
+ P
(p)
−1
·
X
(l)
− X
(p)
(8)
D
2
is Chi-square distributed. If its value is small, then the
path of the vehicle is assumed to be currently quite similar
to the lane’s center line. In order to avoid disturbance due
to punctual erroneous measurements, the distance considered
to evaluate the similarity is a weighted average of so defined
statistical distance, over the N last sample times (Eq.9).
D
∗
k
=
P
N−1
i=0
w
i
D
k−i
P
N−1
i=0
w
i
(9)
However, the weights w
i
are set to be smaller for older
sampled times. N is set so as not to use measurements
older than one sec and to keep reasonable computational
requirements. The distances D
k−i
are calculated with Eq.8.
The intended lane of the vehicle is detected by first calcu-
lating the distance between its path and its current lane’s
center line. If the distance is lower or equal to a threshold
T
h
, the vehicle is supposed to be following that lane and
this is a keep lane maneuver. The value of the threshold
depends on the accuracy of the measurements and has to
be tuned experimentally so as to give realistic results (a use
case is shown is section V). If the distance is above the
threshold, then the vehicle is either going to leave its current
lane or has just entered it. In the first case, the distance
with that lane must be increasing and in the second case,
it must be decreasing. In order to be able to check the
sens of variation of the distance it is necessary to buffer the
calculated distances for at least one iteration. If the distance
is increasing, then the vehicle is leaving its current lane and
the intending lane is the one having the smallest distance,
apart from the vehicle’s current lane. Here, the maneuver
is a change lane, if the intended lane is an adjacent one
otherwise, it is a turn if the intended lane is a connected
lane at an intersection. If the distance is decreasing, then
the vehicle is entering its current lane and the maneuver is
a keep lane (even if it is the second part of an actual lane
change).
Notice that at the beginning of a change lane for instance,
the MRM tends to output a keep lane until the distance
between the vehicle’s path and its current lane’s center line
exceeds the chosen threshold. This may induce a short delay
in the detection of new started maneuvers (See section V).
The lower the threshold, the lower the delay but a too low
threshold will cause instability in case of zigzags within the
lane or because of measurement noise.
For the sake of clarity, only change lane and keep lane
maneuvers will be considered in the following. However,
the proposed approach is not significantly different for a turn
maneuver.
Figure 3: A trajectory in the Frenet frame
IV. TRAJECTORY PREDICTION
The method consists in mixing trajectory prediction based
on maneuver recognition and trajectory prediction based on
a motion model.
A. Trajectory prediction based on maneuver recognition
(T
man
)
The MRM detects the current maneuver but there are many
possible realizations for a single maneuver. Depending on the
driver’s habits, the actual trajectory may be pretty smooth
or pretty aggressive. Moreover, the road geometry will
also have an influence. So, based on the vehicle current
state, the road parameters and the detected maneuver, a
set of trajectories are first generated and the best one is
selected with respect to a cost function described later. The
trajectories are first generated in the Frenet frame along the
center line of the current lane of the vehicle (see Fig.3),
then converted to the initial Cartesian coordinate system.
We know from [14] and [15] that the lateral component
d (t) and the longitudinal component s (t) (t being the time)
of the trajectory of a vehicle moving from the initial state
F
0
=
s
0
, ˙s
0
, ¨s
0
, d
0
,
˙
d
0
,
¨
d
0
in the Frenet frame to the final
state F
1
=
s
1
, ˙s
1
, ¨s
1
, d
1
,
˙
d
1
,
¨
d
1
can each be optimally
modeled as a quintic polynomial. This guaranties the jerk
continuity and provides a unique solution.
1) Initial state of the trajectories in the Frenet frame: All the
trajectories have the same initial state which is derived from
the current state ζ
0
= [x
0
, y
0
, θ
0
, v
0
, a
0
, ω
0
] of the vehicle in
the Cartesian frame. Eq.10 shows the transformation.
d
0
= d
∗
0
˙
d
0
= v
0
sin
θ
0
− θ
−→
T
0
¨
d
0
=
p
(a
2
0
+ γ
0
v
2
0
)sin
θ
0
− θ
−→
T
0
s
0
= 0
˙s
0
= v
0
cos
θ
0
− θ
−→
T
0
¨s
0
=
p
(a
2
0
+ γ
0
v
2
0
)cos
θ
0
− θ
−→
T
0
(10)
d
∗
0
is the Euclidean distance between point (x
0
, y
0
) and the
lane center line, θ
−→
T
0
is the orientation of the tangent vector
−→
T
0
depicted in Fig.3. Appendix-A describes the calculation of
d
∗
0
and
−→
T
0
. The initial curvilinear abscissa s
0
is arbitrarily set
to zero. γ
0
v
2
0
is the current value of the normal acceleration
of the vehicle.
2) Final state of the trajectories in the Frenet frame:
Since the trajectory prediction is made in order to detect
possible collisions, no constraint about obstacles is taken
into account. Only the following assumptions are made:
at the end state, the vehicle is moving right on the center
line of its intended lane (known from the MRM) and has
a constant longitudinal acceleration during the maneuver.
Thus, only partial knowledge is available about the final state
(see Eq.11).
d
1
= d
∗
1
˙
d
1
= 0
¨
d
1
= 0
¨s
1
= a
0
(11)
For a change lane, d
∗
1
equals plus/minus the lane’s width
depending on the direction of the maneuver and is null for a
keep lane. A complete change lane maneuver has a limited
duration denoted t
(K)
. We know from the experiments
described in section V that t
(K)
≈ 6sec. One may extend
or reduce this duration if needed. That does not affect
the proposed method but may change the computational
requirements. A keep lane is obviously shorter. The time
interval
0, t
(K)
is then sampled and each sample time is
used as maneuver ending time t
1
to define a unique trajec-
tory. The set of trajectories originates from this sampling:
t
1
=
t
(i)
i=1..K
. So, an additional piece of information is:
˙s
1
= v
0
+ a
0
· t
1
.
3) Lateral component description: The lateral component of
each trajectory is of the form:
d(t) = c
5
t
5
+ c
4
t
4
+ c
3
t
3
+ c
2
t
2
+ c
1
t + c
0
(12)
Where c
i,i={0,1,2,3,4,5}
are coefficients. Given a starting time
t
0
= 0, a defined ending time t
1
∈ ]0, t
max
] and knowing
the initial and final states, the coefficients c
i,i={0,1,2,3,4,5}
are
easily obtained by solving Eq.13
t
5
0
t
4
0
t
3
0
t
2
0
t
1
0
1
t
5
1
t
4
1
t
3
1
t
2
1
t
1
1
1
5t
4
0
4t
3
0
3t
2
0
2t
1
0
1 0
5t
4
1
4t
3
1
3t
2
1
2t
1
1
1 0
20t
3
0
12t
2
0
6t
1
0
2 0 0
20t
3
1
12t
2
1
6t
1
1
2 0 0
·
c
5
c
4
c
3
c
2
c
1
c
0
=
d
0
d
1
˙
d
0
˙
d
1
¨
d
0
¨
d
1
(13)
4) Longitudinal component description: Since s
1
is un-
known, we loose one degree of freedom and the longitudinal
component of the trajectories will be modeled as a quartic
polynomial.
s(t) = c
4
t
4
+ c
3
t
3
+ c
2
t
2
+ c
1
t + c
0
(14)
Where c
i,i={0,1,2,3,4}
are constant coefficients. The jerk
continuity is still guarantied since the polynomial is a least
three times derivable. The coefficients c
i,i={0,1,2,3,4}
are
obtained by solving Eq.15.
t
4
0
t
3
0
t
2
0
t
1
0
1
4t
3
0
3t
2
0
2t
1
0
1 0
4t
3
1
3t
2
1
2t
1
1
1 0
12t
2
0
6t
1
0
2 0 0
12t
2
1
6t
1
1
2 0 0
·
c
4
c
3
c
2
c
1
c
0
=
s
0
˙s
0
˙s
1
¨s
0
¨s
1
(15)
Notice that by dealing separately with longitudinal and lateral
components, we do not take into account the non holonomic
constraints, which means that this approach cannot be applied
to very low speed scenarios. A set of trajectories is obtained,
each corresponding to a different ending time (Fig.4).
The trajectories are then converted to the Cartesian coordinate
system (see Appendix-B) and the best one is selected with
respect to the cost function described hereafter.
5) Best trajectory selection by minimization of a cost func-
tion: It is admitted that an average driver will seek to mini-
mize the duration of his current maneuver but will also try to
keep some comfort in the cockpit and avoid oscillations and
overshoots. The trajectories generated here (denoted T
(i)
i=1..K
)
have no oscillation but may have overshoots depending on
the initial state (see Fig.4) and the road’s geometry; which is
a realistic fact. Since there is no oscillation and constant
longitudinal acceleration is assumed, the comfort is only
quantified by the normal acceleration during the maneuver.
One can notice in Fig.4 that the trajectories with overshoots
are also the longer ones. So, the cost of a trajectory is defined
as shown in Eq.16.
C
T
(i)
= max (¯a (t)) + α · t
(i)
(16)
where ¯a (t) is the norm of the normal acceleration of the
vehicle at a given time t during the maneuver, t
(i)
is the
duration of the trajectory and α is a positive weighting
coefficients which is used to vary the behavior of the cost
function (a use case is shown in section V). This function
penalizes trajectories having a long duration and by the way
also penalizes trajectories having overshoots. Trajectories
with a high normal acceleration peak are also penalized.
T
man
is the trajectory having the smallest cost:
T
man
= arg min
C
T
(i)
i=1..K
(17)
Instead of considering the maximum value of the normal
acceleration in Eq.16, one may think of computing its average
value on the whole maneuver but the fact is that some of
the generated trajectories having a very small duration but
abnormally high peak of normal acceleration, could then be
selected.
B. Trajectory prediction with motion model (T
mdl
)
Assuming CYRA motion model, the components of the
velocity along each dimension in the Cartesian frame are
(
v
x
(t) = v (t) · cos (ω
0
· t + θ
0
)
v
y
(t) = v (t) · sin (ω
0
· t + θ
0
)
(18)
where v(t) = a
0
· t + v
0
. T
mdl
is obtained on a closed form
by integrating the velocity.
T
mdl
=
(
x (t) =
a
0
ω
2
0
cos (θ (t)) +
v (t )
ω
0
sin (θ (t)) + c
x
y (t) =
a
0
ω
2
0
sin (θ (t)) −
v (t )
ω
0
cos (θ (t)) + c
y
(19)