Further Results with Localization and Mapping Using Range from Radio235 Results In order to evaluate the performance of the filter we turn to the two commonly used error metrics, the Cross-Track Error (XTE) and the Along-Track Error (ATE). The XTE accounts for the position error that is orthogonal to the robot’s path (i.e. orthogonal to the true robot’s orientation), while the ATE accounts for the tangential component of the position error. As part of our error analysis of the path estimates, we observe the average of the absolute values of the XTE and ATE for each point in the path, as well as the maximum and standard deviation of these errors. In the experiment illustrated here, the true initial robot position from GPS was used as the initial estimate. Furthermore, the location of each tag was known. Figure 1 shows the estimated path using the Kalman filter, along with the GPS ground truth (with 2 cm accuracy) for comparison. 0 20 40 60 80 100 120 −10 0 10 20 30 40 50 60 y position (m) x position (m) Path Estimate from Kalman Filter Localization KF GT beacons Fig .1 . Th ep at he st im at ef ro ml oc aliz at ion (r ed ), gr ou nd tr ut h( bl ue )a nd be ac on lo ca ti on s( *) ar es ho wn .T he fil te ru se s od om et ry an da gy ro wi th ra ng em ea su re - me nt sf ro mt he RF be ac on st ol oc aliz ei t se lf .T he pa th be gin s at (0 ,0) an de nds at (3 3,0) ,t ra ve llin ga to ta lo f3 .7 km an d co mp le ti ng 11 id en ti ca ll oo ps ,w it ht he fina l lo op (0 .343 km )s ho wn ab ov e. (N ot et he a xe sa re fli pp ed ). Num e ri ca lr es ul ts ar e given in Table1. Ta bl e1 . Cr oss- Tr ac ka nd Al on g- Tr ac kE rro rs fo rK alm an fil te rL oc ali za ti on es ti - mate forthe entire data setusingthe Kalman Filter with gyro bias compensation. XTE ATE Me an Abs. 0.3439 m 0.3309 m Max. 1.7634 m 1.7350 m Std. Dev.n 0.3032 m 0.2893 m Failure Sensor Silence. An issue that requires attention while dealing with the Kalman filter is that of extensive sensor silence. When the system encoun- ters a long period during which no range measurements are received from the beacons, it becomes heavily dependant on the odometry and its estimate di- verges. Upon recovering from this period of sensor silence, the Kalman filter is misled into settling at a diverged solution. The Figure 2 shows the failure state of the Kalman filter when presented with a period of sensor silence. In this experiment, all range measurements received prior to a certain time were ignored so that the position estimate is derived through odometry alone. As can be seen in the figure, when the range data starts onc e agai n , the Kalman filter fails to converge to an accurate esti ma te . 0 20 40 60 80 100 0 20 40 60 80 y position (m) x position (m) Path Estimate from Kalman Filter Localization Sensor Silence KF GT beacons Recovery from Sensor Silence Fig .2 . Th ep at he st im at ed ur in gt he ex te nde dp er io do f” sim ul at ed ” se ns or sile nc e (c ya n) ,K alm an fil te r’ sr ec ov er yf ro mt he di ve rg ed solu ti on (r ed ), gr ou nd tr ut h( bl ue ) an db ea co nl oc at ion sa re sh ow n. (N ot et he ax es ar efl ip pe d) .T he fil te ri sn ot ab le to properly recoverfromthe diverged solution resultantofthe initial period of sensor silence. Al th ou gh th is is ch arac te ri st ic of al l Kal ma nfi lt er si ng en e ral ,t his pr ob lem is es pe cia ll yc ri ti ca lw hil ed ea li ng w it hr an ge -o nly se ns o rs .D ue to th ee xt ra level of ambiguityassociatedwitheachrange measurementitbecomesfar easier forthe estimate to converge at an incorrect solution. 4.2LocalizationwithParticleFilter As we se ea bo ve ,t he Kal ma nfi lt er ca nf ai lw he nt he as su mp ti on so fl inea ri ty ca nn ot be ju st ified. In th is ca se ,i ti su se fu lt ol oo ka tm et ho ds li ke Pa rt icle 236 J. Djugash, S. Singh, and P. Corke Further Results with Localization and Mapping Using Range from Radio237 Filters that can converge without an initial estimate. Particle Filters are a way of implementing multiple hypothesis tracking. Initially, the system starts with a uniform distribution of particles which then cluster based on measurements. As with the Kalman filter, we use the dead reckoning as a means of prediction (by drifting all particles by the amount measured by the odometry and gyro before a diffusion step to account for increased uncertainty). Correction comes from resampling based on probability associated with each particle. Position estimates are obtained from the centroid of the particle positions. Formulation The particle filter evaluated in this work estimates only position on the plane, not vehicle orientation. Each “particle” is a point in the state space (in this case the x, y plane) and represents a particular solution. The particle resam- pling method used is as described by Isard and Blake [3]. Drift is applied to all particles based on the displacement estimated by dead reckoning from the state at the previous measurment. Diffusion is achieved by applying a Gaussian distibuted displacement with a standard deviation of B m/s which scales according to intersample interval. Given a range measurement r from the beacon at location X b = ( x b , y b ) the probability for the i ’th particle is P ( r, X b , X i ) = 1 σ √ 2 π e − ( r −|X b − X i | ) 2 σ 2 + P 0 (3) whi ch ha sa ma xi mu mi na cir cle of rad ius r ab ou tt he be ac on wit ha rad ia l cr os s-sec ti on th at is Ga us si an .T he mi nim um pr ob ab ilit y, P 0 ,h elp sr ed uc e pr ob lem sw it hp art icle ex ti nc ti on . σ is re la te dt ot he va ri an ce in th er eceiv ed ran ge me as ur em en ts . It wasfound to be importanttogaterange measurements through anor- ma lize de rror an da ran ge me as ur em en tb an d, [7 ]. In th ee ve nt of am ea su re - me nt ou ts ide th er an ge gat ea no pe n- lo o pu pd at ei sp er fo rm ed ,t he pa rt icle s are dis pla ced by th ed ea dr ec ko ning dis pla cem en tw it ho ut re sa mp ling or dif - fu si on . The location of thevehicleistaken as theprobabilityweighted mean of allparticles. Thereisnoattemptmadetoclusterthe particles so if there are ,f or ex am ple ,t wo dis ti nc tp art icle c lus te rs th em ea nw o uld li eb et we en th em .I nit ia ll yt his es ti ma te ha sa si g nific an tl yd iff er en tv al ue to th ev eh icl e’s po si ti on but co nv er ge sr ap idly .H er ew eu se 1000 pa rt icle s, σ =0. 37, an d B =0. 03. Results In theexperimentillustrated here,the initial conditionf or theparticlesis ba se do n no pr io ri nf orm at io n, th ep art icle sa re dis tr ibu te du nif orm ly ov er a la rge bo unding re ct an gl et ha te nc lo se sa ll th eb ea co ns .T he lo ca ti on of ea ch tag was known apriori. Figure 3(a) contains the plot of the particle filter estimated path, along with the GPS ground truth. It should be noted that the particle filter is a stochastic estimation tool and results vary from run to run using the same data. However it is consistently reliable in estimating the vehicle’s location with no prior information. 0 20 40 60 0 20 40 60 80 100 120 x position (m) y position (m) Path Estimate from Particle Filter Localization beacons PF GT (a ) 0 20 40 60 0 20 40 60 80 100 120 x position (m) y position (m) Path Estimate from Particle Filter Localization Sensor Silence PF GT beacons Recovery from Sensor Silence (b ) Fig .3 . (a )T he pa th es ti ma te fr om lo ca liz at ion u sin ga Pa rt ic le Fi l te r( re d) ,g ro und truth(blue)and beacon locations(*) areshown.The filteruses theodometryand ag yr ow it ha bs olu te me asu re men ts fr om th eR Fb ea co ns to pr od uc et hi sp at he st i- ma te .T he Pa rt ic le Fi lt er is no tg iv en a ny in fo rma ti on re gar di ng th ei ni ti al lo ca ti on of th er ob ot ,h en ce it be gin si ts es ti ma te wi th ap ar ti cl ec lo ud uni fo rml yd ist ri but ed wi th am ea na t( -3 .6 m, -2 .5 m) .T he fina ll oo p( 0.343 km )o ft he d at as et is sh ow n he re ,w he re th eP ar ti cl eF ilt er co nve r ge st oas olu ti on .N um er ic al re su lt sa re giv en in Table2.(b) Thepathestimateduringthe extendedperiodof”simulated”sensor si- lence(cyan), Particle filter’srecoveryfromthe diverged solution (red), ground truth (b lu e) an db ea co nl oc at ion sa re sh ow n. Th efi lt er ea sily re co ve rs fr om th ed iv er ge d solu ti on ,e xhi bi ti ng th et ru en at ur eo ft he pa rt ic le fil te r. The next experiment addressesthe problem of extensivesensorsilence discussedinSection4.1.Whenthe Particle filter is presentedwiththe same scenariothatwas giventothe Kalmanfilter earlier we acquirethe Figure 3( b).This figurereveals theabilityofthe Particle filter to re coverfrom an initially divergedestimate. It canbeobservedthatalthoug hinmostcases the pa rt icle filt er pr od uc es al oc al ly no n- st ab le so lut io n( due to re sa mp ling of th e 238 J. Djugash, S. Singh, and P. Corke Further Results with Localization and Mapping Using Range from Radio239 Table 2. Cross-Track and Along-Track Errors for Particle filter Localization esti- mate for the entire data set. XT E AT E Mean Abs. 0.4053 m 0.3623 m Max. 1.6178 m 1.8096 m Std. Dev. 0.2936 m 0.2908 m pa rt icle s) ,i ts ab l it yt or eco ve rf rom a div er ge ds ol ut io nm ak es it an eff ect iv e lo ca liza ti on al gorit hm . 5 SLAM – Simultaneous Localization and Mapping Here we deal with the case where the location of the radio tags is not known ahead of time. We consider an online (Kalman Filter) formulation that esti- mates the tag locations at the same time as estimating the robot position. 5.1 Formulation of Kalman Filter SLAM The Kalman filter approach described in Section 4.1 can be reformulated for the SLAM problem. Process Model: In order to extend the formulation from the localization case to perform SLAM, we need only to include position estimates of each beacon in the state vector. So, q k = x k y k θ k x b 1 y b 1 x bn y bn T (4) where n is the number of initialized RF beacons at time k . The process used to initialize the beacons is described later in this section. Measurement Model: To perform SLAM with a range measurement from bea- con b , located at ( x b , y b ), we modify the Jacobian H(k) (the measurement matrix) to include partials corresponding to each beacon within the current state vector. So, H ( k ) = ∂h ∂q k | q = q = ∂h ∂x k ∂h ∂y k ∂h ∂θ k ∂h ∂x t 1 ∂h ∂y t 1 ∂h ∂x b ∂h ∂y b , ∂h ∂x tn ∂h ∂y tn (5) wher e, ∂h ∂x ti = ∂h ∂y ti =0,f or ti = b, and 1 ≤ i ≤ n. ∂h ∂x b = − ( x k − x b ) √ ( x k − x b ) 2 +(y k − y b ) 2 ∂h ∂x b = − ( y k − y b ) √ ( x k − x b ) 2 +(y k − y b ) 2 (6 ) On ly theterms in H(k)directlyrelatedtothe current range me asurement (i .e., thepartials withrespect to therobot pose andthe posit ionofthe bea- congiving thecurrent measurement) are non-zero. To complet ethe SLAM fo mu la ti on ,P (th ec ov ari an ce ma tr ix )i se xp an de dt ot he co rr ect dimen ti on - al it y( i.e., 2n +3 squ are )w he ne ac hn ew be ac on is init ia lized . Beacon Initialization: For perfect measurements, determining position from range information is a matter of simple geometry. Unfortunately, perfect mea- surements are difficult to achieve in the real world. The measurements are con- taminated by noise, and three range measurements rarely intersect exactly. Furthermore, estimating the beacon location while estimating the robot’s lo- cation introduces the further uncertainty associated with the robot location. The approach that we employ, similar to the method proposed by Olson et al [10], considers pairs of measurements. A pair of measurements is not sufficient to constrain a beacon’s location to a point, since e ach pair can provide up to two possible solutions. Each meas ur em en t pa ir “votes” for its two solutions (and all its neighb ors ) wit hin a tw od im en si on al probability grid to provide esti ma te s of th e be ac on l oc at io n. Idea lly , s olutions that are near ea ch ot he r in th e wo rl d, sh are th e sa me cell wi th in th e gr id. In ord er to accomplish this requirement, the grid size is chosen such that it matches the total uncertainty in the solution: range measurement uncertainty plus Kalman filter estimate uncertainty. After all the votes have been cast, the cell with the greatest number of votes contains (with high probability) the true beacon location. 5.2 Results from Kalman Filter SLAM In this experiment, the true initial robot position from GPS was used as an initial estimate. There was also no initial information, about the beacons, provided to the Kalman filter. Each beacon is initialized in an online method, as described in Section 5.1. Performing SLAM with Kalman filter produces a solution that is globally misaligned, primarily due to the dead reckoning that had accumulated prior to the initilization of a few beacons. Since, until the robot localizes a few beacons, it must rely on dead reckoning alone for navigation. Although this might cause the Kalman filter estimate to settle into an incorrect global solution, the relative structure of the path is still maintained. In order to properly evaluate the performance of SLAM with Kalman filter, we must study the errors associated with the estimated path, after removing any global translational/rotation offsets that had accumulated prior to the initialization of a few beacons. Figure 4 shows the final 10% of the Kalman filter path estimate after a simple affine transform is performed based on the final positions of the beacons and their true positions. The plot also includes the corresponding ground truth path, affine transformed versions of the final beacon positions and the true beacon locations. Table 3 provides the XTE and ATE for the path shown in Figure 4. Several experiments were performed, in order to study the convergence rate of SLAM with Kalman filter. The plot in Figure 5 displays the XTE and its 1 sigma bounds for varying amounts of the data used to perform SLAM (i.e., it shows the result of performing Localization after performing SLAM on different amounts of the data to initialize the beacons). 240 J. Djugash, S. Singh, and P. Corke Further Results with Localization and Mapping Using Range from Radio241 0 20 40 60 80 100 120 −10 0 10 20 30 40 50 60 y position (m) x position (m) Tranformed Path from Kalman Filter SLAM GT Affine Trans. KF AT final beacon est. true beacon positions Fig .4 . Th ep at he st im at ef r om SL AM us in gaK alm a nF ilt er (g re en ), th ec or re - sp on di ng gr ou nd tr u th (b lu e) ,t ru eb ea c on lo ca ti on s( bl ac k *) an dK alm an Fi lt er es ti ma te db ea co nl o ca ti on s( gr een di mo nd) ar es ho wn .( No te th ea xe sa re fli pp ed ) . Asimpleaffinetransform is performedonthe finalestimatebeaconlocationsfrom theKalman Filter in ordertore-align themisaligned global solution.The path showncorresponds to thefinalloop(0.343 km)ofthe full data setafter theaffine transform. Numerical resultsare given in Table3. Table3. Cross-Trackand Along-TrackErrors fo rthe finalloop(0.343 km)ofthe Data Setafter theAffineTransform. XT E AT E Me an Ab s. 0.5564 m 0.6342 m Ma x. 1.3160 m 1.3841 m St d. De v. 0.3010 m 0.2908 m 0 10 20 30 40 50 60 70 80 90 100 0 1 2 3 4 5 6 7 Percent of Data used for SLAM Cross−Track Error (XTE) XTE for the last 10 percent of the dataset (After Affine Transform) XTE Avg. XTE std.(1 sigma) Fig.5. Kalman Filter Covergence Graph. Varyingamountofdataisusedtoperform SLAM,after whichthe locationsofthe initialized beaconsare fixedand simple Kalman filterlocalization is performonthe remainingdata. Th eplot aboveshows theaverage absolute XTEand its1sigmabounds forvarioussub sets of thedata us ed fo rS LA M. 6 Summary This paper has reported on extensions for increasing robustness in localization using range from radio. We have examined the use of a particle filter for recovering from large offsets in position that are possible in case of missing or highly noisy data from radio beacons. We have also examined the case of estimating the locations of the beacons when their location is not known ahead of time. Since practical use would dictate a first stage in which the locations of the beacons are mapped and then a second stage in which these locations are used, we have presented an online method to locate the beacons. The tags are localized well enough so that the localization error is equal to the error in the case where the tag locations are known exactly in advance. References 1. P. Bahland V. Padmanabhan. Radar: An in-buildingrf-baseduserlocation and tracking system.In In Proc.ofthe IEEE Infocom2000,Tel Aviv, Israel,2000. 2. D. Fox, W. Burgard, F. Dellaert,and S. Thrun. Monte carlolocalizatoin:Ef- ficient position estimation formobile robots.In Proceedings of theNational Conference on ArtificialIntelligence (AAAI) ,1999. 3. M. Isardand A. Blake. Condensationc on ditional densityp ropagation forvisual tracking.In International Journal of Computer Vision,1998. 4. R. Wa nt J. Hi gh to we ra nd G. Bo rri el lo. S po to n: An in do or 3d lo ca ti on se ns in g te ch no logy ba se do nr fs ign al st re ng th .T ec hni ca lr ep or t. 5. G. Ka nt or an dS .S in gh .P re lim in ar yr es ul ts in ra ng e- on ly lo ca liz at ion an d ma ppi ng .I n Pr oc eed in gs of IE EE Co nf er en ce on Ro bo ti cs and Au to ma ti on , Wa sh in gt on D. C. ,U SA, Ma y2 002. 6. D. Kurth.Range-onlyrobot localization andslam with radio. Master’s thesis, Ro bo ti cs Ins ti tu te ,C ar ne gie Me llon U ni ve rs it y, Pit ts burgh, PA ,M ay 2004. te ch . re po rt CM U-RI -T R-0 4- 29. 7. D. Kur th ,G .K an to r, an dS .S in gh .E xp er im en ta lr es ul ts in ra ng e- on ly lo ca liz a- ti on wi th ra di o. In Pr oc eed in gs of IR OS 2003 ,L as Ve gas, USA, Oc to be r2 003. 8. A. M. La dd, K. E. Bek ri s, G. Mar ce au ,A .R udys ,D .S .W allac h ,a nd L. E. Ka vr ak i. Robotics-basedlocation sensingfor wireless ethernet.In Eighth ACMMobiCom, Atlanta,GA, September2002. 9. A. Ch ak ra bo rt yN .P ri ya nt ha an dH .B a lak ri sh ma n. Th ec ri c ke tl oc at ion su p- po rt sy st em .I n In Pr oc .o ft he 6t hA nnual AC M/ IE EE In te rn at io nal Co nf er en c e on Mo bi le Co mp ut in ga nd Ne tw or ki ng (M O BI COM 2000) ,B ost on ,M A, Aug us t 2000. 10. EdwinOlson,JohnLeonard,and Seth Teller. Robust range-only beacon local- ization.In Proceedings of Autonomous Underwater Vehicles, 2004,2004. 11. S. Thrun, D. Fox, W. Burgard, andF.Dellaert.Robustmonte carlolocalization formobile robots. ArtificialIntelligence,101:99–141, 2000. 242 J. Djugash, S. Singh, and P. Corke Experiments with Robots and Sensor Networks for Mapping and Navigation Keith Kotay 1 , Ron Peterson 2 , and Daniela Rus 1 1 Massachusetts Institute of Technology, Cambridge, MA, USA { kotay| rus} @csail.mit.edu 2 Dartmouth College, Hanover, NH, USA rapjr@cs.dartmouth.edu Summary. In this paper we describe experiments with networks of robots and sensors in support of search and rescue and first response operations. The system we consider includes a network of Mica Mote sensors that can monitor temperature, light, and the movement of the structure on which they rest. We also consider an extension to chemical sensing in simulation only. An ATRV-Mini robot is extended with a Mote sensor and a protocol that allows it to interact with the network. We present algorithms and experiments for aggregating global maps in sensor space and using these maps for navigation. The sensor experiments were performed outdoors as part of a Search and Rescue exercise with practitioners in the field. Keywords: Sensor network, search and rescue, robot navigation 1Introduction Anetwork of robots and sensors consists of acollection of sensorsdistributed over some area that form an ad-hocnetwork, and acollection of mobile robots that can in teractw ith the sensor net wo rk. Eac hs ensor is equipp ed with some limited memory and processing capabilities, multiple sensing modalities, and communication capabilities. Sensor networks extend the sensing capabilities of the rob ots and allo wt hem to act in resp onse to ev en ts outside their pe rception range. Mobile robots extend sensornetworks throughtheir abilitytobring new sensors to designated locations and move across thesensor field for sensing, data collection, and communication purposes. In this paper we explore this synergy between robot and sensor networks in the context of searchand rescue applications. We extend the mapping and navigation algorithms presented in [8] from the case of as tatic sensorn et wo rk to that of am obile sensor net wo rk. In this algorithm, the sensornetwork models the sensorreadings in terms of “dan- ger” levels sensed across its area and creates aglobalmap in sensor space. P. Corke and S. Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp. 243–254, 2006. © Springer-Verlag Berlin Heidelberg 2006 244 K. Kotay, R. Peterson, and D. Rus The regions that have sensor values above a certain threshold are represented as danger. A protocol that combines the artificial potential field of the sen- sors with a notion of “goal” location for a mobile node (perhaps to take a high resolution picture) computes a path across the map that maintains the safest distance from the danger areas. The focus of this paper is on particular issues related to building systems of sensors and robots that are deployable in realistic physical situations. We present sensor network mapping data from our participation in a search and rescue exercise at Lakehurst, NJ. We then show how these kinds of maps can be used for navigation in two settings: (1) in a simulated scenario involving chemical spills and (2) in a physical scenario implemented on Mica Motes [3] that sense light and guide an ATRV-Mini robot. 2R elated Wo rk This work builds on our researchagendafor providing computationaltools for situationalawareness and “googling” for physical information for first re- sponders [5]. We are inspiredbyprevious work in sensornetworks [2] and robotics [6]. [7] proposesarobot motion planner that rasterizes configuration spaceo bstacles in to as eries of bitmap slices, and then uses dynamic program- ming to compute the distance from eachpointtothe goal and the paths in this space—this is the inspiration for our distributed algorithm. This method guarantees that therobot finds thebest path to the goal. [4] discusses the use of an artificial potential field forrobot motion planning. The concept of using asensor network to generate apotential field of sensed “danger” and then using this information to generate ap ath of minimu md anger from as tart location to agoal location wasproposedin[8]. In this paper,the proximity to danger is based on the number of hops from nodes whichbroadcastdan- ger messages,and the total danger is the summation of the danger potentials from all nodes whichsense danger. Then, given start and goalnodelocations, it is possible for the network to direct the motionofthe agentfrom node to no de along ap ath which mini mizes the exp osure of the agen tt od anger.I na related work, [1] addresses coverageand exploration of an unknown dynamic environmentusing amobile robot and beacons. 3Guiding Algorithm To supp ort guidance, the sensor network computes an adaptivemap in per- ception space. The map is used by mobile nodes to compute safe paths to goal locations. The goals maybemarked by auser or computed internally by the net wo rk. The map is built from lo cally sensedd ata and is represent ed globally as agradientstarting at the nodes thattrigger sensorvalues denotingdanger, using the artificial potential protocoldescribed in [8]. Given suchamap, we [...]... supported under Award No 2000-DT-CX-K001 from the Office for Domestic Preparedness, U.S Department of Homeland Security Points of view in this document are those of the authors and do not necessarily represent the official position of the U.S Department of Homeland Security References 1 M Batalin and G.S Sukhatme Efficient exploration without localization In Int Conference on Robotics and Automation (ICRA), Taipei,... Aerial Imagery Global Cost/Reward Field Estimation Fig 4 High-level view of simple implementation of proposed approach to an Autonomous Ground Vehicle (AGV) and non-augmented cameras for navigation and near-field sensing Efficient global planning suggests the utilisation of satellite (or aerial) height data, visible and hyper-spectral imagery and surveyed meta-data (fence-lines, waypoints, beacons etc.)... paths to perform this maneuver Hence, it is possible to conclude that this formulation allows resolution complete planning queries P Corke and S Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp 269–280, 2006 © Springer-Verlag Berlin Heidelberg 2006 270 M Pivtoraiko and A Kelly Like a grid, the state lattice converts the problem of planning in a continuous function space into one of generating... supporting data manipulation systems will be discussed in detail The proposed model is shown in Figure 1 The model has three significant characterisitics: P Corke and S Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp 2 57 268, 2006 © Springer-Verlag Berlin Heidelberg 2006 258 R Grover et al z Raw Data Likelihood Generator x Liklihood Feedback Z Data Condensation pa pb a Likelihood Generator Liklihood... Govindan, and J Heidemann Embedding the internet Communications of ACM, 43(5):39–41, May 2000 3 J Hill, R Szewczyk, A Woo, S Hollar, D Culler, and K Pister System architecture directions for network sensors In ASPLOS, pages 93–104, 2000 4 D Koditschek Planning and control via potential fuctions Robotics Review I (Lozano-Perez and Khatib, editors), pages 349–3 67, 1989 5 V Kumar, D Rus, and S Singh Robot and. .. Galatali, Juan P Gonzalez, Jay Gowdy, Alexander Gutierrez, Sam Harbaugh, Matthew JohnsonRoberson, Yu Hiroki Kato, Phillip Koon, Kevin Peterson, Bryon Smith, Spencer Spiker, Erick Tryzelaar, and William Red Whittaker, “High speed navigation of unrehearsed terrain: Red team technology for grand challenge 2004,” Technical Report CMU-RI-TR-0 4-3 7, Carnegie Mellon University: Robotics Institute, June 2004 Constrained... 3(4):24–33, December 2004 6 J.-C Latombe Robot Motion Planning Kluwer, New York, 1992 7 J Lengyel, M Reichert, B Donald, and D Greenberg Real-time robot motion planning using rasterizing computer graphics hardware In Proc SIGGRAPH, pages 3 27 336, Dallas, TX, 1990 8 Q Li, M de Rosa, and D Rus Distributed algorithms for guiding navigation across sensor networks In MOBICOM, pages 67 77 , San Diego, October 2003... temperature extremes, and hence produced no data Sensor Radio Connectivity The connectivity between the sensors was measured by each sensor sending and acknowledging 20 ping messages The results are shown in Fig 2 (left) Experiments with Robots and Sensor Networks 30 24 18 12 6 29 23 17 11 5 31 28 22 16 10 4 32 27 21 15 9 3 33 26 20 14 8 2 34 25 19 13 7 2 47 1 Fig 2 Connectivity and acceleration data... L eaf L eaf L eaf L eaf L eaf L eaf Fig 7 The structure of the k-D tree and mutex structure for efficient multi-threaded operation The nodes can all have eight children, corresponding to the binary division of each axis of the original volume Only leaf nodes can contain data and are shown shaded Multiple-read, single-write mutexes are provided at the tree, node and leaf levels for efficient access to the... deployment arena is unstructured, expansive outdoor environments including desert, rural farmland and wooded areas 4.2 Reasoning Tasks and Sensing Requirements Reliable long-term navigation and control for this vehicle under mission scenarios including continuous day/night transitions, all-weather operation and with speeds of up to 9ms−1 requires a minimum set of capabilities Most importantly the vehicle . models the sensorreadings in terms of “dan- ger” levels sensed across its area and creates aglobalmap in sensor space. P. Corke and S. Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp areas. The focus of this paper is on particular issues related to building systems of sensors and robots that are deployable in realistic physical situations. We present sensor network mapping. describe experiments with networks of robots and sensors in support of search and rescue and first response operations. The system we consider includes a network of Mica Mote sensors that can