Simultaneous Localization and Mapping ! SLAM

Andrew Hoguehogue@cs.yorku.ca

York University, Toronto, OntarioJune 20, 2005

Qualifying Oral Examination

Overview

• What is SLAM?

• Why do SLAM?

• Who, When, Where? !! A very brief literature overview

• How has the problem been solved?

• What to do next? Issues, Concerns, Open Problems

What is SLAM?

• SLAM addresses two key problems in Robotics

• Robot Localization, "Where am I?#

• Robot Mapping, "What does the world look like?#

• Goal: Simultaneously estimate both Map & Robot Location!

Where am I?

SLAM Applications

• Oil Pipeline Inspection

• Ocean Surveying & Underwater Navigation

• Mine Exploration

• Coral Reef Inspection

• Military Applications

• Crime Scene Investigation

A Brief History of SLAM

Which came &rst, the chicken or the egg?

• Mapping requires the robot location!

• Localization requires a map!

• Probability is the key

• It is possible to address both problems in Stochastic Framework simultaneously M. Csorba ! PhD Thesis ! Oxford 1997P. Newman ! PhD Thesis ! ACFR 1999

Localization Mapping

Mapping without Localization

Elfes & Moravec Occupancy GridsICRA$85, Computer %89

Kuipers & Byun ! Topological MapsRobotics & Autonomous Systems 1991

Localization without Mapping

Map/Scan Matching Lu & Milios !! AR$97, Cox et al, IEEE Robotics & Automation 91

Geometric BeaconsLeonard & Durrant!

Whyte IEEE Robotics & Automation

%91

Particle Filters

D. Fox PhD Thesis

Bonn 1998

SLAM !• Early work on SLAM done by

Smith, Self and Cheeseman Journal of Robotics Research 1987, Autonomous Robot Vehicles 1990

• Bayesian formulation to estimate Spatial Relationships between Landmarks + estimate Robot Position

• SLAM coined by Leonard & Durrant!Whyte !! IROS$91 and CML 'Concurrent Mapping and Localization( by Thrun et al. Machine Learning 1998

Simultaneous Localization And Mapping SLAM ! Formalized

p(st,!|zt, u

t, n

t)

Robot Location

Map Features

Observations

Control Inputs

Data Associations

z1

z2

z3

zn

.

.

.

.

.

.

n1

n2

n3

nn

u1

u2

u3

un

.

.

.

!1

!2

!3

!n

.

.

.

[x, y, !]

[!p, q̂]

SLAM ! Formalized

p(st,!|zt, u

t, n

t)

Probability of robot being at position st

within environment represented as map !

ut

zt

nt : f(zi) ! !i

given knowledge of the observations

the control inputs 'commanded motion(

and Data Associations

SLAM ! Formalized

! =

!p(zt|st,!, zt!1, ut, nt)p(st,!|zt!1, ut, nt)dst

= !p(zt|st,!, nt)

!p(st|st!1, u

t)p(st!1,!|zt!1, ut!1, nt!1)dst!1}

Measurement Model

}

Motion Model

Normalizing constantensures that the resulting posterior is a probability

p(st,!|zt, u

t, n

t) Using Bayes Rule, Markov Assumption & Simplifying

Solutions to SLAM

• Two Main approaches to "solving# SLAM

• Kalman Filtering Approaches

• Smith et al. Robotics Research 1987, Leonard et al. IROS$91, Dissanyake et al. IEEE R&A 2001, Durrant!Whyte et al. Learning 2001

• Particle Filtering Approaches

• Montemerlo et al. AI 2002, Eliazar IJCAI$03, Grisetti et al. ICRA$05

Kalman Filtering Approaches

µt = [st, !1, !2, . . . , !n]

!t = E[µtµT

t ]

p(st,!|zt, ut, nt) ! N(µt,"t)

N(µ, !2) =1

!

2"!2e

!(x!µ)2

2!2

Kalman Filter approximates the posterior as a Gaussian

Mean 'state vector( contains robot location and map points

Covariance Matrix estimates uncertainties and relationships between each element in state vector

Kalman Filtering Approaches• Advantages

• Simple to implement

• works well in practice

• Disadvantages:

• Assumes Uni!modal Gaussian Probability distributions

• Linear Motion Model

• Time complexity due to matrix inversionO(n3)

O(nlog27) ! O(n2.807)Strictly speaking, matrix inversion is actually

Particle Filtering Approaches

•Gaussians are closed form and simple to estimate but real processes are almost never Gaussian

•Particle Filters solve this by estimating a general PDF using sampling techniques

•Estimate pdf p'( as a set of samples weighted by their likelihood

p(xt) = {xi, wi}

Particle Filtering Approaches

• FastSLAM, Montemerlo ! AI 2002

• Rao!Blackwellised Particle Filter

• Each particle samples trajectory, each particle contains separate EKF for map points

• DP!SLAM, Eliazar and Parr !! IJCAI$03, ICRA$04

• pure particle &ltering approach sampling pose and map space

FastSLAM

p(st,!|zt, u

t, n

t) ! p(st,!|zt

, ut, n

t)

Estimates slightly di)erent posterior

Robot Trajectory or Paths instead of simply pose

This allows for a re!formulation as a Rao!Blackwellised Particle Filter

p(st,!|zt

, ut, n

t) = p(st|zt, u

t, n

t)N!

n=1

p(!n|st, z

t, u

t, n

t)} }Particle Filter

N Separate Kalman FiltersMap per Particle!

Other Approaches

• Sparse Extended Information FiltersThrun et al Robotics Research 2004

• maintains

• Thin Junction Tree FiltersPaskin IJCAI$03

• Submap approaches

• Atlas, Bosse et al. ICRA$03

• HYMMs, Nieto et al. ICRA$04

!!1

, µT!

!1

Issues & Open Problems

• Representational Issues

• Unstructured 3D Environments

• Dynamic Environments

• Data Association Issues

• Loop Closing

• Informed Sensing

Representational Issues• Map representation

• Sparse set of landmarks 'beacons, salient visual features(

• Dense representations 'stereo, dense laser data(

• PDF Representation

• Closed form 'KF or other(, limited to simple pdf forms

• Sample based 'Particle Filtering(, general pdf

Unstructured 3D Environments

• Most SLAM algorithms assume relatively planar motion

• mine mapping, robot has good idea of own motion, world is locally *at

• Some work on Handheld motion 'MDA iSM( but no publications, and not traditional SLAM algorithm

• Guivant et al ! HYMMs ! Robotics Research 2004

• C. Wang ! City mapping ! Thesis CMU 2004

• Davison et al. ! single camera + models ! ICCV$03, IAV$04

Dynamic Environments

• Our world is not static

• Environment changes states

• doors open and close

• Objects move

• people, &sh, other robots

• Lighting is dynamic

• plays havoc on many vision based algorithms

Dynamic Environments• Burgard et al. AI 2000

• localize using vision looking at ceiling, people detection separate using spurious proximity sources

• Hahnel et al. IROS$02, ICRA$03

• map learning using EM alg. Identify data that cannot be explained by the rest of the data, i.e. dynamic objects.

• Montemerlo et al. ICRA$02

• existing map. Use particle &lters for people and localization.

• Wolf et al. ICRA$04

• 2 occupancy grids, dynamic and static

Data Association Issues

• In EKF SLAM, bad data association causes &lter to diverge!

• Hahnel ! Lazy Data Association ! ISRR$03

• Nieto ! FastSLAM Multi!Hypothesis ! ICRA$03

• How to recover from incorrect Data Associations?

• Theoretical analysis of data associations?

• How bad does the algorithm perform with 1 incorrect data association, many incorrect?

nt : f(zi) ! !i

Loop Closing

• Important issue in SLAM

• Ability to close loops allow robot to %&x$ map estimate and minimize error in pose and map

• FastSLAM ! able to close loops automatically, but requires often very informative updates to stay on track

• EKF!based approaches must perform separate algorithm to identify loops and then perform expensive update to entire map

Informed Sensing

• Can sensor parameters be estimated within SLAM framework?

• Sensor data produces pose and maps

• Maps produce estimates of what landmarks are available

• Does this help in reducing errors in maps?

Conclusions

• Applications of SLAM

• Literature Overview

• Formal SLAM overview

• Current Approaches

• Open Problems

Thank You Extra Slides

Bayes RuleP (X ! Y ) = P (X|Y )P (Y )

P (X ! Y ) = P (Y |X)P (X)

P (X|Y ) =P (X ! Y )

P (Y )P (Y ) != 0

P (Y |X) =P (X ! Y )

P (X)P (X) != 0

P (X, Y ) = P (X|Y )P (Y ) = P (Y |X)P (X)

P (X|Y ) =P (Y |X)P (X)

P (Y )Bayes Rule

Expected Value & Moments

E[x] =

! +!

"!

xp(x)dx E [[x ! E[x]]r] =

! +!

"!

(x ! E[x])rp(x)dx

E[c] = c

E [E[x]] = E[x]E[x + y] = E[x] + E[y]

E[xy] = E[x]E[y]

Expected Value Rules

Generalized Central MomentMean

Markov Assumption

• Markov!chain process

• current state is conditionally dependent only upon the previous state

p(xt+1|x0, x1, . . . , xt) = p(xt+1|xt)

p(xt+1) =

!p(xt+1|xt)p(xt)dxt

Probability of variable at time t+1 can be computed as

Kalman Filters• KF

• Linear motion model, zero!mean gaussian noise, unimodal distribution

• EKF

• slightly nonlinear motion model '&rst order approximation(, zero!mean gaussian noise, unimodal distribution

• UKF

• nonlinear motion model '2nd order general, 3rd order gaussian, zero!mean gaussian noise, unimodal distribution

• M. Csorba, Thesis Oxford 1997

• theoretical analysis of EKF SLAM

• correlations arise between errors in vehicle & map estimates ! fundamental to solving SLAM

• limit of map accuracy determined by initial uncertainty in vehicle pose

• P. Newman, Thesis ACFR 1999

• further theoretical results

• Uncertainty in each landmark monotonically decreases

• in the limit as number of observations increase, cov. matrix is fully correlated, rel. are fully known

• initial uncertainty limits landmark uncertainty

Particle Filtering• Advantages

• simple to implement

• represent arbitrary pdf $s, even multi!modal

• adaptive focusing on probable regions of state!space

• deals with non!gaussian noise

• Disadvantages

• high computational complexity, 'many particles(

• di+cult to prove optimal number of particles

• degeneracy & diversity

Degeneracyset of particles do not

relate to reality

Diversityinformative particles now

lost due to resampling

Depletionnot enough particles or

too many copies of same particle introduced

Sparse EIFs

• use standard EKF alg, on inverse covariance

• feature!based EKF!SLAM

• cov matrix is naturally almost sparse

• if not exactly sparse, make it sparse!

• U. Frese ICRA05

• o)!diagonal entries for 2 landmarks decay exponentially with distance traveled betwen observation of &rst and second landmark

R1 R2

L1 L2

R1 R2

L1 L2

Implicit Relationship

SEIF

Sparse EIFs

Covariance Matrix Information Matrix

• Advantages

• Constant time update if bound active landmarks

• Disadvantages

• need to ensure sparsity

• map is inaccessible, must invert information matrix

SEIFs Latest Results• Eustice ICRA05, RSS05

• re!formulated SLAM as view!based instead of feature!based

• information matrix is EXACTLY sparse

• no need to make sparse

• uses scan!matching between frames to register raw sensor data 'gives pose displacements(

Eustice RSS05Map of Titanic

Thin Junction Tree Filters

• Related to SEIFs

• exploit sparseness

• use e+cient data structure, thin junctions to make sparse

• Disadvantages

• Cannot explicitly model cyclic environments

• data association not addressed

Atlas• Topological + Metric information

• graph of coordinate frames, each node is frame, each edge is transformation between frames

• each node contains map of local area

• loop closing not explicit, done separately

• Can do large!scale environments

• Computation is bounded by size of local frame only

HYMMs

• Combines feature maps with other dense info

• partitions space into local triangular regions, similar to Atlas

• Output in examples is a dense occupancy grid

DP!SLAM• Ancestry tree of all particles

• current particles are the leaves

• parent is particle at previous iteration from which current particle was resampled

• map per particle is estimated, but not explicitly

• each grid cell contains a set of observations of cell from all particles in ancestry tree, occupancy is determined by looking at this history

• need to maintain balanced tree for e+ciency

• keep minimal, prune nodes with no children

DP!SLAM• Advantages

• pure particle &ltering approach

• e+cient data structure

• closes large loops automatically '50 meters(

• Disadvantages

• requires many particles for complex environments

• works in %real!time$ but examples given are complex and required 24 hours to complete

FastSLAM Videos

Courtesy of D. Foxhttp://www.cs.washington.edu/ai/

Mobile_Robotics/mcl/

Courtesy of D. Hahnelhttp://www.informatik.uni!

freiburg.de/,haehnel/

Examples• Typically, robot pose is de&ned as -x,y,theta.,

robot moving in a plane

• Typical measurements are range from the robot referenced in the %world$ frame, transformed from the robot frame, early work used sonar, latest work uses laser range &nder

• Typical map is an occupancy grid, fastslam, dpslam all estimate occupancy grids

• Typical control input is odometry or pose displacement from scan matching

SLAM & Navigation

• Use SLAM to guide Navigation

• R. Sim, Diss. McGill 2003, looked more on exploration & navigation strategies separate from SLAM

• Bryson et al. ICRA$05, intelligent planning using mutual information gain and entropy of SLAM covariance matrix as an information metric

• Sim ICRA$05, exploration strategies using information gain of SLAM alg., in simulation only

• Milford, RatSLAM, ICRA$05, goal directed navigation using SLAM map and pose