1 Range-Only SLAM for Robots Operating Cooperatively with Sensor Networks Joseph Djugash Sanjiv Singh George Kantor Wei Zhang Carnegie Mellon University

2 Motivation SLAM – Estimating the robot’s position and the positions of landmarks/beacons in the world

3 Motivation SLAM – Estimating the robot’s position and the positions of landmarks/beacons in the world

4 Motivation SLAM – Estimating the robot’s position and the positions of landmarks/beacons in the world Range-Only sensors provide highly nonlinear measurements

5 Motivation SLAM – Estimating the robot’s position and the positions of landmarks/beacons in the world Range-Only sensors provide highly nonlinear measurements Inter-beacon measurements provide additional constraints

6 Some Related Work Self-localizing sensor networks (Measurements between nodes/beacons) Hendrickson [1992] Moore, Leonard, Rus, and Teller [2004] Pulskamp [1988] Mapping and localization with mobile sensor (Measurements from a moving sensor to landmarks) Hahnel et al. [2004] Olson, Leonard, and Teller [2004] Kurth, Kantor, and Singh [2003]

7 The Atlas Mapping Problem

8 Network Mapping in a Fully Connected Network The Lack of a reference introduces global ambiguity

9 The Sparsely Connected Network

10 Mapped Locations – A Partially Connected Network A clique (fully connected sub-graph) of degree 4 or higher is required to uniquely determine the relative positions of the nodes. E F G B C D A I I C H C I

11 Overview Kalman Filter Approaches Localization SLAM SLAM with inter-beacon measurements Batch Optimization Self-Localization with Virtual Nodes

12 Overview Kalman Filter Approaches Localization SLAM SLAM with inter-beacon measurements Batch Optimization Self-Localization with Virtual Nodes

13 Range-only Localization Highly non-linear measurements make localization challenging Assume Gaussian Noise Model Kalman Filter Linearize range measurements about the estimate of the robot’s position

14 Kalman Filter Localization to SLAM q k = [x k, y k,  k ] T q k = [x k, y k,  k, … x b i, y b i … ] T Localization State Vector: qk ~ 3×1 Pk ~ 3×3 Known Beacon Positions SLAM State Vector: qk ~ (3+2n)×1 Pk ~ (3+2n)×(3+2n) n = # of initialized beacons Unknown Beacons Beacon Initialization is important

15 Range-only SLAM – Initialization Beacon Initialization 1 Occupancy grid based voting scheme Intersections of range measurements vote towards discretized cells within the grid Peaks in the grid represent likely beacon locations Dealing with Noise Requires a large number of measurements Time to initialize beacon increases with noise 1. E. Olson, J. Leonard, and S. Teller, “Robust range-only beacon localization,” in Proceedings of Autonomous Underwater Vehicles, 2004, 2004.

16 Incorporating Inter-beacon Measurements Benefits of Inter-beacon Measurements Initialized beacons can help initialize other beacons Significantly improves time required to initialized new beacons Provides additional constraints that help maintain the accuracy of the estimates Incorrect initialization could significantly affect other beacon’s estimate Robot to Beacon Meas. Only Adding Beacon to Beacon Meas.

17 Overview Kalman Filter Approaches Localization SLAM SLAM with inter-beacon measurements Batch Optimization

18 Time History Information Gain Adding time history increases state dimension and computation time Estimating robot’s pose (x r,y r,z r ) and N beacon positions (x b,y b ) over M odometric time steps State vector without time history: 3 + 2*N dimensions State vector with time history: 3*T + 2*N dimensions (For T=2500 and N=7  17 vs. 7514) Maintaining Time History Current Measurements The Kalman Filter

19 Batch SLAM Minimize the error in all inter-beacon measurements received and the beacon location estimates The Cost Function: Batch optimization achieves the best possible solution at the cost of computation time Batch SLAM ~ 45min vs. KF SLAM ~ 30sec Cost for deviating from robot’s odometry Cost of errors in range measurements

20 System & Experiment

21 Evaluating SLAM Results SLAM using only Robot to Beacon range measurements Step #1 Step #2 Step #3 Perform SLAM – Compute path and beacon estimatesTransform estimates to global Coordinates Perform Rotation and Translation on beacon estimates to best fit true beacon positions Using the transformed estimates of the beacon positions perform Kalman filter localization to evaluate filter performance

22 Conclusion Only robot to beacon measurements Including beacon to beacon measurements Mobile Sensors help remove mapping ambiguities Additional constraints from Inter-beacon measurements improve both localization and map estimates Batch optimization using time history provides the best results at the cost of computation time

23 Thank You! Questions?