Probabilistic Robotics SLAM Simultaneous Localization and Mapping Sebastian

Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping 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 SA-1 9 -1

The SLAM Problem A robot is exploring an unknown, static environment. Given: • The robot’s controls • Observations of nearby features Estimate: • Map of features • Path of the robot 9 -2

Chicken-or-Egg • SLAM is a chicken-or-egg problem • A map is needed for localizing a robot • A good pose estimate is needed to build a map • Thus, SLAM is regarded as a hard problem in robotics • A variety of different approaches to address the SLAM problem have been presented • Probabilistic methods outperform most other techniques 9 -3

Structure of the Landmarkbased SLAM-Problem 9 -4

SLAM Applications Indoors Undersea Space Underground 9 -5

Map Representations Examples: City map, subway map, landmark-based map 9 -6

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

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

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 Pose error correlates data associations 9 -9

SLAM: Simultaneous Localization and Mapping • Full SLAM: Estimates entire path and map! • Online SLAM: Integrations typically done at a time Estimates most recent pose and map! 9 -10

Graphical Model of Full SLAM: 9 -11

Graphical Model of Online SLAM: 9 -12

Techniques for Generating Consistent Maps • Scan matching • EKF SLAM • Fast. SLAM • Probabilistic mapping with a single map and a posterior about poses Mapping + Localization • Graph. SLAM, SEIF 9 -13

Kalman Filter Algorithm 1. Algorithm Kalman_filter( mt-1, St-1, ut, zt): 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. 9. Return mt, St 9 -14

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

EKF-SLAM Map Correlation matrix 9 -16

EKF-SLAM Map Correlation matrix 9 -17

EKF-SLAM Map Correlation matrix 9 -18

Properties of KF-SLAM (Linear Case) [Dissanayake et al. , 2001] Theorem: The determinant of any sub-matrix of the map covariance matrix decreases monotonically as successive observations are made. Theorem: In the limit the landmark estimates become fully correlated 9 -19

Victoria Park Data Set [courtesy by E. Nebot] 9 -20

Victoria Park Data Set Vehicle [courtesy by E. Nebot] 9 -21

Data Acquisition [courtesy by E. Nebot] 9 -22

Raw Odometry (no SLAM) GPS (for reference) Odometry 9 -23

Estimated Trajectory [courtesy by E. Nebot] 9 -24

EKF SLAM Application [courtesy by J. Leonard] 9 -25

EKF SLAM Application odometry estimated trajectory [courtesy by John Leonard] 9 -26

Approximations for SLAM • Local submaps [Leonard et al. 99, Bosse et al. 02, Newman et al. 03] • Sparse links (correlations) [Lu & Milios 97, Guivant & Nebot 01] • Sparse extended information filters [Frese et al. 01, Thrun et al. 02] • Thin junction tree filters [Paskin 03] • Rao-Blackwellisation (Fast. SLAM) [Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03] 9 -27

EKF-SLAM: Complexity • Cost per step: O(n 2), quadratic in the number of landmarks: O(n 2) • Total cost to build a map with n landmarks: O(n 3) • Memory: O(n 2) Approaches exist that make EKF-SLAM O(n^1. 5) / O(n 2. 5) / O(n) 9 -28

EKF-SLAM: Summary • Convergence for linear case! • Can diverge if nonlinearities are large • Has been applied successfully in large -scale environments • Approximations reduce the computational complexity 9 -29

Data Association for SLAM Interpretation tree 9 -30

Data Association for SLAM Env. Dyn. 9 -31

Data Association for SLAM Geometric Constraints Location independent constraints Unary constraint: intrinsic property of feature e. g. type, color, size Binary constraint: relative measure between features e. g. relative position, angle Location dependent constraints Rigidity constraint: "is the feature where I expect it given my position? " Visibility constraint: "is the feature visible from my position? " Extension constraint: "do the features overlap at my position? " All decisions on a significance level 9 -32

Data Association for SLAM Interpretation Tree [Grimson 1987], [Drumheller 1987], [Castellanos 1996], [Lim 2000] Algorithm • backtracking • depth-first • recursive • uses geometric constraints • exponential complexity • absence of feature: no info. • presence of feature: info. perhaps 9 -33

Data Association for EKF SLAM Pygmalion a = 0. 95 , p = 2 9 -34

Data Association for EKF SLAM Pygmalion a = 0. 95 , p = 3 9 -35

Data Association for EKF SLAM Pygmalion texe: 633 ms Power. PC at 300 MHz a = 0. 95 , p = 4 a = 0. 95 , p = 5 9 -36

Summary: EKF SLAM • Extends EKF localization by additional state variables (landmark locations) • Converges in linear-Gaussian world • Data association problem: which measurement corresponds to which landmark? • Data association solved by tree search 9 -37