Introduction to Mobile Robotics SLAM: Simultaneous Localization and Mapping

The SLAM Problem SLAM is the process by which a robot builds a map of the environment and, at the same time, uses this map to compute its location

• Localization: inferring location given a map • Mapping: inferring a map given a location • SLAM: learning a map and locating the robot simultaneously

2

The SLAM Problem

â€˘ 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 â€˘ hus, SLAM is regarded as a hard problem in robotics

3

The SLAM Problem • SLAM is considered one of the most fundamental problems for robots to become truly autonomous.

• A variety of different approaches to address the SLAM problem have been presented

• Probabilistic methods rule! • History of SLAM dates back to the mid-eighties (the stone-age of robotics)

4

The SLAM Problem Given: • The robot’s controls • Relative observations

Wanted: • Map of features • Path of the robot 5

The SLAM Problem

• Absolute robot pose

• Absolute landmark positions

• But only relative measurements of landmarks 6

Mapping with Raw Odometry Video..

7

SLAM Applications SLAM is central to a range of indoor, outdoor, in-air and underwater applications for both manned and autonomous vehicles. Examples:

•At home: vacuum cleaner, lawn mower •Air: surveillance with unmanned air vehicles •Underwater: reef monitoring •Underground: exploration of abandoned mines •Space: terrain mapping for localization 8

SLAM Applications Indoors

Undersea

Space

Underground

9

Map Representations Examples: Subway map, city map, landmark-based map

Maps are topological and/or metric models of the environment 10

Map Representations • Grid maps or scans, 2d, 3d

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

11

Why is SLAM a hard problem? 1. Robot path and map are both unknown

2. Errors in map and pose estimates correlated 12

Why is SLAM a hard problem?

Robot pose uncertainty

â€˘ In the real world, the mapping between observations and landmarks is unknown (origin uncertainty of measurements)

â€˘ Data Association: picking wrong data associations can have catastrophic consequences (divergence) 13

SLAM: Simultaneous Localization And Mapping

• Full SLAM: p(x 0:t ,m | z1:t ,u1:t )

Estimates entire path and map!

• Online SLAM: p(x t ,m | z1:t ,u1:t ) =

∫ ∫ K ∫ p(x

1:t

,m | z1:t ,u1:t ) dx1dx 2 ...dx t−1

Integrations (marginalization) typically done recursively, one at a time

Estimates most recent pose and map!

14

Graphical Model of Full SLAM

p ( x1:t , m | z1:t , u1:t ) 15

Graphical Model of Online SLAM

p ( xt , m | z1:t , u1:t ) = ∫ ∫ Κ

∫ p( x

1:t

, m | z1:t , u1:t ) dx1 dx2 ...dxt −1 16

Graphical Model: Models

Motion model Observation model 17

Remember? KF Algorithm 1.

Algorithm Kalman_filter(µt-1, Σt-1, ut, zt):

2. 3. 4.

Prediction: µ t = At µt −1 + Bt ut

5. 6. 7. 8.

Correction:

9.

Return µt, Σt

Σ t = At Σ t −1 AtT + Rt K t = Σ t CtT (Ct Σ t CtT + Qt ) −1 µ t = µ t + K t ( z t − Ct µ t ) Σ t = ( I − K t Ct ) Σ t

18

EKF SLAM: State representation â€˘ Localization 3x1 pose vector 3x3 cov. matrix

â€˘ SLAM Landmarks are simply added to the state. Growing state vector and covariance matrix!

19

EKF SLAM: State representation • Map with n landmarks: (3+2n)-dimensional Gaussian

Bel(xt,mt ) =

x σx2 σxy 2 σ σ y y xy θ σxθ σyθ l1 , σxl1 σyl1 l2 σxl2 σyl2 M M M lN σxlN σylN

L σxlN L σylN 1 2 L σθlN 1 2 L σl1lN 1 1 12 L σl2lN 2 12 2 M M M O M σθlN σl1lN σl2lN L σl2N

σxθ σyθ σθ2 σθl σθl

σxl σxl σyl σyl σθl σθl σl2 σl l σl l σl2 1

2

• Can handle hundreds of dimensions 20

EKF SLAM: Building The Map â€˘ State Prediction Odometry:

Robot-landmark crosscovariance prediction:

(skipping time index k)

21

EKF SLAM: Building The Map â€˘ Measurement Prediction Global-to-local frame transform h

22

EKF SLAM: Building The Map â€˘ Observation (x,y)-point landmarks

23

EKF SLAM: Building The Map â€˘ Data Association Associates predicted measurements with observation

?

(Gating)

24

EKF SLAM: Building The Map â€˘ Filter Update The usual Kalman filter expressions

25

EKF SLAM: Building The Map â€˘ Integrating New Landmarks State augmented by

landmark-to-landmark cross-covariance:

26

EKF SLAM

Map

Correlation matrix 27

EKF SLAM

Map

Correlation matrix 28

EKF SLAM

Map

Correlation matrix 29

KF-SLAM Properties (Linear Case) • The determinant of any sub-matrix of the map covariance matrix decreases monotonically as successive observations are made • When a new landmark is initialized, its uncertainty is maximal

• Landmark uncertainty decreases monotonically with each new observation

[Dissanayake et al., 2001]

30

KF-SLAM Properties (Linear Case) â€˘ In the limit, the landmark estimates become fully correlated

[Dissanayake et al., 2001]

31

KF-SLAM Properties (Linear Case) â€˘ In the limit, the covariance associated with any single landmark location estimate is determined only by the initial covariance in the vehicle location estimate.

[Dissanayake et al., 2001]

32

Victoria Park Data Set

[courtesy by E. Nebot]

33

Victoria Park Data Set Vehicle

[courtesy by E. Nebot]

34

Data Acquisition

[courtesy by E. Nebot]

35

Estimated Trajectory

[courtesy by E. Nebot]

36

EKF SLAM Application

[courtesy by J. Leonard]

37

EKF SLAM Application

odometry

estimated trajectory [courtesy by John Leonard]

38

SLAM Techniques for Generating Consistent Maps • EKF SLAM • FastSLAM (PF) • Network-Based SLAM • Hybrid Approaches (combination of NW+PF, NW+EKF)

• Topological SLAM (mainly place recognition)

• Scan Matching / Visual Odometry (only locally consistent maps) 39

EKF-SLAM: Complexity • Cost per step: quadratic in n, the number of landmarks: O(n2)

• Total cost to build a map with n landmarks: O(n3)

• Memory: O(n2) Approaches exist that make EKF-SLAM amortized O(n) / O(n2) / O(n2) D&C SLAM [Paz et al., 2006]

40

EKF-SLAM: Summary • Convergence for linear case! • Can diverge if nonlinearities are large. And reality is nonlinear...

• Has been applied successfully in largescale environments

• Approximations reduce the computational complexity

41

Data Association for SLAM Interpretation tree

42

Data Association for SLAM Env. Dyn.

43

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 Îą 44

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

45

Data Association for SLAM

Pygmalion

a = 0.95 , p = 2

46

Data Association for SLAM

Pygmalion

a = 0.95 , p = 3

47

Data Association for SLAM

Pygmalion

texe: 633 ms PowerPC at 300 MHz

a = 0.95 , p = 4 a = 0.95 , p = 5 48

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 (FastSLAM) [Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03] 49