Probabilistic Robotics

SLAM

1Based on slides from the

book's website

2

Given:

• The robot’s control signals.

• Observations of nearby features.

Estimate:

• Map of features.

• Path (sequence of poses) of the robot.

The SLAM Problem

A robot is exploring an unknown, static environment.

3

Structure of the Landmark-based SLAM Problem

4

Mapping with Raw Odometry

5

SLAM Applications

Indoors

Space

Undersea

Underground

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

Why is SLAM a hard problem?

SLAM: robot path and map are both unknown!

Robot path error correlates errors in the map!

8

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!

• Pose error correlates data associations.

Robot poseuncertainty

9

SLAM: Simultaneous Localization and Mapping

• Full SLAM:

• Online SLAM:

Integrations typically done one at a time.

• Aspects:• Continuous: Robot pose, object locations.

• Discrete: feature correspondence i.e. relationship with previously seen objects.

),|,( :1:1:1 ttt uzmxp

121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp

Estimate most recent pose and map!

Estimate entire path and map!

10

Graphical Model of Online SLAM

121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp

11

Graphical Model of Full SLAM:

),|,( :1:1:1 ttt uzmxp

12

Techniques for Consistent Maps

• Scan matching: online.

• EKF SLAM: online and incremental.

• Graph-SLAM: offline with stored information!

• Sparse Extended Information Filters (SEIFs): online with stored knowledge

• Fast-SLAM: Rao-Blackwellized Particle Filters

13

Scan Matching

• Maximize the likelihood of the ith pose and map relative to the (i-1)th pose.

• Calculate the map according to “mapping with known poses” based on the poses and observations.

)ˆ,|( )ˆ ,|( maxargˆ 11]1[

tttt

ttx

t xuxpmxzpxt

robot motioncurrent measurementmap constructed so far

][ˆ tm

14

Scan Matching Example

15

(E)Kalman Filter Algorithm

1. Algorithm (E)Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:3. 4.

5. Correction:6. 7. 8.

9. Return t, t

ttttt uBA 1

tTtttt RAA 1

1)( tTttt

Tttt QCCCK

)( tttttt CzK

tttt CKI )(

),( 1 ttt ug

tTtttt RGG 1

1)( tTttt

Tttt QHHHK

))(( ttttt hzK

tttt HKI )(

16

2

2

2

2

2

2

2

1

21

2221222

1211111

21

21

21

,),(

NNNNNN

N

N

N

N

N

llllllylxl

llllllylxl

llllllylxl

lllyx

ylylylyyxy

xlxlxlxxyx

N

tt

l

l

l

y

x

mxBel

• Map with N landmarks:(3+2N)-dimensional Gaussian.

• Can handle hundreds of dimensions.• Approximately known initial pose.

(E)KF-SLAM

17

Classical Solution – The EKF

• Approximate the SLAM posterior with a high-dimensional Gaussian [Smith & Cheeseman, 1986].

• Single hypothesis data association!

Known/Unknown Correspondences

• Known correspondence:• Easier to solve • Features associated with signatures – visual landmarks.• 3N + 3 dimensions.

• Unknown correspondence:• Harder to solve • Features not associated with unique signatures – range

information.

18

1 2 1

121:1:1:1:1:1:1 ...),|,,(),|,,(c c c

ttttttttt

t

dxdxdxuzcmxpuzcmxp

),|,,( :1:1 tttt uzcmxp

19

EKF-SLAM (Known correspondences)

Map Correlation matrix

20

EKF-SLAM (Known correspondences)

Map Correlation matrix

21

EKF-SLAM (Known correspondences)

Map Correlation matrix

22

Properties of KF-SLAM (Linear Case)

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

[Dissanayake et al., 2001]

Some Observations…

• Over time, the x-y coordinate estimates become fully correlated!

• Implications:• Absolute map coordinates relative to coordinate system

defined by initial robot pose is approximately known.• Map coordinates relative to robot pose known with

certainty asymptotically.

• Local accuracy much better than global accuracy.

23

24

Victoria Park Data Set

[courtesy by E. Nebot]

25

Victoria Park Data Set Vehicle

[courtesy by E. Nebot]

26

SLAM

[courtesy by E. Nebot]

27

Map and Trajectory

Landmarks

Covariance

[courtesy by E. Nebot]

28

Landmark Covariance

[courtesy by E. Nebot]

29

Estimated Trajectory

[courtesy by E. Nebot]

30

EKF SLAM Application (MIT B21)

[courtesy by John Leonard]

31

EKF SLAM Application (MIT B21)

odometry estimated trajectory

[courtesy by John Leonard]

Unknown Correspondences

• Algorithm similar to the case of known correspondences.

• Incremental Maximum Likelihood (ML) estimation of correspondences.

• Strategy:• Propose new landmark and correspondence.• Accept if Mahalanobis distance to previous landmarks

more than a threshold.

32

),,|,,( 1:1:1 ttttt NuzNmxp

33

EKF-SLAM Summary

• Have been applied successfully in real-world environments.

• Convergence results for the linear case.• Can diverge if nonlinearities are large!

• Quadratic in the number of landmarks: O(n2). Not suitable for more than a few 1000 landmarks.

• Additional features:• Map management required to overcome errors due to Gaussian

assumption and spurious landmark creation.• Landmark existence probabilities: landmarks on “probation” • Numerical instability for large sparse matrices.

• Approximations reduce the computational complexity.

34

• 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]

Approximations for SLAM