Connectivity-based Localization in Robot Networks

Transcription

1 Connectivity-based Localization in Robot Networks Tobias Jung, Mazda Ahmadi, Peter Stone Department of Computer Sciences University of Texas at Austin Summary: Localization in an ad-hoc wireless sensor network made up of mobile robots Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.1/14

2 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

3 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

4 Application Base Intelligent exploration Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

5 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

6 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

7 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

8 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

9 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

10 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

11 Application Base Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.2/14

12 Motivation ÔÖÓ Ð Ñ Ë Ð ¹ÓÔØ Ñ Þ Ø ÓÒ Ò Ð ¹ Ð Ò ÛÓÙÐ ÐÓØ Ö Û Ö Ð Ø Ú ÐÓ Ð Þ Ø ÓÒº ÇÙÖ ÐÓ Ð Þ Ò Ø ÒÓ ÙÐØ ÓÖ Ú Ö ÓÙ Ö ÓÒ ººº ÍÒ ÓÖØÙÒ Ø ÐÝ Ï ÓÒ Ö Ø ÓÐÐÓÛ Ò Ö Ð¹ÛÓÖÐ ÔÖÓ Ð Ñ ÇÚ Ö ÐÐ Ó Ø Ú Ò Ð Ô Ö Ø ÒØ Ö Ó ÓÑÑÙÒ Ø ÓÒ Ò ÙÐØ ÒÓÒ¹ÄÇ˵ ÒÚ ÖÓÒÑ ÒØ ÔÔÖÓ ÑÔÐÓÝ Ø Ñ Ó ÒØ ÐÐ ÒØ ÙØÓÒÓÑÓÙ ÖÓ ÓØ Û Ö ÖÓ ÓØ Ö ÐÐÝ ÑÓ Ð Ò Ð Ö Ð Ý ÒÓ Ò Û ØÓ Ø Ö Ö Ø Ò Ù Ø Ò Ø ÑÔÓÖ ÖÝ ÓÑÑÙÒ Ø ÓÒ Ò ØÛÓÖ º = ÖÓ Ù Ø ÔØ Ú Ò ØÓØ ÐÐÝ ÑÓ Ð Ò ØÛÓÖ Ë Ð ¹ÓÒ ÙÖ Ø ÓÒ Ë Ð ¹ÓÔØ Ñ Þ Ø ÓÒ Ë Ð ¹ Ð Ò Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.3/14

13 Problem Our goal: relative localization. Estimate for every robot direction distance to every other node in the network. (I.e. determine location + heading for every node) Challenges: Deployment in uncharted territory = no prior map Robots lack sensor for mapping environment = no SLAM Robots lack GPS, robots lack compass No beacon nodes/landmarks with known location = only information we have is joint connectivity Total number of robots is small = techniques relying on dense coverage fail (MDS-MAP, Hop-count, PCA, GPLVM,...) Robots may have to operate under non-los conditions = RSS distance Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.4/14

14 Our Approach Underdetermined problem: connectivity from just a single time step doesn t carry enough information to reliably estimate underlying locations (robots have a heading). Solution: History-based global approach: jointly estimate positions of robots 1. Combine individual connectivity and motion history 2. Estimate locations such that the induced connectivity graph is most consistent with the observed connectivity graph ( time steps in the history) Key idea: Exploit mobility to obtain connectivity measurements from multiple time steps Exploit individual odometry to align connectivity measurements over time Jointly estimate the start poses for all robots through minimization of an error function (ML solution) Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.5/14

15 Individual motion Pose of i-th robot at time t: x-location, y-location, heading Assume initial pose is unknown (x 3 i, y3 i ) d 2 θ,i y (x 2 i, y2 i ) d 3 θ,i = 0 θi 4 δi 3 θi 3 δ 2 θi 2 i (x 1 i, y1 i ) (0, 0) x For every t, we can observe odometry δ t i d t θ,i distance moved forward (translation) change in heading (rotation) Together with a guess for initial pose, (x start i, yi start, ϕ start i ) we can expand a (hypothetical) path for the robot ˆθ(i, t) = ϕ start i + ˆx(i, t) = x start i + ŷ(i, t) = y start i + t k=3 t k=2 t k=2 d θ (i, k) δ(i, k)cos (ˆθ(i, k) ) δ(i, k)sin (ˆθ(i, k) ). Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.6/14

16 Algorithm outline t=2 Start poses for all robots x (unknown) (+odometry) Estimated locations for all robots (+model for connectivity) Induced connectivity observed connectivity t=3 t=4 t=3 t=2 t=4 t=3 t=4 t=1 Robot #3 t=2 Idea: Find x such that Induced connectivity observed connectivity t=1 Robot #1 t=1 Robot #2 Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.7/14

17 Two basic scenarios 1. Range-based localization Assumes we can observe underlying distance whenever two robots communicate Observable data is thus For every individual robot i δ(i, t) distance moved forward d θ (i, t) change in heading For every pair i, j of robots c(i, j, t) binary connectivity (i, j, t) underlying distance (e.g. TDoA hardware) Solve min x E( x) = 1 2 T t=1 N 1 i=1 N j=i+1 [ˆ x (i, j, t) (i, j, t) ] 2 c(i, j, t) where x := (x start 1, ystart 1, ϕstart 1,..., xstart N, ystart N, ϕstart N ) unknown initial poses ˆ x (i, j, t) distance between (ˆx t i, ŷt i ) and (ˆxt j, ŷt j ) derived from x. Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.8/14

18 Two basic scenarios 2. Connectivity-based localization Assumes we can only observe connectivity. Observable data is thus For every individual robot i δ(i, t) distance moved forward d θ (i, t) change in heading For every pair i, j of robots c(i, j, t) binary connectivity P(connected) P(not connected) distance [m] distance [m] ML approach: solve min x E( x) = T t=1 N 1 i=1 N j=i+1 connected {}}{ {c t ij log ( 1 Φ µ,σ 2(ˆ x (i, j, t) )) + under the observation model for observing connectivity p(c t ij = 1 ˆxt i, ŷt i, ˆxt j, ŷt j ) := 1 Φ µ,σ 2 (ˆ x (i, j, t) ) p(c t ij = 0 ˆxt i, ŷt i, ˆxt j, ŷt j ) := Φ µ,σ 2 (ˆ x (i, j, t) ) not connected {}}{ (1 c t ij (Φ )log µ,σ 2 (ˆ x (i, j, t) ))} where Φ µ,σ 2 is the Gaussian cdf for µ, σ 2. Ignores underlying topology!! Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.9/14

19 Experiments: setup All experiments were done in simulation (CybelePro from IAI) We consider two environments: Indoor: Outdoor: RSS proportional to distance 1 if (i, j, t) R max c(i, j, t) = 0 else For each environment, we consider the scenarios 1. Range-based 2. Connectivity-based Models signal attentuation due to impeding walls [Shell & Mataric, 2009] Observations corrupted by noise: zero noise (0%), low noise (1%), med noise (10%) Number of robots N = 10 Time horizon T = 60sec (simulation step 100msecs, measurements at the rate of 1Hz) Movement of robots controlled by external explorative behavior Minimization via scaled conjugate gradients. Due to nonconvexity multiple random restarts (100). Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.10/14

20 Experiments: results for outdoor environment Range-based: Connectivity-based: 1.8 Average inter node angular error (in 10 degrees) Average inter node distance error (in meters) 1.6 Average reconstruction error (in meters) Average inter node angular error (in 10 degrees) Average inter node distance error (in meters) 1.6 Average reconstruction error (in meters) zero noise low noise med noise 0.2 zero noise low noise med noise Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.11/14

21 Experiments: results for indoor environment Range-based: 8 Connectivity-based: Average inter node angular error (in 10 degrees) Average inter node distance error (in meters) Average reconstruction error (in meters) 7 6 Average inter node angular error (in 10 degrees) Average inter node distance error (in meters) Average reconstruction error (in meters) zero noise low noise med noise 1 zero noise low noise med noise Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.12/14

22 Experiments: ground truth vs. reconstruction 5 0 y coordinate (meters) x coordinate (meters) Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.13/14

23 Summary Problem: Localization in robotic network just from connectivity Challenges: not unlike localization of nodes in sensor networks, but need to estimate location and heading for every node no anchor nodes with known locations, no GPS, no compass,... small number of nodes just have connectivity Our solution: History-based: obtain multiple measurements odometry of individual robot joint connectivity of network over a small time window. Minimize discrepancy between induced and observed connectivity. Connectivity-based Localization in Robot Networks RWSN 6/10/09 p.14/14