The SLAM Problem CSE-571 A robot is exploring an 2 unknown, - PDF document

10/30/16 The SLAM Problem CSE-571 A robot is exploring an 2 unknown, static environment. Robotics Given: ¤ The robot’s controls SLAM: Simultaneous ¤ Observations of nearby features Localization and Mapping Estimate: ¤ Map of features Many slides courtesy of Ryan Eustice, ¤ Path of the robot Cyrill Stachniss, John Leonard Illustration of SLAM SLAM Applications without Landmarks Indoors Undersea 3 Space Underground With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard 1

10/30/16 Illustration of SLAM Illustration of SLAM without Landmarks without Landmarks With only dead reckoning, vehicle With only dead reckoning, vehicle pose uncertainty grows without pose uncertainty grows without bound bound Courtesy J. Leonard Courtesy J. Leonard Illustration of SLAM Illustration of SLAM without Landmarks without Landmarks With only dead reckoning, vehicle With only dead reckoning, vehicle pose uncertainty grows without pose uncertainty grows without bound bound Courtesy J. Leonard Courtesy J. Leonard 2

10/30/16 Illustration of SLAM Mapping with Raw Odometry without Landmarks With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard Repeat, with Measurements of Illustration of SLAM with Landmarks Landmarks ¨ First position: two features observed ¨ Second position: two new features observed Courtesy J. Leonard Courtesy J. Leonard 3

10/30/16 Illustration of SLAM with Landmarks Illustration of SLAM with Landmarks ¨ Re-observation of first two features ¨ Third position: two additional results in improved estimates for features added to map both vehicle and feature Courtesy J. Leonard Courtesy J. Leonard Illustration of SLAM with Landmarks Illustration of SLAM with Landmarks ¨ Re-observation of first four features ¨ Process continues as the vehicle moves results in improved location estimates through the environment for vehicle and all features Courtesy J. Leonard Courtesy J. Leonard 4

10/30/16 SLAM Using Landmarks Test Environment (Point Landmarks) MIT Indoor Track Courtesy J. Leonard Courtesy J. Leonard SLAM Using Landmarks View from Vehicle 1. Move 2. Sense 3. Associate measurements with known features 4. Update state estimates for robot and previously mapped features 5. Find new features from unassociated measurements 6. Initialize new features 7. Repeat MIT Indoor Track Courtesy J. Leonard 5

10/30/16 Simultaneous Localization and Comparison with Ground Truth Mapping (SLAM) ¨ Building a map and locating the robot in the map at the same time ¨ Chicken-and-egg problem map odometry localize SLAM result Courtesy: Cyrill Stachniss Courtesy J. Leonard Definition of the SLAM Problem Three Main Paradigms Given ¤ The robot ’ s controls Kalman Particle Graph- filter filter based ¤ Observations Wanted ¤ Map of the environment ¤ Path of the robot Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 6

10/30/16 Bayes Filter EKF for Online SLAM ¨ Recursive filter with prediction and correction step ¨ We consider here the Kalman filter as a solution to the online SLAM problem ¨ Prediction ¨ Correction Courtesy: Thrun, Burgard, Fox Courtesy: Cyrill Stachniss EKF SLAM Extended Kalman Filter Algorithm ¨ Application of the EKF to SLAM ¨ Estimate robot’s pose and locations of landmarks in the environment ¨ Assumption: known correspondences ¨ State space (for the 2D plane) is Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 7

10/30/16 EKF SLAM: State Representation EKF SLAM: State Representation ¨ Map with n landmarks: (3+2n)-dimensional ¨ More compactly Gaussian ¨ Belief is represented by Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss EKF SLAM: Filter Cycle EKF SLAM: State Representation ¨ Even more compactly (note: ) 1. State prediction 2. Measurement prediction 3. Measurement 4. Data association 5. Update Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 8

10/30/16 EKF SLAM: State Prediction EKF SLAM: Measurement Prediction Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss EKF SLAM: Obtained EKF SLAM: Data Association and Measurement Difference Between h(x) and z Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 9

10/30/16 EKF SLAM: Update Step EKF SLAM: Concrete Example Setup ¨ Robot moves in the 2D plane ¨ Velocity-based motion model ¨ Robot observes point landmarks ¨ Range-bearing sensor ¨ Known data association ¨ Known number of landmarks Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss Initialization Extended Kalman Filter Algorithm ¨ Robot starts in its own reference frame (all landmarks unknown) ¨ 2N+3 dimensions Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 10

10/30/16 Prediction Step (Motion) Update the State Space ¨ Goal: Update state space based on the robot’s ¨ From the motion in the plane motion ¨ Robot motion in the plane ¨ to the 2N+3 dimensional space ¨ How to map that to the 2N+3 dim space? Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss Update Covariance Extended Kalman Filter Algorithm ¨ The function only affects the robot’s motion and not the landmarks Jacobian of the motion (3x3) DONE Identity (2N x 2N) Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 11

10/30/16 This Leads to the Time Propagation Extended Kalman Filter Algorithm Apply & DONE DONE DONE Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss EKF SLAM: Correction Step Range-Bearing Observation ¨ Known data association ¨ Range-Bearing observation : i -th measurement at time t observes the ¨ If landmark has not been observed ¨ landmark with index j ¨ Initialize landmark if unobserved ¨ Compute the expected observation observed estimated relative measurement location of robot’s ¨ Compute the Jacobian of landmark j location ¨ Proceed with computing the Kalman gain Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 12

10/30/16 Jacobian for the Observation Jacobian for the Observation ¨ Based on ¨ Use the computed Jacobian ¨ Compute the Jacobian ¨ map it to the high dimensional space Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss Next Steps as Specified… Extended Kalman Filter Algorithm DONE DONE DONE DONE Apply & DONE Apply & DONE Apply & DONE Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 13

10/30/16 EKF SLAM – Correction (1/2) EKF SLAM – Correction (2/2) Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss Online SLAM Example EKF SLAM Complexity 70 ¨ Cubic complexity depends only on the measurement dimensionality ¨ Cost per step: dominated by the number of landmarks: ¨ Memory consumption: ¨ The EKF becomes computationally intractable for large maps! Courtesy: Cyrill Stachniss 14

10/30/16 EKF SLAM Correlations EKF SLAM Correlations 72 ¨ In the limit, the landmark estimates become fully correlated Blue path = true path Red path = estimated path Black path = odometry ¨ Approximate the SLAM posterior with a high-dimensional Gaussian [Smith & Cheesman, 1986] … ¨ Single hypothesis data association Courtesy: Dissanayake Courtesy: M. Montemerlo EKF SLAM Correlations EKF SLAM Correlations Map Correlation matrix Map Correlation matrix Courtesy: M. Montemerlo Courtesy: M. Montemerlo 15

10/30/16 EKF SLAM Uncertainties EKF SLAM Correlations ¨ The determinant of any sub-matrix of the map covariance matrix decreases monotonically ¨ New landmarks are initialized with maximum uncertainty Map Correlation matrix Courtesy: Dissanayake Courtesy: M. Montemerlo Data Association in SLAM Data Association ¨ Given an environment map 77 ¨ And a set of sensor observations ¨ Associate observations with map elements Robot pose uncertainty ¨ In the real world, the mapping between observations and landmarks is unknown ¨ Picking wrong data associations can have catastrophic consequences ¤ EKF SLAM is brittle in this regard Vision Laser ¨ Pose error correlates data associations 16

10/30/16 Difficulties: clutter Difficulties: clutter ¨ Influence of the type, density, precision and 10000 ¨ Vertical Edge Monocular vision: robustness of features considered: 8000 10000 6000 Laser scanner: 8000 4000 6000 2000 • Small amount of 4000 features (n ) 0 • Small amount of 2000 -2000 measurements (m ) -6000 -4000 -2000 0 2000 4000 6000 0 • Low spuriousness ¨ Many features (n large) High clutter ¨ Many measurements (m large) -2000 -6000 -4000 -2000 0 2000 4000 6000 ¨ no depth information Low clutter ¨ higher spuriousness Difficulties: imprecision Loop-Closing ¨ Both the sensor and the vehicle introduce imprecision ¨ Loop-closing means recognizing an already 10000 10000 mapped area 8000 8000 ¨ Data association under 6000 6000 ¤ high ambiguity 4000 4000 ¤ possible environment symmetries 2000 2000 ¨ Uncertainties collapse after a loop-closure (whether 0 0 the closure was correct or not) -2000 -2000 -6000 -4000 -2000 0 2000 4000 6000 -6000 -4000 -2000 0 2000 4000 6000 Vertical Edge Trinocular Robot imprecision: vision: introduces CORRELATED error variable depth precision good angular precision Courtesy: Cyrill Stachniss 17

10/30/16 Before the Loop-Closure After the Loop-Closure Courtesy: K. Arras Courtesy: K. Arras Example: Victoria Park Dataset Victoria Park: Data Acquisition Courtesy: E. Nebot Courtesy: E. Nebot 18