Probabilistic Robotics

1. Probabilistic Robotics SLAM and FastSLAM

2. The SLAM Problem • SLAM stands for simultaneous localization and mapping • originally developed by Hugh Durrant-Whyte and John J. Leonard • “Mobile robot localization by tracking geometric beacons” - 1991 • The task of building a map while estimating the pose of the robot relative to this map • Why is SLAM hard? Chicken and egg problem: • A map is needed to localize the robot and a pose estimate is needed to build a map • Errors correlate!

3. Indoors Undersea Underground Space SLAM Applications

4. Typical Robot Equipment • Mobile robot • Typically want error under • 2 cm per meter moved • 2° per 45° degrees turned

5. Typical Robot Equipment • Range finding sensors • Sonar • 10 mm accuracy • 4 m range • SICK laser • 10 mm accuracy • 80 m range • IR range finder • 1.5 m range • Vision • Increasingly viable

6. Outline of typical SLAM Process

7. Representations • 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;…

8. The SLAM Problem Given: • The robot’s controls • Observations of nearby features Estimate: • Map of features • Path of the robot A robot moving though an unknown, static environment

9. Why is SLAM a hard problem? SLAM: robot path and map are both unknown! Robot path error increases errors in the map

10. Structure of the Landmark-based SLAM-Problem

20. Why is SLAM a hard problem? • In the real world, the mapping between observations and landmarks is unknown • Picking wrong data associations can have catastrophic consequences Robot pose uncertainty

21. Detecting Landmarks • Look for spikes in range data • Red dots show table legs • Fit geometric primitives • RANSAC line fitting algorithm • Don’t want to fit to all data • Pick a random subset • Fit a primitive • Classify remaining points as outlier or not • Keep if enough points classify well • Keep a full occupancy map • Each cell is a landmark

22. (E)KF SLAM • Represent robot state and landmark position all in one big state • ( rx, ry, l1x, l1y, l2x, l2y,…) • Apply Kalman filter to state update and observations • Uncertainty represented in large covariance matrix

23. Kalman Filter and Linearity • For many applications, the time update and measurement equations are NOT linear • The KF is not directly applicable • Linearize around the non-linearities • Use of the KF is extended to more realistic situations

24. Kalman Filter Extended Kalman Filter The Extended Kalman Filter (EKF) • The Extended Kalman (EKF) is a sub-optimal extension of the original KF algorithm • The EKF allows for estimation of non-linear processes or measurement relationships

25. Linearity Assumption Revisited

26. Non-linear Function

27. EKF Linearization (1)

28. EKF Linearization (2)

29. EKF Linearization (3)

30. (E)KF-SLAM • Map with N landmarks:(3+2N)-dimensional Gaussian • Can handle hundreds of dimensions

31. EKF-SLAM Map Correlation matrix

32. EKF-SLAM Map Correlation matrix

33. EKF-SLAM Map Correlation matrix

34. EKF-SLAM • Can be quadratic in number of landmarks • Non-linearities a problem • Has led to other approaches

35. 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 if the position of the sensor is known.

36. x, y,  Landmark 1 Landmark 2 Landmark M … FastSLAM • Rao-Blackwellized particle filtering based on landmarks [Montemerlo et al., 2002] • Each landmark is represented by a 2x2 Extended Kalman Filter (EKF) • Each particle therefore has to maintain M EKFs Particle #1 x, y,  Landmark 1 Landmark 2 Landmark M … Particle #2 x, y,  Landmark 1 Landmark 2 Landmark M … … Particle N

37. FastSLAM – Action Update Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3

38. FastSLAM – Sensor Update Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3

39. Weight = 0.8 Weight = 0.4 Weight = 0.1 FastSLAM – Sensor Update Particle #1 Particle #2 Particle #3

40. Multi-Hypothesis Data Association • Data association is done on a per-particle basis • Robot pose error is factored out of data association decisions

41. Improved Proposal • The proposal adapts to the structure of the environment

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

43. Results – Victoria Park • 4 km traverse • < 5 m RMS position error • 100 particles Blue = GPS Yellow = FastSLAM Dataset courtesy of University of Sydney

44. Selective Re-sampling • Re-sampling is dangerous, since important samples might get lost(particle depletion problem) • Key question: When should we re-sample?

45. visiting new areas closing the first loop visiting known areas second loop closure Typical Evolution of particle number

46. Intel Lab • 15 particles • four times faster than real-timeP4, 2.8GHz • 5cm resolution during scan matching • 1cm resolution in final map

47. More Details on FastSLAM • M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A factored solution to simultaneous localization and mapping, AAAI02 • D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS03 • M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 2.0: An Improved particle filtering algorithm for simultaneous localization and mapping that provably converges. IJCAI-2003 • G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling, ICRA05 • A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and mapping without predetermined landmarks, IJCAI03