Robotics.usc.edu/~embedded Physics-based Sensing and State Estimation Algorithms for Robotic Sensor...

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


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 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


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


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


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


Localization

(x1,y1)(x0,y0)

(x2,y2)

(x3,y3)

(x4,y4)

(x5,y5)(x6,y6) (x7,y7)

x

y


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


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


Mesh Definition: Damped spring mass system


Mesh Energy

ii

iii

VV

xmV 2

2

1

jj

abj

baj

jjjj

UU

xxz

xxz

zzkU

jj

ji

)(

),(

)(2

1 2

Kineticenergy

Potentialenergy


Mesh Forces and Equations of Motion

j

j

j i

jxi z

U

x

zUFi

min,0

/0

UUVtAs

mFxx iiii

Forces

Equationsof motion


Encoding


SLAM: Simultaneous Localization and Mapping


Multi-robot SLAM


Sensor Network Calibration


Extension

• Relaxation not the only way to minimize energy

• Significantly more efficient techniques exist– Equivalent to MLE approaches


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:


ApproachEquivalently, find the set H that minimizes:


Gradient-based Estimation

• Each estimate• Each observation • Measurement

uncertainty assumed normal

• Relative Absolute

),,ˆ( trqh

),,,,,( bbaa trtro

)ˆ,ˆ(ˆ ba qq

)ˆ()ˆ(2

1)|( THoU


Gradient Descent

U(o|H)μh

μU(O|H)h Oo ˆ

ˆ

•Compute set of poses q that minimizes U(O|H)•Gradient-based algorithm


Results (small environment)


Range Error vs. Time

Robots bumpinto each other


Bearing Error vs. Time


Orientation Error vs. Time


Results (large environment)


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


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


Deployment Stages


Static Deployment using a Potential Field


Coverage vs. Time


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


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


Inspiration: Ant Foraging

• Bring food back to the nest

• Lay chemical trails for each other to follow (stigmergy)


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


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


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


Trail Laying Algorithm: the private crumb list

Moving ?

>Ps sincelast crumb ?

Add crumb to private list

N

N

Y

Y

start


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


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


Trail Reading

Obstacle

Robot

Suggested movement = vector sum of crumbs within sense radius


Experiments

• Localization: Gaussian error model • Communication: UDP broadcast• Local navigation designed for environment• Tested with three controllers

– random– directed– ant


RandomWalk

,v

SteerToAvoid

,v

FollowHeading

Frustration

switchHeadingto goal

Avg crumbheading

Directed

RandomWalk

,v

SteerToAvoid

,v

FollowHeading

Frustration

switch

Ant


Results


Results - Population Size


Results - Localization Error


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


Dynamic Deployment (Patrolling) using Beacons


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


• More at http://robotics.usc.edu/~embedded

• Robot simulatorhttp://playerstage.sourceforge.net

• Support– NSF, ONR, DOE, DARPA, and Intel

http://robotics.usc.edu/~embedded

http://playerstage.sourceforge.net/

Robotics.usc.edu/~embedded Physics-based Sensing and State Estimation Algorithms for Robotic Sensor...

Documents

Transcript of Robotics.usc.edu/~embedded Physics-based Sensing and State Estimation Algorithms for Robotic Sensor...