Probabilistic Robotics Fast SLAM Sebastian Thrun Alex Teichman
Probabilistic Robotics: Fast. SLAM Sebastian Thrun & Alex Teichman Stanford Artificial Intelligence Lab Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian Plagemann, Dirk Haehnel, Mike Montemerlo, Nick Roy, Kai Arras, Patrick Pfaff and others 10 -1
The SLAM Problem § SLAM stands for simultaneous localization and mapping § The task of building a map while estimating the pose of the robot relative to this map § Why is SLAM hard? Chicken-or-egg problem: § a map is needed to localize the robot and a pose estimate is needed to build a map
The SLAM Problem A robot moving though an unknown, static environment Given: § The robot’s controls § Observations of nearby features Estimate: § Map of features § Path of the robot
Map Representations Typical models are: § Feature maps § Grid maps (occupancy or reflection probability maps)
Particle Filters § § Represent belief by random samples § Sampling Importance Resampling (SIR) principle Estimation of non-Gaussian, nonlinear processes § Draw the new generation of particles § Assign an importance weight to each particle § Resampling § Typical application scenarios are tracking, localization, …
Localization vs. SLAM § A particle filter can be used to solve both problems § Localization: state space < x, y, > § SLAM: state space < x, y, , map> § § for landmark maps = < l 1, l 2, …, lm> for grid maps = < c 11, c 12, …, c 1 n, c 21, …, cnm> § Problem 1: The number of particles needed to represent a posterior grows exponentially with the dimension of the state space! § Problem 2: One of these particles has to “guess” the right map from the beginning
Dependencies § Is there a dependency between the dimensions of the state space? § If so, can we use the dependency to solve the problem more efficiently?
Dependencies § Is there a dependency between the dimensions of the state space? § If so, can we use the dependency to solve the problem more efficiently? § In the SLAM context § The map depends on the poses of the robot. § We know how to build a map given the position of the sensor is known.
Mapping using Landmarks l 1 Landmark 1 z 1 observations Robot poses controls x 1 x 0 u 0 z 3 x 2 x 3 u 1 . . . xt ut-1 z 2 Landmark 2 l 2 Knowledge of the robot’s true path renders landmark positions conditionally independent zt
Factored Posterior (Landmarks) poses map observations & movements Factorization first introduced by Murphy in 1999
Factored Posterior (Landmarks) poses map observations & movements SLAM posterior Robot path posterior landmark positions Does this help to solve the problem? Factorization first introduced by Murphy in 1999
Factored Posterior Robot path posterior (localization problem) Conditionally independent landmark positions
Rao-Blackwellization § § This factorization is also called Rao-Blackwellization Given that the second term can be computed efficiently, particle filtering becomes possible!
Fast. SLAM § Rao-Blackwellized particle filtering based on landmarks [Montemerlo et al. , 2002] § Each landmark is represented by a 2 x 2 Extended Kalman Filter (EKF) § Each particle therefore has to maintain M EKFs x, y, Landmark 1 Landmark 2 … Landmark M Particle #2 x, y, Landmark 1 Landmark 2 … Landmark M … Particle #1 Particle N
Fast. SLAM – Action Update Landmark #1 Filter Particle #1 Particle #2 Particle #3 Landmark #2 Filter
Fast. SLAM – Sensor Update Landmark #1 Filter Particle #1 Particle #2 Particle #3 Landmark #2 Filter
Fast. SLAM – Sensor Update Particle #1 Weight = 0. 8 Particle #2 Weight = 0. 4 Particle #3 Weight = 0. 1
Fast. SLAM - Video
Fast. SLAM Complexity § Update robot particles based on control ut-1 § Incorporate observation zt into Kalman filters § Resample particle set N = Number of particles M = Number of map features
Fast. SLAM Complexity § Update robot particles based on control ut-1 § Incorporate observation zt into Kalman filters § Resample particle set N = Number of particles M = Number of map features O(N) Constant time per particle O(N • log(M)) Log time per particle
Log(M) Algorithm Represent particle as tree of Kalman Filters k 4? T F k 2? T k 6? F k 1? T [m] m 1, S 1 T k 3? F [m] m 2, S 2 T [m] m 3, S 3 F k 5? F [m] T [m] m 4, S 4 m 5, S 5 k 7? F [m] m 6, S 6 T [m] m 7, S 7 F [m] m 8, S 8
Log(M) Algorithm new particle k 4? T F k 2? Only update branches that F T k 3? T change during resampling F [m] m[m] 3, S 3 T old particle k 4? phase F k 2? T k 6? F T F k 1? k 3? T T F F [m] [m] [m] m[m] m[m] 1, S 1 2, S 2 3, S 3 4, S 4 m 5, S 5 6, S 6 7, S 7 8, S 8
The importance of scaling O(N 2) O(log. N)
Data Association Problem § Which observation belongs to which landmark? § A robust SLAM must consider possible data associations § Potential data associations depend also on the pose of the robot
Multi-Hypothesis Data Association § Data association is done on a per-particle basis § Robot pose error is factored out of data association decisions
Per-Particle Data Association Was the observation generated by the red or the blue landmark? P(observation|red) = 0. 3 P(observation|blue) = 0. 7 § Two options for per-particle data association § Pick the most probable match § Pick an random association weighted by the observation likelihoods § If the probability is too low, generate a new landmark
Results – Victoria Park § 4 km traverse § < 5 m RMS position error § 100 particles Blue = GPS Yellow = Fast. SLAM Dataset courtesy of University of Sydney
Results – Victoria Park (Video) Dataset courtesy of University of Sydney
Results – Data Association
Fast. SLAM Accuracy
A funny finding (doesn’t generalize) Kalman Filter 250 particles error 100 particles 2 particles steps
Fast. SLAM with Grid Maps § Idea: Replace EKF Landmark map with occupancy grid map § Q: Is this valid?
Mapping Abandoned Coal Mines
Mapping Abandoned Coal Mines
Particles in Mine Mapping
The Importance of Particle without with raw particles data
Fast. SLAM with Grid Maps 3 particles map of particle 1 map of particle 3 map of particle 2
Quality of 2 D Maps 112 m
Outdoor Campus Map § 30 particles § 250 x 250 m 2 § 1. 75 km (odometry) § 20 cm resolution during scan matching § 30 cm resolution in final map
Fast. SLAM Summary § Fast. SLAM factors the SLAM posterior into low-dimensional estimation problems § Scales to problems with over 1 million features § Fast. SLAM factors robot pose uncertainty out of the data association problem § Robust to significant ambiguity in data association § Allows data association decisions to be delayed until unambiguous evidence is collected § Advantages compared to the classical EKF approach § Update Complexity of O(N log. M)
- Slides: 41