Probabilistic Robotics

SLAM

2

Given:

• The robot’s controls (U1:t)

• Observations of nearby features (Z1:t)

Estimate:

• Map of features (m)

• Pose / Path of the robot (xt)

The SLAM Problem

A robot is exploring an unknown, static environment.

3

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 poseuncertainty

Nature of the SLAM Problem

4

Continuous Location of objects in component the map

Robot’s own pose

Discrete Correspondence component

Object is the same or not

5

SLAM: Simultaneous Localization and Mapping

•Full SLAM:

Estimates Entire pose (x1:t) and map (m)

Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)

),|,( t:1t:1t:1 uzmxp

Estimates entire path and map!

6

Graphical Model of Full SLAM:

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

7

SLAM: Simultaneous Localization and Mapping

•Online SLAM:

Estimates Most recent pose (xt) and map (m)

Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)

),|,( t:1t:1t uzmxp

Estimates most recent pose and map!

8

Graphical Model of Online SLAM:

121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp Integrations typically done one at a time

9

SLAM with Extended Kalman Filter

•Pre-requisites

• Maps are feature-based (landmarks)

small number (< 1000)

• Assumption - Gaussian Noise

• Process only positive sightings

No landmark = negative

Landmark = positive

10

EKF-SLAM with known correspondences

Correspondence Data association problem

Landmarks can’t be uniquely identified

Correspondence variable (Cit)

between feature (fit) and real landmark

Titit

it

it Srf

True identity of observed feature

11

EKF-SLAM with known correspondences

Signature Numerical value (average color)

Characterize type of landmark (integer)

Multidimensional vector

(height and color)

12

EKF-SLAM with known correspondences

Similar development to EKF localization

Diff robot pose + coordinates of all landmarks

Combined state vector

TNyNxNyxt

t SmmSmmyxm

xy ,,1,1,1 ...

(3N + 3)

),|( :1:1 ttt uzyp Online posterior

13

Motion update

Mean

Covariance

Iteration through measurements

Test for new landmarks

Initialization of elements

Expected measurement

Filter is updated

EKF-SLAM with known correspondences

Observing a landmark improves robot pose estimate

eliminates some uncertainty of other landmarks

Improves position estimates of the landmark + other landmarks

We don’t need to model past poses explicitly

14

EKF-SLAM with known correspondences

15

Example:

EKF-SLAM with known correspondences

16

Example:

• Uncertainty of landmarks are mainly due to robot’s pose uncertainty (persist over time)

Estimated location of landmarks are correlated

EKF-SLAM with unknown correspondences

• No correspondences for landmarks

• Uses an incremental maximum likelihood (ML) estimator

17

Determines most likely value of the correspondence variable

Takes this value for granted later on

EKF-SLAM with unknown correspondences

18

Motion update

Hypotheses of new landmark

Mean

Covariance

19

General Problem

Gaussian noise assumption Unrealistic

Spurious measurements Fake landmarks

Outliers

Affect robot’s localization

20

Solutions to General Problem

Provisional landmark list

New landmarks do not augment the map

Not considered to adjust robot’s pose

Consistent observation regular map

21

Solutions to General Problem

Landmark Existence Probability

Landmark is observed

Observable variable (o) increased by fixed value

Landmark is NOT observed when it should

Observable variable decreased

Removed from map when (o) drops below threshold

22

Problem with Maximum Likelihood (ML)

Once ML estimator determines likelihood of correspondence, it takes value for granted

always correct

Makes EKF susceptible to landmark confusion

Wrong results

23

Solutions to ML Problem

Spatial arrangement

Greater distance between landmarks

Less likely confusion will exist

Trade off:

few landmarks harder to localize

Little is known about optimal density of landmarks

24

Signatures

Give landmarks a very perceptual distinctiveness

(e,g, color, shape, …)

EKF-SLAM Limitations

• Selection of appropriate landmarks

• Reduces sensor reading utilization to presence or absence of those landmarks

Lots of sensor data is discarded

• Quadratic update time

Limits algorithm to scarce maps (< 1000 features)

• Low dimensionality of maps harder data association problem

25

EKF-SLAM Limitations

• Fundamental Dilemma of EKF-SLAM

It might work well with dense maps (millions of features)

It is brittle with scarce maps

26

BUT

It needs scarce maps because of complexity of the algorithm (update process)

27

SLAM Techniques

•EKF SLAM (chapter 10)

•Graph-SLAM (chapter 11)

•SEIF (chapter 12)

•Fast-SLAM (chapter 13)

28

Graph-SLAM

• Solves full SLAM problem

• Represents info as a graph of soft constraints

• Accumulates information into its graph without resolving it (lazy SLAM)

• Computationally cheap

• At the other end of EKF-SLAM

Process information right away (proactive

SLAM)

Computationally expensive

29

Graph-SLAM

• Calculates posteriors over robot path (not incremental)

• Has access to the full data

• Uses inference to create map using stored data

Offline algorithm

30

Sparse Extended Information Filter (SEIF)

• Implements a solution to online SLAM problem

• Calculates current pose and map (as EKF)

• Stores information representation of all knowledge (as Graph-SLAM)

Runs Online and is computationally efficient

• Applicable to multi-robot SLAM problem

31

FastSLAM Algorithm• Particle filter approach to the SLAM problem

• Maintain a set of particles

• Particles contain a sampled robot path and a map

• The features of the map are represented by own local Gaussian

• Map is created as a set of separate Gaussians

Map features are conditionally independent given the path

Factoring out the path (1 per particle)

Map feature become independent

Eliminates the need to maintain correlation among them

32

FastSLAM Algorithm

• Updating in FastSLAM

Sample new pose update the observed features

• Update can be performed online

• Solves both online and offline SLAM problem

• Instances Feature-based maps

Grid-based algorithm

33

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

34

Thanks !!