Probabilistic Robotics SLAM and Fast SLAM The SLAM

Probabilistic Robotics SLAM and Fast. SLAM

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! 2

SLAM Applications Indoors Undersea Space Underground 3

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

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 5

Outline of typical SLAM Process 6

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; … 7

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 8

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

Structure of the Landmarkbased SLAM-Problem 10

11

12

13

14

15

16

17

18

19

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

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 21

(E)KF SLAM § Represent robot state and landmark position all in one big state § ( rx, ry, l 1 x, l 1 y, l 2 x, l 2 y, …) § Apply Kalman filter to state update and observations § Uncertainty represented in large covariance matrix 22

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 23

The Extended Kalman Filter (EKF) § The Extended Kalman (EKF) is a suboptimal extension of the original KF algorithm § The EKF allows for estimation of nonlinear processes or measurement relationships Kalman Filter Extended Kalman Filter 24

Linearity Assumption Revisited 25

Non-linear Function 26

EKF Linearization (1) 27

EKF Linearization (2) 28

EKF Linearization (3) 29

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

EKF-SLAM Map Correlation matrix 31

EKF-SLAM Map Correlation matrix 32

EKF-SLAM Map Correlation matrix 33

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

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

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 36

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

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

Fast. SLAM – Sensor Update Particle #1 Weight = 0. 8 Particle #2 Weight = 0. 4 Particle #3 Weight = 0. 1 39

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

Improved Proposal § The proposal adapts to the structure of the environment 41

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 42

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

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

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

Intel Lab § 15 particles § four times faster than real-time P 4, 2. 8 GHz § 5 cm resolution during scan matching § 1 cm resolution in final map 46

More Details on Fast. SLAM § M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. Fast. SLAM: A factored solution to simultaneous localization and mapping, AAAI 02 § D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient Fast. SLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS 03 § M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. Fast. SLAM 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, ICRA 05 § A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and mapping without predetermined landmarks, IJCAI 03 47