RBPF Slam Tutorial 2007 PDF
RBPF Slam Tutorial 2007 PDF
RBPF Slam Tutorial 2007 PDF
mapping
localization
path planning
What is SLAM ?
Estimate the pose and the map of a mobile robot at the same time
poses map
[video]
Skip Explanation
[video]
Courtesy of Dieter Fox
Sample-based Posteriors
State hypothesis
Importance weight
Posterior Approximation
The more particles fall into an interval, the higher the probability of that interval How to draw samples form a function/distribution?
Rejection Sampling
Let us assume that f(x)<1 for all x Sample x from a uniform distribution Sample c from [0,1] if f(x) > c otherwise keep the sample reject the sample
c f(x) x x
f(x) c
OK
Three sequential steps: 1. Sampling from the proposal distribution (Bayes filter: prediction step) 2. Compute the particle weight (importance sampling) (Bayes filter: correction step) 3. Resampling
Monte-Carlo Localization For each motion do: Sampling: Generate from each sample in
a new sample according to the motion model
For each observation do: Weight the samples with the observation
likelihood
Resampling
[video]
Courtesy of Dieter Fox
Grids Maps
Grid maps are a discretization of the environment into free and occupied cells Mapping with known robot poses is easy.
a map is needed to localize the robot and a pose estimate is needed to build a map
[video]
Particle filters have successfully been applied to localization, can we use them to solve the SLAM problem?
Observations:
The map depends on the poses of the robot during data acquisition If the poses are known, mapping is easy
Rao-Blackwellization
This is localization, use MCL Use the pose estimate from the MCL and apply mapping with known poses
Example
3 particles
map of particle 1
map of particle 3
map of particle 2
...
xt
z1
z2
zt
Problems in Practice
Each map is quite big in case of grid maps Since each particle maintains its own map Therefore, one needs to keep the number of particles small Solution: Compute better proposal distributions Idea: Improve the pose estimate before applying the particle filter
Pose Correction Using Scan Matching Maximize the likelihood of the i-th pose relative to the (i-1)-th pose
current measurement
robot motion
...
u'2
... ...
u'n
x0
xk
x2k
x nk
zk
z2k
...
z nk
The presented approach is efficient It is easy to implement Scan matching is used to transform sequences of laser measurements into odometry measurements Provides good results for most datasets
Whats Next?
n n
Further reduce the number of particles Improved proposals will lead to more accurate maps Use the properties of our sensor when drawing the next generation of particles
[Doucet, 98]
For lasers is extremely peaked and dominates the product. We can safely approximate by a constant:
is a normalizer
Improved Proposal
End of a corridor:
Corridor:
Free space:
Resampling
n
In case of suboptimal/bad proposal distributions resampling is necessary to achieve convergence Resampling is dangerous, since important samples might get lost (particle depletion problem)
t-3
t-2
t-1
When to Resample?
t-3
n n
t-2
t-1
Key question: When should we resample? Resampling makes only sense if the samples have significantly different weights
Neff Neff
is maximal for equal weights. In this case, the distribution is close to the proposal
If our approximation is close to the proposal, no resampling is needed We only resample when Neff drops below a given threshold (N/2) See [Doucet, 98; Arulampalam, 01]
[video]
[video]
[video]
Gaussians are uni-model distributions In case of loop-closures, the likelihood function might be multi-modal
mode 1
odometry
Sample from odometry first and the use this as the start point for scan matching
Final Approach
n
Conclusion
n
Rao-Blackwellized Particle Filters are means to represent a joint posterior about the poses of the robot and the map Utilizing accurate sensor observation leads to good proposals and highly efficient filters It is similar to scan-matching on a per-particle base with some extra noise The number of necessary particles and re-sampling steps can seriously be reduced How to deal with non-Gaussian observation likelihood functions Highly accurate and large scale map
More Details
n
M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A factored solution to simultaneous localization and mapping, AAAI02 (The classic FastSLAM paper with landmarks) M. Montemerlo, S. Thrun D. Koller, and B. Wegbreit. FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges, IJCAI03. (FastSLAM 2.0 improved proposal for FastSLAM) D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efcient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS03 (FastSLAM on grid-maps using scan-matched input) A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultainous localization and mapping without predetermined landmarks, IJCAI03 (A representation to handle big particle sets)
Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard. Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters, Transactions on Robotics, Volume 23, pages 34-46, 2007 (Informed proposal using laser observation, adaptive resampling) G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling, ICRA05 (Informed proposal using laser observation, adaptive resampling) Cyrill Stachniss, Grisetti Giorgio, Wolfram Burgard, and Nicholas Roy. Analyzing Gaussian Proposal Distributions for Mapping with Rao-Blackwellized Particle Filters, IROS07 (Gaussian assumption for computing the improved proposal)
Implementation available a open source project GMapping on www.OpenSLAM.org Written in C++ Can be used as a black box library Now: 1h Practical Course on GMapping
n n