SIMULTANEOUS LOCALIZATION AND MAPPING BASED ON
MULTI-RATE FUSION OF LASER AND ENCODERS
MEASUREMENTS
Leopoldo Armesto, Josep Tornero
Dept. of Systems Engineering and Control, Technical University of Valencia
Camino de Vera s/n, 46020, Spain
Keywords:
Multi-rate fusion, SLAM, Mobile robots, Kalman filter.
Abstract:
The SLAM problem in static environments with EKF is adapted for multi-rate sensor fusion of encoders and
laser rangers. In addition, the formulation is general and can be adapted for any multi-rate sensor fusion
application. The proposed algorithm, based on well-known techniques for feature extraction, data association
and map building, is validated with some experimental results. This algorithm should been seen as a part of a
complete autonomous robot navigation algorithm, also described in the paper.
1 INTRODUCTION
SLAM problem addresses the simultaneously locate
and build a map using a mobile robot with no previous
knowledge of robot initial localization and the map
(environment). A number of approaches have already
been proposed to solve the SLAM problem since the
seminal paper (Smith et al., 1988) was presented.
The most relevant of these are based on grid-based
methods (Thrun et al., 1998) and parametric methods
(Dissanayake et al., 2001), (Jensfelt and Christensen,
2001), (Castellanos et al., 2001).
Kalman filter approach of SLAM consists on join-
ing the robot state and the set of landmark parameters
of the environment. It is well known that landmark
covariance decreases monotonically. In fact, in the
limit, the determinant of the covariance matrix of a
map containing more than one landmark converges to
zero and is fully correlated (Dissanayake et al., 2001).
The main advantage of this approach is that KF gives
a robust, optimal recursive state estimation to fuse re-
dundant information from different sensors, assuming
Gaussian noise on the system and measurements.
Multi-rate fusion is used when sensors have differ-
ent sampling rates. In any complex application, it is
unrealistic to assume the same sampling period for all
system variables. In mobile robots, sensors such as
laser rangers, sonars, radars, encoders, GPS, vision
systems, etc., have different sampling rates.
In this paper, we present a realistic approach to
the SLAM problem, where sensor measurements are
treated as system outputs at their sampling rates. In
this approach data-missing problems are easily con-
sidered. In particular, we fuse encoder measurements
at fast sampling and laser ranger measurements at
slow sampling. The proposed multi-rate SLAM is
more appropriate for real-time control applications,
because it produces vehicle and map estimations at
the fast sampling rate of the control.
2 SLAM WITH MULTI-RATE
FUSION
2.1 Vehicle and Landmark Models
Let the robot pose be described by the following dis-
crete dynamic equation,
x
r
(k + 1) = f
r
(x
r
(k)) + γ
r
(x
r
(k), w(k))
with x
r
(k) = [x(k) y(k) θ(k) v(k) ω(k)]
T
the robot
state vector with Cartesian positions x(k) and y(k),
orientation θ(k), linear velocity v(k) and angular ve-
locity ω(k). The robot constant velocity model is,
f
r
(x
r
(k)) =
x(k) + T v(k) cos(θ(k))
y(k) + T v(k) sin(θ(k))
θ(k) + T ω(k)
v(k)
ω(k)
where T is the sampling period.
435
Armesto L. and Tornero J. (2004).
SIMULTANEOUS LOCALIZATION AND MAPPING BASED ON MULTI-RATE FUSION OF LASER AND ENCODERS MEASUREMENTS.
In Proceedings of the First International Conference on Informatics in Control, Automation and Robotics, pages 435-439
DOI: 10.5220/0001136804350439
Copyright
c
SciTePress
Figure 1: Vehicle and landmark variables definition
Inputs to the system are linear acceleration a(k)
and angular acceleration α(k) of the robot, which
are unknown and therefore treated as system noises
w(k) = [a(k) α(k)]
T
with Gaussian distribution
w(k) N(0, Q(k)). The noise mapping to robot
states is given by,
γ
r
(x
r
(k), w(k)) =
T
2
2
a(k) cos(θ(k))
T
2
2
a(k) sin(θ(k))
T
2
2
α(k)
T a(k)
T α(k)
Let M denote the map of the environment, which
is a set of segment lines m
i
. Each segment, m
i
is
described by a set of parameters: distance of a line
to the origin ρ
m
i
(k), orientation ϕ
m
i
(k), start point
(x
m
s,i
,y
m
s,i
) and end point (x
m
e,i
, y
m
e,i
), as shown in figure
1. The use of redundant information is due to the fact
that ρ
m
i
(k) and ϕ
m
i
(k) are used in the map state vector
x
m,i
(k) = [ρ
m
i
(k) ϕ
m
i
(k)]
T
, the start and end points
are used to predict only visible segment lines. Since
the landmarks are supposed to be stationary, their dy-
namic description is simply x
m
(k+1) = x
m
(k), with
x
m
(k) = [x
T
m,1
(k) . . . x
T
m,n
(k)]
T
.
In SLAM, the robot and the map states are treated
together x(k)=[x
T
r
(k) x
T
m
(k)]
T
, its covariance is,
P(k) =
·
P
rr
(k) P
rm
(k)
P
T
rm
(k) P
mm
(k)
¸
The prediction of the state and its covariance is,
ˆx(k+1|k) =
·
f
r
(ˆx
r
(k))
ˆx
m
(k)
¸
P
rr
(k+1|k) =F
r
(k)P
rr
(k)F
T
r
(k)+Γ
r
(k)Q(k)Γ
T
r
(k)
P
rm
(k+1|k) =F
r
(k)P
rm
(k)
P
mm
(k+1|k) =P
mm
(k)
where F
r
(k) and Γ
r
(k) are the Jacobians of
f
r
(x
r
(k)) and γ
r
(x
r
(k), w(k)), respectively.
2.2 Measurement Model
In our application, the robot is an industrial fork-
lift with tricycle configuration. We have sensed the
two front wheels with incremental encoders, the rear
wheel with an absolute encoder and we have installed
laser rangers at the front and rear of the vehicle (Mora
et al., 2003). The measurement equations are particu-
larized for these sensors, however it is possible to ex-
tend the model to other sensors such as GPS, vision,
compass, sonar, etc.
Measurement equations of encoders are as follows:
y
inc
=
·
v
1
v
2
¸
=h
inc
(x
r
)+u
inc
=
·
vlω
v+lω
¸
+
·
u
inc
1
u
inc
2
¸
(1)
y
abs
=β = h
abs
(x
r
)+u
abs
=
Mω
v
+u
abs
(2)
where u
inc
and u
abs
are measurement noises with co-
variances R
inc
and R
abs
, respectively. For simplicity
we suppress index k where no confusion can arise.
For each detected line, we the following sensor
model (Jensfelt and Christensen, 2001),
y
las
i
=
·
ρ
las
i
ϕ
las
i
¸
=h
las
i
(x
r
, x
m,i
) + u
las
i
= (3)
=
·
ρ
m
i
p
x
2
+y
2
cos(ξϕ
m
i
)
ϕ
m
i
θ
¸
+
·
u
las
ρ,i
u
las
ϕ,i
¸
where ξ = arctan(y, x) while u
las
i
is the measure-
ment noise with covariance R
las
i
.
Detected landmarks are those which successfully
pass the Data Association Test (DAT), described be-
low. When the DAT fails, the detected feature is in-
cluded in a temporary map M
0
, where the i-th ele-
ment x
tm,i
is defined by the line parameters with re-
spect to the global frame. Until the feature is suf-
ficiently stable, it is not included in M. However,
DAT may fail despite the fact data belong to the same
feature. In that case, in order to avoid repeated land-
marks, a test between the map and the temporary map
is also performed. Only successfully associated ele-
ments are included as measurements,
y
tm
i
=
·
ρ
tm
i
ϕ
tm
i
¸
=h
tm
i
(x
m,i
)+u
tm
i
=
·
ρ
m
i
ϕ
m
i
¸
+
·
u
tm
ρ,i
u
tm
ϕ,i
¸
(4)
where u
tm
i
is the noise with covariance R
tm
i
.
The Jacobians of equations (1)-(4) are respectively
denoted as H
inc
(k), H
abs
(k), H
las
(k) and H
tm
(k).
2.3 Multi-rate Fusion
Assume a general multi-rate sampling with peri-
ods associated to incremental encoders (T
inc
), ab-
solute encoder (T
abs
), laser measurements (T
las
)
and the measurements associated with the tempo-
rary map (T
tm
). The base period is the great
common divisor of all sampling rates, T =
ICINCO 2004 - ROBOTICS AND AUTOMATION
436
gcd(T
inc
, T
abs
, T
las
, T
tm
). We also define period-
icity ratios r
inc
= T
inc
/T , r
abs
= T
abs
/T , r
las
=
T
las
/T , r
tm
=T
tm
/T .
Sensor measurements depend on functions δ
inc
(k),
δ
abs
(k), δ
las
(k), δ
tm
(k), indicating whether their re-
spective measurements are valid (δ(k) = 1) or not
(δ(k) =0). Thus, we can easily: discard erroneous in-
cremental encoder measurements if we detect wheel
slippage; not consider the absolute encoder measure-
ment when the vehicle is stopped, since its measure-
ment equation h
abs
(x(k)) is undetermined for v = 0;
not include features which do not pass the DAT.
Multi-rate fusion is performed by containing avail-
able measurements in rows on the output vector y(k),
y
inc
(k) y(k) iff mod(k, r
inc
)= 0 and δ
inc
(k)= 1
y
abs
(k) y(k) iff mod(k, r
abs
)= 0 and δ
abs
(k)= 1
y
las
(k) y(k) iff mod(k, r
las
)= 0 and δ
las
(k)= 1
y
tm
(k) y(k) iff mod(k, r
tm
)= 0 and δ
tm
(k)= 1
Valid measurements are also combined by rows in
the Jacobian matrix H(k). Noise covariance matrix,
R(k), is a diagonal matrix of valid measurement co-
variances.
The state update is as usual in EKF:
S(k+1) = H(k+1)P(k+1|k)H
T
(k+1)+R(k+1)
K(k+1) = P(k+1|k)H
T
(k+1)S
1
(k+1)
ˆx(k+1) = ˆx(k+1|k)+K(k+1)(y(k+1)h(ˆx(k+1|k)))
P(k+1) = (IK(k+1)H(k+1))P(k+1|k)
3 IMPLEMENTED ALGORITHM
The overall diagram can be seen in figure 2, where
white blocks are executed at a high sampling rate,
gray ones at a low sampling rate and dotted blocks act
as interfaces between high and low sampling rates.
Usually, object path planning and obstacle avoid-
ance algorithms require high computational power
and should be executed at lower sampling rates than
those required by the position controller. In this sense,
the block MR-HOH (multi-rate high order holds) is
used to extrapolate reference points to the control at
high sampling rate from points provided at a low sam-
pling rate (Armesto and Tornero, 2003).
3.1 Feature Extraction
For feature extraction, we have implemented the
RIEPFA (Recursive Iterative End Point Fit Algo-
rithm) (Duda and Hart, 1973). The algorithm per-
forms the segmentation of a laser scan providing
points belonging to the same line segment. Parame-
ters of segment lines are estimated with the Orthogo-
nal Least-Squares method. (Deriche et al., 1992) give
Figure 2: Bock diagram for SLAM with multi-rate fusion,
including obstacle avoidance and position control
the exact formulation for computing line parameter
covariances, assuming that data are affected by Carte-
sian and polar noise.
3.2 Data Association
The objective of data association is to associate mea-
surements to the features in the map. In this work,
data association is based on the Mahalanobis distance
in the innovation space (Bar-Shalom and Fortman,
1988). The innovation matrix of a feature i is given
by:
S
las
i
=H
las
i
·
P
rr
P
ri
P
ir
P
ii
¸
(H
las
i
)
T
+R
las
i
The innovation is z
las
i
= y
las
i
h
las
i
(x
r
, x
m,i
) and
the Mahalanobis distance (z
las
i
)
T
(S
las
i
)
1
z
las
i
η,
where η = 9.0 represents 98.9% of a 2D Gaussian
curve. Additional gating conditions have been consid-
ered, such as the norm ||z
las
i
||, should be lower than
a threshold, z
norm
, and segments separated by more
than a distance, l
max
, are treated separately.
3.3 Map Building and Maintenance
In this section, we describe how a new feature m
N+1
is incorporated into the system vector state,
x
m,N+1
=x
tm,i
, P
N+1N+1
=P
tm,ii
where x
tm,i
is the feature in the temporary map M
0
once it has already been validated. The feature covari-
ance in the temporary map is assigned to the new fea-
ture covariance in the map. For simplicity, the cross-
SIMULTANEOUS LOCALIZATION AND MAPPING BASED ON MULTI-RATE FUSION OF LASER AND
ENCODERS MEASUREMENTS
437
0 5 10 15 20 25 30
0
5
10
X [m.]
Y [m.]
(a) Ground-truth robot trace
0 5 10 15 20 25 30
0
5
10
X [m.]
Y [m.]
(b) SLAM with multi-rate EKF
0 5 10 15 20 25 30
0
5
10
X [m.]
Y [m.]
(c) SLAM with single-rate EKF
Figure 3: Robot trace estimation and map built with the
different Kalman filter approaches.
covariances of the new feature with the robot state and
the rest of the features is initialized to zero.
4 EXPERIMENTAL RESULTS
Data has been taken from a parking lot, where en-
coder measurements were sampled every 50ms and
the laser scans every 500ms for 150 sec. Ground-truth
estimation has been performed using a detailed park-
ing layout, see figure 3(a). In addition, the vehicle
trace and map estimation using the proposed multi-
rate filter is depicted in figure 3(b), where it can be
appreciated that the trace estimation is very similar to
ground-truth estimation and the estimated map con-
tains all the main features of the parking lot.
In order to compare the benefits of using the multi-
rate filter, the single-rate estimation is also performed,
where all measurements were sampled at 500ms. Fig-
ure 4 shows the trace estimation of the three filters,
during the first turn of the trajectory (time between
14.5-26.5sec.). It should be noted that, depending on
the tuning parameters, we have found many numeri-
cal cases where the multi-rate sampling stabilized the
estimation, which was not stable at single-rate.
Landmark standard deviation with the multi-rate
filter are shown in figure 5. Landmark covariance is
not represented until incorporated to the map.
21 22 23 24 25 26
3.5
4
4.5
5
5.5
X [m.]
Y [m.]
Figure 4: Trace comparison between ground-truth (solid
line), MR-EKF (dashed line) and SR-EKF (dotted line).
0 50 100 150
0
1
2
3
4
5
6
7
8
9
x 10
−3
Time [sec.]
Figure 5: Standard deviation of estimated landmarks (σ
2
m
).
Solid lines (walls) and dashed lines (columns).
5 CONCLUSIONS
In this paper, a multi-rate simultaneous localization
and mapping based on Extended Kalman filter has
been presented. Real-time control applications can
clearly benefit from multi-rate SLAM algorithm, be-
cause vehicle and map estimation are computed at the
fast sampling rate of the vehicle control. The algo-
rithm is based on well-known techniques for feature
extraction, data association and map building. The
algorithm should be seen as part of a complete au-
tonomous robot navigation system.
Experimental results have shown that the estima-
tion is successfully performed, with the trace of the
robot and the map very close to the ground-truth es-
timation. In addition, MR-EKF performance is much
improved with respect to SR-EKF.
REFERENCES
Armesto, L. and Tornero, J. (2003). Trajectory extrapola-
tion using multi-rate high order holds. In Symp. on
Intelligent Components and Instruments for Control
Applications.
Bar-Shalom, Y. and Fortman, T. (1988). Tracking and Data
Association. Academic Press, New York.
Castellanos, J., Neira, J., and Tardos, J. (2001). Multisensor
ICINCO 2004 - ROBOTICS AND AUTOMATION
438
fusion for simultaneous localization and map building.
Trans. on Robotics and Automation, 17(6).
Deriche, R., Vaillant, R., and Faugeras, O. (1992). From
noise edges points to 3d reconstruction of a scene: A
robuts approach and its uncertainty analysis. In Ma-
chine Perception and Artificial Inteligence, volume 2,
pages 71–79. World Scientific.
Dissanayake, M., Newman, P., Clark, S., Durrant-Whyte,
H., and Csorba, M. (2001). A solution to the simul-
taneous localization and map building (SLAM) prob-
lem. IEEE Trans. Robot. Automation, 17(3):229–241.
Duda, R. and Hart, P. (1973). Classification and Scene
Analysis. John Wiley and Sons, New York.
Jensfelt, P. and Christensen, H. (2001). Pose tracking using
laser scanning and minimalistic environmental mod-
els. Trans. Robotics and Automation, 17(2):138–147.
Mora, M., Suesta, V., Armesto, L., and Tornero, J. (2003).
Factory management and transport automation. In
IEEE Conference on Emerging Technologies and Fac-
tory Automation, volume 2, pages 508–515.
Smith, R., Self, M., and Cheeseman, P. (1988). A stochastic
map for uncertain spatial relationships. In Int. Symp.
in Robot. Res., pages 467–474.
Thrun, S., Fox, D., and Burgard, W. (1998). A probabilis-
tic approach to concurrent mapping and localization.
Mach. Learning Autonomous Robots, pages 29–53.
SIMULTANEOUS LOCALIZATION AND MAPPING BASED ON MULTI-RATE FUSION OF LASER AND
ENCODERS MEASUREMENTS
439