Probabilistic Mapping - PowerPoint PPT Presentation

1 / 90
About This Presentation
Title:

Probabilistic Mapping

Description:

Efficient Approaches to Mapping with Rao-Blackwellized Particle Filters Wolfram Burgard Department of Computer Science University of Freiburg, Germany – PowerPoint PPT presentation

Number of Views:95
Avg rating:3.0/5.0
Slides: 91
Provided by: Wolfr4
Category:

less

Transcript and Presenter's Notes

Title: Probabilistic Mapping


1
Efficient Approaches to Mapping with
Rao-Blackwellized Particle Filters
Wolfram Burgard
Department of Computer Science University of
Freiburg, Germany
Special thanks to Austin Eliazar, Giorgio
Grisetti, Dirk Hähnel, Mike Montemerlo, Ronald
Parr, Cyrill Stachniss,
2
Dimensions of Robot Mapping
SLAM
localization
mapping
integrated approaches
active localization
exploration
motion control
Makarenko et al., 02
3
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
4
Why is SLAM Hard Ambiguity
End
Start
Courtesy of Eliazar Parr
5
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?

6
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 (mxy) is
    independent
  • Robot positions are known!

7
Updating Occupancy Grid Maps
  • Update the map cells using the inverse sensor
    model
  • Or use the log-odds representation

8
Typical Sensor Model for Occupancy Grid Maps
  • Combination of a linear function and a Gaussian

9
Key Parameters of the Model
10
Occupancy Value Depending on the Measured Distance
11
Deviation from the Prior Belief(the sphere of
influence of the sensors)
12
Calculating the Occupancy Probability Based on
Single Observations
13
Incremental Updating of Occupancy Grids
(Example)
14
Resulting Map Obtained with Ultrasound Sensors
15
Mapping with Raw Odometry
16
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)

17
Scan Matching
  • Maximize the likelihood of the i-th pose and map
    relative to the (i-1)-th pose and map.

18
Scan Matching Example
19
Key Problems
  • How to maintain multiple map and pose hypotheses
    during mapping?
  • How to control the robot?

20
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
21
Rao-Blackwellized Mapping (2)
Compute a posterior over the map and possible
trajectories of the robot
map and trajectory
measurements
robot motion
map
trajectory
22
A Graphical Model of Rao-Blackwellized Mapping
23
FastSLAM
Robot Pose
2 x 2 Kalman Filters

Particle M
Begin courtesy of Mike Montemerlo
24
FastSLAM
25
FastSLAM Simulation
  • Up to 100,000 landmarks
  • 100 particles
  • 103 times fewer parameters than EKF SLAM

Blue line true robot path Red line estimated
robot path Black dashed line odometry
26
Victoria Park Results
  • 4 km traverse
  • 100 particles
  • Uses negative evidence to remove spurious
    landmarks

Blue path odometry Red path estimated path
End courtesy of Mike Montemerlo
27
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?

28
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(xu,x)).
  • Compute importance weight according to the
    likelihood of the observation z given the pose x
    and the map m of the particle.

29
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
30
Mixture Approximation of p(z d)
Choset et al., to appear, Thrun et al., to
appear
31
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
    mxy (correlation model).Konolige, 99
  • Smoothing of Bel(mxy) yields a better gradient
    and improves the robustness.Thrun, 01
    (likelihood fields).

32
RPBF with Grid Maps
3 particles
33
Map Maintenance Challenges
  • High resolution maps are big
  • Typically 100s or 1000s of particles are needed
  • One full map per particle requires
  • O(mn) work (re-sampling)
  • Gigabytes of memory movement
  • Anecdotal reports Tried, but impractical(see
    later)

Begin courtesy of Eliazar Parr
34
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

35
Ancestry Trees
36
Ancestry Trees
37
Ancestry Trees
38
Ancestry Trees
39
Ancestry Trees
40
Ancestry Trees
41
Ancestry Trees
Ancestors with no children can be removed
42
Ancestry Trees
Ancestors with only one child can be merged
43
Ancestry Trees
44
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

45
Map Representation
  • Map is an occupancy grid
  • Avoid one map per particle

Naïve Map Representation
46
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

47
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

48
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 EliazarParr)

A Area observed n Number of
particles m Map size
49
Complexity Summary
  • Total Time O(An2)
  • Compare to O(mn)
  • m gtgt An
  • Linear in observation size
  • Independent of map size

A Area observed n Number of
particles m Map size
50
DP-SLAM Results
scale 3cm
Run at real-time speed on 2.4GHz Pentium 4 at
10cm/s
51
Consistency
52
Results obtained with DP-SLAM 2.0 (offline)
Eliazar Parr, 04
53
Close up
End courtesy of Eliazar Parr
54
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?

55
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).

56
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.

57
Motion Model for Scan Matching
Raw Odometry
Scan Matching
58
Graphical Model for Mapping with Improved Odometry
59
Rao-Blackwellized Mapping with Scan-Matching
Map Intel Research Lab Seattle
Loop Closure
60
RBPF Mapping with Scan-Matching
Map Intel Research Lab Seattle
Loop Closure
61
Rao-Blackwellized Mapping with Scan-Matching
Map Intel Research Lab Seattle
62
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

63
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

64
Dynamically Adapting the Motion Model
  • The previous approach used a constant motion
    model p(xu, 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

65
The Optimal Proposal Distribution
Arulampalam et al., 01
For lasers is extremely
peaked and dominates the product.
66
Resulting Proposal Distribution
Gaussian approximation
67
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

68
Computing the Importance Weight
69
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?

70
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

71
Typical Evolution of neff
72
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
73
Outdoor Campus Map
  • 30 particles
  • 250x250m2
  • 1.75 km (odometry)
  • 20cm resolution during scan matching
  • 30cm resolution in final map
  • 30 particles
  • 250x250m2
  • 1.088 miles (odometry)
  • 20cm resolution during scan matching
  • 30cm resolution in final map

Courtesy by Giorgio Grisetti Cyrill Stachniss
74
Exploration
  • The approaches seen so far are purely passive.
  • By reasoning about control, the mapping process
    can be made much more effective.

75
Where to Move Next?
76
Combining Rao-Blackwellized Mapping with
Exploration
77
Exploration
  • Given
  • Unknown environment.
  • Question
  • How to control the robot so that it efficiently
    learns a map.

78
Decision-Theoretic Formulation of Exploration
cost (path length)
reward (expected information gain)
79
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.

80
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.

81
Application Example
Path estimated by the particle filter
True map and trajectory
82
Map and Pose Uncertainty
pose uncertainty
map uncertainty
83
Goal
  • Integrated approach that considers
  • exploratory actions,
  • place revisiting actions, and
  • loop closing actions
  • to control the robot.

84
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.

85
Dual Representation for Loop Detection
86
Application Example
87
Real Exploration Example
88
Corridor Exploration
89
Comparison
Map uncertainty only
Map and pose uncertainty
90
Example Entropy Evolution
91
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.

92
Potential Projects
  • Dynamic environments
  • Detection of errors
  • Recovery from errors
  • Three-dimensional maps
  • Objects in maps
  • Adaptive models (motion, sensor, )
Write a Comment
User Comments (0)
About PowerShow.com