Outline Robotic Mapping 6.834 Student Lecture Itamar Kahn, Thomas Lin, Yuval Mazor Introduction (Tom) Kalman Filtering (Itamar) J.J. Leonard and H.J.S. Feder. A computationally efficient method for large-scale concurrent mapping and localization. In J. Hollerbach and D. Koditschek, editors, Proceedings of the Ninth International Symposium on Robotics Research, Salt Lake City, Utah, 1999 Hybrid Mapping Approaches (Yuval) S. Thrun, W. Burgard, and D. Fox. A real-time algorithm for mobile robot mapping with applications to multirobot and 3D mapping. In Proceedings of the IEEE Internatinoal Conference on Robotics and Automation (ICRA), San Francisco, CA, 2000. IEEE Conclusion (Tom) Vision / Steps Truly autonomous mobile robots Sense the environment Acquiring models of the environment Reason Act on environment State of the Art 20 years of research Do well on static, structured, limited size Difficulty with dynamic, unstructured, large scale Simulated versus Real-life What is Robotic Mapping? Acquiring spatial models of physical environments with robots What is Robotic Mapping? Sensors with different limitations Cameras, Sonar, Lasers, Radar, Compasses, GPS Paul Newman's mobile robot mapping MIT 1
Main Challenges Noise High Dimensionality Correspondence Problem Changing Environments Robotic Exploration Planning Challenges - Noise Measurement errors accumulate over time Odometry error will accumulate and throw off an entire map Challenges - High Dimensionality 3-D visual maps can take millions of numbers Challenges - Correspondence Problem Do these sensor readings from different times correspond to the same object? Is the blue object the same one it sensed earlier, or it a different object that seems like it's in the same location because of accumulated sensor noise? Challenges - Changing Environments Moving furniture, moving doors Even faster: Moving cars, moving people Hard to distinguish sensor noise and moving items Challenges - Robotic Exploration Planning How robots should explore using incomplete maps 2
Today's Methods All Probabilistic Better models uncertainty, sensor noise Kalman Filtering (Itamar will present), Hybrid Methods (Yuval will present) EM, Occupancy Grids, Multi-Planar Maps (not presenting) Decoupled Stochastic Mapping A Computationally Efficient Method for Large-Scale Concurrent Mapping and Localization John J. Leonard and Hands Jacob S. Feder, MIT, 2000 Introduction DSM: Feature based approach to CML Previous solutions are O(n 2 ), where n is the number of features Results from the number of correlations between the vehicles and features Overview Kalman and Extended Kalman Filters Conventional Stochastic Mapping Decoupled Stochastic Mapping Algorithm Testing Kalman Filter Mini Tutorial The mini tutorial is an adaptation of a tutorial presented at ACM SIGGRAPH 2001 by Greg Welch and Gary Bishop (UNC). Kalman Filter KF operates by Predicting the new state and its uncertainty Correcting with the new measurement The slides of the tutorial are available at http://www.cs.unc.edu/~tracker/ref/s2001/kalman/index.html More information (papers, software, links, etc) is available at http://www.cs.unc.edu/~welch/kalman/index.html IN: Noisy data --> OUT:less noisy 3
Kalman Filter Example 2D Position-Only (e.g., 2D Tablet) Process Model: Measurement Model: Î x k y k state Î = 1 0 x k-1 + ~ x k-1 Î 0 1 Î y k-1 Î ~ y k-1 u k v k measurement state transition state noise x k = Ax k-1 + w k-1 = H x 0 x k + ~ u k Î 0 H y Î y k Î ~ v k measurement matrix state z k = Hx k + v k noise Kalman Filter Example Preparation and Initialization State transition: A = 1 0 Î 0 1 Process Noise Covariance: Measurement Noise Covariance: R = E v * v T Initialization: x 0 = H -1 z 0 P 0 = e 0 Î 0 e { } = Q xx 0 Q = E w * w T Î 0 Q yy Î 0 R yy { } = R xx 0 Predict Correct Kalman Filter Example x - k = Ax k-1 P k - = AP k-1 A T + Q ( ) x k = x k - + K z k - Hx k - P k = ( I - KH)P - Kalman Filter Example Predict x k - = Ax k-1 P k - = AP k-1 A T + Q Correct ( ) -1 ( ) K = P - k H T HP - k H T + R x k = x k - + K z k - Hx k - P k = ( I - KH)P - K = P - k H T ( HP - k H T + R) -1 Kalman Filter Example Extend example to 2D Position-Velocity Process model: state transition state 1 0 dt 0 x 0 1 0 dt y 0 0 1 0 dx dt Î 0 0 0 1 dy Î dt Measurement model: measurement matrix state x H x 0 0 0 y Î 0 H y 0 0 dx dt dy Î dt Kalman Filter But, Kalman filter is not enough!!! Only matrix operations allowed (only works for linear systems) Measurement is a linear function of state Next state is linear function of previous state Can t estimate non-linear variables (e.g., gain, rotation, projection, etc.) 4
Extended Kalman Filter Nonlinear Process (Model) Process dynamics: A becomes a(x) Measurement: H becomes h(x) Filter Reformulation Use functions instead of matrices Use Jacobians to project forward, and to relate measurement to state Stochastic Mapping Size-varying Kalman filter Add and Update of representation Build a map through spatial relationship Stochastic Mapping Estimated locations of the robot and the features in the map [ ] T where x r = x r y r f v [ k] T = [ x 1 [ k] T...x N [ k] T ], such that x i = x i y i x [ k] = x r [ k] T x f [ k] T and x f Estimated error covariance P[ k] = P rr[ k] P rf [ k] Î P fr [ k] P ff [ k] [ ] T [ ] T Stochastic Mapping The dynamic model of the robot is given by ( ) + d x ( u[ k] ) where u[ k] = [ df du] T x[ k +1] = f x[ k], u[ k] The observation model for the system is given by ( ) + d z z[ k] = h x[ k] Augmented Stochastic Mapping Given these assumptions, an extended Kalman filter (EKF) is employed to estimate the state x and covariance P. Decoupled Stochastic Mapping Stochastic Mapping: complexity O(n 2 ) Solution: DSM Divide the environment into multiple submaps Each submap has a vehicle position estimate and a set of features estimates 5
How do we move from map to map? Single-pass vs. Multi-pass DSM Cross-map relocation A B Cross-map updating A B Decoupled Stochastic Mapping Vehicle travels to a previously visited area: Cross-map relocation x B [ k] x A r [ k],p B [ k] Prr A [ k] + P B rr [ j] P B rf [ j] Î x B r [ j] Î P B fr [ j] P B ff [ j] Decoupled Stochastic Mapping Facilitate spatial convergence by bringing more accurate vehicle estimates from lower to higher maps: Cross-map updating [ ] f B x B k - Î x f B,P B k - [ k] [ ] Prr [ j] + F B P B rf [ j] P B fr [ j] 2P B ff [ j] Using EKF, estimate vehicle location in submap B: Use state x A r [ k] as measurement z and covariance P A rr [ k] in A as prediction for state in B. Î B Methods Comparison Testing Full covariance ASM Single-pass DSM Multi-pass DSM 6
Limitations Sensor noise modeled by gaussian process Hybrid Approaches Limited map dimensionality A Real-Time Algorithm for Mobile Mapping with Applications to Multi-Robot and 3D Mapping Sebastian Thrun, Carnegie Mellon University Wolfram Burgard, University of Freiberg Dieter Fox, Carnegie Mellon University Overview Benefits Concurrent mapping and localization using 2D laser range finders Mapping: Fast scan-matching Localization: Sample-based probabilities Motivation: 3D-Maps and large cyclic environments Computation is all real-time Builds 3D maps Handles cycles Accurate map generation in the absence of odometric data Background Incremental Localization Incremental Localization Expectation Maximization Iterate localization for each new sensor scan Can be done in real-time Fail on cyclic environments as error grows unbounded 7
Expectation Maximization (EM) Goal Search most-likely map while considering all past scans Probabilistically, iterate and refine the map Can handle cyclic environments Batch algorithms - not real-time Combine IL and EM in a real-time algorithm that can handle cycles Use posterior estimation like in EM Incremental map construction with maximum likelihood estimators as in IL Mapping A map is a collection of scans and poses m t = { o t,s t } t =0,1,...,t Map likelihood P(m d t ) = hp(m) Ú L P(o t m,s t ) t -1 t Ú t =0 P(s t +1 a t,s t )ds 1...dst t = 0 The most likely map: arg maxp(m d t ) m where: d t ={s o,a o,s 1,a 1,...,s t } Mapping The PDF has an elliptical/banana shape PDF Intuition If a scan shows free space it is unlikely that future scans will show obstacles in that space Darker regions indicate lower probability of an obstacle Posterior pose, s, after moving distance a from s : P(s a, s ) 8
Maximizing Map Likelihood Infeasible to maximize while robot is moving in real-time In the past, the robot had to stop (EM) or risk unbounded error (IL) Conventional Incremental Map Given a scan and odometry reading, determine the most likely pose. Use that pose to increment the map. Never go back to change it. s ˆ t = argmax P(s t o t, a t -1, s ˆ t -1 ) m t +1 = m t U { o t, s ˆ t } Conventional Incremental Map Incremental Map Problem This approach works in non-cyclic environments Pose errors necessarily grow Past poses cannot be revised Search algorithms cannot find solutions to close loops Posterior Incremental Mapping Basic premise: Use Markov localization to compute the full posterior over robot poses Probability distribution over poses based on sensor data: Bel(s t ) = P(s t d t, m t -1 ) Posterior Incremental Mapping Posterior is where the robot believes it is. Can be incrementally updated over time Bel(s t ) = hp(o t s t,m t -1 ) Ú P(s t a t -1, s t - 1)Bel(s t -1 )d st-1 Updated pose and maps: s t = argmax Bel(s t ) m t +1 = m t { o t,s t s t U } 9
Posterior Incremental Mapping Implementation Details Use the posterior belief to determine the most likely pose Uncertainty grows during a loop The robot has a larger window to search to close the loop Take samples of posterior beliefs Save computation and easier to generalize Use gradient descent on each sample to find globally maximum likelihood function. Backwards Correction Handling a Cycle When a loops closes successfully, we can go back and correct our pose estimates D st = s t - s ˆ t Distribute the error st among all poses in the loop Use gradient descent for all poses in the loop to maximize likelihood Multi-Robot Extensions Using posterior estimation extends naturally to environments with multiple robots Each robot need not know any other robot s initial pose BUT every robot localize itself within the map of an initial Team Leader robot Multi-Robot Extensions Use Monte Carlo Localization Initially any location is likely Posterior estimation localizes the robot in the Team Leader s map 10
Results - Cycle Mapping Groundrules: Every scan used for localization Scans appended to map every two meters Random odometric errors (30 or 1 meter) Error generates large error during the cycle but within acceptable range of true pose Posterior estimation finds the true pose and corrects prior beliefs Results - Mapping w/out Odometry Same as before but with no odometric data Traversing the cycles leads to very large error growth Once again, on cycle completion the errors are found and fixed Final map is virtually identical to map generated with odometric data Limitations Non-optimal Nested cycles Dynamic environments Changing the map backwards in time can be dangerous Pseudo-Real Time Brief Comparison Kalman Filtering Hybrid Methods Representation landmark locations point obstacles Sensor Noise Gaussian any Map Dimensionality limited unlimited Dynamic Env's limited no Scenario 1 - Infinite Corridor at Night Which algorithm is better for a robot mapping the infinite corridor late at night, when one janitor is walking around? Vote Kalman Filtering Hybrid Approaches Don't Know Scenario 1 - Infinite Corridor at Night Changing environment problem Kalman - good! (Itamar will explain) Infinite corridor has few features Can handle janitor (limited dynamics) Hybrid - bad! (Yuval will explain) Can't handle dynamic environments 11
Scenario 2 - Airport Parking Lot Which algorithm is better for a robot mapping an airport parking lot with hundreds of cars but no people? Vote Kalman Filtering Hybrid Approaches Don't Know Scenario 2 - Airport Parking Lot High dimensionality problem Kalman - bad! (Itamar will explain) Only handles limited map dimensionality Hybrid - good! (Yuval will explain) Nothing moving Handles unlimited map dimensionality Scenario 3 - Amusement Park Which algorithm is better for a robot mapping a busy amusement park during Christmas? Vote Kalman Filtering Hybrid Approaches Don't Know Scenario 3 - Amusement Park Both fail Kalman - bad! (Itamar will explain) Only does limited dynamics Hybrid - bad! (Yuval will explain) Can't handle such a dynamic environment Almost no algorithms learn meaningful maps in such a dynamic environment Recap Contributions The Mapping Problem Main Challenges Kalman Filtering Hybrid Methods Comparison Provided overview of robotic mapping Presented Kalman Filtering in depth Presented Hybrid Methods in depth 12