Probabilistic Map Building by Coordinated Mobile Sensors

Transcription

1 Probablstc Map Buldng by Coordnated Moble Sensors Jen-Yeu Chen, Student Member, IEEE, and Jangha Hu, Member, IEEE Abstract In ths paper, we develop an effcent algorthm for coordnatng a group of moble robotc sensors to collaboratvely construct the probablstc map (occupancy grd) of a random feld. Each moble sensor s responsble for explorng ts onlnecomputed dynamc Vorono polygon wth a tendency of movng toward the most uncertan area, thus avodng energy and tme waste from wanderng around and surplus exploratons. The performance s measured by the total of the bult probablstc map. Smulaton results show that our algorthm s both tme and energy effcent. I. INTRODUCTION Map buldng s the process of establshng a representaton of a prevous unknown envronment, whch, for nstance, mght be a room n a buldng, a potentally hazardous workng place, or a battle feld. The task of map buldng s usually undertaken by one or multple moble robots whch are equpped wth varous sensors aboard. A group of moble robots wth sensors aboard s also called a moble sensor network n whch a sensor node s a robot. Much of the research effort to date to buld a map has focused on the sngle robot scenaro, whereas n ths paper we develop an effcent scheme for multple robots to cooperatvely construct a map. Because of the complcated nature of the task, map buldng s often coupled wth other tasks. For example, n the front end, when the problem of locatng the robots n the envronment s also consdered, one has the SLAM (smultaneous localzaton and mappng) problem [3], [11], [12]. Alternatvely, map buldng can also be combned wth varous applcatons that utlze the map to be bult, for example, the pursuer s strategy n a pursuer-evader game n an unknown envronment. There are two typcal mappng ways to represent the knowledge of a regon of nterest. The frst approach [3], [11], [12], assumes a set of possble shapes of obstacles, e.g. polygons or rectlnear polygons, n a gven regon. Usually each such shape s parameterzed by a fnte set of parameters whch the robots need to determne. The second approach [4], [5], [6], [8], [9], [13], [14], [15] parttons the regon nto a regular grd of cells (as shown n Fg. 1), and assocates wth each cell a probablty that reflects the possblty that the cell s occuped by an obstacle. The resultng cells together wth the assocated probabltes are often called the occupancy (uncertanty) grd. An occupancy grd s a probablstc map of the regon (envronment) based on known knowledge and t can be updated by new knowledge. J.-Y. Chen and J. Hu are wth School of Electrcal and Computer Engneerng, Purdue Unversty, West Lafayette, IN 4797, USA (Emal: {jenyeu, jangha}@purdue.edu). Each of these two approaches has ts own advantages and dsadvantages, and captures one aspect of the map buldng problem. In ths paper, we focus on the second one. The major keys to the map buldng problem are as follows. Sensor Model. Dependng on the type and capablty of on-board sensors, a sensor model s a functon from the envronment states (n ths case, the confguraton of obstacles and the postons and orentatons of the robots) to a set of measurement readngs, possbly corrupted by noses. In ths paper, a sensor model P(M A) s the condtonal probablty of gettng the measurement M gven that the envronment s wth property A, e.g. occuped by obstacles or not. For example, the readngs from a range detector ndcate the presence of obstacles wthn a cone together wth ther dstances from the robot, and the readngs are accurate wth a certan probablty determned by the dstance of the obstacle, ts devaton from the center of beam, and heat noses of the sensor. The Sensor models are normally nvestgated and determned by takng readngs of the sensor n known envronment states. Map Update Model. Gven a set of measurements taken by the moble robots, one needs to update the map n accordance wth these measurements. Also, t s often the case n practce that the maps kept n dfferent robots are not dentcal, whether ths s due to communcaton delays, or transmsson errors. It s mportant that one should have a mechansm to reconcle these dscrepances. The typcal methods n map update are the Bayes update rule and varous other nference rules. Of these we menton partcularly the graphcal model, whch s suted for nferences under complcated relatons. Control Strategy. At each tme nstance, one has to decde the moton and temporary goal of each moble robot. The control strateges are classfed as centralzed and decentralzed. The centralzed strateges assume the presence of a central controller, and often the precse nformaton on the postons of the robots, whle for decentralzed strateges, each robot makes decsons on ts own usng nformaton that s avalable to t at that tme, whch could vary among dfferent robots, and, occasonally, the decsons could be redundant or even contradctory at the moment. In ths paper, we present a hybrd algorthm to coordnate the moton of a group of robots. A central controller doesn t exst and all robots execute the same coordnated map buldng algorthm

2 based on the same nformaton, a shared map, updated by drect communcatons wth each other. Practcal Constrants. Dependng on the way of communcaton between robots, varous practcal constrants could be adopted. For example, the power lmt and channel capacty of the communcaton unts aboard the robots could constran that at any tme the robots should form a team that no robot s at a dstance farther than a threshold away from the rest of the robots, or each robot should be vsble to at least one other robot, or each robot can only receve the up to date maps from other robot after a certan tme of delay. The ablty to ncorporate these constrants s vtal for the success of the algorthm n practcal stuatons. II. PROBLEM FORMULATION AND THE SOLUTION Suppose there are multple moble robots tryng to map an unknown regon D n R 2. It s possble to apply the current approach to 3D regons after certan modfcatons. The regon D R 2 s parttoned nto a grd of cells, D = {(x, y ) Z 2 : Γ}, where Γ s a fnte set of the ndces of the cells. Each (x, y ) D represents a rectangle [x, x +1] [y, y +1] R 2, whch s ether occuped by an obstacle totally, or s empty totally. See Fg. 1 for an example of a rectangular regon D, where those cells occuped by obstacles are blackened. If at some tme a robot s n a cell (x, y ) whch s not occuped, then at the next tme, t can, accordng to the coordnaton polcy, stay at cell (x, y ), or move to one of the un-occuped mmedately neghborng cells of (x, y ). Fg. 1. Regon D wth obstacles marked. The knowledge of a robot about the envronment D s represented by the probablstc map (occupancy grd) t keeps, whch s defned to be a collecton of probabltes, {p [, 1] : Γ}, such that p s the probablty (the certanty) that cell (x, y ) s occuped. The value of p wll be updated whenever a robot explores (measures) cell. Usually a transformed verson of the probablty map (occupancy grd) s more useful. For each probablty p, the odd of the cell s defned to be o = p 1 p [, ] (1) The logarthmc odd map s the collecton {logo : Γ}. A. Sensor Model We suppose all robots are wth the same sensor model. Each robot takes measurement of each cell ndependently. A measurement of cell = (x, y ) s a random varable denoted as M (k) {, 1}, where k s the number of measurements made at cell regardless from whch robot (snce ther sensor models are the same). M (k) = 1 means that the k- th measurement result suggests cell s occuped whereas = represents the k-th measurement result of cell s un-occuped. After takng measurement, all the robots then update the shared occupancy grd, the probablstc map of the whole regon. We assume the robots can communcate 1 wth each other tmely to update the shared occupancy grd. Ths assumpton s based on the fact that the energy cost of robots movements s larger than that of communcaton. Thus, a coordnaton globally mnmzng the number of robots movements wll beneft the energy savng although t spends some energy on communcatons. Let A {1, } be the random varable that cell s truly occuped (A = 1) or not (A = ) n realty. The sensor model of each robot s hence p(m A ). We assume the sensor model p(m A ) s tme nvarant. For smplcty we denote M (k) p = P(M = A = ), p 1 = P(M = A = 1), p 1 = P(M = 1 A = ) p 11 = P(M = 1 A = 1). (2) The larger p, p 11 are, the more accurate the sensors. For a more realstc model, one can choose dfferent values of these condtonal probabltes for dfferent cells measured, dependng on the sensng pattern of the sensor and the dstance as well as the angle between the sensor and the cell measured, provded that a robot can sense not only ts currently located cell but also the neghborng cells. B. Map Update Model The robots take measurements of the envronment, and update the occupancy grd based on these measurements. Whenever a robot s at a poston (x, y ), t can measure the cells n [(x, y ) + N b ] D, where N b s a cluster of ponts n Z 2 contanng the orgn. Hence the robot can always measure the cell t stays at, as well as a set of cells close to t. The sze of N b reflects how powerful the sensors on board are. Suppose a cell has been measured k tmes. Let M (1,k) = {M (1), M (2)...,M (k) } be the sequence of k measurements on cell. A robot now vsts cell to make a new measurement M (k+1) and then updates the probablty (certanty) of occupancy of cell, p (k+1) = P(A = 1 M (1,k+1) ), (3) 1 In the regon of nterest, robots may communcate wth each other by one-hop drect connectons, mult-hop relays among robots, or connectons va a base staton.

3 where the superscrpt (k+1) of p (k+1) ndcates the (k+1)- th. Also, the odd of cell at (k + 1)-th, denoted o (k+1), s o (k+1) = p(k+1) By the Bayes rule, we have P(A M (1,k+1) ) = P(A M (1,k) = P(M(k+1). (4) 1 p (k+1), M (k+1) M (1,k) ), A ) P(A M (1,k) M (1,k) ) = P(M(k+1) A ) P(A M (1,k) ) ), ) where the last equalty follows from the assumpton that all measurements are ndependent. However, the above equaton s not useful as the probablty ) s not readly avalable. To overcome ths dffculty, we set A = and A = 1 respectvely to obtan P(A = 1 M (1,k+1) ) = P(M(k+1) P(A = M (1,k+1) ) A = 1) P(A = 1 M (1,k) ) ) = P(M(k+1) A = ) P(A = M (1,k) ) The quotent of the above two equatons yelds P(A = 1 M (1,k+1) ) P(A = M (1,k+1) ) = P(M(k+1) A = 1) A = ) P(A = 1 M whch can be rewrtten as where ρ M = o (k+1) { ρ = p1 p ρ 1 = p11 p 1 (1,k) ) P(A = M (1,k) ), ) = ρ M o (k), (5) f M (k+1) = f M (k+1) = 1. Note that the value of ρ M s pre-determned by the sensor model and does not depend on the tme k snce we assume the sensor model to be tme nvarant. Normally, a sensor s of the hgher probablty to have a correct measurement,.e. p p 1 and p 11 p 1. Thus normally < ρ 1 and ρ 1 1. For practcal usage, we take the logarthm of equaton (5) to obtan the map update rule for the logarthmc odd: log o (k+1),. (6) = log ρ M + log o (k), (7) where ρ M s determned by (6). At each tme, robots measure the cells wthn ther measurement range, and update the logarthmc odd map as Fg. 2. Vorono parttons for Eucldean and street dstances. descrbed n equaton (7). Intally, all the probablty p () are.5, reflectng the fact that the robots know nothng about the envronment. Hence log o () =, Γ. As tme goes on and new measurements are taken, n general log o (k) become random processes whose values at each depend on the outcome of the random measurements by robots. Note that log o (k+1) measurement ranges. = log o (k) C. Coordnaton and Performance Metrc f the cell s outsde all robots Our goal s to fnd a dynamc strategy to coordnate the motons of robots such that the resdual uncertanty of the map s mnmzed n a fxed amount of tme. The resdual uncertanty of a probablstc map (occupancy grd) can be defned by the total of the occupancy grd: H(D) = H() = log p + (1 p )log(1 p )), Γ Γ(p (8) where the frst equalty follows from the ndependence of p of each cell. W. l. o. g., we omt the superscrpt (k) n the above defnton. Note that the larger H(D) s, the more uncertan the correspondng probablstc map. Snce robots keep explorng the regon D, the total H(D) s n a decreasng tendency. The p of equaton (8) can be easly obtaned from the updated odd or logarthmc odd by ther relatonshp n equaton (1). In the next secton, we wll present our algorthm for the coordnaton of multple robots that can reduce the resdual uncertanty H(D) of a probablstc map n a tme and energy effcent way. III. ALGORITHM Suppose at some tme, the postons of the robots are (a l, b l ), l = 1,...,m. The Vorono partton s a decomposton of D nto dsjont subsets D 1,..., D m, such that D l = {(x, y ) D : (x, y ) s closest to (a l, b l )}. In case that there s more than one (a l, b l ) closest to (x, y ), we can attrbute (x, y ) to any one of them. D l s called the Vorono polygon of robot l. Shown n Fg. 2 are the Vorono parttons under two dfferent defntons of dstance: Eucldean and street. The phlosophy of our algorthm s that a robot should be responsble for the exploraton of the uncertan area closest to t. To acheve ths goal, every robot computes

4 the Vorono partton of the shared occupancy map and then moves toward the cell of the mnmal absolute odd o n ts Vorono polygon. If a cell has been well explored, t would be of low (hgh certanty) H() and hgh absolute odd, o. Assume that Eucldean dstance s adopted. Our recursve algorthm s outlned below. A regular ncludes all actons except the ntalzaton. The effect of the regster n l s to let the robot to turn back to the orgnal drecton after t made turns to avod obstacles scenaro2 Entropy on regon 3, D 3 scenaro1 (rght down) scenaro2 (rght down)) scenaro3 (left down) scenaro4 (left down) scenaro3 scenaro4 Algorthm: coordnated map buldng by multple robots Intalzaton: each robot chooses ts ntal poston, set ts mode to be pursue and ntalze the logarthmc odd map {log o = : Γ}. A ncludes the followng actons: 1. All the robots take measurements and update the occupancy map accordngly. 2. Gven the postons of robots, (a l, b l ), l = 1,..., m, construct the Vorono partton D 1,..., D m. For each robot l = 1,...,m, If the mode of robot l s pursue, fnd among all cells (x, y ) D l the one mnmzng o + λ (x, y ) (a l, b l ) 2, (9) where λ s a postve weght. Suppose (x, y ) s one mnmzer. Let goal l := (x, y ). The robot l try to move to ts neghbor closest to goal l. If the neghborng cell s empty then move to the cell and keep n pursue mode. If that cell s occuped, turn counterclockwse (left) to fnd a empty neghborng cell and move to the cell unless the robot s trapped. Set the mode of robot l to avod and ntalze a regster, n l =, whch records the number of turns. If the mode of robot l s avod, and n l < 2 then robot l turn clockwse (rght) and try to move to that neghborng cell. If succeed, set n l = n l + 1, else f turn counterclockwse (left) to fnd a empty neghborng cell and move to the cell unless the robot s trapped. If the mode s avod and n l = 2, set the mode to pursue and reset n l =. If the boundary of D s reached, reverse the drecton of turnng, reset n l =. 3. check for some stoppng crtera (total has been less than a pre-decded threshold) f t should be termnated. 5 scenaro Fg. 5. The total (resdual uncertanty) vs. tme. of Regon 3 D 3 A. Smulaton setup IV. SIMULATIONS We comprehensvely smulate our algorthm on regons n varous szes and confguratons. Fg. 3 llustrates an example of 3 moble robots to explore a square regon D, n whch the snapshots of the constructed map (occupancy grd) at dfferent tme s are plotted. We ndcate the probablty of occupancy of each cell n grey-scale. In the begnnng, all the cells are un-explored and n neutral grey. Whle the moble agents move around, they gradually reveal cells occupancy. The darker a cell s color becomes, the hgher the probablty t s occuped. A cell become whte or black as t s explored to be empty or occuped, respectvely. We present smulaton results on four dfferent regons shown n Fg. 4 as four 2x2 cells n dfferent occupancy confguratons. In each case, there are four robots to explore the regon. We set the parameter λ n equaton (9) to.1. Also, the condtonal probabltes to make correct measurement n condton of a cell s occupancy, namely, p and p 11, are both set to.9. A robot can measure ts currently located and neghborng cells. The p and p 11 for the currently located cell and neghborng cells are set to be the same. We thus compare the results of robots wth two dfferent measurement ranges: (1) current cell and one cell ahead n the robot s orentaton, (2) current cell and four neghborng cells left, rght, up, and down. We measure the performance by the resdual uncertanty of exploraton n terms of bnary total, H(D). B. Smulaton results The four curves n Fg. 5 show the results of four dfferent exploraton scenaros for the regon D 3 shown n Fg. 4(c). The four robots ntal postons are (12,4), (13,4), (14,4), and (15,4) for scenaros 1 and 2 and (4,4), (5,4), (6,4), and (7,4) for scenaros 3 and 4. Even though robots ntate at the same postons, the exploraton scenaro can vary due to the

5 Fg. 3. Maps at dfferent tme s. (a) Regon 1, D 1. (b) Regon 2, D 2. (c) Regon 3, D 3. (d) Regon 4, D 4. Fg. 4. Regons consdered n smulaton. random nature of measurements. The ntal total (bnary) s 4 as each cell has ntal (bnary) 1. The total decreases at a constant rate ntally, ndcatng that all the robots are explorng new area. The rate of decrease slows down gradually and then abruptly after a certan tme, at whch pont most of the regon has been explored by at least one robot. The fluctuaton n the curve s due to the random nature of robot measurements. All curves land on the fnal total whch s 3 for D 3 snce 3 cells of D 3 are surrounded by obstacles and can not be explored. At the early stage, the deceasng rate of scenaros 3 and 4 are smaller than that of scenaros 1 and 2. Ths s because, n scenaros 3 and 4, robots whch ntate at the left down corner are confned by obstacles. There are only two doors,.e. cell (2,7) and cell (12,3) for those robots to go through to explore other areas of D 3. A robot may keep searchng explored cells at left down corner before t goes through one of the doors. On the other hand, n scenaros 1 and 2, all robots whch ntate at the rght down corner are free to search un-explored cells. The left sub-fgure of Fg. 6 shows the total decreasng by tme s on four dfferent regons shown n Fg. 4 whereas the rght sub-fgure magnfes the s from 2 to 3 to show the dfferent fnal total entropes of the 4 dfferent regons. The fnal total of Regon 2, D 2, s snce no cell s surrounded by obstacles n D 2. The Regon 4, D 4, wth the largest number of cells beng surrounded by obstacles, hence has the largest fnal total. Rarely but once n a whle, especally at the fnal stage, the total could lghtly buck up and then return to the decreasng trend as shown n the curve 4 n the rght subfgure of Fg. 6. It s possble that two consecutve measurements of a cell are opposte and hence the latter measurement ncreases the (the uncertanty) of the cell. Most cells get consecutve same (and correct) measurements n that the probablty of correct measurement s.9. Each of them contrbutes to the decrement of total. Only few cells have opposte consecutve measurements, contrbutng to the ncrement of total. The effect of ncrement from consecutve opposte measurements wll manfest at total only when (1) most of the cells have been well explored and the changes of ther become extremely small (near ). (2) one cell whch has been explored once (n the case of curve 4) has a measurement opposte from ts prevous one, ncurrng a (bg) ncrement larger than the sum of all the extremely small decrements from other cells. These two condtons wll only happen (wth low possblty) at the late stage of smulaton as n the curve 4 n rght sub-fgure of Fg6. The followng measurements of the cell wll decrease the back. Fg. 7 compares the effect of dfferent measurement ranges of robots. We show the results of two regons, D 1 and D 2. In each case, two measurement ranges are consdered: (1) one neghborng cell n the drecton of robot s orentaton and (2) four neghborng cells. Robots ntate at cells (6,6), (15,6), (6,15) and (15,15). The wder measurement range does help at the early stage n decreasng the total as t explores more cells at a tme. After most of the cells have been explored, the range wll not help much at the late stage of exploraton.

6 Total entroy vs on 4 dfferent regons regon1 D 1 regon2 D 2 regon3 D 3 regon4 D regon1 D 1 regon2 D 2 regon3 D 3 regon4 D 4 Fg. 6. The total (resdual uncertanty) vs. tme on 4 dfferent regons Total on Regon 1, D neghbor 4 neghbors Total on Regon 2, D neghbor 4 neghbors Fg. 7. The total (resdual uncertanty) vs. tme. for dfferent measurement ranges of robots on Regon 1 D 1 and Regon 2 D 2. V. CONCLUSION AND FUTURE WORKS In ths paper, we present a coordnaton algorthm for a group of moble robots wth sensors aboard to effcently construct the probablstc map (occupancy grd) of a random feld. The probablstc sensor model and the mapupdatng model of the probablstc map are also thoroughly developed. We measure the performance of the coordnatng algorthm by the total (resdual uncertanty) of the bult probablstc map. The map constructon task wll fnsh when the total s below a desred threshold. All robots can communcate to update a shared map. By the dynamcally computed Vorono areas (cells), the regon under exploraton s separated nto several areas. Through the coordnaton algorthm, a robot wll be responsble to explore the area closest to t n dstance, effcently savng the energy cost for ts movements. Our smulaton results show that, va collaboraton, robots who only search ther own Vorono polygons obtan the map of the whole regon effcently both n tme and energy costs. In the presented coordnaton algorthm, we assume all robots can communcate to share a probablstc map. Ths assumpton s based on the fact that the energy cost of communcaton s less than the energy cost of robot movement. To further reduce the energy cost, we now are lftng ths assumpton and consderng the stuaton that robots can only communcate wth nearby robots wthn a certan dstance to exchange and ther maps. Thus, dfferent robots wll have dfferent maps at the early stage and then reach the consensus after all. Each robot decdes where to move and explore solely dependng on ts own map. It wll be nterestng to see the drawbacks and benefts of ths modfcaton. REFERENCES [1] I. J. Cox and J. J. Leonard, Modelng a dynamc envronment usng a Bayesan multple hypothess approach, Artf. Intell., 66(2): pp , [2] J. L. Crowley, Coordnaton of acton and percepton n a survellance robot, IJCAI, pp , [3] M.W.M. G. Dssanayake, P. Newman, S. Clark, H. Durrant-Whyte and M. Csorba, A soluton to the Smultaneous Localzaton and Map buldng (SLAM) problem, IEEE Transacton on Robotcs and Automaton, 17(3): pp , 21. [4] A. Elfes, Usng occupancy grds for moble robot percepton and navgaton, Computer, 22(6): pp , [5] A. Elfes, Occupancy grds: A stochastc spatal representaton for actve robot percepton, In Proc. Sxth Conference on Uncertanty n AI [6] A. Elfes, Dynamc control of robot percepton usng mult-property nference grds, In Proc. IEEE Internatonal Conference on Robotcs and Automaton, [7] D. Hahnel, R. Trebel, W. Burgard, S. Thrun, Map buldng wth moble robots n dynamc envronments, In Proc. IEEE Internatonal Conference on Robotcs and Automaton, pp , 23. [8] K. Konolge, Erratc competes wth the bg boys, AI Magazne, 16(2): pp , [9] K. Konolge, Improved occupancy grds for map buldng, Auton. Robots, 4(4): pp , [1] K. Konolge, Large-scale map-makng, In Proc. AAAI, pp , 24. [11] J. Leonard, H. Durrant-Whyte, and I.J. Cox, Dynamc map buldng for an autonomous moble robot, In IROS, pp , 199. [12] J. Leonard and H. Durrant-Whyte, Drected Sonar Sensng for Moble Robot Navgaton, Kluwer Academc Publsher, Boston, [13] H. P. Moravec and A. Elfes, Hgh resoluton maps from wde angle sonar, In Proc IEEE Internatonal Conference on Robotcs and Automaton, pp [14] H. P. Moravec, Sensor fuson n certanty grds for moble robots, AI Magazne, 9(2): pp , [15] H. P. Moravec, M. Blackwell, Learnng sensor models for evdence grds, Robotcs Insttute Research revew, pp. 8-15, [16] C. Ortz, K. Konolge, R. Vncent, B. Morsset, A. Agno, M. Erksen, D. Fox, B. Lmketka, J. Ko, B. Steward, D. Schulz, Centbots: Very large scale dstrbuted robotc teams, In Proc. AAAI, pp , 24. [17] B. Schele, J. L. Crowley, A comparson of poston estmaton technques usng occupancy grds, In Proc. IEEE Internatonal Conference on Robotcs and Automaton,, pp , [18] B. Stewart, J. Ko, D. Fox, K. Konolge, The revstng problem n moble robot map buldng: A herarchcal Bayesan approach, UAI, pp , 23. [19] R. Szabo, Topologcal navgaton of smulated robots usng occupancy grd, CoRR cs.ro/41122, 24. [2] S. Thrun, A probablstc onlne mappng algorthm for teams of moble robots, Int. J. Robotcs Research, 2(5): pp [21] S. Thrun, Learnng occupancy grd maps wth forward sensor models, Auton. Robots, 15(2): pp , 23.