See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/4046199

An Efficient FastSLAM Algorithm for Generating Maps of Large-Scale Cyclic

Environments from Raw Laser Range Measurements

Conference Paper · November 2003

DOI: 10.1109/IROS.2003.1250629 · Source: IEEE Xplore

CITATIONS READS

461 672

4 authors, including:

Wolfram Burgard Dieter Fox

University of Freiburg University of Washington Seattle
754 PUBLICATIONS   54,797 CITATIONS    298 PUBLICATIONS   44,263 CITATIONS   

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

NeuroBots View project

Flourish View project

All content following this page was uploaded by Dieter Fox on 05 June 2014.

The user has requested enhancement of the downloaded file.

 An Efficient FastSLAM Algorithm for Generating Maps
of Large-Scale Cyclic Environments from Raw Laser Range Measurements

Dirk Hähnel1 and Wolfram Burgard1 and Dieter Fox2 and Sebastian Thrun3
1
University of Freiburg, Department of Computer Science, Freiburg, Germany
2
University of Washington, Computer Science and Engineering, Seattle, WA, USA
3
Stanford University, Computer Science Department, Stanford, CA, USA

Abstract As previously proposed by Murhpy [17], our approach

The ability to learn a consistent model of its environ- applies a Rao-Blackwellized particle filter to estimate a
ment is a prerequisite for autonomous mobile robots. A posterior of the path of the robot, in which each particle
particularly challenging problem in acquiring environment has associated to it an entire map. This differs from
maps is that of closing loops; loops in the environment work in [20], where only a single map is retained. To
create challenging data association problems [9]. This scale to large-scale environments, we transform sequences
paper presents a novel algorithm that combines Rao- of laser range-scans into odometry measurements using
Blackwellized particle filtering and scan matching. In our range-scan registration techniques [10]. This way our sys-
approach scan matching is used for minimizing odomet- tem can deal with significantly larger environments than
ric errors during mapping. A probabilistic model of the Murhpy’s approach [17], since the scan matching yields
residual errors of scan matching process is then used for odometry estimates that are an order of magnitude more
the resampling steps. This way the number of samples accurate than the raw wheel encoder data. Simultaneously,
required is seriously reduced. Simultaneously we reduce the transformation of sequences of scans into odometry
the particle depletion problem that typically prevents the measurements reduces the well-known particle deprivation
robot from closing large loops. We present extensive problem [21], since the number of resampling operations
experiments that illustrate the superior performance of our is significantly reduced. By using a learned model of the
approach compared to previous approaches. residual errors of the range registration our approach can
correctly integrate the corrected odometry into the particle
filtering process. As a result, we obtain a drastic reduction
I. Introduction in the number of particles needed to build large-scale
Learning maps with mobile robots is one of the fun- maps, or, put differently, an improved ability to map large
damental problems in mobile robotics. In the literature, environments. This is demonstrated in our experimental
the mobile robot mapping problem is often referred to results section, in which we compare our approach to
as the simultaneous localization and mapping problem previous techniques.
(SLAM) [5], [6], [9], [12], [13], [19]. This is because This paper is organized as follows. In the following
mapping includes both, estimating the position of the robot section, we will discuss techniques for incremental prob-
relative to the map and generating a map using the sensory abilistic mapping and localization. In Section III, we de-
input and the estimates about the robot’s pose. scribe our approach to integrate scan matching with Rao-
One of the hardest problems in robotic mapping is Blackwellized particle filters to achieve a robust approach
that of loop closure. As a robot traverses a large cycle for simultaneous mapping and localization. Section IV
in the environment, it faces the hard data association of presents several experiments illustrating that our approach
correctly connecting to its own map under large position can successfully learn accurate maps with range scan-
errors. This problem has long been acknowledged for its ners in large-scale environments. Additionally, we present
hardness, and a number of approaches have addressed experiments illustrating that our technique outperforms
it [4], [9], [20]. Recently, Murphy and colleagues have existing approaches.
presented Rao-Blackwellized particle filters [8], [17] as
an effective way of representing alternative hypotheses on II. Incremental Probabilistic Mapping and
robot paths and associated maps. Montemerlo et al. [14],
[15] extended this idea to efficient landmark-based SLAM
Localization
using Gaussian representations to and were the first to In probabilistic terms the goal of map learning is to find
successfully realize it on real robots. the map and the robot positions which yield the best
In this paper we present a highly efficient approach to interpretation of the data dt gathered by the robot [19].
simultaneous localization and mapping with laser scans. Here the data dt = {u0:t−1 , z1:t } consists of a stream
of odometry measurements u0:t−1 and perceptions of the u0 u1 ut−1
environment z1:t . The mapping problem can be phrased
as recursive Bayesian filtering for estimating the robot
positions along with a map of the environment:
x0 x1 x2 ... xt

p(x1:t , m | z1:t , u0:t−1 ) = α · p(zt | xt , m)·

zt
Z
z1 z2
p(xt | xt−1 , ut−1 )p(x1:t−1 , m | z1:t−1 , u0:t−2 )dx1:t−1(1)
m
In probabilistic mapping and localization it is typically
assumed that the odometry measurements are governed by
a so-called probabilistic motion model p(xt | xt−1 , ut−1 ) Fig. 1. Graphical model of concurrent mapping and localization
which specifies the likelihood that the robot is at xt given as filtering process.
that it previously was at xt−1 and the motion ut−1 was
measured. On the other hand, the observations follow the
so-called observation model p(z | x), which defines for unknown area and simultaneously observes a high like-
every possible location x in the environment the likelihood lihood of its observations for potential positions that are
of the observation z. under consideration. If the registration of the vehicle in
Unfortunately, estimating the full posterior in Equa- its map can be done with high likelihood, the previous
tion 1 is not tractable in general. One popular approach is poses are corrected backwards in time according to the
to restrict observations to landmark detections, and repre- pose correction that is necessary to properly close the
sents robot positions by Gaussians [6]. In this context, loop. The position posterior is then replaced by a Dirac
the Bayes filter can be approximated efficiently by an distribution which has its mode at the most likely position
EKF for which the state consists of the robot positions when the robot closes the loop. Furthermore, subsequent
along with positions of the landmarks. Other researchers backwards corrections are stopped when the robot reaches
attempted to overcome the restrictions to landmark ob- this node. Whereas this approach can reliably close even
servations by using laser range-finders and incremental large loops, it has the disadvantage that it under-estimates
scan matching [2], [18], [22]. The general idea of these the uncertainty in the robot pose when closing loops.
approaches can be summarized as follows (see also [19]). More recently, Murphy and colleagues [17], [8] have
At any point t−1 in time, the robot is given an estimate of presented Rao-Blackwellized particle filters as an efficient
its pose x̂t−1 and a map m̂(x̂1:t−1 , z1:t−1 ). After the robot way to represent the full posterior of the robot pose.
moved further on and after taking a new measurement zt , Figure 1 depicts a graphical model of Rao-Blackwellized
the robot determines the most likely new pose x̂t such simultaneous mapping and localization. The key idea of
that this approach is to solve the recursive Bayes filter update
by the following equation:
x̂t = argmax{p(zt | xt , m̂(x̂1:t−1 , z1:t−1 ))
xt p(x1:t , m | z1:t , u0:t−1 ) =
·p(xt | ut−1 , x̂t−1 )}. (2) p(m | x1:t , z1:t , u0:t−1 )p(x1:t | z1:t , u0:t−1 ) (3)
It does this by trading off the consistency of the measure- Here, a particle filter is used to represent robot trajec-
ment with the map (first term on the right-hand side in tories x1:t and a different map is conditioned on each
(2)) and the consistency of the new pose with the control sample of the particle filter. The importance weights of
action and the previous pose (second term on the right- the samples are computed according to the likelihoods
hand side in (2)). The map is then extended by the new of the observations in the maximum likelihood map con-
measurement zt , using the pose x̂t as the pose at which structed using exactly the positions this particular particle
this measurement was taken. has taken. The key advantage of this approach is that
Whereas this approach has the advantage that it yields the samples approximate at every point in time the full
accurate results and can be implemented efficiently if the posterior over robot poses and maps. The first successful
registration is performed with respect to a global map or realization on real robots of an extended version of this
with respect to only a fixed number of scans. Its major technique has been presented by Montemerlo et al. [15].
disadvantage lies in the greedy maximization step. When However, particle filters are known to be subject to
the robot has to close larger loops, this approach suffers major approximation errors. One of these errors is known
from registration errors during loop closures and therefore as the particle depletion problem [21]. This problem
tends to fail in large environments. To overcome this can lead to a divergence of the filter and can result in
problem, extensions of this approach have been developed the lack of particles in the vicinity of the correct state.
which maintain a posterior about the position of the In the SLAM context this can prevent the robot from
vehicle [9], [19]. The key idea of these techniques is closing a given loop. There are two parameters that have
to delay the maximization until the robot detects that a a major influence on the approximation error. First, the
loop has been closed. This is usually done by identifying number of particles needs to be high enough to represent
that the robot enters an already known area from an the posterior. However, too many samples can prevent
 200 200
measured pose β’ 150 150

β 100
50
100
50
0 0
d’ -50 -50
initial pose d -100 -100
final pose 0 100 200 300 400 500 600 0 100 200 300 400 500 600

α α’ Movement in cm Movement in cm

path Fig. 3. Sample densities obtained with the models for the raw
odometry (left image) and for scan matching (right image) for
ten incremental movements of a real robot.
Fig. 2. Parameters of the probabilistic motion model.

errors after convergence of scan matching. Using a data set

the filtering process from being fast enough for online- recorded in the Intel Research Lab Seattle, we applied our
processing. Furthermore, the number of resampling steps system equipped with a manually designed motion model.
needs to be limited in order to avoid that the samples We then took the resulting map (see Figure 9) as ground
converge too quickly to the maximum likelihood state truth and compared the raw odometry and the results of
which is undesirable especially in ambiguous situations. the scan matching with the positions corrected by the
On the other side, too few resampling steps could result in routine. The error model we use has three parameters as
a divergence of the filter since many samples are wasted it assumes that in every single movement there are three
on unlikely states and the uncertainty typically introduced errors involved (see also Figure 2). First, whenever the
by robot motions would exceed the certainty gained by robot starts to move, it makes a small rotational error
incorporating observations of the environment. α0 − α. Second, the robot introduces a certain error d0 − d
In the following section we will describe our solution to the distance between the final location and the starting
to this problem. This approach transforms sequences of position. Finally, the true final orientation differs by a
laser measurements into odometry measurements using a certain amount from the measured orientation which is
scan matching procedure and utilizes the remaining laser expressed by a non-zero difference β − β 0 . The means
scans for map estimation. and the variances of the relative errors in these three
parameters were learned by comparing the approximated
III. Combining Laser-based FastSLAM with displacement after convergence of the scan matching
Scan Matching routine with the (estimated) ground truth information.
Alternative models of odometry errors and corresponding
Our approach to reduce the problems described above is
techniques for parameter estimation have been proposed
to use a scan matching routine to correct the odometry
by Borenstein and Feng [3], Doh et al. [7], as well as
and to use this corrected path information as input for the
Bengtsson and Baerveldt [1].
sampling step in the Rao-Blackwellized particle filter.
The 2d scan matching we apply, which is described Figure 3 plots the resulting sample densities obtained
in detail in [10], aligns a scan relative to the previous when relying on pure odometry (left image) and the
scans by computing an occupancy grid map [16] from the densities obtain with the error model for the scan matching
previous measurements. To avoid the time consuming ray- process (right image). As the figure shows, the samples are
tracing operation required to compute the likelihood of a much more focused if the scan matching routine is used.
measurement p(z | x) we apply an approximation which This leads to the desired effect that the variance of the
considers only the endpoint of a beam [11], [19]. This way, posterior is reduced, that fewer samples can be used, and
p(z | x) can be computed efficiently using fast look-up that larger loops can be closed.
operations. To also be able to incorporate maximum range A graphical model of our approach to integrate re-
measurements, our system assumes that the cell 20cm in sults from the scan matching process into the Rao-
front of that in which the maximum range measurement Blackwellized sampling routine is depicted in Figure 4
ends must be unoccupied. (c.f. 1). The key idea is to compute every k steps a
A key question when combining a scan matching rou- new odometry measurement u0j out of the k − 1 previous
tine with a probabilistic technique is how to estimate the observations z and the k most recent odometry readings.
uncertainty of the scan matching process so as to cor- The k-th observation is then used to compute the weights
rectly incorporate the corresponding uncertainty during the of the samples in the particle filter. Note that this clear
prediction step of the sampling procedure. In our current separation between laser scans used for odometry and
system we use a parametric model of the odometry error laser scans used for map estimation ensures that all
and learn the parameters of this model using data acquired information is used only once.
during experiments. To learn the parameters of the model One important aspect when using Rao-Blackwellized
used for the experiments described here we performed an particle filters for mapping is the efficient update of the
experiment in which we generated a statistics of alignment maps of the individual particles. Montemerlo et al. [15]
 u0 ... uk−1 uk ... u2k−1 un·k ... u(n+1)·k−1
z 1 ... z k−1 z k+1... z 2k−1 ... z n·k+1 ... z(n+1)·k−1

u’1 u’2 ... u’n

x0 xk x2k ... xn·k

zk z 2k ... z n·k

Fig. 4. Graphical model of the integration of scan matching

Fig. 5. Mapping of the Intel Research Lab with the raw
and probabilistic mapping.
odometry data.

proposed a tree-structure to efficiently update the map. In

the system described here, we only use a limited number
of scans gathered by the robot to update the map of
a particle. The scans chosen need to intersect with the
area visible according to the pose of the corresponding
particle. This way, the update of the map of every particle
can be achieved in constant time. Although this is an
approximation only, we never found any evidence that the
quality of the resulting maps was decreased significantly.

IV. Experimental Results Fig. 6. Map of the Intel Research Lab after scan matching (left)
and obtained in real-time with 100 samples (right).
The approach described above has been implemented and
tested using different robotic platforms and in different
environments as well as in extensive simulation runs. In 500 particles. Whereas this map is more accurate and
all experiments, we found out that the system can operate has a similar crispness as the scan matching map, the
online and can also robustly close large and nested loops. time to compute this map was several hours. Figure 7
visualizes the trajectories of all samples shortly before
A. Mapping Large-Scale Environments with and after closing the major loop in this data set. As the
Multiple Cycles left image illustrates, the robot is quite uncertain about its
position relative to the starting position upon its return.
The first experiment was carried out using a Pioneer 2
However, after a few resampling steps the uncertainty has
robot equipped with a SICK LMS laser range-finder in
been reduced drastically (right image).
the Intel Research Lab, Seattle, WA. The size of this
environment is 28m × 28m. The robot traveled 491m A second example map obtained with our approach is
with an average speed of 0.19m/s. Figure 5 shows the map depicted in Figure 8. The map shows the fourth floor
generated based on the raw odometry data provided by the of the 50m × 12m large Sieg Hall of the University of
robot. As can be seen from the figure, the robot suffers Washington. As can be seen from the figure, the robot
from serious errors in odometry so that the resulting went several times around the circle and still successfully
map is useless without any correction. Figure 6 (left) learned a consistent map. This map was generated in real-
shows the map created with our scan matching technique. time using 100 samples. The grid resolution was 10cm.
Although local structures of the map appear to be very
accurate, the map is globally inconsistent. For example
many structures like walls, doors etc. can be found twice
and with a small offset between them. Finally, the right
image of Figure 6 shows the resulting map obtained with
our system. Although the sharpness of this map is not as
high as that of the map created only with scan matching,
they are globally consistent. The map was created in real-
time, i.e. the computation time needed to process the data
did not exceeded the time to record them. We used 100
samples, a number we found to yield satisfactory results Fig. 7. Trajectories of all 100 samples shortly before (left) and
on all data sets. Figure 9 shows a map created using after (right) closing the loop.
Fig. 8. Map of the Sieg Hall at the University of Washington
created in real-time.

B. Comparison to Previous Online Techniques

The second experiment is designed to show the advantage
of our integrated technique over previous approaches that
represent a posterior over poses in a single map only [19],
[9]. For this experiment we used a data set generated for
the Wean Hall of the Carnegie Mellon University using
our B21r simulator. The size of this environment is 32m Fig. 9. Map of the Intel Research Lab obtained offline with
× 10m. In the simulation the robot moved 251m with 500 samples.
an average speed of 0.78m/s. To obtain realistic data, we
added a serious amount of noise to the ground truth data
provided by the simulation system. The resulting input
trajectory is depicted in Figure 10 (left). Please note,
that pure scan matching again failed to correctly close
the loop using this data set. We implemented the particle
filter strategy presented by Thrun et al. [20], [19]. In our
system we achieve this by using only that sample with the
highest likelihood during the resampling process whenever
the robot closes a loop. After this, we continue with the
normal resampling procedure described above. The middle
image in Figure 10 shows the result of this procedure. The
point in time when the system discovered that it closed
the loop and the resulting inconsistencies are also labeled.
Fig. 11. Map created with the standard Rao-Blackwellized
The inconsistencies are a consequence of the fact that only
particle filtering technique in real-time using 100 samples and
the particle with the highest importance factor survives at based on the raw odometry data.
the time the robot closes the loop. Since this particle does
not always correspond to the correct position of the robot could not observe a case in which the standard algorithm
(as it is the case in this example) and since the motion converged. An example map typically obtained using the
model cannot compensate for this error, the resulting standard algorithm is depicted in Figure 11. The fact
map contains errors. In contrast to that, our approach, that our algorithm reliably converges with 100 samples
that integrates scan matching with a Rao-Blackwellized indicates that the integration of the scan matching routine
particle filter, yields a consistent map of the environment yields an enormous improvement.
(see right image of Figure 10) since it provides accurate
predictions and simultaneously maintains the robot pose
uncertainty in the posterior. V. Conclusions
Finally, we analyzed whether the standard Rao- In this paper we presented a highly efficient algorithm
Blackwellized particle filter without odometry correction for simultaneous mapping and localization using laser
by scan matching provides the same performance as our scans that combines a scan matching procedure with Rao-
approach. For this purpose we run the standard procedure Blackwellized particle filtering. The scan matching routine
using the input data for the Intel Research Lab. We used is used to transform sequences of laser measurements
shorter resampling steps (three times more often than into odometry measurements. The corrected odometry
in the other runs to avoid a fast divergence) and 200 and the remaining laser scans are then used for map
samples which was the maximum number of samples estimation in the particle filter. The lower variance in
that allowed updates in real-time on our 1.8GHz Pentium the corrected odometry reduces the number of necessary
IV PC. Since the standard procedure was not able to resampling steps and this way decreases the particle deple-
learn a consistent map, we repeated this experiment with tion problem. In practical experiments we demonstrated
increasing numbers of samples. It turned out that under that our approach allows to learn maps of large-scale
1000 samples, which was the maximum number our PC environments in real-time with as few as 100 samples.
equipped with 768MB of main memory could handle, we Simultaneously, it outperforms previous approaches with
 END END

END

Inconsistencies

loop closure

START
START
START

View publication stats

Fig. 10. (left) Map obtained from the raw input data in the simulation experiment. In the middle, the resulting map is shown if the
mapping process is continued with the maximum likelihood sample after closing the loop at the marked place. The inconsistencies in
the right part of the map show that the loop is not correctly closed. The map on the right was built using our approach.

respect to robustness and efficiency. [10] D. Hähnel, D. Schulz, and W. Burgard. Mapping with
mobile robots in populated environments. In Proc. of the
Acknowledgments IEEE/RSJ International Conference on Intelligent Robots
This work has partly been supported by the EC un- and Systems (IROS), 2002.
der contract number IST-2000-29456 and by the Ger- [11] K. Konolige. Markov localization using correlation. In
man Science Foundation (DFG) under contract number Proc. of the International Joint Conference on Artificial
SFB/TR8-03. It has also been sponsored by DARPA’s Intelligence (IJCAI), 1999.
MARS, CoABS, MICA, and SDR Programme (contract [12] J.J. Leonard and H.J.S. Feder. A computationally efficient
numbers N66001-01-C-6018,NBCH1020014, F30602-98- method for large-scale concurrent mapping and localiza-
2-0137, F30602-01-C-0219, and NBCHC020073) and by tion. In Proc. of the Ninth Int. Symp. on Robotics Research
(ISRR), 1999.
the NSF under grant numbers IIS-9876136, IIS-9877033,
and IIS-0093406. [13] F. Lu and E. Milios. Globally consistent range scan
alignment for environment mapping. Autonomous Robots,
4:333–349, 1997.
VI. REFERENCES [14] M. Montemerlo and S. Thrun. Simultaneous localization
[1] O. Bengtsson and A. Baerveldt. Localization in changing and mapping with unknown data association using Fast-
environment - estimation of a covariance matrix for the SLAM. In Proc. of the IEEE International Conference on
idc algorithm. In Proc. of the IEEE/RSJ International Robotics & Automation (ICRA), 2003.
Conference on Intelligent Robots and Systems (IROS), [15] M. Montemerlo, S. Thun, D. Koller, and B. Wegbreit.
pages 1931–1937, 2001. FastSLAM: A factored solution to simultaneous mapping
[2] P. Besl and N. McKay. A method for registration of 3d and localization. In Proc. of the National Conference on
shapes. Trans. Patt. Anal. Mach. Intell. 14(2), pages 239– Artificial Intelligence (AAAI), 2002.
256, 1992. [16] H.P. Moravec. Sensor fusion in certainty grids for mobile
[3] J. Borenstein and Feng. L. Measurement and correction of robots. AI Magazine, pages 61–74, Summer 1988.
systematic odometry errors in mobile robots. IEEE Journal [17] K. Murphy. Bayesian map learning in dynamic environ-
of Robotics and Automation, 12(6):869–880, 1996. ments. In Neural Info. Proc. Systems (NIPS), 1999.
[4] M. Bosse, P. Newman, M. Soika, W. Feiten, J. Leonard, [18] T. Röfer. Using histogram correlation to create consistent
and S. Teller. An atlas framework for scalable mapping. laser scan maps. In Proc. of the IEEE/RSJ International
In Proceedings of the IEEE International Conference on Conference on Intelligent Robots and Systems (IROS),
Robotics and Automation (ICRA), 2003. 2002.
[5] J.A. Castellanos, J.M.M. Montiel, J. Neira, and J.D. Tardós. [19] S. Thrun. A probabilistic online mapping algorithm for
The SPmap: A probabilistic framework for simultaneous teams of mobile robots. International Journal of Robotics
localization and map building. IEEE Transactions on Research, 20(5):335–363, 2001.
Robotics and Automation, 15(5):948–953, 1999.
[20] S. Thrun, W. Burgard, and D. Fox. A real-time algorithm
[6] G. Dissanayake, H. Durrant-Whyte, and T. Bailey. A com- for mobile robot mapping with applications to multi-robot
putationally efficient solution to the simultaneous locali- and 3D mapping. In Proc. of the IEEE International
sation and map building (SLAM) problem. In ICRA’2000 Conference on Robotics & Automation (ICRA), 2000.
Workshop on Mobile Robot Navigation and Mapping, 2000.
[21] R. van der Merwe, A. Doucet, N. de Freitas, and E. Wan.
[7] N. Doh, H. Choset, and W. K. Chung. Accurate relative The unscented particle filter. Technical Report CUED/F-
localization using odometry. In Proc. of the IEEE Inter- INFENG/TR 380, Cambridge University, Department of
national Conference on Robotics & Automation (ICRA), Engineering, 2000.
2003.
[22] G. Weiß, C. Wetzler, and E. von Puttkamer. Keeping track
[8] A. Doucet, J.F.G. de Freitas, K. Murphy, and S. Russell.
of position and orientation of moving indoor systems by
Rao-Blackwellised particle filtering for dynamic Bayesian
correlation of range-finder scans. In Proc. of the IEEE/RSJ
networks. In Proc. of the Conference on Uncertainty in
International Conference on Intelligent Robots and Systems
Artificial Intelligence (UAI), 2000.
(IROS), pages 595–601, 1994.
[9] J.-S. Gutmann and K. Konolige. Incremental mapping
of large cyclic environments. In Proc. of the IEEE
Int. Symp. on Computational Intelligence in Robotics and
Automation (CIRA), 1999.