slam applications what is slam?hogue/qual_slides.pdf · solutions t o slam ¥ two m ain approaches...
TRANSCRIPT
Simultaneous Localization and Mapping ! SLAM
Andrew [email protected]
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 <ering 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 <ers 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 <er 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 <ering 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