Probabilistic Robotics

Probabilistic Robotics

SLAM and FastSLAM

2

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!

The SLAM Problem

3

SLAM ApplicationsIndoors

Space

Undersea

Underground

4

Typical Robot Equipment Mobile robot

Typically want error under 2 cm per meter moved 2° per 45° degrees turned

5

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

6

Outline of typical SLAM Process

7

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

8

Given: The robot’s

controls Observations of

nearby featuresEstimate: Map of features Path of the

robot

The SLAM ProblemA robot moving though an unknown, static environment

9

Why is SLAM a hard problem?

SLAM: robot path and map are both unknown!

Robot path error increases errors in the map

10

Structure of the Landmark-based SLAM-Problem

11

12

13

14

15

16

17

18

19

20

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

21

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

22

(E)KF SLAM Represent robot state and landmark

position all in one big state ( rx, ry, l1x, l1y, l2x, l2y,…) Apply Kalman filter to state update and

observations Uncertainty represented in large

covariance matrix

23

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

Kalman Filter and Linearity

24

The Extended Kalman (EKF) is a sub-optimal extension of the original KF algorithm

The EKF allows for estimation of non-linear processes or measurement relationships

The Extended Kalman Filter (EKF)

),(),,(1

kkk

kkkk

vxhzwuxfx

kkk

kkkk

vxHzwuBxAx

1

Kalman Filter Extended Kalman Filter

25

Linearity Assumption Revisited

26

Non-linear Function

27

EKF Linearization (1)

28

EKF Linearization (2)

29

EKF Linearization (3)

30

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

ll

yx

mxBel

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

Can handle hundreds of dimensions

(E)KF-SLAM

31

EKF-SLAM

Map Correlation matrix

32

EKF-SLAM

Map Correlation matrix

33

EKF-SLAM

Map Correlation matrix

34

EKF-SLAM Can be quadratic in number of

landmarks Non-linearities a problem

Has led to other approaches

35

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.

Dependencies

36

FastSLAM Rao-Blackwellized particle filtering based on

landmarks [Montemerlo et al., 2002] Each landmark is represented by a 2x2

Extended Kalman Filter (EKF) Each particle therefore has to maintain M EKFs

Landmark 1 Landmark 2 Landmark M…x, y,

Landmark 1 Landmark 2 Landmark M…x, y, Particle#1

Landmark 1 Landmark 2 Landmark M…x, y, Particle#2

ParticleN

…

37

FastSLAM – Action Update

Particle #1

Particle #2

Particle #3

Landmark #1Filter

Landmark #2Filter

38

FastSLAM – Sensor Update

Particle #1

Particle #2

Particle #3

Landmark #1Filter

Landmark #2Filter

39

FastSLAM – Sensor Update

Particle #1

Particle #2

Particle #3

Weight = 0.8

Weight = 0.4

Weight = 0.1

40

Multi-Hypothesis Data Association

Data association is done on a per-particle basis

Robot pose error is factored out of data association decisions

41

Improved Proposal The proposal adapts to the structure

of the environment

42

Per-Particle Data Association

Was the observationgenerated by the redor 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

43

Results – Victoria Park

4 km traverse < 5 m RMS

position error 100 particles

Dataset courtesy of University of Sydney

Blue = GPSYellow = FastSLAM

44

Selective Re-sampling Re-sampling is dangerous, since

important samples might get lost(particle depletion problem)

Key question: When should we re-sample?

45

Typical Evolution of particle number

visiting new areas closing the

first loop

second loop closure

visiting known areas

46

Intel Lab 15 particles four times faster

than real-timeP4, 2.8GHz

5cm resolution during scan matching

1cm resolution in final map

47

More Details on FastSLAM M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A

factored solution to simultaneous localization and mapping, AAAI02

D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS03

M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 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, ICRA05

A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and mapping without predetermined landmarks, IJCAI03