Combined road prediction and target tracking in collision avoidance
Andreas Eidehall Fredrik Gustafsson
Vehicle Dynamics and Active Safety Department of Electrical Engineering
Volvo Car Corporation University of Link
¨
oping
SE-405 31 G
¨
oteborg, Sweden SE-581 83 Link
¨
oping, Sweden
aeidehal@volvocars.com fredrik@isy.liu.se
Abstract
Detection and tracking of other vehicles and lane geom-
etry will be required for many future intelligent driver
assistance systems. By integrating the estimation of these
two features into a single filter, a more optimal utiliza-
tion of the available information can be achieved. For
example, it is possible to improve the lane curvature
estimate during bad visibility by studying the motion of
other vehicles.
This paper derives and evaluates various approximations
that are needed in order to deal with the non-linearities
that are introduced by such an approach.
I. Introduction
Future intelligent automotive systems such as adaptive
cruise control, collision avoidance or lane guidance will
require detailed knowledge about the vehicle surroundings
[6]. In this paper, vehicle surroundings will refer to lane
geometry and other vehicles. Typically, lane information
is obtained from a vision system and other vehicles are
detected with a radar.
The importance of integrating data from object tracking
and road geometry tracking has quite recently been rec-
ognized [3], [2], [7]. The main idea is to try to improve
the road geometry estimate by studying the motion of
other vehicles and vice versa. For example, if a couple
of tracked vehicles suddenly all start moving right, one
of two things can have happened. The first is that they
all started a lane change manoeuvre and the road remains
straight. The other is that we are entering a curve and the
vehicles are still following the center of their lanes. These
possibilities can be treated in a Bayesian framework,
together with the information from the lane tracker, to
build a new estimator. In order to do this we need to
construct a new object measurement equation based on
the road geometry.
II. Overview
The position on the road for each vehicle is denoted (x,y).
This means that x is the driven distance along the road and
y the lateral position in the lane. We will need to relate
this to absolute, or Cartesian, position denoted ( ˜x, ˜y). The
purpose of Section III-A is to find this transformation
( ˜x, ˜y)=T(x, y). To derive this, we need a model of the
road, and we start with a general model describing the
road curvature as c(x)=c
0
+c
1
x. This describes a clothoid
curve and is a commonly used parametrization in collision
avoidance applications. The trigonometric formulas that
arise do not give an explicit expression for T(x, y). If such
an expression is needed, either the trigonometric functions
can be Taylor expanded, or a simpler model with c
1
= 0
can be used (constant curve radius). These approximations
are treated in Section III-C.
To be able to apply a Kalman filter, we first define a state
vector that contains road geometry, the host vehicle’s and
the tracked vehicles’ positions. Section IV-A gives the
measurement equations, where the host vehicle’s sensors
are expressed as functions of the state vector. A suitable
motion model for the host vehicle and the tracked vehicles
is suggested in Section IV-B.
Having said this, the results presented in Section V should
be possible to appreciate, without understanding all of the
detail in the model and filter described in Section III and
IV.
III. Measurement equation derivation
A. Coordinate system derivation
We will start by deriving a two dimensional coordinate
transformation which is a mapping T from a curved
coordinate system (x, y) which follows the road to a
cartesian coordinate system ( ˜x, ˜y) which is attached to the
host vehicle, see Figure 1. The first part of the derivation
is similar to what was done in [4].
We start with a planar curve r(x), where x is distance
along a curve. Assume the curvature along the curve is
given by c = c(x).Now,if
ˆ
t(x) is the tangent vector, where
x
t
0
r(x)
t(x)
x
~
~
y
β
r
0
c(x)
radius
=
1
n(x)
y
H
Fig. 1. The vectors used in the derivation of T .
the hat means |
ˆ
t(x)| = 1, we define the normal vector n(x)
as
n(x)=
d
ˆ
t
dx
(x)
From vector analysis we get an alternative expression for
n(x) where we use the fact that it is perpendicular to
ˆ
t(x)
and that its length is precisely c(x). We first define
R(α)
cosα −sinα
sinα cosα
Then n(x)=c(x)R(−
π
2
)
ˆ
t(x). Thus, we end up with the
differential equation
d
ˆ
t
ds
(x)=c(x)R(−
π
2
)
ˆ
t(x)
which has the solution
ˆ
t(x)=exp(R(−
π
2
)
x
0
c(τ)dτ)
ˆ
t
0
= R(−
x
0
c(τ)dτ)
ˆ
t
0
(1)
This can then be used to obtain an expression for the
position:
r(x)=
x
0
ˆ
t(τ)dτ + r
0
=
=
x
0
R(−
τ
1
0
c(τ
2
)dτ
2
)dτ
1
ˆ
t
0
+ r
0
(2)
The vectors r(x) and
ˆ
t(x) are illustrated in Figure 1.
To construct the coordinate transformation T we first
define one of our coordinates to be the distance along
the curve: T(x, 0)=r(x). To define the other coordinate
we require T to be orthogonal, i.e.
dT(x,y)
dy
⊥
ˆ
t(x) for all x
A natural choice is to simply extend a straight line at
T(x,0) along −R(−
π
2
)
ˆ
t(x). This choice will also give us
a positively oriented transformation. T will look like
˜x
˜y
= T(x,y)=r(x) − R(−
π
2
)
ˆ
t(x)y (3)
B. Choosing curvature function
The road is often modelled as segments of straight lines,
arcs and clothoids, see for example [4]. Clothoids are
segments were the curvature changes linearly with the
distance along the curve. According to [1], this agrees
well with how roads are constructed. The function
c(x)=c
0
+ c
1
x (4)
will suffice for all these cases. Of course, when approach-
ing a curve we might, for example, have situations were
the section 0 - 50 meters of our field of view is a straight
line and the section 50 - 100 meters is a clothoid, a case
which can not be modelled with a linear curvature law.
Plugging (4) into (1) and using
ˆ
t
0
=
cosβ
sinβ
we get
ˆ
t(x)=R(−c
0
x− c
1
x
2
/2)
cosβ
sinβ
=
=
cos(c
0
x+ c
1
x
2
/2) sin(c
0
x+ c
1
x
2
/2)
−sin(c
0
x+ c
1
x
2
/2) cos(c
0
x+ c
1
x
2
/2)
cosβ
sinβ
=
cosβ cos(c
0
x+ c
1
x
2
/2)+sinβ sin(c
0
x+ c
1
x
2
/2)
−cosβsin(c
0
x+ c
1
x
2
/2)+sinβ cos(c
0
x+ c
1
x
2
/2)
=
cosβ −sinβ
sinβ cosβ
cos(c
0
x+ c
1
x
2
)
−sin(c
0
x+ c
1
x
2
)
= R(β)
cos(c
0
x+ c
1
x
2
)
−sin(c
0
x+ c
1
x
2
)
(5)
(5) gives the second term of (3). In order to get an
expression for the first term we need to integrate (5),
which can not be done analytically. Instead we need to
do some sort of approximation.
Before we continue we shall use the free parameter r
0
to
describe an offset perpendicular to
ˆ
t
0
by simply setting
r
0
= y
off
R(−
π
2
)t
0
= y
off
01
−10
cosβ
sinβ
= R(β)
0
−y
off
The vector r
0
is shown in Figure 1.
C. Approximations
Approximation A: Omitting the clothoid parameter
Using c
1
= 0 in (5) we get
ˆ
t(x)=R(β)
cos(c
0
x)
−sin(c
0
x)
which, used in (2) gives
r(x)=R(β)
sin(c
0
x)
cos(c
0
x) − 1
1
c
0
− R(β)
0
y
off
and, since rotations commute, the coordinate transforma-
tion (3) becomes
T
a
(x,y) r(x) − R(
π
2
)t(x)y =
= R(β)
(1+ c
0
y)sin(c
0
x)
(1+ c
0
y)cos(c
0
x) − 1− c
0
y
off
1
c
0
Approximation B: Linearizing the trigonometric
functions
If we use sinτ = τ and cosτ = 1 then (5) becomes
ˆ
t(x)=R(β)
1
−c
0
x− c
1
x
2
/2
Plugging this into (2) we get
r(x)=R(β)
x
−c
0
x
2
/2− c
1
x
3
/6
− R(β)
0
y
off
and from (3) we get the coordinate transformation
T
b
(x,y) R(β)
x+ y(c
0
x+ c
1
x
2
/2)
y− y
off
− c
0
x
2
/2− c
1
x
3
/6
Approximation C: As B, plus further approximations
We now do some further approximation steps
T
b
(x,y) ≈ R(β)
x
y− y
off
− c
0
x
2
/2− c
1
x
3
/6
≈
≈
1 −β
β 1
x
y− y
off
− c
0
x
2
/2− c
1
x
3
/6
=
=
x− β(y− y
off
− c
0
x
2
/2− c
1
x
3
/6)
βx+ y− y
off
− c
0
x
2
/2− c
1
x
3
/6
≈
≈
x
y− y
off
+ βx − c
0
x
2
/2− c
1
x
3
/6
T
c
(x,y)
This is a commonly used approximation.
IV. Filter construction
To be able to use a Kalman filter, we will build three
state space models, based on the approximations from the
previous section. The states for the host vehicle are shown
in Figure 2. Note that we have β = −Ψ
rel
. We need c
1
as state in all three filters for dynamic reasons. Observed
vehicles will have the states x
i
,˙x
i
and y
i
, where i runs
through all detected objects.
y
off
Ψ
rel
W
radius =
c + c x
1
0
Ψ
abs
x
x
~
y
~
y
1
H
Fig. 2. W, y
off
, Ψ
rel
, c
0
and c
1
are the host vehicle states. The mapping
T transforms from the coordinate system (x, y) to the coordinate system
( ˜x, ˜y).
A. Measurement equations
The measurements for the host vehicle are Ψ
m
rel
, c
m
0
, L
m
and R
m
where the last two are the distances to the left
and right lane marking. Superscript m denotes measured
quantities. For other vehicles we measure the position, ˜x
m
and ˜y
m
. These relate to the states as
L
m
t
= −W
t
/2− y
off,t
+ e
1,t
R
m
t
= W
t
/2− y
off,t
+ e
2,t
Ψ
m
rel,t
= Ψ
rel,t
+ e
3,t
c
m
0,t
= c
0,t
+ e
4,t
(6a)
˜x
m
i,t
˜y
m
i,t
= T(x
i,t
,y
i,t
)+
e
5,t
e
6,t
i
(6b)
where T can be replaced with any of the approximations.
Of course, T depends on all the host vehicle states
as well. The variables (e
1
,...,e
6
) are some stochastic
measurement noise.
B. Motion models
Since x and y are the curved road coordinates, the motion
model of other vehicles can be greatly simplified. For
example, it allows us to use the equation ˙y
i
= 0 which
simply means that we assume that other vehicles will fol-
low their own lanes. In the longitudinal direction we will
use the ¨x
i
= 0 − a
host
cosΨ
rel
, a
host
being the measured
acceleration of the host vehicle so that with sample time
T
s
we get the motion equations:
x
i
t+1
= x
i
t
+ T
s
˙x
i
t
+ a
host,t
cosΨ
rel,t
T
2
s
/2+ w
1,t
˙x
i
t+1
= ˙x
i
t
+ a
host,t
cosΨ
rel,t
T
s
+ w
2,t
(7a)
y
i
t+1
= y
i
t
+ w
3,t
For the road geometry parameters we first clarify that Ψ
rel
is the angle offset to the lane and Ψ
abs
is the angle to
some fix reference. We can obtain a relationship between
the two by taking the time derivative of Ψ
rel
Ψ
rel
= Ψ
abs
+ γ ⇒
˙
Ψ
rel
=
˙
Ψ
abs
+
˙
γ =
˙
Ψ
abs
+
v
r
=
˙
Ψ
abs
+ c
0
v
where r is the current road radius, v the velocity and γ
denotes the angle between the lane and some fix reference.
˙
Ψ
abs
can typically be measured with a yaw rate sensor.
We also have
˙y
off
= sinΨ
rel
v ≈ Ψ
rel
v
Using
˙
W = 0 and ˙c
1
= 0 we can write the motion
equations for the host vehicle states:
W
t+1
= W
t
+ w
4,t
y
off,t+1
= y
off,t
+ vT
s
Ψ
rel,t
+ w
5,t
Ψ
rel,t+1
= Ψ
rel,t
+ vT
s
c
0,t
+ T
s
˙
Ψ
abs,t
+ w
6,t
(7b)
c
0,t+1
= c
0,t
+ vT
s
c
1,t
+ w
7,t
c
1,t+1
= c
1,t
+ w
8,t
The variables (w
1
,...,w
8
) are stochastic process noise.
V. Evaluation
A. Geometric comparison
From [1] we get the following guidelines for road con-
struction. For a 50 km/h road, the minimum radius is
140 meters and for a 90 km/h road it is 550 meters. The
recommended maximum clothoid parameters for these
curves are given by the formula
c
1
=
k
v
3
where k = 0.45 (m/s
3
) which is the maximum ”jerk”
and v the velocity, giving the clothoid 1.7 · 10
−4
and
2.9 · 10
−5
(1/m
2
) for the 50 km/h and the 90 km/h
curve respectively. In Figure 3 we have compared the
approximations with the exact transformation. Note that
on straight lines and on circle segments, T
a
coincides with
the exact transformation.
−40 −30 −20 −10 0 10 20 30
0
10
20
30
40
50
60
70
x (m)
y (m)
Exact
Approximation A
Approximation B
Approximation C
Fig. 3. Illustration of the different approximations. A road with edges
at y = −5 and y = 5 has been transformed with the three approximations
and with the true transformation, based on typical a 50 km/h curve.
B. Curvature estimate accuracy
To analyze the different models further, an extended
Kalman filter was implemented, based on the different
approximations. The accuracy of the lane geometry was
then evaluated during a test drive. A vehicle equipped
with a camera and a radar was used to record data. Lane
geometry measurements were given by the camera and
measurements of other vehicles were given both from
the camera and the radar. Some additional data from the
vehicle, such as velocity and yaw rate, were also collected.
The true values of the lane geometry were obtained from
a detailed map.
Four filters were then run on the same data, the three
approximations that was derived in Section III-C, plus
a completely decoupled model, i.e. tracking of obstacles
and lane geometry done separately.
Filter tuning
Filter tuning is the process of adjusting the entries in
the ”Q” and ”R” matrices, which are often interpreted
as process and measurement noise covariance. If we
constrain these to be diagonal, we have 14 parameters
to tune, as shown in Table I.
The tuning was started by first using ”physical” intuition
trying to judge errors in measurements and changes in
different states. This was then used as a starting point
for hours of manual tuning in order to get acceptable
performance from all three filters.
Host Obstacles
Process noise 53
Measurement noise
42
TABLE I
N
UMBER OF TUNING PARAMETERS
After that, a more systematic approach was used. A scal-
ing parameter was applied to selected filter parameters.
For example, if the host states process noise covariance
matrix is called Q
host
, the filters was run with this matrix
replaced with λQ
host
, where λ for example ranges from
10
−2
to 10
2
. Then the mean error in some parameter,
usually the curvature c
0
, was computed. This procedure
was then applied to different combinations of parameters
or single parameters. This is of course only a sub-
optimization in the 14 dimensional parameter space.
10
−2
10
−1
10
0
10
1
10
2
10
3
10
4
0
0.5
1
1.5
2
2.5
x 10
−4
Mean curvature error
Road measurements noise covariance scaling
Approximation A
Approximation B
Approximation C
Decoupled
Fig. 4. Curvature error during bad visibility. The measurement
noise of the road measurements has been scaled from 10
−2
to 10
4
. The
plot shows the error in the curvature estimate for the different filters.
This also includes a decoupled filter where the road geometry and the
obstacles are treated separately. Note that Approximation A becomes
unstable for high measurement noise values.
Results
Figure 4 and Figure 5 shows some preliminary results
of the performance of the different filters. The three
approximations have been compared to a decoupled linear
filter, where lane geometry and obstacles were tracked
separately. Figure 4 shows a data sequence recorded
during bad visibility. It shows that the performance can be
improved by using a combined filter, which has also been
demonstrated in for example [7] and [5]. Figure 5 shows a
data set recorded during good visibility. In this case, there
is only a small improvement by using integrated filtering.
10
−2
10
−1
10
0
10
1
10
2
10
3
10
4
0
0.5
1
1.5
2
2.5
x 10
−4
Mean curvature error
Road measurements noise covariance scaling
Approximation A
Approximation B
Approximation C
Decoupled
Fig. 5. Curvature error during good visibility. The measurement
noise of the road measurements has been scaled from 10
−2
to 10
4
. The
plot shows the error in the curvature estimate for the different filters.
To make the experiment more interesting, we have only
allowed the measurement noise of the road measurements
to vary between the two cases, all other parameters are
kept constant. It can be seen in the Figures 4 and 5 that
the optimal performance is reached at higher measurement
noise for the bad visibility case than the good visibility
case. This is intuitive, if bad visibility was detected by for
example the vision system, you would typically increase
the process noise of road measurements in the Kalman
filter in order to rely more on other measurements and on
the motion model.
It should be noted that the data in Figure 4 and Figure 5
are from different roads and during different traffic condi-
tions. Therefore, care should be taken before comparing
the performance in the two experiments.
Also, even though the curvature is important in many
applications, it is just one of many parameters. A more
thorough evaluation of the performance could be done if
the particular application of the filter was known.
In the next section we examine a property that is important
in many collision avoidance applications.
C. Lane assignment
Lane assignment is the problem of deciding in which
lanes the tracked vehicles are currently driving. This is
where the quality of the lane geometry estimate becomes
utterly important, even the slightest error in heading angle
or curvature will result in a significant lateral error for
vehicles at a long distance, say 70 - 100 meters.
All three model approximations from Section III-C were
