An Efficient Hierarchical Localization for Indoor Mobile Robot with Wireless Sensor and Pre-Constructed Map

Transcription

1 The 5h Inernaional Conference on Ubiquious Robos and Ambien Inelligence (URAI 2008) An Efficien Hierarchical Localizaion for Indoor Mobile Robo wih Wireless Sensor and Pre-Consruced Map Chi-Pang Lam 1 Wei-Jen Kuo 2 Chun-Feng Liao 2 Ya-Wen Jong 2 Li-Chen Fu 1,2 Joyce Yen Feng 3 r @nu.edu.w r @nu.edu.w cfliao@ieee.org jessi3py@gmail.com lichen@nu.edu.w fengyen@nu.edu.w 1 Deparmen of Elecrical Engineering, Naional Taiwan Universiy, Taipei, Taiwan 2 Deparmen of Compuer Science and Informaion Engineering, NTU, Taipei, Taiwan 3 Deparmen of Social Work, NTU, Taipei, Taiwan Absrac - This paper proposes a feasible localizaion mehod for robo in indoor environmen wih wireless device. Localizaion and navigaion have been imporan issues in mobile robo researches. Paricle filer based localizaion is wide used in recen researches. Bu no only paricle based localizaion mehods suffer compuaion overhead, paricle depleion, and he long ime duraion needed o iniialize robo posiion, i also relies heavily on moion model. To deal wih hese problems, we combine he g wireless senor nework and a pre-consruced map o provide an indoor robo localizaion sysem in hierarchical manner. We apply his sysem o a home service robo, running in an environmen wih common family furnishing, o show our hierarchical sysem ha is feasible and efficien, and also solves he above problems of paricle filer. Keywords - Robo localizaion, robo navigaion, hierarchical localizaion, paricle filer I. Inroducion Home environmen is one of he mos common environmens ha we always siuae. If robos can localize hemselves a home and smoohly navigae hemselves o everywhere in he house, i will be an imporan symbol of achieving he goal of ubiquious robos service in our daily life. To fulfill his arge, he robo should rely on he cooperaion of muliple sensors. Various approaches have been inroduced o solve such problem. In oudoor environmens, GPS-based localizaion is popular for such applicaion; in indoor environmen, exising localizaion echniques for mobile robos can be classified ino wo caegories: relaive localizaion, e.g. wih he help of odomery and inerial navigaion, and absolue localizaion, e.g. uilizing acive beacon, landmark and environmen map [17]. The goal of our research is o esablish a feasible and efficien echnique such ha a robo can move smoohly from one specific place o anoher place in an indoor environmen. We inend o ake he advanage of he exising wireless senor nework based localizaion echnique and o combine i wih he map-based localizaion mehod o accomplish his goal. We also aim o realize his navigaion sraegy on a home service robo, Julia, as shown in Fig. 1, made by a research eam Fig. 1 Julia in an indoor environmen from Advanced Conrol Lab. of EE Deparmen and Inelligen Roboics Lab. of CSIE Deparmen boh a Naional Taiwan Universiy. Much research has been done on localizaion of mobile robos. Localizaion is a ask composed of wo subasks, i.e., global posiion esimaion and local posiion racking. Global posiion esimaion akes place when he robo is given a previously consruced map and he informaion only abou where he robo is somewhere on he map. Once he posiion of he robo is known, local posiion racking akes over by keeping rack of he posiion over ime. The simples and mos direc approach o localizaion problem is dead reckoning, which depends only on odomery daa. However, wih dead reckoning, he pose error will be accumulaed and hence his mehod can provide a reliable localizaion. Anoher approach is o use riangulaion echnique, which is a well-known echnique for finding a posiion by means of bearings from fixed poins in he environmen [14]. When sensors are unreliable, his mehod remains several problems [1]. Kalman filer is also applied o deal wih localizaion problem, and is resul is opimal when some assumpions hold, such as linear model and Gaussian noise disribuion [8]. A bulk approach is using Bayesian inference as he basic elemen of he localizaion algorihm, namely, Markov model [9]; and paricle filer [2] algorihm, such

2 The 5h Inernaional Conference on Ubiquious Robos and Ambien Inelligence (URAI 2008) as Mone Carlo localizaion [12]. Bu no only paricle based localizaion mehods suffer compuaion overhead, paricle depleion, and he long ime duraion needed o iniialize robo posiion, i also relies heavily on moion model. To deal wih he deficiency of paricle filer, we can ake advanage of wireless sensor o obain a rough posiion and hen adop a probabilisic approach o compensae ha deficiency. In his paper, we propose such philosophy o help esablish a more reliable and efficien localizaion and navigaion resul. We consruc he map of he environmen wih laser ranger finder in advance. This map is represened by an occupancy grid map. When he robo sars o localize iself, i firs obains he knowledge abou which room i locaes by use of he wireless sensor, named Ekahau Posiion Engine [10], which is an indoor room-level racking sysem. The posiioning engine works in real-ime and combines signal srengh wih sie calibraion o display posiions of persons or asses on a map of he space. The engine can in fac show he posiion of any device conneced o he Wi-Fi nework, such as lapops, PDAs or Wi-Fi ags. By Ekahau he robo will know is rough posiion. Afer ha, he robo begins o compare he curren laser daa colleced wih our pre-consruced map, saring from he surrounding posiion ha Ekahau provides, and hen provides a pose predicion. We regard his pose predicion as an indoor GPS measuremen and fuse i wih odomery daa by Exended Kalman Filer o indenify he accurae locaion of he robo. We implemen his sysem o a home service robo, running in an environmen wih common family furnishing, o show his hierarchical manner is feasible and efficien, and also solves he major deficiencies of paricle filer menioned above. This paper consiss of as follows. In secion II, he mapping algorihm for our pre-consruced map is provided. Our hierarchical localizaion manner is inroduced in secion III and experimenal resuls are shown in secion IV. II. Mapping wih Laser range finder Recenly, laser range finders are moving ou of he laboraory, and has become he main equipmen for indoor localizaion[3][4][5]. Julia also has a horizonally fixed URG-04LX laser mouned on he fron. Sweeping 33cm above ground level wih a broad view of 240 degrees, Julia is able o deec obsacles or moving objecs wihin 5.6 meers of range more efficienly, due o fewer blind spos. Mapping is a cenral ask for auonomous mobile robo s navigaion and self-localizaion. In order o achieve minimum error rae during real ime localizaion, a variey of grid sizes are esed on our sysem. The resul shows ha, for our environmen, using 9cm grid size gives he bes performance and accuracy. This value may vary among differen kinds of environmen and sensor ype. In our approach, mapping is done off-line wih scan maching algorihm. Sae of he ar scan maching mehods are algorihms ha compare wo scans direcly, or using special feaures, such as hisogram, exraced lines, and landmarks. In his paper, insead of applying he commonly used ieraive closes poin (ICP) algorihm, we use direc comparison of scans which akes less compuaion ime and is also easier o implemen. Moreover, he direc scan maching mehod oupus he exac locaion of he laser scanner on a gridmap, raher han he ransformaion beween wo raw scans [6]. Wih he robo wandering around and a human monioring his procedure, i is urned off manually when finished. Normally, we should have odomeer readings as one of he inpu measuremens, bu since he robo wanders a a very low speed, 0.2 m/sec, a simpler mehod presened below can be applied: Algorihm mapping( x 1, z ) /*his sep samples M possible posiions of he robo, a weigh is aached o each posiion according o he measuremen of laser scanner*/ for m = 1 o M do [ m] [ m] sample x ~ p( x x 1 ) [ m] [ m] w = p( x z, x ) 1 endfor /*search for he posiion wih larges weigh*/ for m = 1 o M do [ m] [ m] find x wih he greaes w endfor [ m] reurn x The inpu of his algorihm is he previously esimaed pose x 1 and he mos recen measuremen z. Similar o he paricle filer [7], M paricles of possible robo posiion are sampled, and he paricle wih greaes weigh [ m ] w is seleced as he curren pose x ˆ. The resuling pose w, x ˆ is hen used o generae a new map according o (1): 1 M = M { xˆ, z } (1) 1 M is he map consruced so far from measuremens z o z a corresponding poses x o x 1 1. The mapping 1 procedure will hen generae a map M which will laer be used for localizaion. III. Hierarchy Localizaion A. Probabilisic-based localizaion When he localizaion process is firs sared up, he robo has no idea where i is, his kind of siuaion is known as he kidnapped robo problem. Insead of acquiring he robo s locaion in one sep, a hierarchical approach is used. Firs of all, he Ekahau sysem provides a room level localizaion, and hen an exhausive search wihin he room using grid level localizaion gives us an

3 The 5h Inernaional Conference on Ubiquious Robos and Ambien Inelligence (URAI 2008) accurae posiion of he robo. This hierarchical procedure is shown as Fig.2. In our work, we aach a Wifi Tag (T301) o he robo s head o obain is posiion. Afer appropriae calibraion, he racking accuracy of Ekahau can reach o 1 meer. Here we use Ekahau o achieve room level racking as he accuracy varies largely depending on he nework and oher environmen facors. Since room level localizaion gives rough posiion of he robo, insead of exhausive search of he whole environmen, we search only inside ha cerain room for he mos reasonable iniial posiion of he robo. No only can room level localizaion speed up he iniializaion process, i also correcs grid level localizaion when he robo is likely o experience serious posiioning errors. Grid level localizaion can be accomplished by using he gridmap generaed during he mapping sage. In order o achieve more efficien localizaion, he gridmap will no be modified during localizaion phase. The equaion below describes how localizaion is done: 1 xˆ = arg max{ Pz ( x, M ) Px ( xˆ, u)} (2) 1 x 1 In equaion (2), Pz ( x, M ) is he measuremen model, which sands for he probabiliy ha measuremen z is observed, given pose x and he map 1 M consruced so far; P( x xˆ 1, u) represens he moion model, which is he probabiliy ha he robo is a x given previous pose xˆ and conrol u 1. In order o obain he measuremen model, we apply equaion (3): 682 M x i y Pz i 1 i 1 ( x, M ) = [ ][ ] = (3) 682 Since a single scan of he laser scanner provides 682 range daa, each range value is ransformed from polar coordinae o Caresian coordinaes on he gridmap according o he pose x. Here, we call each poin ( x i, y ) i an end-poin of a specific laser beam. Individual cells of he gridmap conain one of he following hree values: 0 : free space 0.5 : unknown (no ye explored) 1: occupied The erm 682 i= 1 M [ x ][ y ] oals all gridmap values of he i i end poins. The resul is hen divided by he oal number of daa, i.e. 682, o acquire he probabiliy of he measuremen model. In order o compue he curren locaion of he robo x ˆ, here is sill one more ask we need o solve, ha is, he moion model of he robo. More specifically, we are required o obain he disribuion of possible pose from conrol inpu u [7], which is comprised of ranslaional velociy v and roaional velociy w, and boh can be Ekahau room level global localizaion Robo pose Fig. 2 Flow char of hierarchical localizaion acquired by commands given o he wheel encoder as in following algorihm: Algorihm sample_moion_model( u, x 1 ) ˆv = v + sample_gaussian( α v 1 + α w 2 ) ŵ = w + sample_gaussian ( α v 3 + α w 4 ) ˆ γ = sample_gaussian ( α v 5 + α w 6 ) x = x sinθ + sin( θ + Δ ) y = y + cosθ cos( θ + Δ ) θ = θ + ŵδ+δ ˆ γ reurn x = ( x, y, θ ) T In his algorihm, α 1 hrough α6 are parameers of moion noise sampled by Gaussian disribuion. The funcion sample_gaussian(s) generaes a random sample wih zero mean and sandard deviaion s. We also perurb he final orienaion by adding a random erm ˆ γ. On accoun of discreizaion of he gridmap, a sampling version of moion model will be applicable. For each ieraion of he localizaion algorihm, 500 paricles are sampled. Is purpose is o sample all possible poses by adding Gaussian noise o conrol inpuu. Therefore, o maximize (2), he sampled pose x a which he measuremen model gives a maximum probabiliy will be reurned as x ˆ. B. Sensor fusion Grid level localizaion EKF sensor fusion In order o manage he noise effec, we use exended Kalman filer (EKF) o fuse he daa, including odomery, gyro, and he posiion measured from probabilisic-based localizaion ha aced as a virual GPS pose. Odomery is used in predicion sep, and gyro and posiion measured from probabilisic-based localizaion are used in updae sep. EKF provides a good pose predicion and noise variance racking. The process is described below:

4 The 5h Inernaional Conference on Ubiquious Robos and Ambien Inelligence (URAI 2008) Predicion sep: T cos θ kk s 0 uk sk+ 1 k = skk + T cos θ kk 0 + wk s ωk 0 1 (4) T Pk+ 1 k = AkPk kak + Qk (5) A = 1 0 Tu sinθ s k k k 0 1 T u cosθ k s k k k 0 0 T s (6) a b where s = [ x y θ] T is he posiion and orienaion sae of robo. T s is he sampling ime of our localizaion sep. uk and ωk refer o he velociy and angular velociy respecively. wk ~ (0, Qk) is he process noise. P k + 1 k is a prior esimae of covariance. We use he pose from probabilisic-based localizaion as our measuremen z k : xl zk = y l + vk (7) θ l where vk ~ (0, Rk) represens he measuremen noise. We updae our predicion by he following equaion, where K k is he Kalman gain, P is he error k + 1 k + 1 covariance marix a ime k+1. Updae equaion: 1 K = P ( P + R ) (8) k k+ 1 k k+ 1 k k s = s + K ( z x ) (9) k+ 1 k+ 1 k k k k k+ 1 k P = ( I K ) P (10) k+ 1 k+ 1 k k+ 1 k IV. Experimenal Resuls Experimens are conduced o show he feasibiliy of he proposed hierarchical localizaion and navigaion sraegy. Figure 2(a) shows our experimen environmen, which consiss of wo bedrooms, one living room and one dining room. For he high level represenaion of indoor environmen, a pre-consruced opological map is used. All rooms and he main passable ways in he house are se as opological poins; moreover, in order o allow robo o ener he room much smooher, opological poins are se in he fron and rear of each door ha connecs wo rooms. The red poins in Fig. 2(a) are he opological poins we se and he green poins in Fig. 2(a) show he locaion of four Wi-Fi access poins (one for each room) for Ekahau Posiion Engine. And Fig. 2(b) is our pre-consruced map of he same environmen. We came up wih using a gridmap of 9cm per grid. The whie color grids in Fig. 2(b) represen free area, gray color grids are undiscovered area c Fig. 2 (a) The srucure of our experimen environmen. (b) The pre-consruced map by laser range finder. (c) Overlap of (a) and (b) Fig. 3 Ekahau on he robo head and black grids mean occupied area. Figure 2(c) is he overlap of Fig. 2(a) and Fig. 2(b). From Fig. 2(c) we can find ha here are some inexaciudes in our pre-consruced map. Several undiscovered area is inside he house because furniure is siuaed in hose area; free area ouside he house because laser peneraes hrough he window. Though our pre-consruced map is no perfec, he localizaion algorihm allows cerain oleraion of inaccuracies because i aims o find he mos possible predicion of robo posiion. The wireless device - Ekahau, shown in Fig. 3 - is mouned on he head of Julia. We use AciveMQ [11], an open source embeddable MOM (Message Oriened Middleware) o implemen our sysem middleware. AciveMQ uses a cross-plaform messaging proocol, and suppors several programming languages such as C, C++, C#, and Java. To connec our applicaion wih AciveMQ, we adop he CMS (C++ Messagine Service) [13], which is a subprojec of AciveMQ. CMS is

5 The 5h Inernaional Conference on Ubiquious Robos and Ambien Inelligence (URAI 2008) an API for C++ for inerfacing wih Message Brokers such as Apache AciveMQ. Figure 5, Fig. 6 and Fig. 7 show he rajecory of Julia, running from living room o second bedroom, from second bed room o firs bedroom and from firs bedroom o dining room, respecively. In he firs place, robo obains is own room locaion from wireless device, and hen i chooses he neares opological poin as he sar poin and he opological poin of our goal room as he end poin. Afer ha, a shores pah algorihm is applied o plan a pah according o our pre-consruced map for robo o go. Because here are always people walking in he house or obsacles on he floor, we mus modify our pah reacively. To deal wih such problem, nearness diagram navigaion [15] is used as our collision avoidance algorihm because his algorihm has been a good implemenaion for robo navigaion in roublesome, cluered and complex scenarios, which home environmen is always saisfied. From our experimens, we see ha he localizaion resul can be affeced by available laser daa. If he robo is siuaed in a relaively spacious space, available daa will become fewer, so some drif happens o he localizaion resul. However, hrough sensor fusion, we can reduce such deficiency so ha he localizaion resul afer EKF is much smooher. Our navigaion sysem remains good racking of he robo pose. Figure 5 and Fig 7 also show he siuaion ha an obsacle is laid in he original pah. Navigaion sysem sars o change he robo pah reacively no only o avoid he obsacle bu also o reach he goal raher han o follow he original reference pah, which is blocked by an obsacle. Taking a close look a Fig. 6, when he robo isn avoiding any obsacle, upon comparing he reference pah wih he rajecory of grid level localizaion, we discover ha he precision of local localizaion is wihin 18cm due o using 9cm grid size. Anoher experimen is done o conduc he efficiency of localizaion when he robo does no iniially know where i is or loses is posiion due o maching failure. Through our hierarchical localizaion manner, when employing Ekahau, he robo can iniially obain which room i locaes, and hen searches for is posiion in his room locally insead of searching he whole environmen. The larges room, living room, in our environmen is abou 5.7m x 2m, so our searching area should cover a leas he whole room. Table 1 is he comparison of using hierarchical localizaion manner - idenifying he room ha he robo locaes from Ekahau hen is precise locaion by searching a mach beween he room in pre-consruced map and he curren laser daa, shown in Fig.4a - and global maching manner, idenifying he robo posiion only by searching a mach beween he whole pre-consruced map and he curren laser daa, shown in Fig.4b. We go a reliable localizaion resul using hierarchical manner, wih which sandard deviaion of our Gaussian sample is much smaller han global searching. The number of paricles ha are used o ge a reliable localizaion resul is 15000, which is fewer han 70000, he number of paricles used in global manner. Since using fewer paricles, hierarchical manner saves ime o localize he robo. Alhough Mone Carlo localizaion also solves he global localizaion problem, i iniially requires he robo o wander around wihou knowing i posiion unil mos of he paricles converge o a single poin. Bu in our approach, he precise posiion can be locaed on he firs sep, due o large number of paricles, ha is, an exhausive search in a cerain room for he opimal locaion. a b Fig.4 (a) Example of hierarchical localizaion manner. If we know he room ha he robo locaes firs, we can only searching for is posiion from ha room. (b) Example of global maching manner. We should search he whole environmen o idenify he robo posiion. Table 1 Comparison of room level localizaion and global localizaion. Hierarchical Global Maching Number of paricles Time consumpion 4.57 sec 13.7 sec Fig. 5 Moion rajecory from living room o 2 nd bedroom Fig. 6 Moion rajecory from 1 s room o dining room

6 The 5h Inernaional Conference on Ubiquious Robos and Ambien Inelligence (URAI 2008) Fig. 7 Moion rajecory from dining room o living room VI. Conclusion Since using radiional paricle filer may encouner paricle depleion and large paricle numbers, we propose a hierarchical scheme o deal wih hese deficiencies. We combine he wireless localizaion device wih probabilisic-based localizaion o ac as an indoor GPS measuremen, fused wih odomery daa o improve he efficiency and reliabiliy of indoor localizaion. The main conribuion of our hierarchical sraegy is o speed up he iniializaion and recovery process because we only need o search cerain area of he pre-consruced map raher han he whole environmen for maching he srucure of curren laser daa. We successfully implemen his algorihm o a home service robo, Julia, which successfully navigaes o desire room and posiion wih collision free movemen. Acknowledgemens This work was suppored by he Naional Science Council of he Republic of China under Gran NSC E [5] A. Diosi, G. Taylor, and L. Kleeman, "Ineracive SLAM using Laser and Advanced Sonar," in Roboics and Auomaion, ICRA Proceedings of he 2005 IEEE Inernaional Conference on, 2005, pp [6] V. Trung-Dung, O. Aycard, and N. Appenrod, "Online Localizaion and Mapping wih Moving Objec Tracking in Dynamic Oudoor Environmens," in Inelligen Vehicles Symposium, 2007 IEEE, 2007, pp [7] Sebasian Thrun, Wolfram Burgard and Dieer Fox, "Probabilisic Roboics" [8] J. J. Leonard and H. Durran-Whye, Mobile robo localizaion by racking geomeric beacons, IEEE Trans. Robo. Auoma., vol. 7, pp , June [9] D. Fox, W. Burgard, and S. Thrun, Markov localizaion for mobile robosin dynamic environmens, J. Arif. Inell. Res., vol. 11, pp , Nov [10] hp:// [11] hp://acivemq.apache.org/ [12] D. Fox, W. Burgard, F. Dellaer, and S. Thrun, Mone Carlo localizaion: Efficien posiion esimaion for mobile robos, in Proc. 16h Na. Conf. Arificial Inelligence, Orlando, FL, 1999, pp [13] hp://acivemq.apache.org/cms/ [14] Beke, M. and Gurvis, L., Mobile robo localizaion using landmarks, IEEE Transacions on Roboics and Auomaion, Apr. 1997, Vol.13, pp [15] Minguez, J. and Monano, L., Nearness Diagram Navigaion: Collision Avoidance in Troublesome Scenarios, Roboics and Auomaion, IEEE Transacions on, Feb. 2004, Vol. 20, pp References [1] G. Dudek and M. Jenkins, Compuaional Principles of Mobile Roboics. Cambridge, U.K.: Cambridge Univ. Press, [2] Ioannis M. Rekleiis. Cooperaive Localizaion and Muli-Robo Exploraion. PHD hesis. School of Compuer Science, McGill Universiy, Monreal, Quebec, Canada, February [3] L. Zhang and B. K. Ghosh, "Line segmen based map building and localizaion using 2D laser rangefinder," in Roboics and Auomaion, Proceedings. ICRA '00. IEEE Inernaional Conference on, 2000, pp vol.3. [4] K. Lingemann, H. Surmann, A. Nucher, and J. Herzberg, "Indoor and oudoor localizaion for fas mobile robos," in Inelligen Robos and Sysems, (IROS 2004). Proceedings IEEE/RSJ Inernaional Conference on, 2004, pp vol.3.