Multi-Robot Map-Merging-Free Connectivity-Based Positioning and Tethering in Unknown Environments

Transcription

1 Mult-Robot Map-Mergng-Free Connectvty-Based Postonng and Tetherng n Unknown Envronments Somchaya Lemhetcharat and Manuela Veloso February 16, 2012 Abstract We consder a set of statc towers out of communcaton range of each other, n an envronment wth no global coordnates. We address the problem of deployng moble robots, ntally not necessarly wthn range of each other or of the statc towers, to serve as gateways to connect the towers. Our robot postonng algorthm conssts of a heurstcally controlled exploraton wth sharng of labeled relatve postonng nformaton, wthout the need to merge maps. After connectvty s acheved, we further address the problem of the robots locatng and tetherng to an agent usng only measurements of sgnal strengths, wthout any communcaton between the agent and the team of robots. We contrbute a two-step tetherng algorthm that uses a datadrven mult-robot RSSI-dstance model. We llustrate our connectvty algorthm n smulaton and compare the effcency of dfferent proposed heurstcs. We demonstrate the effcacy of the most promsng heurstc n a varety of realstc ndoor scenaros. We fnally present results of the successful performance of our tetherng algorthm n smulaton and wth real robots n an actual offce envronment. Keywords: Mult-robot control; Mult-robot teamwork; Mult-robot WF-based communcaton 1 Introducton We are nterested n plannng for multple dstrbuted robots to acheve a common postonng goal, wthout the need for map-mergng. Concretely, we address the problem of usng a set of moble robots to ensure connectvty between a number of statc communcaton towers sparsely deployed n an unknown envronment and not wthn range of each other (Fg. 1a). The robots are themselves communcaton nodes and can communcate wth the statc towers and wth one another, when wthn range. We assume that the robots have no knowledge of the envronment, both n terms of the obstacles and the postonng of the statc towers. The obstacles, such as walls, nterfere wth the sgnal propagaton, and pose challenges n terms of modelng the sgnal propagaton. In open space, models of wreless sgnal decay allow the sgnal strength to provde a good dstance estmate [4], whle n the presence of poorly modeled obstacles, sgnal strength provdes multple dstance hypotheses, preventng the use of the network sgnal strength for localzaton. After the postonng goal s accomplshed, we are nterested n usng the deployed robot team to locate and tether to,.e., follow, an autonomous agent. The agent does not need to communcate wth the robot team, but each team member can measure the receved sgnal strength ndcator (RSSI) of connectons to each other and to the agent. We assume that the agent s ntally 1

2 a) b) R T R R R R R T T Fgure 1: a) 3 moble robots poston themselves to connect 3 statc towers. b) An autonomous agent (outlned n bold) enters the envronment, and one of the robots tethers to t based on RSSI measurements. Bold lnes ndcate obstacles, dashed lnes ndcate wreless connectvty, and arrows ndcate movement. connected to a subset of the team, and remans connected to some member of the team as t moves. The goal s for one of the robots n the team to tether to the autonomous agent as the agent moves ndependently n the envronment (Fg. 1b). In order to do so, based on the subset of robots connected to the target, our soluton sets that all but one of the robots n the team become statonary and act as RSSI landmarks, and the remanng moble robot,.e., the seeker, tethers to the target. Wth our mult-robot algorthm, the robots nfer the target s locaton and moton solely based on RSSI data. However, we note that, whle we focus on RSSI and nfer dstance nformaton from t, our algorthm s general and applcable to any form of sensng that provdes dstance nformaton, even f t s hghly nosy, e.g., a mcrophone that uses volume to estmate dstance. There are several real scenaros that are nstances of the general problem we address. For example, emergency teams that need to assst n areas not fully covered wth communcaton towers or where the connectvty s lost because of a dsaster, can carry and drop small moble robots to autonomously navgate and poston themselves so that the connectvty s extended n the crss area. More generally, ths problem s not specfc to the connectvty goal, and could be extended to other mult-robot postonng needs wth other objectves. Once the robot team has been deployed, the robots can then be used to located and tether to a member of the emergency team, or to a vctm of the dsaster. We consder the deployment of autonomous moble robots n the envronment wth the goal of actng as communcaton gateways and landmarks. The robot navgaton s drven by the communcaton sgnals. The robots can dentfy each other and the towers from communcated dentfers. In addton, the robots use only RSSI measurements to located and tether to the autonomous agent, and does not requre the agent to communcate to the team, e.g., that t s movng or the drecton of ts moton. Furthermore, our approach s targeted to be run on many small, low-cost robots ndoors, where global postonng va GPS or wreless trangulaton s unavalable. We do not use any assumptons about the nature of sgnal degradaton n the envronment, and nstead buld a data-based model of RSSI. Also, our approach does not requre that the robots are homogeneous, or even know about the capabltes of the other robots we fnd solutons to the problem readly wthout plannng the full jont-actons of all the robots. A robot wll never nstruct another robot to head to a locaton that the latter has never vsted, and so ths ensures that the latter robot has the capabltes to 2

3 reach ts target. The organzaton of our artcle s as follows: n Secton. 3, we descrbe the problem, our assumptons, and a general overvew of our approach and contrbutons. In Secton 4, we explan our algorthm for achevng connectvty and assocated data structures n detal, as well as theoretcal guarantees. We then dscuss and analyze the plannng heurstcs used by the robots n Secton 5. We descrbe the RSSI-dstance model that we generate from real-world data n Secton 6 and formally descrbe the second phase of our algorthm, to locate and tether to the autonomous agent, n Secton 7. Sectons 8 and 9 llustrate the results of runnng our algorthm n dfferent scenaros for the connectvty and tetherng goals respectvely, and we summarze our contrbutons and dscuss future work n Secton Related Work Prevous work addressed the problem of dspersng a robotc swarm to provde coverage, usng wreless sgnal ntensty as a measure of dstance between robots, assumng open space between the robots [12]. Robots can poston themselves to optmze sensor readngs from the envronment, usng Vorono graphs [19]. Our goal s to provde connectvty between statc towers, usng the robots as gateways, and not to maxmze coverage of an envronment. By deployng RFID tags as coordnaton ponts, robots buld a jont map and can coordnate to explore an envronment [21]. Smlarly, a sensor network can also be deployed after mappng an unknown envronment [5]. Our approach does not requre any form of map-mergng or common global reference frame, or leavng markers n the envronment. Instead, the robots use poston labels to refer to other robots postons, wthout knowng where these postons are n the envronment. Also, our goal s not the exploraton of an unknown space, but to establsh connectvty. In an envronment wth unknown obstacles, a robot team that s ntally connected can reason about connectvty mantenance, and constran ther moblty n order to avod dsconnectng the network [17]. Smlarly, connected robot teams can reason about whch lnks to delete whle mantanng connectvty, through dstrbuted consensus and market based auctons [14]. From an ntal connected network, robots can acheve bconnectvty,.e., every robot s connected to at least 2 other robots, to enable robustness n the network f any robot fals [2]. We address the case when moble robots start from unknown and unconnected postons. Connectvty of moble robots can be acheved through coalescence, where robots that are connected coalesce nto a cluster and stay connected as they explore the space together [15]. Our goal s to connect statc towers usng robots as gateways. These robots are capable of explorng the envronment, whle the towers reman fxed n ther locatons. In addton, our approach does not requre that robots stay connected together; robots can dsconnect from other robots and explore ndependently. Usng the receved sgnal strength ndcator (RSSI) to nfer dstance has been extensvely studed. A data-drven approach has be used to ft a straght lne between RSSI and log 10 (dstance) n open space [18], and the RSSI to 2 known landmarks can be compared n order to localze a sensor n open space [11]. When the confguraton of walls and obstacles are known, a model can be developed that provdes an accurate measure of RSSI [3]. We use a data-drven approach to generate our RSSI-dstance model, but we do not assume that the envronment s open-space. Our model s general and does not requre knowledge of the confguratons of walls and obstacles n the envronment, or any known landmarks. 3

4 In addton to RSSI, tme-of-flght can be used to measure the dstance between recevers and transmtters n a wreless sensor network [9, 16], but requres specalzed hardware such as precse clocks and synchronzaton between sensor motes. Our approach does not requre any specalzed hardware on the robots; wreless communcaton hardware typcally provdes RSSI measurements whch we use for locatng and tetherng to the target. Localzaton of a robot can be performed usng wreless (WF) sgnal strengths, by developng a WF sgnal map that a robot uses to localze n real tme as t receves RSSI nformaton [4, 1]. Smlarly, range-only localzaton can be performed by placng known RFID tags n the envronment [8]. We consder the case where the robots have no pror map of the envronment, and there are no known statc landmarks. Furthermore, the goal s not localzaton of the robot team, but for the seeker robot to reman tethered to the target. Connectons of a moble robot team can be modeled as a bnary relatonshp (connected/not connected) wth a probablstc relatonshp between dstance and connectvty [6]. The robots are ntally randomly postoned n the world, and the goal of the team s to estmate ther relatve postons. In our approach, the robots are randomly postoned as well, but we use RSSI to nfer dstance, and use nformaton from the team of robots n order to locate and tether to a target robot. A data-drven approach can be used to model RSSI and dstance probablstcally, and the goal of the seeker robot s to locate and tether to the target robot [20]. We use a smlar data-drven approach, but our model determnes the mnmum and maxmum dstance of a par of robots gven ther RSSI. In addton, we consder the case where a team of robots s used to locate and tether to the target robot, nstead of a sngle seeker robot. Furthermore, we do not assume that the target robot communcates wth the team, or synchronzes ts drecton wth the seeker, through a compass or other motons. We nfer the moton of the target completely through RSSI observatons among the robot team. In order to track a moble target, nfrared transmtters and recevers can be used to determne the relatve dstance and bearng from a seeker robot to the target [10]. Range-only measurements can be used for a car-lke robot to follow a moble target [13]. We use only RSSI to obtan an estmate of the dstance to the target robot, wth no bearng nformaton. Furthermore, we use a team of robots to provde addtonal nformaton about the target s locaton and moton, n order for the seeker robot to tether to t effectvely. 3 Challenges, Assumptons, and Approach In ths secton, we formally descrbe the problem and dentfy ts techncal challenges. We present our assumptons and an overvew of the soluton we contrbute. 3.1 Problem Statement and Challenges A team of n autonomous robots are deployed n an unexplored envronment contanng m statc (non-movng) communcaton towers. The frst goal s to fnd a confguraton of robots such that all the towers are connected. In Fg. 2a, towers T1 and T2 are not wthn drect communcaton range, and moble robots R1 and R2 have postoned themselves such that T1 and T2 are connected n the communcaton network, by usng R1 and R2 to relay network packets. Upon establshng a connected network, a subset of n n of these robots wll be used to locate and tether to, 4

5 .e., follow, an autonomous agent (that s not part of the team and does not communcate wth the team) that carres a rado whose sgnal strengths to the teams rados can be measured. The envronment contans physcal obstacles, such as walls, that mpede the robots movement as well as degrade sgnal propagaton. As such, t s dffcult for the robots to have an accurate model of sgnal propagaton (due to sgnal degradaton, reflecton and nterference) that wll allow them to obtan an accurate estmate of the dstance to the towers or other robots, as an accurate plannng state. In Fg. 2b, R1 s connected to towers T1, T2, and T3, wth equal sgnal strengths, due to the degradaton of sgnal propagaton n ar and through the walls. a) b) T2 R1 R2 T1 T2 T1 R1 Fgure 2: a) Connectvty example wth 2 towers (T1 and T2) that are not wthn communcaton range, and 2 moble robots (R1 and R2). b) R1 s connected to 3 towers, wth equal sgnal strengths. Bold lnes ndcate obstacles (walls) that degrade sgnal propagaton, and dashed lnes ndcate connectvty. The robots do not have a map of the world, nor do they possess any form of global postonng. Thus, there s no global coordnate system, and coordnates used by each robot are relatve to ts startng poston and orentaton. Hence, robots cannot share coordnates wth other robots as they do not have a common meanng. Robots can only communcate when n range. In addton, other than communcatng va network packets and measurng sgnal strengths, they are ncapable of sharng nformaton (e.g., by leavng physcal markers n the world). 3.2 Assumptons We lst the assumptons of our approach, dscuss the mplcatons, and possble ways to overcome the assumptons: Assumpton 1. The number and dentfcaton of the towers (m) are known. The dentfcaton of the towers can generally be retreved va the network protocol. If m s unknown, we can use an teratve deepenng approach combned wth tme-lmted exploraton at each teraton, to prevent the algorthm from runnng nfntely. Assumpton 2. The envronment s bounded. Assumpton 3. There exsts at least one confguraton for k n robots that connects all m towers. T3 5

6 Assumptons 2 and 3 lmt, n a very straghtforward way, the amount of exploraton that the robots need to ental n order to fnd a soluton. More concretely, Assumpton 2 ensures that the space that the robots need to explore n order to fnd the soluton s somehow bounded. In practce, Assumpton 2 s not lmtng, as the space s lmted by the fnte number of towers. Assumpton 3 s also qute reasonable n that t ensures that there s a soluton. Ths, n turn, mples that the exploraton algorthm does not go on forever and eventually termnates, f t s complete. Notce also that we do not requre all robots to be part of the soluton, whch means that we do not need to know beforehand how many robots are necessary to attan a soluton, as long as we have some upper bound on ths number. Assumpton 4. The exploraton algorthm for the robots are such that, at any tme t, P(τ C (t) < ) = 1, where C denotes a general confguraton of the robots n the envronment and τ C (T ) s the frst return tme to C after a gven tme T. Ths assumpton states that each robot can revst any confguraton, n a fnte amount of tme, that may be relevant to fnd a soluton. Ths assumpton s used to guarantee that the relevant network nformaton s passed between the robots and eventually propagates to all robots n the team. In practce, gven the relatvely large range wthn whch the robots and towers can communcate, the soluton confguraton can be vsted effectvely. In general, the exploraton algorthm can be desgned so that each robot ncrementally extends ts area of exploraton, cyclcally returnng to the areas already explored. Assumpton 5. Communcaton s nstantaneous, costless and error-free. In practce, communcaton s not nstantaneous and s subject to error. Also, robots may come nto and out of range of one another as messages are sent, causng messages to be lost. Ths assumpton, however, only affects the effcency of our algorthm. Our algorthm s completely asynchronous, and can use any communcaton protocol to make communcaton more relable. Thus, we focus on achevng the goal, and abstract our problem from errors n communcaton. Also, whle communcaton s error-free, ths assumpton does not exclude the varyng of sgnal strengths of connectons when robots are n range. Assumpton 6. The communcaton devces of the robot team and the autonomous agent s known and dentcal. Ths assumpton states that the characterstcs of the rados of the robot team, as well as the autonomous agent that the team has to locate and tether to, are known a pror. Ths assumpton allows some nformaton to be obtaned from measurements of sgnal strengths between rados, even f the sgnal strength measurements are nosy and vary based on the structure of the envronment. Our approach s applcable to a relaxed form of ths assumpton f the rados are not dentcal but known, then models of the dfferent rados can be learned before applyng our soluton. 3.3 Overvew of Approach and Contrbutons In the frst phase of the algorthm, the robots explore the envronment, and collect nformaton on connectvty as they do so. When robots meet (get wthn communcaton range), they share 6

7 ther nformaton, whch allows them to readly fnd a soluton confguraton. Once a soluton s found, the robots head to ther soluton postons and provde connectvty to all the towers n the envronment, and prepare for the second phase of the algorthm. The second phase begns when the autonomous agent comes wthn range of at least 1 of the robots n the team. At that pont, the subset of the team that are connected to each other and to the agent begns locatng and tetherng to the agent. To do so, one of the robots move n a pattern and records sgnal strength measurements between robots of the team and to the autonomous agent. From these sgnal strengths, the relatve locaton of the autonomous agent s found, and the robot uses a probablstc model to track the moton of the agent and tethers to t. We now descrbe some mportant features of our approach, outlnng ts contrbutons: Instead of sharng coordnates (whch s mpossble, snce there s no global coordnate system and map-mergng s not performed), the robots create poston labels whch they share wth each other. A robot can reference another robot s poston, wthout knowng where that poston s n the actual envronment (Sec. 4.1). Instead of sharng and mergng maps, the robots buld a more effectve representaton of connectvty - a network graph (Sec. 4.2), and share ths nformaton whenever they meet (Sec. 4.3). Mergng a network graph nvolves just the unon of vertces and edges. Furthermore, the network graph representaton allows sharng of nformaton that can be propagated across the team of robots effcently. In ths network graph representaton, determnng whether the goal can be acheved wth present knowledge equates to searchng the graph for a connected soluton, subject to certan constrants on the edges n the network graph. Searchng for such a soluton s an NP-complete problem, but we contrbute a method that reduces the search space and runs effcently n practce (Sec. 4.4). Once a soluton s found, each robot smply has to travel to ts soluton poston. Thus, the dffculty of the overall plannng problem conssts of effectvely explorng the space for confguratons that can be useful for the connectvty goal. We propose multple plannng heurstcs to perform the exploraton (Sec. 5), and present extensve smulaton results n representatve scenaros (Sec. 8). Although the receved sgnal strength ndcator (RSSI) data s nosy and vares dependng on the envronment, we contrbute a RSSI-dstance model that s learned from real-world data and can be used n new envronments wth unknown confguratons of walls and obstacles (Sec. 6). We contrbute an algorthm that s capable of locatng and tetherng to an autonomous agent, usng only RSSI measurements between the robot team and the autonomous agent, wthout requrng the agent to communcate, e.g., that t s movng or the drecton of ts moton (Sec. 7), and present experments both n smulaton and on real robots n an actual offce envronment (Sec. 9). 7

8 (x 1, y 1 ) R T a P β P α Ta (x 2, y 2 ) R Tb T b (a) Spatal representaton of robot R n two dstnct postons, (x, y) and (x, y ). The bold lnes ndcate obstacles, and dashed lnes represent connectvty. (b) Graphcal representaton of the same connectons of robot R usng poston labels P α and P β. Fgure 3: Poston labels and graphcal representaton of connectvty of robot R. 4 Dstrbuted Network Connectvty Let R = {R1,..., Rn} be the robots deployed n the envronment, and T = {T1,..., Tm} be the statc towers. Each robot R moves autonomously n the envronment and the purpose of the robot team s to fnd a confguraton C such that all the towers n T are connected, as llustrated n Fg. 2, where a confguraton s a vector of postons n the envronment, one for each robot. Notce that, n any gven envronment, multple such confguratons may exst, and we make no requrements as to whch one the robots should adopt. The goal s to fnd any such confguraton. 4.1 Poston Labels In order for the robots to refer to postons n the world wthout usng global coordnates, they use poston labels: Defnton 4.1. Let R R be a robot. A poston label P α s a name that refers to a poston (ndexed by α) of R. We llustrate the use of poston labels through an example (see Fg. 3). Suppose that a gven robot, R, at some tme t 1, s at coordnates (x 1, y 1 ), where the subscrpt denotes the fact that the coordnates (x 1, y 1 ) are expressed n terms of R s reference frame. Let R be connected to towers Ta and Tb n ths poston. At some other tme t 2, R s at coordnates (x 2, y 2 ), and s connected only to Ta. The spatal postons and connectons of R at t 1 and t 2 are shown n Fg. 3a. The lack of a global coordnate system prevents robots other than R to assgn any meanng to the coordnates (x 1, y 1 ) and (x 2, y 2 ) and as such, R assgns a label to each of the two postons, and stores a mappng of the poston labels to the coordnates, e.g., P α = (x 1, y 1 ) ; P β = (x 2, y 2 ) Each robot can convert poston labels of ts own postons nto coordnates n ts own reference frame, and these poston labels can be shared readly among all the robots. For example, when R meets another robot Rj, R can share that t s connected to Ta and Tb when at poston P α, 8

9 tme t 1 tme t 2 tme t 1 R 1 P21 P11 T1 P21 P11 T1 P11 P12 T1 R1 P21 T2 R2 R3 P31 R 2 P21 P11 T1 T2 P32 P21 P11 T1 tme t 2 P31 P32 R1 T1 P12 P21 R2 R3 T2 R 3 P31 T2 T2 P32 P31 P21 P11 T1 (a) Spatal representaton of 3 moble robots and 2 towers n an envronment. (b) Network graphs for the 3 robots shown n Fg. 4a. The shaded vertces ndcate postons that the robots can convert nto coordnates. Edges between vertces ndcate connectvty. The soluton found s outlned n bold. Fgure 4: Network graph representaton shared between robots to fnd a soluton and s connected to Ta when at poston P β. Rj does not need to know the coordnates of these postons; t only needs to know that R s capable of connectng to Ta and/or Tb at those postons, and that R can travel to the postons f need be. In partcular, ths connectvty nformaton can be stored n the form of a graph, as shown n Fg. 3b. R merely has to share the graph shown n Fg. 3b to allow Rj to store the new connectvty nformaton. 4.2 Macro Network Graph Representaton for Connectvty Informaton We developed a data representaton, that we call a network graph, whch allows robots to store, share and merge connectvty nformaton readly. Defnton 4.2. A network graph G s an undrected graph G = (V, E), where each vertex (or node) v V s a poston label (e.g., P α ) or a tower (e.g., Ta), and each edge e E s a par {v 1, v 2 }, where v 1, v 2 V. Edges represent connectons between vertces (robots/towers) and the weghts of the edges represent the sgnal strengths of the connectons. To llustrate the usage and benefts of a network graph, consder Fg. 4. At tme t 1, the robots R1, R2 and R3 are at postons P1 1, P2 1, and P3 1 respectvely. The physcal postons of the robots and ther connectons are shown n Fg. 4a and the network graphs of the robots are shown n Fg. 4b. Note that the robots synchronze and merge ther graphs when connected, whch s why R1 and R2 have dentcal graphs. At tme t 2, R1 and R3 move to postons P1 2 and P3 2 respectvely; R2 stays n poston P2 1. At ths tme, R2 can share nformaton regardng R1 wth R3, even though R1 and R3 have never met. Ths allows both R2 and R3 to dscover a soluton where R1, R2 and R3 are at postons P1 1, P2 1, and P3 2 respectvely. The network graphs of the robots are shown n Fg. 4b, and the soluton found s outlned n bold. The network graph representaton offers multple benefts. Frst of all, robots can readly share nformaton. When two robots R and Rj meet, they can update ther ndvdual network 9

10 a) T1 P1 1 b) T1 R1 R3 T2 P2 1 P1 2 P2 2 P3 1 R2 P3 2 T2 Fgure 5: a) A network graph of 3 robots and 2 towers. b) The correspondng macro network graph of the same 3 robots and 2 towers. The poston labels are not shown n the macro network graph, even though the nformaton s embedded n the macro nodes. graphs and unfy ther knowledge n all parts of the graph, ndependently of ther current poston. Furthermore, the updatng of graphs s asynchronous n the sense that not all robots need to have the same network representaton at all tmes. In addton, a confguraton that ensures connectvty of all towers can be obtaned drectly from the graph. Formally, a soluton s that connects all towers n a graph G = (V, E) exsts ff a sub-graph G = (V, E ) G exsts such that all towers Ta T are connected, and each robot R s n at most one poston,.e., (P α, P β V α = β). As the robots explore the envronment, they create new poston labels, whch ncreases the sze of the network graph. The decson of when to create a new poston label s based on factors such as the granularty desred n dscretzng the envronment, and has a drect mpact on the rate of growth of the network graph. In ths artcle, we do not dscuss when s best to create a poston label, and assume that ths decson s made by a provded functon; n the experments (descrbed later), we use the dscretzaton of the envronment to create new poston labels. The sze of the network graph ncreases as the robots explore the envronment, and searchng ths graph becomes computatonally expensve as more poston labels as created over tme. In order to cope wth ths growth, we consder a macro network graph representaton, n whch all nodes correspondng to each robot are collapsed nto a sngle macro node. Fg. 5 shows a network graph and ts correspondng macro network graph. Defnton 4.3. A macro node v s an equvalence class defned over the set of vertces of the orgnal network graph G = (V, E), that corresponds to a sngle robot or tower, e.g., the macro node R = {P α : P α V, α}. Defnton 4.4. A macro edge e = {v 1, v 2 } s an equvalence class defned over the set of edges n the orgnal network graph G = (V, E), that corresponds to all connectons between the 2 macro nodes (.e., v 1 and v 2 ), e.g., the macro edge {R, Rj} = {{P α, Pj β } : {P α, Pj β } E, α, β}. Defnton 4.5. A macro network graph s an undrected graph H = (V, E) where V s the set of all macro nodes, and E s the set of all macro edges. A network graph can be represented as a macro network graph, and vce versa. In a macro network graph (we henceforth drop the usage of the word network for brevty), V m + n, where 10

11 m and n are the number of towers and robots respectvely, and E ( ) m+n 2. Any partcular macro edge {v 1, v 2 } E means that, n the orgnal network graph, the robots/towers correspondng to nodes v 1 and v 2 share at least one connecton. Each macro edge can also be seen as a set of constrants on the robots postons. These constrants lmt the possble robot postons n order to have the connecton descrbed by the macro edge. Each macro edge e E s assocated wth the correspondng equvalence class or constrant set that must store all edges n the orgnal network graph and correspondng sgnal strength nformaton. Ths means, n partcular, that the macro graph representaton s equvalent to the orgnal network graph representaton n terms of space-effcency. However, the macro graph representaton provdes sgnfcant advantages when searchng for a soluton, whch we wll soon show. We conclude by observng that a soluton s a connected subgraph of H that ncludes all macro nodes correspondng to towers n T, and each robot R can be n a poston P α such that all constrants are met n the soluton subgraph. Further detals are provded n Sec Communcaton Phase As the robots explore the world, they ndvdually mantan a macro graph whch they use to store connectvty nformaton. Upon comng wthn communcaton range, robots share ther correspondng macro graphs and update them to nclude the nformaton comng from the other robots. Ths process can be decomposed nto several steps whch we now descrbe. Theorem 4.6. Suppose R1,..., Rn are connected, wth macro graphs H 1,..., H n respectvely before the communcaton phase. After the communcaton phase, R1,..., Rn wll have the same macro graph H, such that H {1,...,n} H ). Proof. Let the current postons of the R1,..., Rn be P1 α1,..., Pn αn. Let the robots drectly connected to robot R be R R. Each robot R frst adds constrants to ts macro graph H (f the constrants do not already exst) regardng ts drect connectons to all robots n R,.e., { } P α, Pj αj, Rj R. After ths step, R s new macro graph s H H. Next, R shares ts updated macro graph H wth all the other robots (both drectly and ndrectly connected) n the followng way: R sends H to ts drect neghbors, who merge H wth ther macro graphs. The neghbors then share ther updated macro graphs wth ther neghbors, and so on. Ths synchronzaton can take multple rounds of communcaton untl no new nformaton s avalable to all the connected robots. Thus, R receves macro graph nformaton from the other robots, and R ncorporates the shared nformaton. After ths operaton, R s new macro graph s: H = H (H 1... H 1 H H n) = j {1,...,n} H j Therefore, after the communcaton phase, every connected robot has the same macro graph H, where: 11

12 H = H {1,...,n} {1,...,n} H H (snce H H, {1,..., n} ) 4.4 Fndng a Soluton The robots buld a macro network graph and search t to fnd a soluton confguraton: Defnton 4.7. A macro edge e = {R, v} s applcable to a poston label P α f: v = Rj and {P α, Pj β } s n the equvalence class of e or for some β v = Ta and {P α, Ta} s n the equvalence class of e Defnton 4.8. A soluton s of a macro network graph H s a sub-graph H s = (V s, E s ) H such that all the towers n T are connected, and each robot can be at a sngle poston,.e., R V s, P α such that e E s, R e P α s applcable to e. To reduce the search space for a soluton s (or equvalently H s ), we add the restrcton that H s s acyclc f H s contans cycles, then macro edges from H s can be removed (elmnatng the cycles) whle stll ensurng that all the towers are connected. In order to fnd such a soluton H s, the search begns at one of the towers (e.g., Ta), by nsertng all macro edges connected to Ta nto a queue (.e., Q = {e E : e = {R, Ta}, R V}), and runnng the functon fnd soluton recursvely (see Algo. 1 for the pseudocode). The fnd soluton algorthm proceeds as follows: P contans the possble postons that the robots can be n ntally, all robots can be n all postons. Gven a queue Q of macro edges and the frst macro edge e n the queue, the algorthm attempts to use e and recurse, as well as not use e and recurse. Ths ensures that all combnatons of usng and not usng macro edges are tested. In addton, as the algorthm recurses, Q, the queue of macro edges to consder, V c, the set of vertces (robots/towers) already connected, E used, the set of macro edges used, and E skp, the set of macro edges that were skpped, are updated. Although t may seem that ths search performs a complete search of the macro graph, the search tree s pruned quckly, due to the constrants n each of the macro edges. Thus, P becomes more and more constraned, lmtng the number of macro edges stll avalable for use, and reducng the search space consderably Updatng Constrants Gven a set of possble postons P = {P 1,..., P n }, where P refers to possble postons of R, and a macro edge e = {v 1, v 2 }, we update P such that the constrants of the macro edge e are satsfed. In order to do so, we take the ntersecton of the constrants of e and the relevant elements of P. 12

13 Next, we terate through all e V s and further constran P (snce the reduced postons of R may further constran postons of Rj through a prevously-used macro edge e = {R, Rj}). After all the propagatons have completed, f s.t. P = 0, then P becomes nvald. We have the followng result: Theorem 4.9. For a problem wth m towers and n robots verfyng Assumptons 1 through 5, all robots wll fnd at least one soluton w.p.1. Proof. From Assumpton 2, there s at least one confguraton n whch all towers are connected (a soluton). The fact that the envronment s bounded n the sense of Assumpton 3 and that the exploraton algorthm s thorough n the sense of Assumpton 4 guarantees that at least one robot eventually determnes a soluton (the probablty of ths not happenng goes to 0 as t ). From Thm. 4.6, robots synchronze ther macro graphs when they meet. Usng Assumptons 4 and 5, ths mples that the network structure eventually propagates to all robots. Thus, f a soluton to the connectvty problem s found by some robot R, then all the robots wll fnd ths soluton w.p.1 n the lmt, as t. As a result, the soluton propagates to all robots n the lmt, thus establshng the desred result. Algorthm 1 fnd soluton(q, P, V c, E used, E skp ) 1: f Q s empty then 2: return false 3: end f 4: e = popqueue(q) 5: P = updateconstrants(e, P ) 6: f P s vald then 7: Q = updatequeue(e, Q) 8: V c = addvertex(e, V c ) 9: E used E used {e} 10: f T V c then 11: soluton (V c, E used ) 12: return true 13: end f 14: f fnd soluton(q, P, V c, E used, E skp) = true then 15: return true 16: end f 17: end f 18: E skp E skp {e} 19: f fnd soluton(q, P, V c, E used, E skp ) = true then 20: return true 21: end f 22: return false 13

14 4.5 Algorthm for Dstrbuted Network Connectvty The robots run a fully-dstrbuted algorthm that allows them to fnd and converge to a soluton. A flow-chart of the algorthm s shown n Fg. 6. The fully-dstrbuted algorthm shown n Algo. 2, whch ncludes a state heurstc functon. The robot can be n one of four states, namely Explore, Share Soluton, Goto Soluton, and Stop. Each robot R starts n the Explore state, wth an empty macro graph. Fg. 7 shows the state transton dagram for the robots. Algorthm 2 Dstrbuted Network Connectvty Algorthm 1: H {} // H s the macro graph 2: state Explore 3: loop 4: (T, R ) = getconnectons() 5: // Create poston label 6: P α = getpostonlabel(current coordnates) 7: // Update macro graph H wth connectons to towers 8: for all Ta T do 9: addconstrant( {Ta, P α }, H) 10: end for 11: // Update macro graph H wth connectons to robots 12: // Synchronze macro graphs 13: performcommuncatonphase(r, H) 14: // Check for soluton and update state 15: f graph was updated then 16: s = checkforsoluton(h) 17: f s s vald and s current soluton then 18: current soluton s 19: sol posn = getsolutonposton(s) 20: neghbors = getneghborstoinform(s) 21: state Share Soluton 22: end f 23: end f 24: f state = Share Soluton & nformed(neghbors) then 25: state Goto Soluton 26: else f state = Goto Soluton & P α = sol posn then 27: state Stop 28: end f 29: // Plan the next acton 30: acton = getnextacton(state) 31: // Execute the acton 32: executeacton(acton) 33: end loop At each tme step, the robot R generates a lst of towers T T, and a lst of robots R R that are n range. For each tower Ta T, robot R adds a constrant nto ts macro graph as R s current poston label (.e., P α ), the tower s ID (.e., Ta), and the sgnal strength of the connecton. 14

15 Update Macro Graph Communcaton Phase Check for soluton Execute acton Plan next acton Fgure 6: Flowchart of the fully-dstrbuted algorthm Explore found soluton Share Soluton dfferent soluton found dfferent soluton found shared soluton wth neghbors Stop arrved at soluton poston Goto Soluton Fgure 7: State transton dagram for each robot. The robots start n the Explore state. When all robots are n the Stopped state, the soluton confguraton has been acheved and all towers are connected. R then enters a communcaton phase, where t adds constrants nto ts macro graph as R s current poston label, the poston labels of the robots R s drectly connected to (R ), and the sgnal strengths of the connectons. R then shares and synchronzes ts macro graph wth all the robots t s drectly connected to (R ). After ths phase, all robots that are connected wll have the same macro graph. Followng the communcaton phase, R now searches the macro graph for a soluton f the macro graph was updated. Lastly, R swtches ts nternal states f necessary, based on whether a soluton has been found Convergng to a Soluton In Sec. 4.4, we have establshed that, wth enough exploraton, all robots eventually determne a soluton. However, n envronments where multple solutons exst, t s possble that at any tme step not all robots determne the same soluton. Therefore, t s necessary to ensure that, n the presence of multple solutons, all robots adopt and move to the same soluton. The process of ensurng consensus n a common soluton arses from a common and determnstc soluton selecton mechansm. As seen before, the algorthm to fnd a soluton (Algo. 1) s determnstc. Furthermore, snce robots synchronze ther macro graphs when they meet, connected robots wll fnd the same soluton. 15

16 Once a robot R has found a soluton, t attempts to fnd ts neghbors n the soluton and synchronze ts macro graph wth them (Share Soluton state). After all ts neghbors have synchronzed ther macro graphs, R heads to ts soluton poston (Goto Soluton state). Fnally, after arrvng at the soluton poston, R wll stop movng (Stop state). If at any tme, a better soluton s found (e.g., by recevng new nformaton from other robots), R wll restart ts convergence process and attempt to fnd ts neghbors agan. If a robot R fnshes sharng ts soluton wth ts neghbors and moves to ts fnal poston whle other robots are stll negotatng, ether the other robots settle n ther postons correspondng to the soluton adopted by robot R or some robot (that adopted a dfferent soluton) wll not stop untl t connects to robot R. At ths pont, they synchronze ther macro graphs and adopt the same soluton. If the soluton found s dfferent, R restarts ts sharng process. Ths means that, snce the number of robots s fnte, they all eventually settle n one soluton and move to the correspondng poston. Ths concluson s stated n the followng result: Theorem For a problem wth m towers and n robots verfyng Assumptons 1 through 5, all robots converge to the same soluton w.p.1. 5 Plannng an Acton for Connectvty As mentoned above, the robot can be n one of 4 states: Explore, Share Soluton, Goto Soluton, and Stop (see Fg. 7). In the Explore state, the goal of the planner s to traverse the world such that the robots wll eventually fnd a soluton confguraton s that connects all the towers. In the Share Soluton state, a soluton s has been found, and the goal s to communcate ths soluton s to neghbor robots n s. In the Goto Soluton state, the planner has to fnd the shortest path from the current locaton to the robot s poston n the soluton s. Lastly, n the Stop state, the planner has no work to do and merely stops the robot n place. We now descrbe a number of dfferent heurstcs that are used for exploraton: Random Movement The smplest heurstc was random movement, where a robot would choose an acton randomly from the lst of possble actons. There was no weghtng of the actons, so f k actons were avalable, they would each have a 1 k probablty of beng selected. Ths heurstc provdes a baselne for comparson, snce t s arguably the most nave form of exploraton. Coverage of the Space The next heurstc we consdered was a coverage algorthm. We adapted the node-countng algorthm descrbed n [7]. Each robot kept a counter C c of how many tmes t vsted a cell c. Then, when choosng an acton, t pcks the adjacent cell c such that C c s the mnmum among all adjacent cells. In the case where more than one cell has the mnmum value, t pcks randomly among the mnmum cells. All cells are ntalzed to have a counter of 0, so unexplored cells always have prorty over explored cells. 16

17 Weghted Exploraton Ths heurstc was smlar to the above coverage algorthm, except that the robot uses a weghted dce to decde among ts adjacent cells, nstead of choosng the least-vsted cell (.e., wth the mnmum value). We defned a rato γ, that represents the exploraton vs explotaton probabltes. Gven k 1 unexplored adjacent cells, and k 2 explored adjacent cells, t chooses to explore wth k 1 γ k 1 γ+k 2 (1 γ) probablty, and explot otherwse. If t chooses to explore, t pcks one of the unexplored cells randomly. Otherwse, f t chooses to explot, t pcks an explored cell, weghted on how many tmes t has prevously vsted that cell. For each explored cell c k and correspondng counter C ck, we defne p k = 1 Cc k mn j C cj max j C cj mn j C cj + α, where α s a weghtng term such that the cell wth the maxmum counter wll not have a 0 probablty of beng chosen. Gven the p k of the adjacent cells, p the robot pcks a cell c k wth a probablty of k j p. j For example, suppose the adjacent cells are (c 1, 0), (c 2, 0), (c 3, 3), (c 4, 5), (c 5, 2), where each tuple represents an adjacent cell and ts correspondng counter (where 0 means unexplored). The robot 2γ wll choose to explore wth a 2γ+3(1 γ) probablty. If t decdes to explore, t wll pck ether c 1 or c 2 wth equal probablty. If t decdes to explot, t wll pck c 3 wth a probablty of c 4 wth a probablty of Stay at Towers α and c 1+α ( 1 2 +α)+(α)+(1+α) 5 wth ( 1 +α)+(α)+(1+α) α ( 1 2 +α)+(α)+(1+α), In ths heurstc, the robots had one of 2 roles: stay at an assgned tower, or avod towers. A robot R s assgned the role of stayng at tower Ta f n ts macro graph, t has the most connectons to Ta. Otherwse, the robot R assumes the avod towers role. In the stay at tower role, f the robot R s not currently connected to ts assgned tower Ta, then t plans the shortest path (usng breadth-frst search) to the nearest cell that connects t to Ta (based on the map t bult whle explorng the world). If the robot s already connected to Ta, then t decdes to explore or explot usng a weghted dce, smlar to the weghted exploraton heurstc above. However, n ths case, t gnores all adjacent explored cells that do not have a connecton to Ta. Thus, the weghtage only occurs for unexplored cells, and explored cells that are known to have a connecton to Ta. In ths way, R stays close to Ta and may lose connecton only f t goes to an unexplored cell that s out of Ta s range. In the avod towers role, nstead of choosng between explore and explot, the robot chooses between explore, explot, and vstng a tower, wth probabltes α, β, and 1 α β respectvely. The robot chooses between these 3 optons based on the number of adjacent cells that match the requrement:.e. f there are k 1 unexplored cells, k 2 explored cells that do not have a connecton to a tower, and k 3 explored cells that have a connecton to a tower, then the robot chooses to k 1 α k 1 α+k 2 β+k 3 (1 α β) 2 β k 1 α+k 2 β+k 3 (1 α β) k explore wth probablty, explot wth, and vsts a tower cell otherwse. If t chooses to explot or vst a tower, then the relevant cells are selected probablstcally based on ther counter values, smlar to the weghted exploraton heurstc. By usng the heurstc, the robots that do not have an assgned tower tend to vst areas that have no connectons to any tower, and explore new regons. Ths allows new towers to be dscovered quckly, as well as connectons to be found between towers. We experment on ths heurstc n detal n Sec

18 6 Modelng Dstance Based on RSSI After phase 1 of the algorthm s completed, the team of robots are placed n a confguraton that connects the statc wreless towers. An autonomous agent s also present n the envronment n phase 2, and the goal of the robot team s now to locate and tether to the autonomous agent. To do so, we use the receved sgnal strength ndcator (RSSI) between the robots and the autonomous agent to nfer the dstance between them. In an ndoor envronment, where there are walls, furnture and other obstacles, sgnal attenuaton and mult-pathng makes t dffcult to estmate dstance accurately [3]. Furthermore, we are nterested n the case where robots are placed n unknown envronments, so they do not have a map that can be used to model mult-pathng and attenuaton. 6.1 Collectng Real-World Data We use a data-drven approach, smlar to [20], n order to create an RSSI-dstance model n a complex ndoor envronment. We used a par of Robot Creates, each wth a Gumstx Verdex mcroprocessor and 2 attached rado antennas (Fg. 8). The antennas allow the robots to communcate wrelessly, and provde the RSSI of ther connecton (n ntegers of dbm). We then collected (RSSI, dstance) pars, over a range of 0 to 40m. The black dots n Fg. 9 shows a scatter plot of the collected data. Wthn the range of dstances, there were many confguratons of walls, furnture, and other obstacles. Thus, the data collected provded a large sample of possble confguratons of the world, and not a model of open space. Fgure 8: One confguraton of Robot Creates used to collect real-world RSSI data. Many confguratons of walls and obstacles between the robots were used n the data collecton. 6.2 Creatng the RSSI-Dstance Model The black and gray lnes n Fg. 9 show the maxmum and mnmum boundares of the model created, based on the data collected. The ntuton behnd the lnes s that the maxmum and mnmum RSSIs should decrease monotoncally as the dstance ncreases. In ths manner, the maxmum RSSI-dstance and mnmum RSSI-dstance lnes are composed of horzontal segments and slopes of negatve gradent. Our RSSI-dstance model dffers from [20] n that we construct 18

19 Fgure 9: The RSSI-Dstance model created from real-world data. The black and gray lnes show the maxmum and mnmum boundares of the model, and the black dots show the data used to generate the model. The red crosses show data collected from a dfferent part of the buldng. the mnmum and maxmum bounds nstead of a trapezod, because our data dd not show lower lkelhood near the boundares. Usng the RSSI-dstance model M, we can determne the mnmum and maxmum RSSI for a gven dstance,.e., (dst mn, dst max ) = M rss (RSSI), and the mnmum and maxmum dstance for a gven RSSI,.e., (RSSI mn, RSSI max ) = M dstance (dst). From the functon M dstance, we also defne a lkelhood functon M lkelhood, such that there s a constant pror wthn the upper and lower bounds of RSSI,.e., M lkelhood (dst, RSSI) = 1 f RSSI mn RSSI RSSI max and 0 otherwse. Whle the model we create does not explctly capture the number of walls and obstacles n the envronment, t mplctly handles all possble confguratons of walls and obstacles that were present n the data-collecton phase. Thus, wth suffcent data, the model generated can handle all possble confguratons of walls (of dfferent thckness and materals) and obstacles. We collected real-world data from one floor of the buldng to create the model, and used data collected from a dfferent floor of the buldng to valdate the model. The red crosses n Fg. 9 show the valdaton data. Whle not all the crosses fall wthn the model, the dstance between the cross and the boundary of the model s very small (< 1m). Thus, the model s suffcently accurate and s applcable for use n stuatons when robots have no map of the envronment, but have representatve data used to create the model. 7 Tetherng to a Target wth Multple Robots In the second phase of our algorthm, an autonomous agent enters the envronment, and the goal of the robot team s to locate and tether to ths autonomous agent. We defne R R, where R = n n, to be the subset of the robot team that s connected to each other and to the autonomous agent, and we defne R n +1 to be the autonomous agent. 19

20 R R n+1 R 0 R j Fgure 10: An example scenaro: dotted lnes and gray areas ndcate connectons and obstacles respectvely. R 0 frst moves to determne the target R n +1 s locaton (black arrows), and when R n +1 begns movng, R 0 tethers to t (blue arrows). Defnton 7.1. The mult-robot tetherng doman s a tuple {L, R, X, O, f O }, where: L s the set of locatons n the world; R = {R 0,..., R n +1} s the set of robots, where R 0 s the seeker, R 1,..., R n are n cooperatve teammates, and R n +1 s the autonomous agent; X s the set of states of the world,.e., x (t) X contans the locatons of the robots, walls and obstacles at tme t, and x (t) x (t) s the locaton of R at tme t; O s the set of possble observatons,.e., connectons among robots and the correspondng RSSI; f O : X O s the observaton generaton functon, whch determnes sgnal strengths of connectons among the robots, gven the state of the world. The confguraton of the walls and obstacles n the world are ntally unknown to the robots, and they are ntally placed n a random confguraton. In addton, the observaton functon f O s unknown to the robots, although they have a model M of RSSI and dstance (Sec. 6). In a realworld envronment, f O would nclude the physcal attrbutes of sgnal propagaton and attenuaton, mult-pathng and other factors, whle M provdes a mnmum and maxmum dstance gven RSSI and vce versa. The mult-robot tetherng problem s for one of the robots n the team, whch we call the seeker R 0, to tether to the target R n +1,.e., mnmze t T x(t) 0 x (t) n +1, as R n +1 moves n the envronment, usng nformaton from the mult-robot team R 0,..., R n. However, R n +1 s not part of the mult-robot team and does not communcate or nform the team that t s movng; the team must nfer R n +1 s locaton and moton through RSSI measurements only. In ths artcle, we focus on the case where R 1,..., R n reman statonary n the world, and communcate ther connectons and RSSI to R 0, and we do not dscuss how R 0 s selected. In the frst step of our algorthm, we assume that R n +1 s statonary, and the mult-robot team determnes R n +1 s ntal locaton x (0) n +1. In the second step of the algorthm, after R n +1 s locaton has been determned, R n +1 begns movng and R 0 remans tethered to t. Fg. 10 shows an example scenaro of the problem and our algorthm. 20

21 7.1 Locatng the Target At each tme step t, an observaton o (t) s generated by the observaton generaton functon f O, and each robot R observes o (t) o (t). R 0,..., R n communcate and merge ther ndvdual observatons. Even though the target robot R n +1 does not communcate, ts connectons can be nferred from the team s observatons, e.g., f R n +1 and R are connected, then that connecton s shared by R. Thus, o (t) = n =0 o(t), and o (t) s avalable to the mult-robot team. The robots are ntally randomly postoned n the world. However, from the RSSI of ther connectons, they can nfer the maxmum and mnmum dstances between them, usng the RSSIdstance model M. In addton, all the robots except R 0 are statonary n ths step, and we assume that R 0 s capable of keepng track of ts locaton relatve to ts ntal locaton,.e., t has good odometry and sensor feedback n order to compensate for dead-reckonng errors. Thus, x (t) 0 s known to R 0 n ts own coordnate frame. Creatng Constrants from Observatons For each observaton o (t) O, we defne a functon CreateConstrants(o (t) ) that creates constrants C, based on the RSSI of connectons c,j o (t) (Algo. 3). The RSSI-dstance model M s used to nfer the maxmum and mnmum dstance (d mn and d max ) of 2 robots (R and R j ) by usng the RSSI of ther connecton. Snce only R 0 moves n ths step of the algorthm, x (t) = x (0) 0. Thus, CreateConstrants(o (t) ) returns constrants of the forms d mn x (0) x (0) j d max and d mn x (t) 0 x (0) d max. Algorthm 3 Creatng Constrants from an Observaton CreateConstrants(o (t) ) C {} for all (R 0, R, s 0, ) o (t) do (d mn, d max ) M rss (s 0, ) addconstrant(c, d mn x (t) 0 x(0) d max ) end for for all (R, R j, s,j ) o (t) s.t., j 0 do (d mn, d max) M rss (s,k ) addconstrant(c, d mn x(0) x (0) j d max) end for return C Movng the Seeker and Mergng Constrants In order to locate the target more effectvely, the seeker moves along a pre-determned path, and creates new constrants as t does so. x (t) 0 s known to R 0 n ts own coordnate frame, and so these constrants form rngs of possble locatons. For example, f x (t) 0 = (0, 0), d mn = 5, d max = 10, then x (0) s a rng of radus 5 to 10 centered about the orgn. Thus, ntersectons of these rngs over tme provde a good estmate of the robots ntal locatons. 21

22 Fgure 11: Constrants on R s poston generated from connectons to R 0 (n blue), and R j (n red). However, as Fg. 9 shows, for a gven RSSI, the range of possble dstances can be very large, e.g., d mn and d max for -60 dbm s 5.5m to 39.7m respectvely. As such, the ntersectons of rngs from constrants nvolvng R 0 help to narrow a robot s poston (Fg. 11), but not exactly locate t unless R 0 travels a large dstance. Parwse dstance constrants between other robots are necessary for more accurate postons of the robots. Fndng Possble Jont Locatons of Robots All the constrants whle the seeker s movng can be merged to form a set C. Possble jont locatons of all the robots,.e., x (0) 1,..., x(0) n +1, can be nferred from ths set of constrants C. However, t s nfeasble to smply consder all possble combnatons of locatons and checkng f C s satsfed, as the sze of the jont locaton space grows exponentally as n ncreases. As such, we developed an algorthm to generate possble jont locatons of the robots, as shown n Algo. 4. The algorthm frst ntalzes the set of possble locatons of each robot as L, the set of locatons n the doman. Next, by consderng the constrants nvolvng R 0, ths set of ndvdual locatons s reduced n sze. The next step of the algorthm consders jont-locatons of 2 robots, and elmnates combnatons that do not satsfy C. The algorthm then contnues teratng on the number of robots to consder n the jont-space, whle elmnatng combnatons that do not satsfy C. In ths way, the sze of possbltes does not grow unbounded as C elmnates unsatsfable jont locatons, and converges to a small number of complete jont-locatons of all robots, gven suffcent movement of the seeker. 7.2 Tetherng to the Target After the frst step s completed, the robot team has a good estmate of the target s locaton, and the seeker R 0 s to reman tethered to the target robot R n +1 as the target moves n the world. The target does not communcate wth the other robots, and the team has to nfer ts movement through observatons. The goal s to mnmze the dstance between the seeker s locaton x (t) 0 and the target s locaton x (t) n +1 over a tme perod. To mantan an accurate estmate of x (t) n +1, we use a probablstc approach, and apply Bayesan updates to the observatons. From Def. 7.1, x (t) X s the state of the world at tme t, where x (t) ; we denote x (t 1:t 2 ) = x (t1),..., x (t2). Smlarly, o (t) and u (t) are all the observatons and control nputs at tme t respectvely. We defne bel (x (t) ) = P (x (t) o (1:t 1), u (1:t) ), whch s the belef of R s poston at tme t, gven observatons up to tme t 1 and bel (x (t) ) = P (x (t) o (1:t), u (1:t) ) as the belef of the robot s x (t) 22

23 Algorthm 4 Generatng possble jont locatons GetLoc(x (0) 0..., x (T ) 0, o (0),..., o (T ) ) // Generate all constrants C T t=0 CreateConstrants(o(t) ) // Intalze ndvdual robot locatons for = 1,..., n + 1 do X L // L s the set of all possble locatons end for // Constran ndvdual locatons usng R 0 s connectons for all (d mn x (t) 0 x(0) d max ) C, x X do f x (t) 0 x < d mn or x (t) 0 x > d max then X X \ {x } end f end for // Generate jont locatons X X 1 for = 2,..., n + 1 do X X; X {} for all x X, (x 1,..., x 1 ) X do f SatsfyConstrants(C, (x 1,..., x )) then X X {(x 1,..., x )} end f end for end for return X poston after ncorporatng the latest observaton. From the Markovan assumpton, bel (x (t) ) x (t) s.t.x (t) n +1 P (o (t) x (t) ) bel j (x (t) j ) (1) Eqn. 1 s ntractable to compute, snce the combnatons of x (t) grows exponentally as n ncreases, and P (o (t) x (t) ) depends on the observaton generaton functon f O, whch s unknown to the robots. However, by usng the RSSI-dstance model M (Sec. 6), we model that the RSSI between any 2 robots depends only on ther dstance, and not on the locatons of other robots or obstacles. Thus, we can smplfy P (o (t) x (t) ): P (o (t) x (t) ) = P (c (,j) x (t) ) (2) c (,j) o (t) = P (c (,j) x (t), x (t) j ) (3) c (,j) o (t) = P (c (,j) dst,j ), where dst,j = x (t) x (t) j (4) c (,j) o (t) 23 j=0

24 Algorthm 5 Calculatng summed lkelhoods GetLkelhood(M, o (t), bel) // sum c(,j), stores x (t),x (t) j P (c x (t), x (t) )bel (x (t) j ) for all c (,j) = (R, R j, s,j ) o (t) do sum c(,j), 0; sum c(,j),j 0 end for for all L 1, L 2 L do for all c (,j) = (R, R j, s,j ) o (t) do sum c(,j),+ = M lkelhood (s,j, L 1 L 2 )bel (L 1 ) sum c(,j),j+ = M lkelhood (s,j, L 1 L 2 )bel j (L 2 ) end for end for return (sum c(,j),, sum c(,j),j) c (,j) o (t) Also, we make an approxmaton that for each robot R, P (o (t) x (t) ) s only dependent on ts connecton to R 0, and the connectons between R and other robots. Ths allows a smplfcaton of Eqn. 1: bel (x (t) ) x (t) s.t.x (t) x (t) s.t.x (t) n +1 P (o (t) x (t) ) bel j (x (t) j ) (5) j=0 n +1 P (c (0,), c (,1),..., c (,n +1) x (t) ) bel j (x (t) j ) (6) bel (x (t) )P (c (0,) x (t) 0, x(t) ) bel (x (t) )P (c (0,) dst 0, ) x (t) s.t.x (t) 0,x(t) c (,j) o (t) x (t) j j=0 P (c (,j) x (t) c (,j) o (t), x (t) j )bel j(x (t) j ) (7) P (c (,j) dst,j )bel j (x (t) j ) (8) Eqn. 7 uses the fact that x (t) 0 s known and x (t) s gven as a parameter, and that the probabltes of the connectons between R and other robots are ndependent (usng the model M). Eqn. 8 reverses the order of the sum and product, snce the combnatons of x (t) j are ndependent and can be factorzed n that way. In partcular, snce L, the set of locatons, s common for all robots, x (t) P (c (,j) dst,j )bel j (x (t) j ) j can be computed n O( L ) steps smultaneously. Furthermore, x (t) L, so by loopng across all combnaton L 1, L 2 L, the belefs of all robots can be computed n quadratc tme, nstead of beng exponental n the number of robots. We ncrementally update the belefs of each robot at every tmestep n ths manner. Although each connecton n o (t) can be treated ndependently, n practce ths s also a quadratc operaton, snce t nvolves a summaton across combnatons of 2 robot locatons. Hence, our approach provdes hgher accuracy whle mantanng the same com- 24

25 putatonal cost. In partcular, Algo. 5 computes sum c(,j), and sum c(,j),j = x (t),x (t) j P (c (,j) x (t), x (t) j = x (t),x (t) P (c (,j) x (t), x (t) j )bel (x (t) ) j )bel j(x (t) ) for all c (,j) o (t) smultaneously. The estmated locaton of the target s updated as part of ths ncremental belef update step, and the seeker then plans the shortest path (around obstacles f necessary) to the target s estmated locaton, n order to mnmze the tether dstance. As the seeker encounters obstacles, t updates ts local map and replans the path to the target. 8 Experments n Establshng Connectvty In ths secton, we descrbe the extensve experments that we performed n smulaton on the frst phase of our algorthm for the robot team to establsh connectvty among the statc wreless towers. 8.1 Setup We created a smulator that models a dscrete 2D world, whch allows horzontal and vertcal walls (n between the dscrete cells) to be placed anywhere n the world. The smulator calculates sgnal strength between any two cells n the world, based on an exponental decay rate, as well as degradaton from the walls. We dd not smulate nterference between robots, or reflecton of sgnals from the walls; the underlyng algorthm would not be sgnfcantly affected even f the sgnal strength calculatons were dfferent. In ths 2D world, each robot had 4 possble actons, namely to move north, south, east, or west. In the event that an acton would cause the robot to ht a wall, the acton would fal and the robot would stay where t orgnally was; otherwse the robot would move n the drecton specfed. We mplemented our algorthm as descrbed n Sec. 4, as well as the dfferent heurstcs descrbed n Sec. 5. In addton, we created 3 dfferent scenaros: an Offce World, a Corrdor World, and a Lobby World (see Fg. 12) Offce World The Offce World was cells n sze, and contaned 2 small offces on the top, and a larger offce at the bottom left (see Fg. 12a). An L-shaped corrdor ran n between the top offces and the bottom. A tower was placed nsde each offce, at a dstance such that they could not communcate drectly. Due to the small sze of the Offce World, we fxed the number of robots to 5 and selected ntal startng postons of the robots. Ths world was desgned as a proof-of-concept of the algorthm, as well as to compare the performance of the dfferent heurstcs gven a fxed ntal state Corrdor World The Corrdor World was cells n sze, and contaned 8 offces. 4 offces were arranged horzontally on the top of the world, and the other 4 were arranged horzontally at the bottom (see Fg. 12b). A long corrdor ran n between the top row of offces and the bottom, and 4 towers were placed n a zgzag fashon n the offces. j 25

26 Fgure 12: Representatve worlds that were expermented on: a) Offce World b) Corrdor World, c) Lobby World. Black lnes ndcate walls/obstacles, flled crcles represent towers, and hollow crcles (only n Offce World) show the fxed ntal confguraton of robots. Corrdor and Lobby Worlds had random ntal confguratons of robots. The Corrdor World provded a realstc depcton of many corrdors n unversty hallways whch are flanked by offces on both sdes Lobby World The Lobby World was cells n sze, and contaned a large lobby n the mddle (30 30). Around the lobby were 12 small offces - 3 on each sde, as well as 4 slghtly larger offces located at each corner of the lobby (see Fg. 12c). We placed 4 towers n ths world, 1 n each of the corner offces. The Lobby World provded a depcton of a large lobby area, that s connected to many small rooms. Ths world was desgned not only to smulate real-world stuatons, but also to provde a worst-case scenaro for our algorthm. By havng a large open area, the robots would be able to move around freely and create a dense macro graph, where each robot vertex would have every other robot vertex as a neghbor. Ths could cause the search for soluton to take extremely long, and so we wanted to test the effectveness of our algorthm n such a scenaro. 26

27 8.2 Comparng the Heurstcs We ran the dfferent heurstcs n the Offce World scenaro (whch had 5 robots n a fxed ntal confguraton), wth 100 trals per heurstc. In each tral, we measured how much tme the algorthm took n order to fnd a soluton confguraton. We then compared the percentage of trals that found a soluton n t seconds on less; Fg. 13a shows the comparson of the dfferent heurstcs n the Offce World. All the heurstcs performed well n ths scenaro, wth the stay-at-towers heurstc performng slghtly better than the others. It found 100% of the solutons wthn 12s of executon, compared to 23s of the weghted exploraton heurstc. The coverage and random heurstcs found 99% of the solutons wthn 36s and 69s respectvely. We beleved that all the heurstcs performed relatvely well n the Offce World scenaro due to the fact that many soluton confguratons exsted, and that the robots began n a confguraton that was close to many solutons. Therefore, we performed smlar experments on the Corrdor and Lobby Worlds, wth 5 robots n each case, and a random ntal confguraton for the robots n each tral. As shown n Fgs. 13b and 13c, the stay-at-towers heurstc outperformed all other heurstcs by a large margn, n terms of the tme taken to fnd a soluton, as well as the percentage of solutons found gven a fxed amount of computaton tme. It s nterestng to note that the coverage and weghted exploraton heurstcs performed more poorly than the random heurstc. Ths s because n a large world, a large emphass on exploraton reduces the chance that robots wll meet n a useful confguraton (where useful refers to a partal confguraton that can later be used as part of a soluton), snce they rarely return to prevously vsted cells. As such, t s dffcult for the robot team to fnd a soluton confguraton, even after they have ndvdually explored the entre envronment. After these experments, we concluded that the stay-at-tower heurstc was the most promsng, as t performed well n all the scenaros. We then tested ths heurstc extensvely n the Corrdor and Lobby Worlds, as descrbed below. 8.3 Further Experments for Corrdor and Lobby We used the stay-at-towers heurstc exclusvely for all the experments descrbed below, as t was the most promsng heurstc (see Sec. 8.2). In each experment, we fxed the number of robots and ran 1000 trals. The ntal confguraton of the robots was randomly selected n each tral, and we measured how long the algorthm took to fnd a soluton. Snce our algorthm s dstrbuted, but we were runnng a smulaton where all the robots performed ther computatons, we dvded the total amount of tme elapsed by the number of robots beng smulated. We then compared the percentage of trals that found a soluton n a lmted amount of tme per robot. To be consstent wth the prevous experments comparng the heurstcs (whch ran for 100s for 5 robots), the upper lmt for each tral was set to 20s per robot,.e., 100s for 5 robots, 300s for 15 robots, etc. In Fg. 14a, we show the results n the Corrdor World, wth the number of robots varyng from 5 to 50. Smlarly, n Fg. 14b, we show the results n the Lobby World, also wth the number of robots varyng from 5 to 50. In Fg. 15a, we show some random ntal confguratons of 5 robots and the correspondng solutons found n the Corrdor World; n Fg. 15b, we do the same for the Lobby World. In both the Corrdor World (Fg. 14a) and Lobby World (Fg. 14b), all the graphs trend upwards towards 100%, the graphs are not flat at the end of 20s, and would reach 100% f the algorthm was 27

28 (a) Offce World (b) Corrdor World (c) Lobby World Fgure 13: Percentage of trals that found a soluton n t seconds or less wth 5 robots n the dfferent scenaros. (a) Corrdor World (b) Lobby World Fgure 14: Percentage of trals that found a soluton wth varyng number of robots runnng stayat-towers heurstc. run to nfnty. An nterestng observaton s that the graph for 5 robots crosses that of 15 robots n both scenaros, whch occurs because searchng a network graph of 5 robots s much qucker than searchng one wth 15 robots at each tme step, O(n 2 ) new constrants are added, where n s 28

29 (a) Corrdor World (b) Lobby World Fgure 15: The top row shows random ntal confguratons of 5 robots (hollow crcles) and 4 towers (flled crcles); the bottom row shows the correspondng solutons found. The black lnes represent walls/obstacles, and the blue lnes ndcate connectons between robots and towers. Number of steps Number of robots Corrdor World Lobby World ± ± ± ± ± 12 8 ± ± ± 2.7 Table 1: Number of steps to fnd a soluton the number of robots, and thus, whle the sze of the graph grows polynomally wth the ncrease n the number of robots, searchng for a soluton n the graph takes exponentally longer. As the number of robots ncreases beyond 15, the number of possble soluton confguratons ncreases dramatcally, and solutons are found much more quckly. Table 1 shows the average number of steps to fnd a soluton n Corrdor and Lobby worlds. As the number of robots ncreases, the number of steps decreases substantally, whch reflects the ncrease n the number of possble soluton confguratons. Also, when 50 robots are randomly placed n the world, there s a hgh probablty that they already are n a soluton confguraton, whch s why the average number of steps taken s close to 0. 9 Tetherng Experments We mplemented the second phase of our algorthm, to locate and tether to an autonomous agent, on the Robot Creates and n smulaton, and descrbe our experments below. 29

30 9.1 Expermentng n Smulated Envronments We created a 2D dscrete smulator, that allows walls to be placed n between cells of the world. For our experments, we used a world, wth rooms and corrdors to smulate an offce envronment. Each cell s 1m 1m, and thus L, the set of locatons n the world, s a set of 900 dscrete cells. The robots n the smulator can move n the 4 cardnal drectons. Multple robots are able to occupy the same cell, as we are smulatng the Robot Creates, and ther physcal dmensons are small enough for at least 4 of them to occupy a 1m 1m area. The robots actons move them exactly 1m n the desred drecton, unless a wall blocks the path. We smulate perfect odometry on the robots, as mperfect odometry can be compensated by other sensory feedback. The robots buld occupancy grds that allow path-plannng. To smulate connectons between robots, we used the RSSI-dstance model M descrbed n Sec. 6. For a gven dstance d, M dstance (d) returns the maxmum and mnmum RSSI (RSSI mn and RSSI max respectvely) that can be acheved at the dstance. We then return an RSSI value sampled unformly between RSSI mn and RSSI max. At each tme step, the current postons of the robots are used to generate connectons and RSSIs. Each robot receves a lst of drect connectons and ther RSSIs. The team of cooperatve robots, R 0,..., R n, communcate and merge ther receved nformaton, as descrbed n Sec Locatng the Target Successfully In the frst phase of our experments, we ntalzed R 0 n the mddle of the world, and randomly placed the other robots. R 0 was the only moble robot, and t moved n a square of length 10m,.e., North, then East, then South, then West, and collected observatons, as descrbed n Sec We vared n, the number of cooperatve teammates, from 0 to 5, and measured the error n the estmated locaton of R n+1 usng the L2 metrc. The estmated locaton of R n+1 was calculated by consderng all vald jont-locaton hypotheses of the robots, and takng an average of R n+1 s locaton n each hypothess. For each value of n, we ran 500 experments. Fg. 16 shows the error of the estmaton as the number of teammates vary. When there are no teammates (only the seeker and target exst), the error n the estmated poston of the target s 3.89 ± 3.88m. As the number of teammates ncrease, ths error decreases monotoncally, to 1.35 ± 1.23m when there are 5 teammates. Thus, havng more teammates ncreases the accuracy of estmaton, as more nformaton s collected at each tme step. Also, teammates provde addtonal dstance constrants, whch ad n narrowng down possble hypotheses n the target s locaton Tetherng to the Target Effectvely In the second phase of our experments, the target robot moves, and the seeker mnmzes ts dstance to the target. We smulated the target s movement by frst flppng a balanced con to decde f the target takes an acton. If an acton s to be taken, then the target flps a weghted con to decde f t mantans ts current drecton (70%) or heads n a new drecton (30%) wth equal probablty for each drecton. We ntalzed the seeker at the center of the world, and placed the target robot n the cell north of the seeker, to smulate that the seeker moved to the target pror to the target s moton. The other robots n the team were randomly postoned n the world. The target robot moved autonomously wthout communcatng ts actons. Usng the algorthm explaned n Sec. 7.2, the 30

31 Fgure 16: The effect of teammates n estmatng the target s locaton. For each number of teammates, 500 trals were run wth random ntal postons of teammates and target. seeker updated a probablstc model of the target s locaton. However, snce the seeker s unaware of f and when the target moves, as well as the drecton of moton, at each tme step, the seeker performs a blur on the probablstc map of the target s locaton. To do so, the seeker assumes that the target s acton u (t) n+1 = move, such that: { α f L 1 s adjacent to L 2 P (L 2 L 1, move) = 0 otherwse P (L 1 L 1, move) = 1 P (L 2 L 1, move) L 2 L 1 We set α = 0.2, vared the number of teammates from 0 to 4, and measured the dstance between the seeker and target n the L1 metrc. We used the L1 metrc because the robots moved n the 4 cardnal drectons, so the L1 metrc calculates the mnmum tme steps requred for the seeker to reach the target. Fg. 17a shows the average dstance between the seeker and target, over 100 tme steps, averaged across 250 runs. In each run, the teammates postons were generated randomly. We also vared the standard devaton of the ntal estmate of the robots locatons, from σ = 0m to σ = 6m. The results show that ncreasng the number of teammates reduces the average tethered dstance, even when the ntal estmate of the locatons s mperfect. Thus, the seeker s capable of followng the target as t moves n the world, even though the target does not nform the other robots when t moves, and the drecton that t moves n. Further, the probabltes used by the target to decde ts moton are also unknown to the seeker; the seeker uses a general blur to capture all possble motons of the target n each tme step. Fg. 17b shows the average dstance between the seeker and target as a functon of tme steps, when σ = 0m. The target moves randomly n the world and the seeker follows t. When there are no teammates n the world, the dstance between the seeker and target quckly rses, and reaches a steady-state of 9.7 ± 6.2m after 84 tme steps. As the number of teammates ncreases, the seeker acheves a steady-state more quckly and at a closer dstance (e.g., 2.5 ± 1.9m after 42 tme steps wth 4 teammates). Thus, ncreasng the number of teammates enables faster convergence and better tetherng. 31

32 (a) The average tethered dstance to the target robot n smulated envronments, over 250 trals of 100 tme steps each, wth σ varyng from 0m to 6m. (b) Tethered dstance between the seeker and the target over tme n smulaton. Fgure 17: Tethered dstance between the seeker and the target n smulaton. (a) Multple Robot Creates deployed n a real offce. (b) The average tethered dstance between the seeker and target robots n a real offce envronment, over a perod of 10 mnutes for each tral. Fgure 18: Tetherng experments wth real robots. 9.2 Measurng Real Robot Performance We mplemented the tetherng algorthm on the Robot Creates, and evaluated the performance n a real offce (Fg. 18a). The seeker and target robots were placed 1.5m apart, and the seeker s ntal estmates of the other robots had a standard devaton σ = 2.5m. The target robot traveled n a straght lne, whle the seeker estmated ts locaton and tethered to t. We marked the locaton of the seeker and target robots every mnute for a total of 10 mnutes n each tral. Due to the lmted memory and CPU on the robots, the experments were lmted to a 10m 10m area. We vared the number of teammates from 0 to 2, and ran 5 trals for each confguraton of the robots; there were 2 possble confguratons of a sngle teammate. Fg. 18b shows the average dstance between the seeker and target. The results clearly show that as the number of teammates ncreases, the average dstance decreases, whch s smlar to the results n smulaton. The average dstance s a smaller number compared to the smulaton results due to the envronment s sze the real robot experments were performed n a 10m 10m world whle the smulatons were 30m 30m. 32