Range-Only SLAM for Robots Operating Cooperatively with Sensor Networks

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 robots position and the
  positions of landmarks/beacons in the world

3
Motivation

• SLAM Estimating the robots position and the
  positions of landmarks/beacons in the world

4
Motivation

• SLAM Estimating the robots position and the
  positions of landmarks/beacons in the world
• Range-Only sensors provide highly nonlinear
  measurements

5
Motivation

• SLAM Estimating the robots 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
I
D
E
B
G
H
C
A
F
A clique (fully connected sub-graph) of degree 4
or higher is required to uniquely determine the
relative positions of the nodes.
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 robots position

14
Kalman FilterLocalization to SLAM

• Localization
• State Vector
• qk 31
• Pk 33
• Known Beacon Positions

• SLAM
• State Vector
• qk (32n)1
• Pk (32n)(32n)
• n of initialized beacons
• Unknown Beacons
• Beacon Initialization is important

qk xk, yk, ?kT
qk xk, yk, ?k, xbi, ybi T
15
Range-only SLAM Initialization

• Beacon Initialization1
• 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
Robot to Beacon Meas. Only

• 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 beacons estimate

Adding Beacon to Beacon Meas.
17
Overview

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

18
Time History Information Gain
Maintaining Time History
Current Measurements
The Kalman Filter

• Adding time history increases state dimension and
  computation time
• Estimating robots pose (xr,yr,zr) and N beacon
  positions (xb,yb) over M odometric time steps
• State vector without time history 3 2N
  dimensions
• State vector with time history 3T 2N
  dimensions(For T2500 and N7 ? 17 vs. 7514)

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 robots 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 estimates
Transform 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?