Simultaneous Localization and Mapping ! SLAM Andrew Hogue [email protected]York University, Toronto, Ontario June 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
12
Embed
SLAM Applications What is SLAM?hogue/qual_slides.pdf · Solutions t o SLAM ¥ Two M ain approaches t o "solving # SLAM ¥ Kalman F iltering Approaches ¥ Smith et al. R obotics R
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
• 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
• 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