Simultaneous Localization and Mapping! SLAM

Transcription

1 Overview Simultaneous Localization and Mapping! SLAM What is SLAM? Qualifying Oral Examination Why do SLAM? Who, When, Where?!! A very brief literature overview Andrew Hogue hogue@cs.yorku.ca How has the problem been solved? York University, Toronto, Ontario June 20, 2005 What to do next? SLAM Applications What is SLAM? Where am I? 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! Issues, Concerns, Open Problems Oil Pipeline Inspection Ocean Surveying & Underwater Navigation Mine Exploration Coral Reef Inspection Military Applications Crime Scene Investigation

2 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 It is possible to address both problems in Stochastic Framework simultaneously M. Csorba! PhD Thesis! Oxford 1997 P. Newman! PhD Thesis! ACFR 1999 Localization Mapping Mapping without Localization Localization without Mapping Elfes & Moravec Occupancy Grids ICRA$85, Computer %89 Map/Scan Matching Lu & Milios!! AR$97, Cox et al, IEEE Robotics & Automation 91 Kuipers & Byun! Topological Maps Robotics & Autonomous Systems 1991 Geometric Beacons Leonard & Durrant! Whyte IEEE Robotics & Automation %91 Particle Filters D. Fox PhD Thesis Bonn 1998

3 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(s t, Θ z t, u t, n t ) Robot Location Observations Data Associations [x, y, φ] Map n 1 [ p, ˆq] Control Inputs n Features 2 n 3 θ 1 θ 2 θ 3 θ ṅ. z 1 z 2 z 3 z ṅ. u 1 u 2 u 3 u ṅ.. n n SLAM! Formalized p(s t, Θ z t, u t, n t ) Probability of robot being at position s t within environment represented as map Θ given knowledge of the observations the control inputs 'commanded motion( and Data Associations z t u t n t : f(z i ) θ i p(s t, Θ z t, u t, n t ) = ηp(z t s t, Θ, n t ) SLAM! Formalized } Measurement Model Using Bayes Rule, Markov Assumption & Simplifying p(s t s t 1, u t )p(s t 1, Θ z t 1, u t 1, n t 1 )ds t 1 } Motion Model η = p(z t s t, Θ, z t 1, u t, n t )p(s t, Θ z t 1, u t, n t )ds t Normalizing constant ensures that the resulting posterior is a probability

4 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 Kalman Filter approximates the posterior as a Gaussian p(s t, Θ z t, u t, n t ) N(µ t, Σ t ) N(µ, σ 2 ) = 1 e (x µ)2 2σ 2 2πσ 2 Mean 'state vector( contains robot location and map points µ t = [s t, θ 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 Disadvantages: Assumes Uni!modal Gaussian Probability distributions Linear Motion Model Time complexity O(n 3 ) due to matrix inversion Strictly speaking, matrix inversion is actually O(n log27 ) O(n ) 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(x t ) = {x i, w i }

5 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 Estimates slightly di)erent posterior p(s t, Θ z t, u t, n t ) p(s t, Θ z t, u t, n t ) Robot Trajectory or Paths instead of simply pose This allows for a re!formulation as a Rao!Blackwellised Particle Filter N p(s t, Θ z t, u t, n t ) = p(s t z t, u t, n t ) p(θ n s t, z t, u t, n t ) } n=1 Filter }Particle N Separate Kalman Filters Map per Particle! Other Approaches Sparse Extended Information Filters Thrun et al Robotics Research 2004 maintains Thin Junction Tree Filters Paskin IJCAI$03 Submap approaches Σ 1, µ T Σ 1 Atlas, Bosse et al. ICRA$03 HYMMs, Nieto et al. ICRA$04 Issues & Open Problems Representational Issues Unstructured 3D Environments Dynamic Environments Data Association Issues Loop Closing Informed Sensing

6 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

7 Data Association Issues n t : f(z i ) θ i 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? 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 Conclusions 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? Applications of SLAM Literature Overview Formal SLAM overview Current Approaches Open Problems

8 Thank You Extra Slides Bayes Rule P (X Y ) = P (X Y )P (Y ) P (X Y ) = P (Y X)P (X) P (X Y ) = P (X Y ) P (Y ) 0 P (Y ) P (Y X) = P (X Y ) P (X) P (X) 0 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[x] = Expected Value & Moments Mean + xp(x)dx E [[x E[x]] r ] = Generalized Central Moment + Expected Value Rules E[c] = c E [E[x]] = E[x] E[x + y] = E[x] + E[y] E[xy] = E[x]E[y] (x E[x]) r p(x)dx

9 Markov Assumption Markov!chain process current state is conditionally dependent only upon the previous state p(x t+1 x 0, x 1,..., x t ) = p(x t+1 x t ) Probability of variable at time t+1 can be computed as p(x t+1 ) = p(x t+1 x t )p(x t )dx t 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 Degeneracy set of particles do not relate to reality Diversity informative particles now lost due to resampling Depletion not enough particles or too many copies of same particle introduced

10 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 Implicit Relationship SEIF L1 L2 R1 R2 Covariance Matrix Sparse EIFs 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 Thin Junction Tree Filters 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 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

11 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 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 Computation is bounded by size of local frame only 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

12 FastSLAM Videos Courtesy of D. Hahnel 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 Courtesy of D. Fox Mobile_Robotics/mcl/ 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