efficient approaches to mapping with rao- blackwellized particle filters department of computer...
TRANSCRIPT
![Page 1: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/1.jpg)
Efficient Approaches to Mapping with Rao-
Blackwellized Particle Filters
Department of Computer ScienceUniversity of Freiburg, Germany
Wolfram Burgard
Special thanks to: Austin Eliazar, Giorgio Grisetti, Dirk Hähnel, Mike Montemerlo, Ronald Parr, Cyrill Stachniss, …
![Page 2: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/2.jpg)
Dimensions of Robot Mapping
[Makarenko et al., 02]
mapping
motion control
localizationSLAM
active localization
exploration
integrated approaches
![Page 3: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/3.jpg)
Types of SLAM-Problems
Grid maps or scans
[Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01;…]
Landmark-based
[Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…
![Page 4: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/4.jpg)
Why is SLAM Hard: Ambiguity
Start
End
Same position
[Courtesy of Eliazar & Parr]
![Page 5: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/5.jpg)
Properties of Standard EKF-SLAM
Requires pre-defined landmarks/features
Complexity O(n2) where n is the number of landmarks
Data association problem
How can we solve the SLAM problem for not feature-based representations?
![Page 6: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/6.jpg)
Occupancy Grid Maps
Introduced by Moravec and Elfes in 1985 Represent environment by a grid. Estimate the probability that a location is
occupied by an obstacle. Key assumptions
Occupancy of individual cells (m[xy]) is independent
Robot positions are known!
yx
xyt
tttt
mBel
zuzumPmBel
,
][
110
)(
),,,|()(
![Page 7: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/7.jpg)
Updating Occupancy Grid Maps
Update the map cells using the inverse sensor model
Or use the log-odds representation
1
][1
][1
][
][
1][
1][
][
1
1
,|1
,|11
xy
t
xyt
xyt
xyt
ttxy
t
ttxy
txyt mBel
mBel
mP
mP
uzmP
uzmPmBel
1][][ ,|log tt
xyt
xyt uzmoddsmB )(log: ][][ xy
txy
t moddsmB
xP
xPxodds
1:)(
][log xytmodds
][1
xytmB
![Page 8: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/8.jpg)
Typical Sensor Model for Occupancy Grid Maps
Combination of a linear function and a Gaussian:
![Page 9: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/9.jpg)
Key Parameters of the Model
![Page 10: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/10.jpg)
z+d1 z+d2
z+d3z
z-d1
Occupancy Value Depending on the Measured Distance
![Page 11: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/11.jpg)
Deviation from the Prior Belief(the sphere of influence of the sensors)
![Page 12: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/12.jpg)
Calculating the Occupancy Probability Based on Single Observations
![Page 13: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/13.jpg)
Incremental Updating of Occupancy Grids (Example)
![Page 14: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/14.jpg)
Resulting Map Obtained with Ultrasound Sensors
![Page 15: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/15.jpg)
Mapping with Raw Odometry
![Page 16: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/16.jpg)
Techniques for Generating Consistent Maps
Scan matching (online) Probabilistic mapping with a single map and
a posterior about poses Mapping + Localization (online)
EKF SLAM (online, mostly landmarks or features only)
EM techniques (offline) Lu and Milios (offline) Rao-Blackwellized particle filters
(landmarks and grids)
![Page 17: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/17.jpg)
Scan Matching
Maximize the likelihood of the i-th pose and map relative to the (i-1)-th pose and map.
)ˆ,|( )ˆ ,|( maxargˆ 11]1[
ttt
ttt
xt xuxpmxzpx
t
robot motioncurrent measurement
map constructed so far
![Page 18: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/18.jpg)
Scan Matching Example
![Page 19: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/19.jpg)
Key Problems
How to maintain multiple map and pose hypotheses during mapping?
How to control the robot?
![Page 20: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/20.jpg)
Rao-Blackwellized Mapping
Observation: Given the true trajectory of the robot, all measurements are independent.
Idea: Use a particle filter to represent potential
trajectories of the robot (multiple hypotheses). For each particle we can analytically compute
the map of the environment (mapping with known poses).
Each particle survives with a probability that is proportional to the likelihood of the observation given that particle and its map.
[Murphy et al., 99]
![Page 21: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/21.jpg)
Rao-Blackwellized Mapping (2)
),|,( 1:0:1:1 ttt uzmxp
),|(),,|( 1:0:1:11:0:1:1 tttttt uzxpuzxmp
Compute a posterior over the map and possible trajectories of the robot :
robot motionmap trajectory
map and trajectory
measurements
![Page 22: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/22.jpg)
A Graphical Model of Rao-Blackwellized Mapping
m
x
z
u
x
z
u
2
2
x
z
u
... t
t
x 1
1
0
10 t-1
![Page 23: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/23.jpg)
FastSLAM
Robot Pose 2 x 2 Kalman Filters
Landmark 1 Landmark 2 Landmark N…x, y,
Landmark 1 Landmark 2 Landmark N…x, y, Particle#1
Landmark 1 Landmark 2 Landmark N…x, y, Particle#2
Landmark 1 Landmark 2 Landmark N…x, y, Particle#3
ParticleM
…
[Begin courtesy of Mike Montemerlo]
![Page 24: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/24.jpg)
FastSLAM – Simulation
Up to 100,000 landmarks
100 particles
103 times fewer parameters than EKF SLAM
Blue line = true robot pathRed line = estimated robot pathBlack dashed line = odometry
![Page 25: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/25.jpg)
Victoria Park Results
4 km traverse 100 particles Uses negative
evidence to remove spurious landmarks
Blue path = odometryRed path = estimated path
[End courtesy of Mike Montemerlo]
![Page 26: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/26.jpg)
Key Questions
Can we apply Rao-Blackwellized particle filters to mapping with large grid-maps?
How can we compactly represent the individual maps carried by the particles?
How can we reduce the number of particles needed?
![Page 27: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/27.jpg)
Tasks to be Solved
Mapping (occupancy grids) Each particle carries its own map m. The history of each particle represents a
potential trajectory of the robot. Localization
Propagate the particles according to the motion model (draw from p(x|u,x’)).
Compute importance weight according to the likelihood of the observation z given the pose x and the map m of the particle.
![Page 28: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/28.jpg)
Computing the Likelihood of a Measurement: Ray Casting
1. Determine the distance to the closest obstacle in the direction of the measurement (ray-casting).
2. Approximate the likelihood p(z | m, x) by the likelihood p(z | d) of z given the “expected measurement d for x.”
[Fox et al., 98]
![Page 29: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/29.jpg)
Mixture Approximation of p(z | d)
[Choset et al., to appear, Thrun et al., to appear]
![Page 30: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/30.jpg)
Computing the Likelihood of a Measurement: Correlation Models
Determine the cell [xy] a beam ends in. Approximate the likelihood p(z | m, x) by
the occupancy probability Bel(m [xy])
contained in m[xy] (correlation model).[Konolige, 99]
Smoothing of Bel(m[xy]) yields a better gradient and improves the robustness.[Thrun, 01] (likelihood fields).
![Page 31: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/31.jpg)
RPBF with Grid Maps
map of particle 1 map of particle 3
map of particle 2
3 particles
![Page 32: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/32.jpg)
Map Maintenance Challenges
High resolution maps are big Typically 100’s or 1000’s of particles are
needed One full map per particle requires
O(|m|·n) work (re-sampling) Gigabytes of memory movement
Anecdotal reports: Tried, but impractical(see later)
Begin courtesy of Eliazar & Parr
![Page 33: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/33.jpg)
DP-SLAM: Distributed Particle Mapping
Exploit sampling/re-sampling steps of PF Common ancestry = Redundant map sections
History representation: Ancestry Tree Leaves correspond to current particles
New map Representation Store multiple maps in a single grid
![Page 34: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/34.jpg)
Ancestry Trees
![Page 35: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/35.jpg)
Ancestry Trees
![Page 36: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/36.jpg)
Ancestry Trees
![Page 37: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/37.jpg)
Ancestry Trees
![Page 38: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/38.jpg)
Ancestry Trees
![Page 39: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/39.jpg)
Ancestry Trees
![Page 40: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/40.jpg)
Ancestry Trees
Ancestors with no children can be removed
![Page 41: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/41.jpg)
Ancestry Trees
Ancestors with only one child can be merged
![Page 42: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/42.jpg)
Ancestry Trees
![Page 43: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/43.jpg)
Ancestry Trees
Maintain a minimal tree (improves complexity) Exactly n leaves Branching factor at least 2 Depth no more than n
Explicitly store the ancestry info Node = Ancestor particle with unique ID Stores parent link, map updates
![Page 44: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/44.jpg)
Map Representation
Map is an occupancy grid Avoid one map per particle
Naïve Map Representation
![Page 45: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/45.jpg)
DP-Mapping
Distribute particles over a single map
Each grid square stores: ID of each ancestry node
that has seen this square Associated observations No redundant data No unnecessary data
![Page 46: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/46.jpg)
Localization
For each laser cast of the current particle Trace laser cast through grid For each grid square return map occupancy
Store observations as balanced trees (keyed on IDs)
Linear storage ancestry Logarithmic access/updates
![Page 47: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/47.jpg)
Complexity
Localization: O(An2) n particles check A grid
squares Worst case cost n to check
occupancy(harder than it sounds)
Map Maintenance: O(Anlogn) Additions, Deletions: O(Anlogn) Ancestry Tree Maintenance : O(Anlogn) Amortized analysis
(see papers by Eliazar&Parr)
A = Area observed
n = Number of particles
|m| = Map size
![Page 48: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/48.jpg)
Complexity Summary
Total Time : O(An2) Compare to O(|m|n) |m| >> An
Linear in observation size Independent of map size
A = Area observed
n = Number of particles
|m| = Map size
![Page 49: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/49.jpg)
DP-SLAM Results
Run at real-time speed on 2.4GHz Pentium 4 at 10cm/s
scale: 3cm
![Page 50: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/50.jpg)
Consistency
![Page 51: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/51.jpg)
Results obtained with DP-SLAM 2.0 (offline)
Eliazar & Parr, 04
![Page 52: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/52.jpg)
Close up
End courtesy of Eliazar & Parr
![Page 53: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/53.jpg)
Observations
DP-SLAM is an efficient and elegant way to store the individual maps assigned to the particles.
Complexity O(An2) where n is the number of particles
How can we reduce the number of particles?
![Page 54: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/54.jpg)
Techniques to Reduce the Number of Particles Needed
Better proposals (put the particles in the right place in the prediction step).
Avoid particle depletion (re-sample only when needed).
![Page 55: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/55.jpg)
Generating better Proposals
Use scan-matching to compute highly accurate odometry measurements from consecutive range scans.
Use the improved odometry in the prediction step to get highly accurate proposal distributions.
![Page 56: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/56.jpg)
Motion Model for Scan Matching
'
'
d'
final pose
d
measured pose
initial pose
path
Raw OdometryScan Matching
![Page 57: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/57.jpg)
Graphical Model for Mapping with Improved Odometry
m
z
kx
1u'
0uzk-1
...1z ...
uk-1 ...k+1z
ukzu2k-1
2k-1...
x0
k
x2k
z2k
...
u'2u' n
...
xn·k
zu u(n+1)·k-1n·k
n·k+1
...(n+1)·k-1z...
n·kz
...
...
![Page 58: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/58.jpg)
Rao-Blackwellized Mapping with Scan-Matching
Map:
Inte
l R
ese
arc
h L
ab
Seatt
le
Loop Closure
![Page 59: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/59.jpg)
RBPF Mapping with Scan-Matching
Map:
Inte
l R
ese
arc
h L
ab
Seatt
le
Loop Closure
![Page 60: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/60.jpg)
Rao-Blackwellized Mapping with Scan-Matching
Map:
Inte
l R
ese
arc
h L
ab
Seatt
le
![Page 61: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/61.jpg)
Comparison to Previous Techniques Standard Rao-Blackwellized mapping
with grid maps (Intel Research Lab data set)
Wean Hall (32m x 10m), noise added to odometry (simulation)
Scan Matching Single map plus posterior about
poses
![Page 62: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/62.jpg)
Comparison to the Original Approach• Same model for observations
• Odometry instead of scan matching results
• Number of particles varying from 500 to 2.000
• Typical result:
![Page 63: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/63.jpg)
Dynamically Adapting the Motion Model The previous approach used a
constant motion model p(x|u, x’).
It needs to be more peaked than the model for raw odometry.
Accordingly, it will fail in situations in which scan matching yields bad results (e.g., in wide open spaces)
Goal: better proposal distribution
![Page 64: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/64.jpg)
The Optimal Proposal Distribution
For lasers is extremely peaked and dominates the product.
[Arulampalam et al., 01]
We can safely approximate by a constant:
![Page 65: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/65.jpg)
Resulting Proposal Distribution
Gaussian approximation:
![Page 66: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/66.jpg)
Estimating the Parameters of the Gaussian for each Particle
xj are a set of sample points around the point x* the scan matching has converged to.
is a normalizing constant
![Page 67: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/67.jpg)
Computing the Importance Weight
![Page 68: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/68.jpg)
Selective Re-sampling
Re-sampling is dangerous, since important samples might get lost(particle depletion problem)
In case of suboptimal proposal distributions re-sampling is necessary to achieve convergence.
Key question: When should we re-sample?
![Page 69: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/69.jpg)
Number of Effective Particles
Empirical measure of how well the goal distribution is approximated by samples drawn from the proposal
We only re-sample when neff drops below a given threshold (n/2)
See [Doucet, ’98; Arulampalam, ’01]
![Page 70: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/70.jpg)
Typical Evolution of neff
visiting new areas closing the
first loop
second loop closure
visiting known areas
![Page 71: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/71.jpg)
Example (Intel Lab)
15 particles
four times faster than real-timeP4, 2.8GHz
5cm resolution during scan matching
1cm resolution in final map
Courtesy by Giorgio Grisetti & Cyrill Stachniss
![Page 72: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/72.jpg)
Outdoor Campus Map
30 particles
250x250m2
1.75 km (odometry)
20cm resolution during scan matching
30cm resolution in final map
Courtesy by Giorgio Grisetti & Cyrill Stachniss
30 particles
250x250m2
1.088 miles (odometry)
20cm resolution during scan matching
30cm resolution in final map
![Page 73: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/73.jpg)
The approaches seen so far are purely passive.
By reasoning about control, the mapping process can be made much more effective.
Exploration
![Page 74: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/74.jpg)
Where to Move Next?
![Page 75: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/75.jpg)
Combining Rao-Blackwellized Mapping with Exploration
mapping
motion control
localizationSLAM
active localization
exploration
![Page 76: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/76.jpg)
Decision-Theoretic Formulation of Exploration
reward (expected information gain)
cost (path length)
![Page 77: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/77.jpg)
Naïve Approach to Combine Exploration and Mapping
Learn the map using a Rao-Blackwellized particle filter.
Apply an exploration approach that minimizes the map uncertainty.
![Page 78: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/78.jpg)
Disadvantage of the Naïve Approach
Exploration techniques only consider the map uncertainty for generating controls.
They avoid re-visiting known areas.
Data association becomes harder.
More particles are needed to learn a correct map.
![Page 79: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/79.jpg)
Application Example
Path estimated by the particle filter
True map and trajectory
![Page 80: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/80.jpg)
Map and Pose Uncertainty
pose uncertainty map uncertainty
![Page 81: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/81.jpg)
Goal
Integrated approach that considers
exploratory actions, place revisiting actions, and loop closing actions
to control the robot.
![Page 82: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/82.jpg)
Dual Representation for Loop Detection
Trajectory graph stores the path traversed by the robot.
Grid map represents the space covered by the sensors.
Loops correspond to long paths in the trajectory graph and short paths in the geometric map.
![Page 83: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/83.jpg)
Dual Representation for Loop Detection
![Page 84: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/84.jpg)
Application Example
![Page 85: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/85.jpg)
Real Exploration Example
![Page 86: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/86.jpg)
Corridor Exploration
![Page 87: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/87.jpg)
Comparison
Map and pose uncertainty:
Map uncertainty only:
![Page 88: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/88.jpg)
Example: Entropy Evolution
![Page 89: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/89.jpg)
Summary Rao-Blackwellization is well-suited for
maintaining multiple hypotheses during occupancy grid mapping.
Grid-based approaches can be scaled to larger environments by
using appropriate data structures for the maps carried by the individual particles (DPSLAM), by
using improved motion models (better proposals), by
using adaptive re-sampling schemes, and by
actively controlling the actions of the robot.
![Page 90: Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard](https://reader035.vdocument.in/reader035/viewer/2022070409/56649e795503460f94b78891/html5/thumbnails/90.jpg)
Potential Projects
Dynamic environments Detection of errors Recovery from errors Three-dimensional maps Objects in maps Adaptive models (motion, sensor, …) …