Transcript cnself 1614

Physics-based Sensing and
State Estimation Algorithms for
Robotic Sensor Networks
Gaurav. S. Sukhatme
Robotic Embedded Systems Lab
University of Southern California
IPAM
5/17/2002
robotics.usc.edu/~embedded
Sensing and State Estimation Algorithms for
Robotic Sensor Networks
based on Nature
Gaurav. S. Sukhatme
Robotic Embedded Systems Lab
University of Southern California
IPAM
5/17/2002
robotics.usc.edu/~embedded
Robotics and Sensor Networks
• Sensor Nets: Couple information gathering,
processing, and communication resources to
the physical environment to monitor,
understand, and ultimately affect the physical
world
• Robots:
– Improve nature and quality of sensing
– Effect changes in the physical world by
manipulation and motion
robotics.usc.edu/~embedded
Sensor Coordinated Actuation
• Physical phenomena occur at diverse spatial and
temporal scales
• Synoptic sampling at arbitrarily fine spatio-temporal
scales is impossible with a finite number of sensors
• Actuation provides a means to focus sensing where it
is needed, when it is needed
• To enable sensing of uncharted phenomena, in
dynamic, unknown, and partially observable
environments, actuation cannot be based on planning
a priori
• Actuation for improved sensing critically depends on
using sensor data in situ to reposition and refocus
sensors
robotics.usc.edu/~embedded
Challenges
• Coordinate the actions of large number of
actuated nodes carrying sensors without
central control (or even representation)
• Sub-problems: Localization, Navigation,
Deployment, Mapping, Calibration, Energy
Management, Connectivity
robotics.usc.edu/~embedded
Localization, Mapping, Deployment &
Navigation
• Cooperative Localization and Mapping: robots
estimate poses relative to each other using
intermittent observations and maximum likelihood
estimation
• Deployment: robots spread out through local
interactions (potential fields) to cover an unknown
environment with sensors
• Navigation: robots leave trail markers (like ants) and
navigate from source to sink
robotics.usc.edu/~embedded
Localization
(x6,y6)
(x7,y7)
(x5,y5)
(x4,y4)
y
(x0,y0)
(x3,y3)
(x1,y1)
(x2,y2)
x
robotics.usc.edu/~embedded
Localization
(x6,y6)
(x7,y7)
(x5,y5)
(x4,y4)
y
(x0,y0)
(x3,y3)
(x1,y1)
(x2,y2)
x
robotics.usc.edu/~embedded
Localization
• Past approaches (ours and others) include
– filtering inertial sensors for location estimation
– using landmarks (based on vision, laser etc.)
– using maps
• Algorithms vary from Kalman filters, to
Markov localization to Particle filters
• Our approach for sensor networks
– Exploit communication for in-network localization
– Physics-based models
robotics.usc.edu/~embedded
Static Localization
• System contains beacons and
beacon detectors
• Assumptions:
• beacons are unique,
• beacon detectors determine
correct identity.
• Static localization:
• determine the relative pose of
each pair of beacons/detectors
robotics.usc.edu/~embedded
Mesh Definition: Damped spring
mass system
robotics.usc.edu/~embedded
Mesh Energy
Kinetic
energy
Potential
energy
1
Vi  mi xi2
2
V  Vi
1
U j  k j ( z j  z j )2
2
z j  ( xai , xb j )
i
( z j  xb j  xa j )
U  U j
j
robotics.usc.edu/~embedded
Mesh Forces and Equations of Motion
Forces
Fi   xiU   
j
Equations
of motion
z j U j
xi z j
0  xi  xi  Fi / mi
As
t   V  0,U  U min
robotics.usc.edu/~embedded
Encoding
robotics.usc.edu/~embedded
SLAM: Simultaneous Localization and
Mapping
robotics.usc.edu/~embedded
Multi-robot SLAM
robotics.usc.edu/~embedded
Sensor Network Calibration
robotics.usc.edu/~embedded
Extension
• Relaxation not the only way to minimize
energy
• Significantly more efficient techniques exist
– Equivalent to MLE approaches
robotics.usc.edu/~embedded
Team Localization using MLE
• Construct a set of estimates H = {h} where:
h is the pose of robot r at time t.
• Construct a set of observations O = {o} where o is
either:
the measured pose of robot rb relative to robot ra at time t, or
the measured change in pose of robot r between times ta and tb.
• Assuming statistical independence between observations
find the set of estimates H that maximizes:
robotics.usc.edu/~embedded
Approach
Equivalently, find the set H that minimizes:
robotics.usc.edu/~embedded
Gradient-based Estimation
• Each estimate
• Each observation
• Measurement
uncertainty assumed
normal
• Relative Absolute
ˆ, r, t )
h  (q
o  (  , , ra , ta , rb , tb )
U (o | H ) 
1
(   ˆ )T (   ˆ )
2
ˆ  ( qˆa , qˆb )
robotics.usc.edu/~embedded
Gradient Descent

μˆ 
U(O|H)  
U(o|H)
ˆ
h
oO h μ
•Compute set of poses q that minimizes U(O|H)
•Gradient-based algorithm
robotics.usc.edu/~embedded
Results (small environment)
robotics.usc.edu/~embedded
Range Error vs. Time
Robots bump
into each other
robotics.usc.edu/~embedded
Bearing Error vs. Time
robotics.usc.edu/~embedded
Orientation Error vs. Time
robotics.usc.edu/~embedded
Results (large environment)
robotics.usc.edu/~embedded
Team Localization
–
–
–
–
–
–
–
Runs on any platform as long as it can compute its motion via
inertial sensing
Unique beacons: robots, people, fixed locations etc.
No model of the environment
Indifferent to changes in the environment
Robust to sensor noise
Permits both centralized and distributed implementation
Will soon be available as part of Player on sourceforge as a
service
A. Howard, M. Mataric, G. Sukhatme, Localization for Mobile Robot Teams: A MLE
Approach, USC Technical Report IRIS-01-407, 2001
A. Howard. M. Mataric, G. Sukhatme, Localization for Mobile Robot Teams using MLE,
submitted to IROS 2002
A. Howard, M. Mataric, G. Sukhatme, Team Localization: A MLE Approach, submitted to IEEE
TRA
robotics.usc.edu/~embedded
The Deployment Algorithm
Each robot is controlled according to virtual forces
Fi  U  (Uo  U n )  Fo  Fn
U o|n
1
 ko|n 
i ri
1
Fo|n  ko|n  2
i ri
x  ( F  x ) / m
robotics.usc.edu/~embedded
Deployment Stages
robotics.usc.edu/~embedded
Static Deployment using a Potential Field
robotics.usc.edu/~embedded
Coverage vs. Time
robotics.usc.edu/~embedded
Physics is Great but …
• Many social animals exhibit self organized, fault
tolerant, redundant, scalable, real time
behavior
• Social insects in particular exhibit all of the
above and are remarkably simple
– The emergent complexity is in the interaction
– The system works because of communication
• Robot navigation using trail following
robotics.usc.edu/~embedded
Resource Transportation
• Team of N robots starts
from a `home’ location
A in an unknown
environment
• Explore to find supply of
resource at B
• Carry resource home
and return for more
• Exemplified by foraging
of ants & bees
robotics.usc.edu/~embedded
Inspiration: Ant Foraging
• Bring food
back to the
nest
• Lay chemical
trails for each
other to follow
(stigmergy)
robotics.usc.edu/~embedded
Trails in Localization Space
• Abstract away the chemical trail
• Communicate landmarks in shared
localization space
• Announce trails only when successful
• Pros: adaptive, robust, distributed, scalable +
lightweight, simple
• Cons: not necessarily optimal, robots must be
localized
robotics.usc.edu/~embedded
Implementation requirements
• Low bandwidth communication
• Exploration/Navigation:
– In order to generalize, trail following should be
independent of local navigation strategy
– Local navigation can (must!) be designed to match the
environment
– Large populations make random walk attractive
• Localization:
– trail following should be robust wrt. realistic localization
error
robotics.usc.edu/~embedded
Robot “Crumb Trails”
• A crumb is a landmark/signpost in localization
space, indicating the direction to move next
• Trail represented by a linked list of “crumb” data
structures
• Each crumb contains
– a position (x,y)
– a suggested heading 
– a timestamp t
robotics.usc.edu/~embedded
Trail Laying Algorithm: the private crumb list
start
N
Moving ?
Y
N
>Ps since
last crumb ?
Y
Add crumb to private list
robotics.usc.edu/~embedded
Trail Laying Algorithm: the public crumb list
start
Carrying
resource ?
Y
N
N
N
Reached
Goal ?
Reached
Goal ?
Y
Y
Announce private crumbs,
clear private crumb list
robotics.usc.edu/~embedded
Trail Laying Algorithm:
fresh crumbs
start
Received
announced
crumb ?
N
Y
Add crumb to public list
N
Is there a
crumb older
than Qs ?
Y
Delete crumb
robotics.usc.edu/~embedded
Trail Reading
Obstacle
Robot
Suggested movement =
vector sum of crumbs within sense radius
robotics.usc.edu/~embedded
Experiments
•
•
•
•
Localization: Gaussian error model
Communication: UDP broadcast
Local navigation designed for environment
Tested with three controllers
– random
– directed
– ant
robotics.usc.edu/~embedded
Directed
Ant
Frustration
switch
Frustration
Heading
to goal
RandomWalk FollowHeading
switch
RandomWalk FollowHeading
SteerToAvoid
v, 
Avg crumb
heading
v, 
SteerToAvoid
v, 
robotics.usc.edu/~embedded
v, 
Results
robotics.usc.edu/~embedded
Results - Population Size
robotics.usc.edu/~embedded
Results - Localization Error
robotics.usc.edu/~embedded
robotics.usc.edu/~embedded
Resource Transport Summary
• Demonstrated cooperative search and transport
• Our trail laying/following method
–
–
–
–
–
is robust wrt. real-world odometry error
has low computation and bandwidth requirements
scales linearly with population
is independent of navigation strategy
scales to N sources and sinks
robotics.usc.edu/~embedded
Dynamic Deployment (Patrolling) using Beacons
robotics.usc.edu/~embedded
Conclusions
• Sensor Nets: sensing and communications
coupled to the physical world
• Robots can:
– Improve nature and quality of sensing
– Effect changes in the physical world by
manipulation and motion
• Localization and Deployment: efficient,
scalable services for robotic sensor networks
robotics.usc.edu/~embedded
• More at http://robotics.usc.edu/~embedded
• Robot simulator
http://playerstage.sourceforge.net
• Support
– NSF, ONR, DOE, DARPA, and Intel
robotics.usc.edu/~embedded