Automation & Robotics Research Institute The … lectures/Koushil Thai...Automation & Robotics...
Transcript of Automation & Robotics Research Institute The … lectures/Koushil Thai...Automation & Robotics...
Automation & Robotics Research InstituteUniversity of Texas at Arlington
Automation & Robotics Research Institute The University of Texas at Arlington
F.L. LewisMoncrief-O’Donnell Endowed Chair
Head, Controls & Sensors Group
Wireless Sensor Networks
Automation & Robotics Research InstituteUniversity of Texas at Arlington
Wireless Sensor Networks2006 Conferences on Cybernetics & Intelligent Systems (CIS)
and Robotics, Automation, & Mechatronics (RAM)
Bangkok
GC I-Ming Chen
Automation & Robotics Research InstituteUniversity of Texas at Arlington
Localization of a Wireless Sensor Network with Unattended Ground Sensors and Some
Mobile Robots
Koushil SreenathMS Thesis, 2006
Frank Lewis, Dan PopaThe University of Texas at Arlington, United States
Automation & Robotics Research InstituteUniversity of Texas at Arlington
Localization - Contributions
Simultaneous Localization of WSNDistributed Kalman filtersGeometric constraints of radio connectivity
Adaptive Localization StrategyNavigate to a location to maximally reduce localization uncertainty.
Automation & Robotics Research InstituteUniversity of Texas at Arlington
Localization
Localization TechniquesRSSI – Signal strengthTOA – Time of ArrivalAOA – Angle of ArrivalSignal pattern matchingGeometric constraints
Range-free
Mobile Robot with GPS
UGS
Automation & Robotics Research InstituteUniversity of Texas at Arlington
1. Sensor Localization using Mobile Robot
[ ]TyxX αφ=
( )
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
==
αωα
φαφα
αφ sin
sincoscoscos
,L
vvv
yx
txaXt
t
t
&
&
&
&
&
⎥⎥⎦
⎤
⎢⎢⎣
⎡== Bot
y
BotxBotBot
kRσ
σσσ
00
,
const
BotyBot
yconst
BotxBot
xRangeRangeσ
σσ
σ == ,
1. At each discrete time instant,2. if robot broadcast received by sensor3. then4. Update sensor state and uncertainty estimates using KF measurement update equations.5. else6. Propagate estimates using time update equations.7. end if
Robot Dynamics
Sensor localization algorithm
[ ]Tiiik yxx =
ik
ik
ik
ik
ik
ik
ik wGuBxAx ++=+1
( ) ( ) ( )ik
ik
ik
ik
ix
ii RvQwPxx ,0,,0,,000 ===
⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡=
1001
,1001
,0000
,1001 i
kik
ik
ik HGBA
Sensor Dynamics
ik
ik
ik QPP +=
−+1
ik
ik xx ˆˆ 1 =
−+
Sensor time update
1
11
11
1
11
−
+−
++
−−++ ⎥
⎦
⎤⎢⎣
⎡+= i
kkTi
kik
ik HRHPP
⎟⎠⎞⎜
⎝⎛ −+=
−+++
−+++
−++
ik
ik
ikk
Tik
ik
ik
ik xHzRHPxx 111
111111 ˆˆˆ
Sensor measurement update
Kalman Filter
Automation & Robotics Research InstituteUniversity of Texas at Arlington
1. Sensor Localization using Mobile Robot
Automation & Robotics Research InstituteUniversity of Texas at Arlington
2. Simultaneous Localization of Static Sensor and Mobile Robot
( ) ( )wtGtuXaX += ,,&
[ ][ ] ugs
kkugsugs
k
gpskk
gpsgpsk
vktXhZ
vktXhZ
+=
+=
),(
),(
( ) ( ) ( ) ( ) ( ) ( )ugsugsk
gpsgpsk RvRvQtwPXX ,0,,0,,0,,0 00 ====
( ) ( )⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
0000000000100001
,sin
sincoscoscos
, tGL
vvv
yx
tXat
t
t
αωα
φαφα
αφ&
&
&
&
[ ] [ ] ⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡=
yx
ktXhyx
ktXh kugs
kgps ),(,),(
( )
( ) ( )
( ) ( )⎥⎦
⎤⎢⎣
⎡=
∂∂
=
⎥⎦
⎤⎢⎣
⎡=
∂∂
=
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡−−−
=∂
∂=
00100001,
00100001,
0000cos000
sinsincoscos00cossinsincos00
),(,
XkXhXH
XkXhXH
Lvvvvv
XtXa
tXA
ugsugs
gpsgps
t
tt
tt
α
φαφαφαφα
Robot Dynamics( )( ) ( ) TT GQGtXPAPtXAP
tuXaX
++=
=
,ˆ,ˆ,,ˆˆ
&
&
Robot time update
( ) ( )( ) ( )[ ] ( )
( )[ ]kXhZKXX
tPXHKItP
RXHtPXHXHtPK
kugsugs
kkkk
kkugs
kk
ugsk
Tugskk
ugsk
Tugskk
,ˆˆˆ
ˆ
)ˆ()()ˆ(ˆ1
−−
−−
−−−−−−
−+=
−=
⎥⎦⎤
⎢⎣⎡ +=
Updates from UGS
( ) ( )( ) ( )[ ] ( )
( )[ ]kXhZKXX
tPXHKItP
RXHtPXHXHtPK
kgpsgps
kkkk
kkgps
kk
gpsk
Tgpskk
gpsk
Tgpskk
,ˆˆˆ
ˆ
)ˆ()()ˆ(ˆ1
−−
−−
−−−−−−
−+=
−=
⎥⎦⎤
⎢⎣⎡ +=
const
iyi
yconst
ixi
x
iy
ixiiiugs
k
RangeRange
PR
σσ
σσ
σσ
σσ
==
⎥⎥⎦
⎤
⎢⎢⎣
⎡=⎥⎦
⎤⎢⎣⎡ +=
,
00
,22
Robot measurement update
Updates from GPS
1. Navigate robot along desired path.2. Broadcast location information at discrete intervals.3. if broadcast from GPS received4. Update robot state and uncertainty estimates using GPS measurement equation.5. end if6. if broadcast from sensor received7. Update robot state and uncertainty estimates using UGS measurement equation.8. end if
Robot localization algorithm
⎥⎦⎤
⎢⎣⎡ +=
22 BotBotXYk PR σ
Robot measurement update
Jacobian linearization
Automation & Robotics Research InstituteUniversity of Texas at Arlington
2. Simultaneous Localization of Static Sensor and Mobile Robot
Automation & Robotics Research InstituteUniversity of Texas at Arlington
3. Simultaneous Adaptive Localization
1. Broadcast Navigation request, NAV-REQ, packet.2. Wait to receive Localization request, LOC-REQ, packets.3. for all LOC-REQ with the same friendly neighbor4. Combine uncertainty scalars of the requesting sensors.5. end for6. Pick friendly neighbor with maximum combined uncertainty scalar of the requesting sensors.7. if multiple maximas arise8. Among the maxima, pick the most localized friendly neighbor.9. end if10. Navigate around the picked friendly neighbor executing the simultaneous localization algorithm11. Repeat Steps 1-10 as required.
Adaptive localization algorithm
LOC-REQ packet flow
NAV-REQ packet flow
Formal Comm. Packet Structure & Protocol
Automation & Robotics Research InstituteUniversity of Texas at Arlington
Simultaneous Adaptive LocalizationAdaptive localization
Localization error Localized sensors
Automation & Robotics Research InstituteUniversity of Texas at Arlington
ARRI Distributed Intelligence & Autonomy LabDIAL
UnattendedGroundSensors
SmallmobileSensor-Dan Popa
Testbed containing MICA2 network (circle), Cricket network (triangle), Sentry robots, Garcia Robots & ARRI-bots
Automation & Robotics Research InstituteUniversity of Texas at Arlington
Publications
K. Sreenath, F.L. Lewis and D.O. Popa, "Simultaneous Adaptive Localization of a Wireless Sensor Network," ACM Mobile Computing and Communication Review, submitted.
Automation & Robotics Research InstituteUniversity of Texas at Arlington
Conclusion
LocalizationSimultaneous localization of WSN
Localization of UGS by Mobile RobotLocalization of Mobile Robot by GPS & UGSJoint localization
Adaptive localization algorithmFormal communication packet protocol