+ All Categories
Home > Documents > 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram...

9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram...

Date post: 29-Dec-2015
Category:
Upload: gertrude-murphy
View: 216 times
Download: 0 times
Share this document with a friend
37
9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping 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 Sebastian Thrun & Alex Teichman Stanford Artificial Intelligence Lab
Transcript
Page 1: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-1SA-1

Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping

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

Sebastian Thrun & Alex Teichman

Stanford Artificial Intelligence Lab

Page 2: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-2

Given:

• The robot’s controls

• Observations of nearby features

Estimate:

• Map of features

• Path of the robot

The SLAM Problem

A robot is exploring an unknown, static environment.

Page 3: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-3

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

Page 4: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-4

Structure of the Landmark-based SLAM-Problem

Page 5: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-5

SLAM Applications

Indoors

Space

Undersea

Underground

Page 6: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-6

Map Representations

Examples:City map, subway map, landmark-based map

Page 7: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-7

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

Page 8: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-8

Why is SLAM a hard problem?

SLAM: robot path and map are both unknown

Robot path error correlates errors in the map

Page 9: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-9

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

Page 10: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-10

SLAM: Simultaneous Localization and Mapping

•Full SLAM:

•Online SLAM:

Integrations typically done one at a time

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

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

Estimates most recent pose and map!

Estimates entire path and map!

Page 11: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-11

Graphical Model of Full SLAM:

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

Page 12: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-12

Graphical Model of Online SLAM:

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

Page 13: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-13

Techniques for Generating Consistent Maps

•Scan matching

•EKF SLAM

•FastSLAM

•Probabilistic mapping with a single map and a posterior about poses Mapping + Localization

•GraphSLAM, SEIF

Page 14: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-14

Kalman Filter Algorithm

1. Algorithm 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 )(

Page 15: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-15

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

(E)KF-SLAM

Page 16: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-16

EKF-SLAM

Map Correlation matrix

Page 17: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-17

EKF-SLAM

Map Correlation matrix

Page 18: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-18

EKF-SLAM

Map Correlation matrix

Page 19: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-19

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]

Page 20: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-20

Victoria Park Data Set

[courtesy by E. Nebot]

Page 21: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-21

Victoria Park Data Set Vehicle

[courtesy by E. Nebot]

Page 22: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-22

Data Acquisition

[courtesy by E. Nebot]

Page 23: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-23

Raw Odometry (no SLAM)

Odometry

GPS (for reference)

Page 24: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-24

Estimated Trajectory

[courtesy by E. Nebot]

Page 25: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-25

EKF SLAM Application

[courtesy by J. Leonard]

Page 26: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-26

EKF SLAM Application

odometry estimated trajectory

[courtesy by John Leonard]

Page 27: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-27

• 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

Page 28: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-28

EKF-SLAM: Complexity

•Cost per step: O(n2), quadratic in 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-SLAMO(n^1.5) / O(n2.5) / O(n)

Page 29: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-29

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

Page 30: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-30

Data Association for SLAM

Interpretation tree

Page 31: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-31

Data Association for SLAM

Env. Dyn.

Page 32: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-32

Data Association for SLAM

Geometric Constraints

Location independent constraints

Unary constraint:intrinsic property of featuree.g. type, color, size

Binary constraint:relative measure between featurese.g. relative position, angle

Location dependent constraints

Rigidity constraint:"is the feature where I expect it givenmy 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

Page 33: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-33

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

Page 34: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-34

Data Association for EKF SLAM

Pygmalion

a = 0.95 , p = 2

Page 35: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-35

Data Association for EKF SLAM

a = 0.95 , p = 3

Pygmalion

Page 36: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-36

Data Association for EKF SLAM

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

texe: 633 ms

PowerPC at 300 MHz

Pygmalion

Page 37: 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti,

9-37

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


Recommended