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

4/29/20 The SLAM Problem CSE-571 2 A robot is exploring an 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 1 2 Illustration of SLAM SLAM Applications without Landmarks 3 Indoors Undersea Space Underground With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard 3 4 1

4/29/20 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 5 6 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 7 8 2

4/29/20 Illustration of SLAM Mapping with Raw Odometry without Landmarks With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard 9 10 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 11 12 3

4/29/20 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 13 14 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 15 16 4

4/29/20 SLAM Using Landmarks Test Environment (Point Landmarks) MIT Indoor Track Courtesy J. Leonard Courtesy J. Leonard 17 18 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 19 20 5

4/29/20 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 21 22 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 23 24 6

4/29/20 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 25 26 EKF SLAM EKF SLAM: State Representation ¨ Application of the EKF to SLAM ¨ Map with n landmarks: (3+2n)-dimensional Gaussian ¨ Estimate robot’s pose and locations of landmarks in the environment ¨ Belief is represented by ¨ Assumption: known correspondences ¨ State space (for the 2D plane) is Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 27 28 7

4/29/20 EKF SLAM: State Representation EKF SLAM: State Representation ¨ More compactly ¨ Even more compactly (note: ) Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 29 30 EKF SLAM: Filter Cycle EKF SLAM: State Prediction 1. State prediction 2. Measurement prediction 3. Measurement 4. Data association 5. Update Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 31 32 8

4/29/20 EKF SLAM: Obtained EKF SLAM: Measurement Measurement Prediction Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 33 34 EKF SLAM: Data Association and EKF SLAM: Update Step Difference Between h(x) and z Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 35 36 9

4/29/20 EKF SLAM: Concrete Example Initialization Setup ¨ Robot starts in its own reference frame (all landmarks unknown) ¨ Robot moves in the 2D plane ¨ 2N+3 dimensions ¨ Velocity-based motion model ¨ Robot observes point landmarks ¨ Range-bearing sensor ¨ Known data association ¨ Known number of landmarks Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 37 38 Prediction Step (Motion) Extended Kalman Filter Algorithm ¨ Goal: Update state space based on the robot’s motion ¨ Robot motion in the plane ¨ How to map that to the 2N+3 dim space? Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 39 40 10

4/29/20 Update the State Space Extended Kalman Filter Algorithm ¨ From the motion in the plane DONE ¨ to the 2N+3 dimensional space Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 41 42 Update Covariance This Leads to the Time Propagation ¨ The function only affects the robot’s motion and not the landmarks Apply & DONE Jacobian of the motion (3x3) Identity (2N x 2N) Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 43 48 11

4/29/20 EKF SLAM: Correction Step Extended Kalman Filter Algorithm ¨ Known data association : i -th measurement at time t observes the ¨ landmark with index j DONE ¨ Initialize landmark if unobserved DONE ¨ Compute the expected observation ¨ Compute the Jacobian of ¨ Proceed with computing the Kalman gain Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 51 52 Range-Bearing Observation Jacobian for the Observation ¨ Range-Bearing observation ¨ Based on ¨ If landmark has not been observed observed estimated ¨ Compute the Jacobian relative measurement location of robot’s landmark j location Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 53 58 12

4/29/20 Jacobian for the Observation Next Steps as Specified… ¨ Use the computed Jacobian DONE DONE ¨ map it to the high dimensional space Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 59 61 EKF SLAM – Correction (1/2) Extended Kalman Filter Algorithm DONE DONE Apply & DONE Apply & DONE Apply & DONE Courtesy: Cyrill Stachniss Courtesy: Cyrill Stachniss 62 63 13

4/29/20 EKF SLAM – Correction (2/2) EKF SLAM Complexity ¨ 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 Courtesy: Cyrill Stachniss 64 65 EKF SLAM Correlations Online SLAM Example 69 ¨ In the limit, the landmark estimates become fully correlated Courtesy: Dissanayake 69 70 14

4/29/20 EKF SLAM Correlations Data Association in SLAM 71 72 Robot pose uncertainty ¨ In the real world, the mapping between observations and landmarks is unknown ¨ Picking wrong data associations can have catastrophic Blue path = true path Red path = estimated path Black path = odometry consequences ¨ Approximate the SLAM posterior with a high-dimensional ¤ EKF SLAM is brittle in this regard Gaussian [Smith & Cheesman, 1986] … ¨ Pose error correlates data associations ¨ Single hypothesis data association Courtesy: M. Montemerlo 71 72 Before the Loop-Closure Loop-Closing ¨ Loop-closing means recognizing an already mapped area ¨ Data association under ¤ high ambiguity ¤ possible environment symmetries ¨ Uncertainties collapse after a loop-closure (whether the closure was correct or not) Courtesy: K. Arras Courtesy: Cyrill Stachniss 73 74 15

4/29/20 Example: Victoria Park Dataset After the Loop-Closure Courtesy: K. Arras Courtesy: E. Nebot 75 76 Victoria Park: EKF Estimate Victoria Park: Data Acquisition 78 Courtesy: E. Nebot Courtesy: E. Nebot 77 78 16

4/29/20 Victoria Park: EKF Estimate Victoria Park: Landmarks Courtesy: E. Nebot Courtesy: E. Nebot 79 80 Victoria Park: Landmark Andrew Davison: MonoSLAM Covariance 81 Courtesy: E. Nebot 81 82 17

4/29/20 EKF SLAM Summary Sub-maps for EKF SLAM 84 ¨ Quadratic in the number of landmarks: O(n 2 ) ¨ Convergence results for the linear case. ¨ Can diverge if nonlinearities are large! ¨ Have been applied successfully in large-scale environments. ¨ Approximations reduce the computational complexity. [Leonard et al 1998] 83 84 Graph-SLAM Literature • Full SLAM technique EKF SLAM ¨ “Probabilistic Robotics”, Chapter 10 • Generates probabilistic links ¨ Smith, Self, & Cheeseman: “Estimating Uncertain Spatial Relationships in Robotics” • Computes map only occasionally ¨ Dissanayake et al.: “A Solution to the Simultaneous Localization and Map Building (SLAM) Problem” ¨ Durrant-Whyte & Bailey: “SLAM Part 1” and “SLAM • Based on Information Filter form Part 2” tutorials Courtesy: Cyrill Stachniss 85 86 18