Self-Organized Distributed Localization Based on Social Odometry

Transcription

1 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry Álvaro Gutérrez, Félx Monastero-Hueln Unversdad Poltécnca de Madrd, Span Alexandre Campo IRIDIA, CoDE, Unversté Lbre de Bruxelles, Belgum Lus Magdalena European Centre for Soft Computng, Span 1 Introducton Most of the localzaton exstng works address the problem of a sngle robot, but only few works address t from the mult-robot pont of vew. In a smple way, each sngle robot could mplement ts own localzaton algorthm by gnorng the presence of the other robots n the envronment. Followng ths approach, we could use a sngle robot localzaton algorthm n every robot n the team, but robots would gnore useful nformaton avalable n ther neghbors. For these reasons, n the lterature, mult-robot localzaton problem s referred to as a collaboratve/cooperatve localzaton problem (Panzer et al., 2006). In ths chapter, we develop, analyze and study a localzaton strategy, n whch robots nether share any movement constrant nor access to centralzed nformaton. Ths soluton explots self-organzed cooperaton n a swarm of robots to reduce each ndvdual locaton error. In a nutshell, each robot s knowledge conssts of an estmate of ts own locaton and an assocated confdence level that decreases wth the dstance travelled. Ths nformaton s purely local and results from ndvdual experence wth the envronment and other robots. In order to maxmze ts confdence level, each ndvdual updates ts estmates usng the nformaton avalable n ts neghborhood. Hence, each ndvdual wll adopt the estmate of ts mmedate neghbors wth a probablty that ncreases wth the rato of confdence levels. Ths adaptve dynamcs, prevously defned as Socal Odometry (Gutérrez et al., 2008b), confers to each robot the possblty of ganng knowledge from others, by mtatng estmated locatons that offer hgher confdence levels. Estmated locatons, confdence levels and actual locatons of the robots dynamcally change n order to gude each robot to ts goal. Ths smple onlne socal odometry allows the populaton of robots to both reduce ndvduals errors and effcently reach a common objectve. Moreover, ths collectve navgaton uses successfully mnmal local communcaton, to promote an effcent collectve performance and removes the need of statonary robots. We ntroduce the Socal Generalzed Induced Kalman Flter (SGIKF) and the Socal Generalzed Induced Ferm Flter (SGIFF) based on a resemblance to the Kalman Flter (KF) and ts Induced Kalman Flter (IKF) dervaton (Gutérrez, 2009). The flter development flows to the confdence level concept, nspred by the spectral norm of the covarance matrx.

2 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry For the algorthm valdaton, real localzaton and communcaton systems are needed to provde stuated and abstract communcaton. Abstract communcaton refers to communcaton protocols n whch only the content of the message carres a meanng and the physcal sgnal (the medum) that transports the message does not have any semantc propertes (Støy, 2001). Dfferently, stuated communcaton means that both the physcal propertes of the sgnal that transfers the message and the content of the message contrbute to ts meanng (see (Clancey, 1997) for more detals). For example, when an ndvdual tells us to come here the content of the message has not all the nformaton, but the locaton of the sender wth respect to our poston makes the message gets the meanng of come to my poston. One way to do so s to let the communcatng robots extract the locaton of the communcatng source from the sgnal receved. Therefore, these systems are commonly called localzaton and communcaton systems. Note that abstract communcaton tends to be used n mult-robot systems n whch a wreless network provdes the requred structures to transmt messages from a specfc sender to a specfc recever (Tubashat & Madra, 2003). The use of wreless networks mples experments wth long range communcaton. Because of the number of robots and the dmensons of the expermental envronments typcally used, such communcaton span across the whole group of robots. In swarm robotcs, ths s n contradcton wth the concept of localty whch s seen as an mportant ngredent to acheve scalablty (Dorgo & Şahn, 2004). Therefore, n our expermental valdaton we make use of a local communcaton sensor (Gutérrez et al., 2008a) whch offers stuated and abstract communcaton to the robots. 2 Related work Probablstc methods have been appled wth remarkable success to robot localzaton (Smmons & Koeng, 1995; Cassandra et al., 1996; Burgard et al., 1996; Burgard et al., 1998; Gutmann et al., 1999; Gutmann et al., 2001; Chong & Kleeman, 1997; Wang, 1988). Most of these approaches are based on Markov localzaton methods whch make use of dead-reckonng and absolute or relatve measurements. The key dea s that each robot mantans an estmate on ts poston whch wll be updated accordng to ts odometry calculatons and measurements n the envronment. The most used probablstc method has been the Kalman Flter (KF) (Kalman, 1960; Larsen et al., 1998; Martnell & Segwart, 2003). It s an optmal flter that estmates a state vector contanng the robot poston and the parameters characterzng the odometry error ntroduced by the desgner. The KF makes a number of assumptons on the system, measurements and dfferent noses that are nvolved n the estmaton problem. It presupposes that the system and measurements are adequately modeled by a lnear dynamc system, and that noses are ndependent and whte. Moreover, the KF presumes that the ntal state of the system s also ndependent and Gaussan dstrbuted. Although the KF s an effcent recursve flter, t requres to ntroduce n the robots external nformaton that models the envronment and t s also computatonally costly. For nstance, n (Thrun et al., 2000) robots navgate n an ndoor envronment where a map s gven to the robots and make use of the KF. Each robot senses the envronment, correct ts measure accordng to the observaton of others robots and exchange the nformaton wth the rest of the team. In ths work, the way the nformaton s exchanged s not presented. In (Roumelots & Bekey, 2002), each robot measures ts relatve orentaton and shares the nformaton wth the rest of the group. Durng the navgaton cycle, each robot collects data from ts proproceptve sensors to perform the predcton step of KF whle t shares nformaton from the exteroceptve sensors wth the rest of the team durng the update. In (Howard et al., 2002) a method based on a combnaton of maxmum lkelhood estmaton and numercal optmzaton was ntroduced. Ths method allows the error on the robot localzaton to be reduced by usng the nformaton comng from relatve observatons among the robots n the team. A dstrbuted multrobot localzaton strategy was ntroduced based on an Extended KF (EKF) used to fuse proproceptve and

3 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena exteroceptve sensor data. In (Sm & Dudek, 2004), the authors deal wth the automatc learnng of the spatal dstrbuton gven a set of mages offered by all the members of the group, thanks to a centralzed system whch takes care of all the nformaton. In (Roumelots & Reklets, 2004), the robots are equpped wth a compass whch ncreases the effcency of the flter. In (Panzer et al., 2006) robots dynamcally correct the estmaton autonomously evaluated when a second robot comes across and they exchange ther estmates. The algorthm combnes the robustness of an EKF wth ts nterlaced (IEKF) mplementaton (Setola & Vasca, 1999). Robots make use of a centralzed system (wreless communcaton) for the data exchange. In (Martnell, 2007) a new approach based on a seres of EKFs herarchcally dstrbuted s ntroduced. The team s broken down nto several groups and, for each group, an EKF estmates the locatons of all the members of the group n a local frame attached to one robot. However, ths robot s specfc and s consdered as the group leader. Other approaches for explorng the envronment are based on map constructon (Dudek & Mackenze, 1993), whch are also costly n computatonal terms. Most of these mplementatons buld maps ncrementally by teratng localzaton for each new readng of each sensor on a robot. Dfferent works have been mplemented recently n the mult-robot localzaton approach based on local communcaton. In (Fox et al., 2000) robots navgate n an envronment to whch they know the map and mprove ther global localzaton each tme they meet another robot exchangng ther estmates. In (Roumelots & Bekey, 2002) each robot collects sensor data regardng ts own moton and shares ths nformaton wth the rest of the team durng the update cycles. A sngle estmator, n the form of a KF, processes the avalable postonng nformaton from all the members of the team and produces a poston estmate for every one of them. In (Martnell, 2005) the authors extend the EKF approach presented n (Roumelots & Bekey, 2002) by consderng a general relatve observaton between two robots. In order to explot the nformaton contaned n any relatve observaton between two robots, the authors derve the EKF equatons to ntegrate a generc relatve observaton, based on the bearng, dstance and orentaton of another robot. In (Panzer et al., 2006), the authors mplement a collaboratve localzaton system n smulaton. When two robots are suffcently close to communcate they exchange nformaton about the robot pose and a fxed landmark. The data transmtted are used as vrtual sensors for the recevng robot. Whle beng successful n achevng ther goals, all these frameworks present several lmtatons: ) they have a hgh computatonal consumpton as a result of optmal flters and map constructon, ) some robots are not allowed to move whle others are trackng the dstance between them, thus representng a msuse of resources, ) robots must mantan vsual contact wth the rest of the group at all tmes, and v) n some cases the robots have to communcate wth a central devce to update or download maps, synchronze movements, or update postons. The collectve behavor, Socal Odometry, proposed n ths chapter avods the aforementoned problems n a collectve and self-organzed manner. 3 The robot: Hardware and moton 3.1 Robot hardware For the experments we have used the e-puck robot (see Fgure 1). E-pucks are modular, robust and non-expensve robots desgned by (Mondada et al., 2009) for research and educatonal purposes. They are small wheeled cylndrcal robots, 7 cm n dameter, equpped wth a varety of sensors, and whose moblty s ensured by a dfferental drve system. The e-puck robot uses two mnature stepper motors wth gear reducton. The motor has 20 steps per revoluton and the gear has a reducton of 50:1. A crcular rng s used as a tre for the wheel frcton. The dstance between the wheels s about 53 mm. The maxmum speed of the wheels s about 1,000 steps/s, whch corresponds to one wheel revoluton per second.

4 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry Fgure 1: E-pucks robots. The e-puck hardware and software are fully open source provdng low-level access to every electronc devce and offerng extenson possbltes 1. E-pucks are equpped wth 8 nfrared proxmty sensors, a 3D accelerometer, a rng of 8 LEDs and a CMOS camera. Extenson boards communcate wth the man board through an I2C, SPI or RS232 bus. Fnally, a Bluetooth communcaton s avalable for programmng the robot and communcatng data to a computer or to other Bluetooth devces. 3.2 Communcaton hardware We have equpped each robot wth a local communcaton board, the E-puck Range & Bearng board (Gutérrez et al., 2008a;?). The board allows robots to communcate locally, obtanng at the same tme both the range and the bearng of the emtter wthout the need of any centralzed control or any external reference. The board reles on nfrared communcatons wth frequency modulaton and s composed of two nterconnected modules for data and power measurement. The E-puck Range & Bearng board s controlled by ts own processor. Each board ncludes 12 sets of IR emsson/recepton modules (see Fgure 2). Each of these modules s equpped wth one nfrared emttng dode, one nfrared modulated recever and one nfrared photodode 2. The modules, as shown n Fgure 3, are nearly unformly dstrbuted on the permeter of the board; so, the dstance between them s approxmately 30. A Manchester code s mplemented to allow any data sent at a fxed dstance to be receved wth the same ntensty by the recever. The mplementaton of the Manchester code allows a maxmum data rate of 5 kbps. The range of communcaton can be software controlled from 0 cm to 80 cm. (a) (b) Fgure 2: (a) Top and (b) bottom vew of the range and bearng board. 1 Further detals on the robot platform can be found at 2 For an exhaustve descrpton of the board see

5 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena (a) (b) Fgure 3: (a) Emtters and (b) recevers dstrbuton around the permeter of the board. Fgure 4: Path made by the wheels of a dfferental drve robot durng a turn. 3.3 Robot moton Let us assume a robot performs a movement n accordance wth Fgure 4, whch can be expressed as the followng trgonometrc equatons: r k θ k = U l,k (1) (r k + b) θ k = U r,k (2) where b s the wheelbase of the robot, measured as the dstance between the two deal contact ponts between each wheel and the floor, r k s the left wheel turnng radus and θ k s the turnng angle at tme k. By subtractng Equaton 1 from Equaton 2, the turnng angle s obtaned: θ k = ( U r,k U l,k )/b (3) Addng Equaton 1 and Equaton 2 and substtutng Equaton 3, we obtan the lnear movement of the center pont c of the robot as: ρ k = ( U r,k + U l,k )/2 (4) To obtan the robot s movement equaton, let the locaton of a robot at tme k 1 be: x k 1 = [ x k 1 y k 1 θ k 1 ] T (5) where (x k 1,y k 1 ) are the Cartesan coordnates and θ k 1 s the orentaton wth respect to a global reference.

6 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry Fgure 5: The robot s dsplacement on a plane. In accordance wth Fgure 5, a rotaton θ k and a translaton ρ k move the robot to a new locaton x k. x k 1 + ρ k cos(α k ) x k = y k 1 + ρ k sn(α k ) α k + δ k (6) If we approxmate the arc movement of the mdpont of the robot for the chord ( ρ k ) as shown n Fgure 4 and Fgure 5, we observe that angle α k = π γ k + (θ k 1 π/2), where γ k = π/2 θ k /2. Therefore α k = θ k 1 + θ/2. Moreover, θ k = α k + δ k, where δ k = π/2 γ k. Hence, θ k = θ k 1 + θ k By substtutng these angles nto Equaton 6, we obtan the movement equaton of the robot as 3 : Let us represent Equaton 7 as x k = x k 1 + ρ k cos(θ k 1 + θ k /2) ρ k sn(θ k 1 + θ k /2) θ k (7) x k = f (x k 1,u k,v k ) (8) where x k 1 s the [ x k 1 y k 1 θ k 1 ] T state vector, uk s the [ U r,k U l,k ] T nput vector and vk denotes the system nose. v k N (0,Q) ndcates a whte nose wth a zero mean and covarance matrx Q, whch models the uncertantes of the odometry model. Lookng at Equaton 7 and assumng the system has no errors, the state vector s redefned as x k = f (x k 1,u k,0). For modellng the relablty of the localzaton measurement, we defne P as the covarance matrx of our measure: where P 0 = 0 and Q k s defned as: P k = A k P k 1 A T k +W k Q k W T k (9) 3 Notce that ths equaton approxmates the robot s movement and t s not exact. Nevertheless, we wll use t as f the robot s movement was calculated as an nfntesmal dsplacement.

7 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena Fgure 6: Non-systematc error propagaton when a robot s travellng n a straght lne. [ kr U Q k = r,k 0 0 k l U l,k where k r and k l are error constants representng the nondetermnstc errors of the nteracton of the floor and the rght and left wheel respectvely. Fnally, A k and W k are the Jacobans of f ( ) wth regard to L k 1 and u k, respectvely: A k = 1 b 1 0 ρ k sn(ζ k ) 0 1 ρ k cos(ζ k ) cos(ζ k) ρ k 2b sn(ζ 1 k) 2 cos(ζ k) + ρ k 2b sn(ζ k) W k = 1 2 sn(ζ k) + ρ k 2b cos(ζ 1 k) 2 sn(ζ k) ρ k 2b cos(ζ k) where ζ k = θ k 1 + θ k 2 and θ k and ρ k are defned by Equatons 3 and 4. Usng the covarance matrx, the poston estmaton can be represented as an ellpsod surroundng the robot poston when no errors are computed. Projectng ths ellpsod n the x y plane, we model each computed robot poston as a characterstc error ellpse, whch ndcates a regon of uncertanty for the actual poston (Smth & Cheeseman, 1987). Ths regon ncreases wth the dstance travelled (see Fgure 6), untl some absolute poston measurement resets t. ] 1 b (10) (11) 4 Socal Odometry 4.1 A foragng task We set up a foragng task whch serves as an example for the algorthm dervaton. Insde an arena we ntroduce two goal areas (.e. nest and prey) whch have to be located by the robots (See Fgure 7). Once the areas have been located, the robots must forage from nest to prey endlessly. Robots use dead-reckonng to estmate and reach the nest and prey locatons. When robot fnds the nest or the prey, t stores ts a pror estmated locaton nformaton (ts actual poston and orentaton) as x nest, x prey, and respectvely. Addtonally, the robot keeps track of the dstance travelled snce t left the nest or the prey denoted by p nest, and pprey, respectvely, whch represents the nverse of the a pror confdence level the robot has about ts estmated nformaton (see Fgure 8). At each tme step, robot checks f there s another robot to communcate wth. If there s not, t updates ts a posteror estmated goal locatons and confdence levels as:

8 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry Fgure 7: Arena for a smple foragng task. Fgure 8: Robots nformaton about the nest and prey areas and the nverse of ther confdence levels. x nest, j represents the a pror estmated nest poston of robot at tme k and xprey, represents the a pror estmated prey poston of robot j at tme k. The robots keep track of the dstance travelled as the nverse of the confdence levels (p nest, j and pprey, ). x nest, p nest, x prey, p prey, = x nest, = p nest, = x prey, = p prey, In the next step (k + 1), the a pror estmated goal locatons are updated wth the robot s movement n the tme step duraton ( x k+1), and the nverse of the a pror confdence levels are updated wth the dstance travelled ( dk+1 ) n the tme step duraton: x nest, k+1 k = xnest, + x k+1 p nest, k+1 k = pnest, + d k+1 x prey, k+1 k = xprey, + x k+1 p prey, k+1 k = pprey, + d k+1 Therefore, f there s no encounter between the robots, the confdence level contnues to decrease untl the robot arrves at the nest or prey or untl t gets lost. If two robots meet, they communcate and update ther estmates. In what follows we show all the dfferent goal locaton exchange optons, where goal represents ether the nest or prey: None of the two robots know the goal locatons: Robots do not exchange any nformaton. (12) (13)

9 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena One robot knows a goal locaton: Let us assume that robot s the one who has prevously vsted the goal. Its a pror estmated locaton s x goal, and the nverse of ts correspondng a pror confdence level s p goal,. Robot j replaces ts nformaton wth the one provded by robot : x goal, j p goal, j = x goal, = p goal, (14) Both robots know the goal locaton: In ths case, the two robots exchange ther nformaton as follows: and x goal, p goal, x goal, j p goal, j ( = SO = SO ( = SO = SO x goal,, xgoal, j, pgoal,, pgoal, j ( p goal,, pgoal, j x goal, j, xgoal,, pgoal, j, pgoal, ( p goal, j, pgoal, ) ) (15) ) ) (16) where SO s the update functon dscussed n Secton 4.2. We wll show that thanks to the SO functons, when two robots update ther nformaton, they end up wth the same estmated knowledge and confdence level: x goal, p goal, = x goal, j = p goal, j (17) 4.2 Socal Odometry flters In (Gutérrez et al., 2009) we defned the Socal Induced Kalman Flter (SIKF) as a flter nspred by the KF and the spectral norm of the covarance matrx. The SIKF predcton stage was denoted by: and the correcton phase denoted by: x = f ( ) x k 1 k 1,u k 1,0 (18) p = Ak P k 1 k 1 AT k +V k Q k 1 V T k (19) 2 p g k = p + p j x = ( 1 g ) ( ) k x + g k x j + x j k (20) (21) where g k represents a scalar value nduced from the KF gan. p = ( 1 g k) p (22)

10 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry Notce that n these equatons, we are makng use of scalar values for both the predcton and correcton stage, nspred by the spectral norm of the gan and covarance matrces. Moreover, we assume that a robot acts as a sensor for robot j and vce versa 4. Followng these prncples, two dfferent flters are presented: Socal Generalzed Induced Kalman Flter (SGIKF): It follows the SIKF equatons, where the confdence level depends on the dstance travelled. Socal Generalzed Induced Ferm Flter (SGIFF): A modfcaton of the SIKF where g k s modfed accordng to a sgmod functon and the confdence level depends on the dstance travelled. In both flters, when a robot encounters a neghbor, t transmts ts estmated locaton and confdence level. Hence, each robot has the opportunty to adopt the estmates of other robots present n ther neghborhood Socal Generalzed Induced Kalman Flter Snce the spectral norm of the covarance matrx P grows endlessly untl a communcaton s establshed or the robots arrve at one of the goals, we defne the nverse of the a pror confdence level (p ) of robot as the dstance travelled (dk ) snce the robot left a specfc area. Therefore the predcton stage for the nduced covarance matrx s defned as: p = d k (23) Ths mplementaton allows the robot not to calculate the covarance matrx at each tme step, and therefore to save computatonal tme Socal Generalzed Induced Ferm Flter In the SGIFF, the nverse of the a pror confdence level, represented by p, s also calculated as the dstance travelled snce the robot left a specfc area. To calculate g k, we adopt the so called parwse comparson rule (Traulsen et al., 2006; Santos et al., 2006; Traulsen et al., 2007) often adopted n evolutonary/socal dynamc studes, to code the socal learnng dynamcs, whch makes use of the Ferm dstrbuton (see also Fgure 9): g 1 k = 1 + e β( p ) where p = p p j and β measures the mportance of the relatve confdence levels n the decson makng. For low values of β, the decson makng proceeds by gnorng the confdence levels, whereas for hgh values of β we obtan a pure mtaton dynamcs commonly used n cultural evoluton (Hammersten, 2003) defned by a sharp step functon. In the frst case, the confdence level works as a small perturbaton to a smple average between the two estmates, whle n the latter, each robot s ready to completely gnore the estmate whch has a smaller relatve confdence level. Hence, we use a weghted average to obtan the new locaton x and confdence level p usng the Ferm functon: x = ( 1 g ) ( ) k x + g k x j + x j k 4 Notce that the KF propertes cannot be translated to the SIKF; however some propertes are emprcally tested n Secton 6. (24) (25)

11 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena Fgure 9: The Ferm functon whch allows the robots to decde between ther own estmate and the nformaton provded by the others. p = ( 1 g k) p + g k p j (26) A comparson chart about the SGIKF and SGIFF equatons s shown n Table 1. In the SGIFF, we observe the a posteror confdence level of robot adds a termnus based on the confdence level of robot j (g k p j ). Ths s because, followng a socal dynamcs approach, we want to offer a more ratonal than statstcal approach to the SGIFF. When two robots communcate, they mplctly agree on a mddle pont between both confdence levels. Therefore, when a communcaton s establshed, the robot wth a worst confdence level wll mprove t and the one wth the better confdence level wll reduce t. Table 1: Socal Odometry flters comparson. x SGIKF ) f ( x k 1 k 1,u k 1,0 SGIFF ) f ( x k 1 k 1,u k 1,0 p p k 1 k 1 + d k p k 1 k 1 + d k g k x p p +p j ( ) ( 1 g k x + g k x j + x j k 1 1+e β ( p p j ) ) (1 g k ) x + g k ( x j + x j k ) p ( 1 g k ) p ( 1 g k ) p + g k p j 4.3 Communcaton In our experments, robots do not share a global coordnates system, so they rely on ther communcaton axs to transform the nformaton transmtted by ther neghbor nto ther own frame. Ths nformaton can be

12 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry X axs (Robot s headng) AREA A ROBOT dy φ Y Axs γ α j Y Axs j λ γ ROBOT j α j φ dyj j Communcated drecton j X Axs (Robot s headng) Communcaton axs Fgure 10: Robots sharng nformaton about the estmated locaton of area A. Robot has prevously vsted area A and communcates ts estmates (dy and φ ) to robot j. locally transmtted thanks to the range and bearng nformaton provded by the E-puck Range & Bearng board. Fgure 10 shows an example of how nformaton about the estmated locaton of Area A, prevously vsted by robot, s transmtted from robot to robot j. In a frst step, robot transmts ts estmate of the dstance dy and drecton φ of area A to robot j. For the drecton, the value transmtted s the angle α, obtaned from φ usng the communcaton beam as reference axs: α = φ γ, where γ s the bearng provded by the E-puck Range & Bearng board. In a second step, robot j transforms the receved data nto ts own coordnates system. Frst, t calculates the drecton provded by robot as φ j = γ j + α π, followed by the calculaton of the locaton ( x j,ỹ j) of area A: where λ j s the range provded by the E-puck Range & Bearng board. x j = λ j cos ( γ j) + dy cos ( φ j) ỹ j = λ j sn ( γ j) + dy sn ( φ j) (27) 5 Control archtecture We have desgn the robots controllers based on a behavor-based archtecture. The behavor s mplemented followng a Subsumpton archtecture (Brooks, 1986). At the lowest level, each behavor s represented usng an augmented fnte state machne (AFSM). Each AFSM performs an acton and s responsble for ts world percepton. The robots are ntally located at random postons nsde a fxed area n the center of the arena. Insde ths area robots do not perceve the central place (nest) or the resource ste (prey) areas. Once a robot fnds the nest or the prey, t stores ts poston and contnues wth a random walk untl t fnds the other area. When both areas have been located, the robots try to go from one area to the other endlessly. Because of the movement errors, robots mght arrve at some coordnates that they wrongly estmate nsde the area. At ths pont a robot consders tself lost, t resets ts estmated locatons and starts carryng out a random walk untl t fnds both areas agan. If a robot correctly arrves at one of the areas, t stores the new poston coordnates. When two robots encounter they

13 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena exchange ther estmates about the goal areas. The nformaton s exchanged and updated followng the flters presented n Secton 4.2. Fgure 11: Depcton of an e-puck. S, [1,8] refer to the nfrared sensors used as IR proxmty sensors. G, [1,3] refer to the ground sensors. M l and M r are the left and rght motor respectvely. In the controllers, robots make use of the followng nputs and outputs: Clock: the system clock runs at 10 Hz. Some of the AFSMs need the clock nput to perform the odometry movement calculatons. Infrared Sensors (S ): 8 nfrared sensors are dstrbuted around the the robot s permeter (see Fgure 11). They are used to detect the presence of obstacles or neghbors wth whom the robots can communcate. Ground sensors (G ): 3 nfrared sensors are located n the lower-front part of the robot (see Fgure 11). Robots dfferentate the areas dependng on the color of the ground. The nest s represented as black, prey as grey and the rest of the arena as whte. RANDB Emtter and Recever: the emsson and recepton processed sgnals of the E-puck Range & Bearng board (see Fgure 3). Motors (M r,m l ): the two dfferental drve motors (see Fgure 11). Fgure 12 shows the archtecture dagram of the controller. Each layer corresponds to a robot behavor and arrows connectng the dfferent AFSMs, the suppressor and reset sgnals. The AFSMs are descrbed below: Avod: the state machne returns a vector takng nto account all the IR sensors above a certan threshold. The drecton sent to the motors s the opposte of ths vector. Forage: the robot has locaton nformaton about both the nest and the prey areas. It moves from nest to prey and back followng the shortest path. If the robot arrves at one of the two areas, detected by the ground sensors, t stores the new estmated poston and goes towards the other area. If the robot arrves at a place where the area was supposed to be but s not, t resets ts Goal Locatons memory. As a consequence, the robot enters the wander AFSM. Wander: the robot carres out a random walk. If a nest or prey area s found, the robot stores ts poston n ts Goal Locatons memory. Receve Data: the robot translates the nformaton receved nto ts own reference axs.

14 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry Fgure 12: The behavor structure of the foragng agents for a controller wth communcaton defned by four levels of competence (layers) and fve AFSMs. The suppressor operators are represented by crcles wth an S enclosed, whle the reset s represented by R. Send Data: the robot transforms the nformaton to communcate accordng to the common reference axs (communcaton axs), and sends t to ts neghbors. 6 Expermental evaluaton In the followng, we report expermental results wth smulated and real robots n whch the outcomes of four controllers usng communcaton strateges and one not usng communcaton are compared. 6.1 Experments n smulaton In our smulaton, an e-puck s modelled as a cylndrcal body of 3.5 cm n radus that holds 8 nfrared sensors dstrbuted around the body, 3 ground sensors on the bottom-front part of the body and a range and bearng communcaton sensor. A dfferental drve system composed of two wheels s fxed to the body of the smulated robot. For the three types of sensors, we have sampled real robot measurements and mapped the data nto the smulator. Furthermore, we added unformly dstrbuted nose to smulate the standard devaton of the dfferent sensors. ±20 % nose s added to the nfrared sensors and ±30 % to the ground sensors. In the range and bearng sensor, nose s added to the bearng (±20 ) and range (±2.5 cm) values. Moreover, each message emtted can be lost wth a probablty that vares lnearly from 1 % when the sender-recever dstance s less than 1 cm, to 50 % when the two robots are 15 cm from each other. Three dfferent expermental setups (ESs) have been chosen to study the convergence and comparson of the dfferent algorthms. All setups are carred out n a smlar arena (see Fgure 7) where the dmensons and number of robots are changed as detaled n Table 2. For each expermental setup we have tested fve dfferent communcaton strateges:

15 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena Table 2: Descrpton of the three smulated expermental setups for the sngle objectve localzaton experment. ES1 ES2 ES3 Area dmensons (m 2 ) 1.2 x x x 8.5 Intal area radus (m) Nest and prey radus (m) Number of robots Experment duraton (s) No communcaton (NC): Robots do not communcate. Sngle SGIKF (SSGIKF): Robots use the SGIKF to correct the nformaton receved related to the area they are gong to. Double SGIKF (DSGIKF): Robots use the SGIKF to correct the nformaton receved related to both areas. Sngle SGIFF (SSGIFF): Robots use the SGIFF to correct the nformaton receved related to the area they are gong to. Double SGIFF (DSGIFF): Robots use the SGIFF to correct the nformaton receved related to both areas. Each expermental setup has been repeated 30 tmes. The performance of the robots n the foragng task under study s measured as the number of total round trps completed from the nest to the prey and back durng the duraton of the experment Recrutment process We frst study the tme t takes for all the robots to locate the nest and prey areas. When robots do not communcate they are explorng the envronment untl they fnd both areas. Each robot uses the same algorthm, but they gnore useful nformaton avalable n ther neghbors (.e. other robots that have already found one or both areas). In any of the other four strateges, when robots are lookng for one of the areas and they fnd a neghbor, they stop and check f the neghbor s transmttng nformaton about the unknown area. In ths case, they take the neghbor s estmates as ther own and go towards the communcated estmate. Fgure 13 shows the tme t takes for all the robots to vst both areas at least once. We observe how the convergence on the path speeds up wth the communcaton strateges. A robot, whch has found one or both areas, s able to recrut other robots. All the communcaton strateges perform approxmately n the same way. The recrutment tme effcency speeds up four tmes on the communcaton strateges compared to the NC for the ES1 and the ES2. Moreover, note than n the ES3 not all the robots, wth a NC controller, are able to fnd the path wthn the 7,200 s run Retreval process We have also tested the benefts of the dfferent communcaton strateges n a retreval process. We defne the retreval process as the task n whch robots have to transport vrtual tems from the prey to the nest area. Each tme a robot completes a run from the nest to the prey and comes back to the nest, we consder the robot has succeeded n ts task and count one round trp. We have tested the dfferent communcaton strateges n the three

16 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry (a) (b) (c) Fgure 13: Recrutment results for the fve communcaton strateges for the (a) ES1 (b) ES2 and (c) ES3, n a sngle objectve scenaro n smulaton. (30 replcatons for each boxplot). Each box comprses observatons rangng from the frst to the thrd quartle. The medan s ndcated by a horzontal bar, dvdng the box nto the upper and lower part. The whskers extend to the farthest data ponts that are wthn 1.5 tmes the nterquartle range. Outlers are shown as crcles.

17 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena ESs. In any of the communcaton strateges, robots show a better performance than the NC. In the NC strategy, robots rely only on ther estmates and once a robot has lost the correct locaton of ts goal, t has to explore the envronment and fnd the areas by chance, whch explans the poor performance of ths strategy. Fgure 14 shows results for the two SGIKFs compared to the NC. We observe that the SGIKFs ncrease 2.5 tmes the performance of the retreval process wth respect the NC for the ES1. For the other two ESs, robots wthout communcaton are not able to perform more than 50 successful retrevals whle the SGIKFs acheve 1400 and 2000 retrevals n average for the ES2 and the ES3 respectvely. We observe that the sngle mplementaton of the flter, when robots only communcate ther estmate about the goal they are amng at, performs better than the double one. Ths s because when robots are movng from one area to the other, they have better nformaton on the goal they are comng from than the one they are amng at. When a robot encounters another neghbor (robot j), ths last one s typcally comng from the place robot s amng at. Therefore, robot mproves ts goal estmate wth the more relable nformaton of robot j (the estmate on where t s comng from). If robot j communcates nformaton about the goal t s amng at, as n the double flter mplementaton, t s dsruptng the nformaton from robot whch wll be propagated to the rest of the neghbors that robot encounters later. Fgure 15 shows the same experments for the SGIFFs wth dfferent β parameters. We have defned 8 values where β [10 5,100]. A maxmum retreval process s acheved when β = 10 2 for the ES1 and ES2, and β = 10 3 for the ES3. Fnally, note that the best βs perform better than n the SGIKFs, whle any of the other β perform worse than the SGIKF mplementatons Stablty In any of the communcaton strateges we observe not all of the robots are able to keep themselves on the path. When many robots try to enter the nest and prey areas at the very same tme or there s not enough robots on the path to communcate wth, the robots get lost. However, when robots get lost they are typcally around the nest or prey area. Therefore, a lost robot, wth a communcaton strategy, suddenly fnds another neghbor from whom t gets ts estmates and has a hgh probablty of fndng the path agan. Fgure 16 shows the average of the robots on the path for the three expermental setups. In all the communcaton strateges, % of the robots (dependng on the flter used) are always n the path n ts steady state. However, no more than 20 % of the total number of robots are able to stay on the path for the NC strategy. 6.2 Experments wth real robots Real experments are carred out n a 1.7x1.2 m 2 arena, durng 1800 s wth 10 e-pucks. Robots start n a central round area of 0.2 m radus n random postons and orentatons. Data collecton s managed through the Bluetooth connecton. Due to the Bluetooth lmtatons, two dfferent computers are used to communcate wth 5 dfferent robots each. Robots are ntalzed at the same tme thanks to a standard TV remote control. Addtonally, robots keep track of a tmer whch s ntalzed wth the remote control and allows the robots to have the same tme reference. When a robot arrves at one of the two goals (.e. nest or prey) or t gets lost, the robot sends a Bluetooth command to the computer, ndcatng the state n whch t fnds tself (.e. nest, prey or lost) and the tme at whch t has been produced. After 1800 s the controller stops and robots must be randomly ntalzed agan. The 8 IR sensors are used as nput to the avod and communcaton behavors. The nest and prey areas are detected wth the ground sensors. The communcaton range has been lmted to 15 cm to avod the IR sgnals to spread over the arena, as n the smulaton experments. We have tested the 5 dfferent communcaton strateges defned n Secton 6.1. For the SGIKFs, only the best β value obtaned n smulaton (.e. β = 10 2 ) has been programed. Fnally, each expermental setup was repeated 30 tmes to allow for statstcal comparsons. In Fgure 17 we observe the recrutment process acheved by the real robots. Compared to the smulaton

18 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry (a) (b) (c) Fgure 14: Retreval results comparson between the NC, SSGIKF and DSGIKF for the (a) ES1 (b) ES2 and (c) ES3, n a sngle objectve scenaro n smulaton. (30 replcatons for each boxplot).

19 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena (a) (b) (c) Fgure 15: Retreval results for dfferent β values of the SGIFFs for the (a) ES1 (b) ES2 and (c) ES3, n a sngle objectve scenaro n smulaton. (30 replcatons for each boxplot).

20 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry (a) (b) (c) Fgure 16: Stablty results comparson for the fve communcaton strateges for the (a) ES1 (b) ES2 and (c) ES3, n a sngle objectve scenaro n smulaton.

21 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena results we notce a smlar behavor and the same mprovement n average. However, the standard devaton grows because of communcaton mperfectons dscussed later. Fgure 17: Recrutment results for 10 real e-pucks n a 1.7x1.2 m 2 arena. Fgure 18 shows the retreval process comparson between the 5 dfferent strateges. We observe a small reducton n the number of round trps. In average 120 round trps for the SGIKFs compared to the 150 obtaned n smulaton and 140 nstead of 170 for the SGIFFs. Therefore there s only a dfference of 20% between real and smulated experments. However, robots wth any of the communcaton strateges are able to carry out n average twce more retrevals than wth the NC strategy. Fgure 18: Retreval results for 10 real e-pucks n a 1.7x1.2 m 2 arena. Fnally, Fgure 19 shows the stablty of the robots on the path. A larger oscllaton s apprecated compared to the smulated experments. We observe, for the communcaton strateges, an average of 5 robots n the path compared to the 6 robots obtaned n smulaton. The dfferences observed between the real and smulated experments are manly due to the workng of the range and bearng sensor. These dfferences arse from the mperfecton of the communcaton model mplemented n smulaton. Another problem s the reflecton obtaned from the borders of the arena, whch dstort some of the bearng measures when robots are near those borders. Moreover, we have observed some nterferences between the range and bearng sensor and the IR proxmty sensors. These alteratons n the

22 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry Fgure 19: Stablty results for 10 real e-pucks n a 1.7x1.2 m 2 arena. range and bearng produce extra errors not modelled n the smulaton, whch make the robots mscalculate the nformaton gven by other neghbors. However, we observe the same behavor n all the qualtatve measures and only a decrease of % n the quanttatve ones. Moreover, the tme used n the communcaton s longer n the real robots scenaro. Ths s because errors n the communcaton makes the communcaton layer repeat some messages. Therefore, robots wat a lttle more tme (around 20-30%) n the communcaton process, whch explans the reducton of the number of vrtual tems retreved n the experments wth real robots. 7 Conclusons In ths chapter we have descrbed a socal localzaton strategy n whch robots use parwse local communcaton to share knowledge about specfc locatons to mprove ther performance n a foragng task. By lettng the robots use the estmates of others, we engneer an effcent and decentralzed knowledge sharng mechansm whch allows the robots to acheve ther goals, both from an ndvdual and group perspectve. Ths smple mechansm drves the system to a successful collectve pattern mprovng the ndvduals behavor. We have also compared two dfferent Socal Odometry strateges: the Socal Generalzed Induced Kalman Flter, nspred on the Kalman Flter and the Socal Generalzed Induced Ferm Flter as an mtaton based dynamc algorthm. Our experments have demonstrated that complex behavors can result from smple local nteractons based on smple behavor-based controllers such as the Subsumpton archtecture. A smple foragng task has been created as test-bed for the algorthm test. Local communcaton allows the robots to mprove drastcally the group performance and shows t to be stable. Addtonally, we observe that the algorthms mplement an mplct recrutment process that speeds up the ntal exploraton phase mandatory to acheve the foragng task. Fnally, the performance of the Socal Odometry flters allows an optmstc forecast concernng the use of onlne self-organzed methodologes n the feld of swarm robotcs.

23 Álvaro Gutérrez, Félx Monastero-Hueln, Alexandre Campo & Lus Magdalena References Brooks, R. A. (1986). A robust layered control system for a moble robot. IEEE Jorunal of Robotcs and Automaton, 2(1), Burgard, W., Derr, A., Fox, D., & Cremers, A. B. (1998). Integratng gglobal poston estmaton and poston trackng for moble robots: The dynamc markov localzaton aapproach. In Proceedngs of the IEEE Internatonal Conference on Intellgent Robots and Systems (pp. 1 6). Pscataway, NJ: IEEE Press. Burgard, W., Fox, D., Henng, D., & Schmdt, T. (1996). Estmatng the absolute poston of a moble robot usng poston probablty grds. In Proceedngs of the Thrteenth Natonal Conference on Artfcal Intellgence (pp ). Cambrdge, MA: AAAI Press/MIT Press. Cassandra, A. R., Kaelblng, L. P., & Kuren, J. A. (1996). Actng under uncertanty: Dscrete bayesan models for moble-robot navgaton. In Proceedngs of the IEEE Internatonal Conference on Intellgent Robots and Systems (pp ). Pscataway, NJ: IEEE Press. Chong, K. & Kleeman, L. (1997). Accurate odometry and error modellng for a moble robot. In Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton (pp ). Pscataway, NJ: IEEE Press. Clancey, W. J. (1997). Stuated Cognton: On Human Knowledge and Computer Representatons. Cambrdge Unversty Presss, Cambrdge, UK. Dorgo, M. & Şahn, E. (2004). Guest edtoral. Specal ssue: Swarm robotcs. Autonomous Robots, 17(2 3), Dudek, G. & Mackenze, P. (1993). Model-based map constructon for robot localzaton. In Proceedngs of Vson Interface (pp ). Toronto, ON: CIPPR Socety Press. Fox, D., Gurgard, W., Kruppa, H., & Thrun, S. (2000). Autonomous Robots, 8(3), Gutérrez, A. (2009). Socal Odometry: Dstrbuted Locaton Knowledge for Swarm Robotcs Based on Local Communcaton. PhD thess, Escuela Técnca Superor de Ingeneros de Telecomuncacón - Unversdad Poltécnca de Madrd. Gutérrez, A., Campo, A., Dorgo, M., Amor, D., Magdalena, L., & Monastero-Hueln, F. (2008a). An open localzaton and local communcaton emboded sensor. Sensors, 8(11), Gutérrez, A., Campo, A., Dorgo, M., Monastero-Hueln, F., & Donate, J. (2009). Open e-puck range and bearng mnaturzed board for local communcaton n swarm robotcs. In Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton (pp. n press). Pscataway, NJ: IEEE Press. Gutérrez, A., Campo, A., Santos, F. C., Pncrol, C., & Dorgo, M. (2008b). Socal odometry n populatons of autonomous robots. In M. Dorgo, M. Brattar, C. Blum, M. Clerc, T. Stützle, & A. F. Wnfeld (Eds.), Ant Colony Optmzaton and Swarm Intellgence, 6th Internatonal Conference, ANTS 2008, Brussels, Belgum, September 2008, Proceedngs, volume LNCS 5217 of Lecture Notes n Computer Scence (pp ). Berln, Germany: Sprnger-Verlag. Gutmann, J., Wegel, T., & Nebel, B. (2001). A fast, accurate, and robust method for self-localzaton n polygonal envronments usng laser-rangefnders. Advanced Robotcs Journal, 14(1), Gutmann, J.-S., Wegel, T., & Nebel, B. (1999). Fast, accurate and robust self-localzaton n polygonal envronments. In Proceedngs of the IEEE Internatonal Conference on Intellgent Robots and Systems (pp ). Pscataway, NJ: IEEE Press. Hammersten, P. (2003). Genetc and Cultural Evoluton of Cooperaton. Cambrdge, MA: MIT Press. Howard, A., Matarć, M., & Sukhatme, G. (2002). Localzaton for moble robot teams usng maxmum lkelhood estmaton. In Proceedngs of the IEEE Internatonal Conference on Intellgent Robots and Systems (pp ). Pscataway, NJ: IEEE Press. Kalman, R. E. (1960). A new approach to lnear flterng and predcton problems. Transactons of the ASME Journal of Basc Engneerng, 82(Seres D),

24 Self-Organzed Dstrbuted Localzaton Based on Socal Odometry Larsen, T., Bak, M., Andersen, N., & Ravn, O. (1998). Locaton estmaton for autonomously guded vehcle usng an augmented Kalman flter to autocalbrate the odometry. In FUSION 98 Spe Conference (pp ). Las Vegas, NV: CSREA Press. Martnell, A. (2005). Mult-robot localzaton usng relatve observatons. In Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton (pp ). Pscataway, NJ: IEEE Press. Martnell, A. (2007). Improvng the precson on mult robot localzaton by usng a seres of flters herarchcally dstrbuted. In Proceedngs of the IEEE Internatonal Conference on Intellgent Robots and Systems (pp ). Pscataway, NJ: IEEE Press. Martnell, A. & Segwart, R. (2003). Estmatng the odometry error of a moble robot durng navgaton. In Proceedngs of the 1st European Conference on Moble Robots (pp ). Warszawa, Poland: Zturek Research Scentfc Inst. Press. Mondada, F., Bonan, M., Raemy, X., Pugh, J., Canc, C., Klaptocz, A., Magnenat, S., Zufferey, J. C., Floreano, D., & Martnol, A. (2009). The e-puck, a robot desgned for educaton n engneerng. In Proceedngs of the 9th Conference on Autonomous Robot Systems and Compettons (pp ). Castelo Branco, Portugal: IPCB-Insttuto Poltécnco de Castelo Branco. Panzer, S., Pascucc, F., & Setola, R. (2006). Multrobot localsaton usng nterlaced extended kalman flter. In Proceedngs of the IEEE Internatonal Conference on Intellgent Robots and Systems (pp ). Pscataway, NJ: IEEE Press. Roumelots, S. & Bekey, G. (2002). Dstrbuted multrobot localzaton. IEEE Transactons on Robotcs and Automaton, 18(5), Roumelots, S. & Reklets, I. (2004). Propagaton of uncertanty n cooperatve multrobot localzaton: Analyss and expermental results. Autonomous Robots, 17(1), Santos, F. C., Pacheco, J. M., & Lenaerts, T. (2006). Cooperaton prevals when ndvduals adjust ther socal tes. PLoS Computatonal Bology, 2(10), e140. Setola, L. G. R. & Vasca, F. (1999). An nterlaced extended kalman flter. IEEE Transactons on Automatc Control, 44(8), Sm, R. & Dudek, G. (2004). Self-organzng vsual maps. In Proceedngs of the Natonal Conference on Artfcal Intellgence (pp ). Cambrdge, MA: MIT Press. Smmons, R. & Koeng, S. (1995). Probablstc robot navgaton n partally observable envronments. In Proceedngs of the Internatonal Jont Conference on Artfcal Intellgence (pp ). San Mateo, CA: Morgan Kaufmann. Smth, R. C. & Cheeseman, P. (1987). On the representaton and estmaton of spatal uncertanly. Internatonal Journal of Robotcs Research, 5(4), Støy, K. (2001). Usng stuated communcaton n dstrbuted autonomous moble robots. In Proc. of the 7 th Scandnavan Conf. on artfcal ntellgence (pp ). Amsterdam, NL: IOS Press. Thrun, S., Burgard, W., & Fox, D. (2000). A real-tme algorthm for moble robot mappng wth applcatons to mult-robot and 3D mappng. In Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton (pp ). Pscataway, NJ: IEEE Press. Traulsen, A., Nowak, M., & Pacheco, J. (2006). Stochastc dynamcs of nvason and fxaton. Physcal Revew - Sere E, 74(1), Traulsen, A., Pacheco, J., & Nowak, M. (2007). Parwse comparson and selecton temperature n evolutonary game dynamcs. Journal of Theoretcal Bology, 246(3), Tubashat, A. & Madra, S. (2003). Sensor networks: An overvew. IEEE Potentals, 22(2), Wang, C. M. (1988). Locaton estmaton and uncertanty analyss for moble robots. Autonomous Robot Vehcles, 1(1),