Key Problems
• Localization
– “where am I ?”
• Fault Detection
– “what’s wrong ?”
• Mapping
– “what is my environment like ?”
1/53
Localization
• Combining Local and Global Information
– localize with local sensing most of the time
– use a map or other absolute information
when available to correct
• Challenges
– how often to use global information ?
– unified framework for local and global
sensing ?
2/53
Localization
• Where is the robot
?
– on my map ?
– in a global
coordinate system ?
– relative to other
robots ?
3/53
Localization: Autonomous
Mobile Nodes
• Integrate angular, linear rates of motion
from onboard sensors (Durant-Whyte 91)
• Focus on reducing orientation error
• Combine a compass (or any absolute
direction sensor) with a gyroscope (a
rate sensor)
4/53
The Kalman Filter
Sensor
measurements
Scaled
residual
+
Update
Propagate
System
model
Sensor
model
X
residual
-
Kalman
gain
5/53
System Model: Robot Kinematics
xk  1  xk  vavgdt sin k
yk  1  yk  vavgdt cos k
k  1  k   / tdt
vavg  ( vL  vR ) / 2
6/53
Filter
Measurement vector
Propagate
x  [ / t vL vR]
xk  1 / k  xk / k
Pk  1 / k  Pk / k  Q
T
Update
xk  1 / k  1  xk  1 / k  K ( res )
Pk  1 / k  1  ( I  K ) Pk  1 / k
7/53
Results
• 1 order of
magnitude
improvement
using calibrated
odometry and
Kalman filtering
compared to
raw odometry
(Goel, Roumeliotis and Sukhatme 99)
8/53
Absolute Position Sensing (GPS)
• Dual estimator strategy works better
than single filter with all sensor inputs
(Goel, Roumeliotis and Sukhatme 99)
GPS
GPS
available ?
KF1
x,y
KF2
x,y
yes
no
Encoders,Gyros
9/53
Application to Fault Detection
– Multiple Kalman
Filters in parallel
– Each with a
different model of
either the sensors
or the robot
kinematics
– Analyze residual
signatures
10/53
Flat Tire: Residuals vs. Time
KF Nominal
Right tire
flat
Left tire
flat
(Roumeliotis, Sukhatme, Bekey 98a)
11/53
Gyro Failure
Residuals post-processed with Bayesian hypothesis testing
(Roumeliotis, Sukhatme and Bekey 98b)
12/53
Multi-node Mapping
13/53
Algorithm Outline
• Individual nodes detect features
• Individual nodes create topological
maps with approximate metric
information
• Match algorithm finds best match using
heuristic pruning
• Combined map displayed to user
14/53
Landmark Detection and
Mapping
Sonars
Odometry
Line-Segment
Extractor
left &
right
left &
right
Color Blob
Laser Scan
front
Opening
Corridor
End-of-corridor
Door Detector
Door Detector
Detector
Detector
Detector
(Color Vision)
(Laser)
Map
Builder
15/53
Map Representation
• Augmented graph
– Vertices are landmark elements
– Links are metric connections
struct node{
id
type // corner, junction, door
x, y
// approx coordinates
struct link[4]
// 4 possible
// directions
visit_counter
detection_counter
}
struct link{
connected_to_id
type
// open space, blocked,
// door
heading
// in local frame
compass
distance
travel_counter
}
16/53
Area Explored
Robot 1
Robot 2
Both
17/53
Individual Maps
18/53
Candidate Transformations
• Two maps (graphs) with n and m nodes
respectively
• General graph isomorphism problem is
NP complete
• Most known algorithms are procedural
in nature and exponential in complexity
19/53
Match Heuristics
• H1: pair landmarks of same type
attribute (e.g doors with doors, corners
with corners)
• H2: consider only landmarks that
describe invariant spatial features (e.g.
corners and junctions)
20/53
Scaling
• Reduces the
number of
candidate
transformations
to about 20%
of n*m
21/53
Final Matching
• Check for mismatch in absolute heading
value
• Compute translation and rotation
transformation for each remaining pair
• The transformation that yields the
highest number of overlapping
landmarks is the best match
22/53
Results of Match Algorithm
23/53
Results
• Tested on over 50 trials over ~2 km of
traverse
– heuristic pruning never resulted in best
match being discarded
– in 96% of the cases, top two matches
produced contain the correct result
(Dedeoglu and Sukhatme 00)
24/53
Pointers
• Papers
robotics.usc.edu/~gaurav/publications.html
• Projects
SCOWR
MARS
TMR
SUO
netweb.usc.edu/scowr
robotics.usc.edu/projects/mars
robotics.usc.edu/projects/tmr
robotics.usc.edu/~avatar
25/53