robotics.usc.edu/~embedded
Physics-based Sensing and State Estimation Algorithms for
Robotic Sensor Networks
Gaurav. S. Sukhatme
Robotic Embedded Systems LabUniversity of Southern California
IPAM5/17/2002
robotics.usc.edu/~embedded
Sensing and State Estimation Algorithms for
Robotic Sensor Networksbased on Nature
Gaurav. S. Sukhatme
Robotic Embedded Systems LabUniversity of Southern California
IPAM5/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
(x1,y1)(x0,y0)
(x2,y2)
(x3,y3)
(x4,y4)
(x5,y5)(x6,y6) (x7,y7)
x
y
robotics.usc.edu/~embedded
Localization
(x1,y1)(x0,y0)
(x2,y2)
(x3,y3)
(x4,y4)
(x5,y5)(x6,y6) (x7,y7)
x
y
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
ii
iii
VV
xmV 2
2
1
jj
abj
baj
jjjj
UU
xxz
xxz
zzkU
jj
ji
)(
),(
)(2
1 2
Kineticenergy
Potentialenergy
robotics.usc.edu/~embedded
Mesh Forces and Equations of Motion
j
j
j i
jxi z
U
x
zUFi
min,0
/0
UUVtAs
mFxx iiii
Forces
Equationsof motion
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
ApproachEquivalently, find the set H that minimizes:
robotics.usc.edu/~embedded
Gradient-based Estimation
• Each estimate• Each observation • Measurement
uncertainty assumed normal
• Relative Absolute
),,ˆ( trqh
),,,,,( bbaa trtro
)ˆ,ˆ(ˆ ba qq
)ˆ()ˆ(2
1)|( THoU
robotics.usc.edu/~embedded
Gradient Descent
U(o|H)μh
μU(O|H)h Oo ˆ
ˆ
•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 bumpinto 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
nonoi FFUUUF )(
i i
nono rkU
1||
i i
nonor
kF 2||
1
mxFx /)(
Each robot is controlled according to virtual forces
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
Moving ?
>Ps sincelast crumb ?
Add crumb to private list
N
N
Y
Y
start
robotics.usc.edu/~embedded
Trail Laying Algorithm: the public crumb list
Carrying resource ?
Reached Goal ?
N
Y
N
start
Reached Goal ?
N
Announce private crumbs, clear private crumb list
Y
Y
robotics.usc.edu/~embedded
Trail Laying Algorithm: fresh crumbs
Receivedannounced
crumb ?
Is there acrumb older
than Qs ?
N
Y
Y
start
Add crumb to public list
Delete crumb
N
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
RandomWalk
,v
SteerToAvoid
,v
FollowHeading
Frustration
switchHeadingto goal
Avg crumbheading
Directed
RandomWalk
,v
SteerToAvoid
,v
FollowHeading
Frustration
switch
Ant
robotics.usc.edu/~embedded
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 simulatorhttp://playerstage.sourceforge.net
• Support– NSF, ONR, DOE, DARPA, and Intel