Efficient Approaches to Mapping with Rao-Blackwellized Particle Filters

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. Same position 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 (m[xy]) is independent • Robot positions are known!

7. Updating Occupancy Grid Maps • Update the map cells using theinverse 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. z+d1 z+d2 z z+d3 z-d1 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. current measurement robot motion map constructed so far 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 map robot motion trajectory

22. u u u 0 1 t-1 ... x x x x 0 1 2 t m z z z 1 2 t A Graphical Model of Rao-Blackwellized Mapping

23. Particle #1 x, y,  x, y,  x, y,  x, y,  Landmark 1 Landmark 1 Landmark 1 Landmark 1 Landmark 2 Landmark 2 Landmark 2 Landmark 2 Landmark N Landmark N Landmark N Landmark N … … … … Particle #2 Particle #3 FastSLAM Robot Pose 2 x 2 Kalman Filters … Particle M [Begin courtesy of Mike Montemerlo]

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

25. 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]

26. Key Questions • Can we apply Rao-Blackwellized particle filters to mapping with largegrid-maps? • How can we compactly represent theindividual maps carried by the particles? • How can we reduce the number of particles needed?

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

28. Computing the Likelihood of a Measurement: Ray Casting • Determine the distance to the closest obstacle in the direction of the measurement (ray-casting). • 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]

29. Mixture Approximation of p(z | d) [Choset et al., to appear, Thrun et al., to appear]

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

31. RPBF with Grid Maps map of particle 3 map of particle 1 map of particle 2 3 particles

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

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

34. Ancestry Trees

35. Ancestry Trees

36. Ancestry Trees

37. Ancestry Trees

38. Ancestry Trees

39. Ancestry Trees

40. Ancestry Trees Ancestors with no children can be removed

41. Ancestry Trees Ancestors with only one child can be merged

42. Ancestry Trees

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

44. Map Representation • Map is an occupancy grid • Avoid one map per particle Naïve Map Representation

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

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

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

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

49. DP-SLAM Results scale: 3cm Run at real-time speed on 2.4GHz Pentium 4 at 10cm/s

50. Consistency