1. Efficient Approaches to Mapping with Rao- Blackwellized Particle Filters Department of Computer Science University of Freiburg, Germany Wolfram Burgard Special thanks to: Austin Eliazar, Giorgio Grisetti, Dirk Hähnel, Mike Montemerlo, Ronald Parr, Cyrill Stachniss, …

2. Dimensions of Robot Mapping [Makarenko et al., 02] mapping motion control localization SLAM active localization exploration integrated approaches

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 Start End Same position [Courtesy of Eliazar & Parr]

5. Properties of Standard EKF- SLAM Requires pre-defined landmarks/features Complexity O(n 2 ) 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 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. z+d 1 z+d 2 z+d 3 z z-d 1 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. robot motioncurrent measurement map constructed so far

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 : robot motionmap trajectory map and trajectory measurements

22. 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 10t-1

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

24. FastSLAM

25. FastSLAM – Simulation Up to 100,000 landmarks 100 particles 10 3 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(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.

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 m [xy] (correlation model). [Konolige, 99] Smoothing of Bel(m [xy] ) yields a better gradient and improves the robustness. [Thrun, 01] (likelihood fields).

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

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

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

41. Ancestors with no children can be removed

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

43. Ancestry Trees

44. 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(An 2 ) 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

49. Complexity Summary Total Time : O(An 2 ) 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

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

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(An 2 ) 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 m z k x 1 u' 0 u z k-1... 1 z u k-1... k+1 z u k z u 2k-1... x 0 k x 2k z... u' 2 n... x n·k z uu (n+1)·k-1n·k n·k+1... (n+1)·k-1 z... n·k z...

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(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

65. The Optimal Proposal Distribution For lasers is extremely peaked and dominates the product. [Arulampalam et al., 01] We can safely approximate by a constant:

66. Resulting Proposal Distribution Gaussian approximation:

67. Estimating the Parameters of the Gaussian for each Particle  x j 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 n eff drops below a given threshold (n/2) See [Doucet, ’98; Arulampalam, ’01]

71. Typical Evolution of n eff visiting new areas closing the first loop second loop closure visiting known areas

72. Example (Intel Lab)  15 particles  four times faster than real-time P4, 2.8GHz  5cm resolution during scan matching  1cm resolution in final map Courtesy by Giorgio Grisetti & Cyrill Stachniss

73. Outdoor Campus Map  30 particles  250x250m 2  1.75 km (odometry)  20cm resolution during scan matching  30cm resolution in final map Courtesy by Giorgio Grisetti & Cyrill Stachniss  30 particles  250x250m 2  1.088 miles (odometry)  20cm resolution during scan matching  30cm resolution in final map

74. The approaches seen so far are purely passive. By reasoning about control, the mapping process can be made much more effective. Exploration

75. Where to Move Next?

76. Combining Rao-Blackwellized Mapping with Exploration mapping motion control localization SLAM active localization 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 reward (expected information gain) cost (path length)

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 and pose uncertainty: Map uncertainty only:

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, …) …