Simultaneous Localization and Mapping !

SLAM
Qualifying Oral Examination Andrew Hogue hogue@cs.yorku.ca
York University, Toronto, Ontario June 20, 2005

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?
Where am I?

SLAM Applications
Oil Pipeline
Inspection

SLAM addresses two key problems in

Robotics

Ocean Surveying &

Underwater Navigation

Robot Localization, "Where am I?# Robot Mapping, "What does the world look like?#

Goal: Simultaneously estimate both Map &

Robot Location!

Mine Exploration Coral Reef Inspection Military Applications Crime Scene

Investigation

Which came &rst, the chicken or the egg?

A Brief History of SLAM

Mapping requires the robot location! Localization requires a map! Probability is the key
in Stochastic Framework simultaneously Localization Mapping

It is possible to address both problems

M. Csorba ! PhD Thesis ! Oxford 1997 P. Newman ! PhD Thesis ! ACFR 1999

Mapping without Localization

Elfes & Moravec Occupancy Grids
ICRA$85, Computer %89

Localization without Mapping

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

Particle Filters Kuipers & Byun ! Topological Maps

Robotics & Autonomous Systems 1991

Geometric Beacons
Leonard & Durrant! Whyte
IEEE Robotics & Automation %91

D. Fox PhD
Thesis Bonn 1998

SLAM !

Simultaneous Localization And Mapping

SLAM ! Formalized

Early work on SLAM done by

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

p(st , |z t , ut , nt )
Robot Location
[x, y, ]

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

Observations

Data Associations

[p, q ]

Map Features

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

1 2 .3 . . n

z1 z2 z3 . . . zn

Control Inputs

u1 u2 u3 . . . un

n1 n2 n3 . . . nn

SLAM ! Formalized

SLAM ! Formalized
p(st , |z t , ut , nt )
= p(zt |st , , nt )

p(st , |z t , ut , nt )
Probability of robot being at position within environment represented as map

Using Bayes Rule, Markov Assumption & Simplifying

st

p(st |st1 , ut )p(st1 , |z t1 , ut1 , nt1 )dst1

} }

given knowledge of the observations zt the control inputs 'commanded motion( ut and Data Associations nt : f (zi ) i

Measurement Model Motion Model

p(zt |st , , z t1 , ut , nt )p(st , |z t1 , ut , nt )dst

Normalizing constant ensures that the resulting posterior is a probability

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

Kalman Filtering Approaches

Kalman Filter approximates the posterior as a Gaussian

p(st , |z t , ut , nt ) N (t , t )
N (, 2 ) = 1 2 2 e
(x)2 2 2

Mean 'state vector( contains robot location and map points

Particle Filtering Approaches

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

t = [st , 1 , 2 , . . . , n ]
Covariance Matrix estimates uncertainties and relationships between each element in state vector

t = E[t T ] t

Kalman Filtering Approaches

Advantages

Simple to implement works well in practice

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

Disadvantages:

Assumes Uni!modal Gaussian Probability distributions Linear Motion Model Time complexity O(n3 ) due to matrix inversion
Strictly speaking, matrix inversion is actually

p(xt ) = {xi , wi }
O(nlog2 7 ) O(n2.807 )

Particle Filtering Approaches

FastSLAM, Montemerlo ! AI 2002
Rao!Blackwellised Particle Filter Each particle samples trajectory, each particle contains separate EKF for map points

FastSLAM
Estimates slightly di)erent posterior

p(st , |z t , ut , nt ) p(st , |z t , ut , nt )
Robot Trajectory or Paths instead of simply pose This allows for a re!formulation as a Rao!Blackwellised Particle Filter
N

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

pure particle &ltering approach sampling pose and map space

p(s , |z , u , n ) = p(s |z , u , n )

p(n |st , z t , ut , nt )
n=1

}
Particle Filter

}
N Separate Kalman Filters Map per Particle!

Other Approaches
Sparse Extended Information Filters
Thrun et al Robotics Research 2004

Issues & Open Problems

Representational Issues Unstructured 3D Environments Dynamic Environments Data Association Issues Loop Closing Informed Sensing

maintains

1 , T 1

Thin Junction Tree Filters

Paskin IJCAI$03

Submap approaches

Atlas, Bosse et al. ICRA$03 HYMMs, Nieto et al. ICRA$04

Representational Issues
Map representation Sparse set of landmarks 'beacons, salient visual
features(

Unstructured 3D Environments
Most SLAM algorithms assume relatively planar
motion mine mapping, robot has good idea of own motion, world is locally *at publications, and not traditional SLAM algorithm

Dense representations 'stereo, dense laser data(

PDF Representation Closed form 'KF or other(, limited to simple pdf

forms

Some work on Handheld motion 'MDA iSM( but no Guivant et al ! HYMMs ! Robotics Research 2004 C. Wang ! City mapping ! Thesis CMU 2004 Davison et al. ! single camera + models ! ICCV$03, IAV$04

Sample based 'Particle Filtering(, general pdf

Dynamic Environments

Our world is not static

Dynamic Environments
Burgard et al. AI 2000

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

Environment changes states

doors open and close people, &sh, other robots plays havoc on many vision based algorithms

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.

Objects move Lighting is dynamic

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

n : f (zi ) i
In EKF SLAM, bad data association causes &lter
to diverge!
t

Loop Closing
Important issue in SLAM Ability to close loops allow robot to FastSLAM ! able to close loops
%&x$ map estimate and minimize error in pose and map automatically, but requires often very informative updates to stay on track separate algorithm to identify loops and then perform expensive update to entire map

Nieto ! FastSLAM Multi!Hypothesis ! ICRA$03 How to recover from incorrect Data

Hahnel ! Lazy Data Association ! ISRR$03 Associations?

Theoretical analysis of data associations? How bad does the algorithm perform with 1
incorrect data association, many incorrect?

EKF!based approaches must perform

Informed Sensing
Can sensor parameters be estimated within
SLAM framework?

Conclusions
Applications of SLAM Literature Overview Formal SLAM overview Current Approaches Open Problems

Sensor data produces pose and maps Maps produce estimates of what landmarks are
available

Does this help in reducing errors in maps?

Thank You

Extra Slides

Bayes Rule
P (X Y ) = P (X|Y )P (Y ) P (X Y ) = P (Y |X)P (X)

Expected Value & Moments

Mean
+

Generalized Central Moment

+

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

E[x] =

xp(x)dx

E [[x E[x]] ] =

(x E[x])r p(x)dx

Expected Value Rules

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

Bayes Rule P (X|Y ) =

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

E[c] = c E [E[x]] = E[x] E[x + y] = E[x] + E[y] E[xy] = E[x]E[y]

Markov Assumption
Markov!chain process
current state is conditionally dependent only upon the previous state

Kalman Filters
KF

Linear motion model, zero!mean gaussian noise, unimodal distribution slightly nonlinear motion model '&rst order approximation(, zero!mean gaussian noise, unimodal distribution nonlinear motion model '2nd order general, 3rd order gaussian, zero!mean gaussian noise, unimodal distribution

EKF

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

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

UKF

p(xt+1 ) =

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

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 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 high computational complexity, 'many particles( di+cult to prove optimal number of particles degeneracy & diversity

Degeneracy
set of particles do not relate to reality

Diversity
informative particles now lost due to resampling

P. Newman, Thesis ACFR 1999

Disadvantages

Depletion
not enough particles or too many copies of same particle introduced

Sparse EIFs
use standard EKF alg, on inverse
covariance
Implicit Relationship SEIF

Sparse EIFs
L1 L2

feature!based EKF!SLAM

cov matrix is naturally almost sparse

if not exactly sparse, make it

sparse!

Covariance Matrix R1 R2

Information Matrix

U. Frese ICRA05

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

Advantages

Constant time update if bound active landmarks need to ensure sparsity map is inaccessible, must invert information matrix

Disadvantages

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 RSS05 Map 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

HYMMs
Combines feature maps with other dense info partitions space into local triangular regions,
similar to Atlas

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

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

DP!SLAM
Advantages

pure particle &ltering approach e+cient data structure closes large loops automatically '50 meters( requires many particles for complex environments works in %real!time$ but examples given are complex and required 24 hours to complete

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

Disadvantages

need to maintain balanced tree for e+ciency keep minimal, prune nodes with no children

FastSLAM Videos

Examples
Typically, robot pose is de&ned as -x,y,theta.,
robot moving in a plane

Courtesy of D. Hahnel http://www.informatik.uni! freiburg.de/,haehnel/

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 dpslam all estimate occupancy grids displacement from scan matching

Typical map is an occupancy grid, fastslam,

Courtesy of D. Fox http://www.cs.washington.edu/ai/ Mobile_Robotics/mcl/

Typical control input is odometry or pose

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