Auonomous Humanoid Navigaion Using Laser and Odomery Daa Ricardo Tellez, Francesco Ferro, Dario Mora, Daniel Pinyol and Davide Faconi Absrac In his paper we presen a novel approach o legged humanoid navigaion on indoor environmens using classical probabilisic SLAM mehods based on odomery informaion and laser measuremens. We use wo small lasers insalled in he robo fee o capure he laser daa. Odomery is obained by calculaing he posiion of each fee a every ime sep. The SLAM problem is solved by using a muli-laser SLAM soluion ogeher wih a holonomic moion model. Navigaion skills also include a pah planning module wih obsacle avoidance for auonomous navigaion in indoor environmens, and he whole process is performed wihou exernal compuaional power. Opionally, localizaion robusness is increased by adding he deecion of landmarks using a camera. Resuls obained are presened for he 1.5 m all Reem-B humanoid robo. I. INTRODUCTION Simulaneous Localizaion and Mapping in legged humanoid robos is a difficul issue, mainly due o he fac ha radiional SLAM echniques based on laser and odomery daa canno be used. The main reason is ha laser devices are very heavy and canno be mouned onboard a humanoid. On op of ha, in some cases where researchers have achieved o include lasers on a humanoid [1], [2], he large number of degrees of freedom makes very difficul o ake sabilized measuremens of laser-odomery. For hose reasons, SLAM implemenaions in humanoids are mainly based on he use of vision. Insead of using laser devices, cameras are used o idenify landmarks, convering he SLAM problem ino a vision one [3], [4], [5], [6]. For insance, he QRIO robo uses a sereo camera o self-localize and idenify possible pahs o goals [7]. The HRP robo achieved o use a small laser mouned on he robo s mouh [1], [2] in addiion o he use of he camera o creae complex 3D represenaions of he environmen. However, due o is inabiliy o correcly measure he laser posiion, is use was merely relegaed o he idenificaion of obsacles. In his paper, a novel approach is proposed based only on laser and odomery daa. Two small lasers are mouned on he fee of he robo and used o navigae. Using his seup, he robo is able o solve he whole problem of navigaion, ha is, generae maps in real ime, localize on i, and even auonomously navigae using pah planning and obsacle avoidance. Addiionally, and as an opional feaure, he robo makes use of is sereo vision camera o increase he localizaion robusness and allow quick recovery in los siuaions. The paper is divided as follows: in secion II, a descripion of he humanoid plaform is provided including he hardware seup required o perform navigaion. Secion III describes he algorihm used o perform mapping. Secion Pal Technology Roboics Paris 175, 4-1, Barcelona, Spain, ellezawork@gmail.com Fig. 1. Reem B robo used for he experimens IV describes he localizaion sysem. In secion V, here is a descripion of he pah planning mehod used, ogeher wih he obsacle avoidance module. Each secion includes he resuls obained when applied o he real robo Reem-B. II. HUMANOID PLATFORM The humanoid robo used is called Reem-B, a humanoid robo of 1.5 meers all buil by Pal Technology. Is weigh is abou 60 Kg wih an auonomy of abou 120 minues. Among oher abiliies, Reem-B can carry loads as heavy as 12 Kg, and walk a a maximum speed of 1.5 Km/h wih a grea sabiliy. The walking algorihm is based on a zero momen poin approximaion [8]. In order o feed he SLAM algorihm wih sensor daa, he robo is equipped wih wo small Hokuyo lasers [9] mouned on is fee (figure 2). Each laser is configured o scan 180 degrees on an angle ha is roaed 30 degrees from he foo cener. This roaion allows he deecion of obsacles behind he robo, and avoids self-deecion of he oher leg. Angle resoluion is of one degree, wha makes a oal of 360 measuremens o be processed by he algorihm a every ime sep. To provide a sabilized daa measuremen, he movemen of he fee when sepping is mainained parallel o he ground by he walking algorihm. This fac, assures ha he measuremens made by a laser in movemen corresponds o a unique inclinaion. Measuremens aken in his way are surprisingly very sable, as can be observed in figure 3. This siuaion allows he use of he laser scanned daa on a regular basis wih independence of movemen saus of he foo. In order o use his informaion i is assumed ha here are no obsacles wih a heigh beween he laser heigh when he
walks around i, and, if correcly uned, no furher processing is required. Once he map is compleed, he robo can direcly change is operaion mode o localizaion (see secion IV), and sar using he map for localizaion and pah planning, wihou requiring any addiional sep. Fig. 2. Image of he wo lasers mouned on he fee (lef), and diagram of he scanned range of each laser seen from op (righ) Fig. 3. A measuremen made by he lef laser foo a 180 degrees, when he robo looks parallel o a wall, a 6 cm aprox. A some poin in ime, he foo is moved up and forward (showed by he doed line). The graphic shows ha here is no noable difference beween he measuremen wih he foo on he ground or wih he foo on he air. fee is on he ground and when he fee is a is maximum heigh (see secion VI for a discussion on ha). Odomery is obained wih an inverse kinemaic compuaion by he walking algorihm, providing a each sep he (x,y,z) posiion and (θ, ϕ, φ) angles of each foo wihin a global coordinaes axis, in a similar way as provided for a 2D mobile base. For our algorihm, only he (x,y) coordinaes and he θ angle (as projeced over he (x,y) axis) are used o indicae he orienaion of each laser, in a similar way as in ypical SLAM wih wheeled robos. The z coordinae is no used in he algorihm, and he oher wo angles (ϕ, φ), which are he angles projeced over he axis (x,z) and (y,z) respecively, are mainained consan during he whole walking movemen. The robo is equipped wih wo PC104 sack based compuers, which provide all he compuaional power required o perform all he navigaion and conrol asks. This means ha all he algorihms required o perform mapping, localizaion, pah planning and obsacle avoidance are execued inside he robo. Only he visualizaion ools used o ake picures of he maps and observe localizaion saus where run on exernal compuers o he robo iself. This fac makes he Reem-B one of he mos auonomous robos of he world of is size. III. SLAM ALGORITHM Localizaion and mapping abiliies of he robo are based on he use of paricle filers. For he implemenaion of he SLAM feaure, we have seleced an algorihm called DP- SLAM [10], [11]. This is a very ineresing algorihm, since i allows an almos real-ime generaion of maps, wihou requiring poserior pos-processing. This means ha our robo creaes is environmen map a he same ime ha i A. DP-SLAM algorihm The DP-SLAM algorihm uses a paricle filer o mainain a join probabiliy over robo pose and maps. This implies ha he algorihm is able o solve map ambiguiies during he paricle filer execuion. Furhermore, i doesn need any addiional phase o produce he map and/or close loops. The difference wih similar algorihms [12], [13], [14] is he echnique i uses o mainain he large number of maps efficienly by exploiing redundancies beween maps. The auhors have developed a mehod called disribued paricle mapping. Insead of explicily mainaining muliple maps, he algorihm mainains a single map marix ha represens he grid map, and includes in each grid he observaions made by differen paricles. DP-SLAM mainains an ancesry-ree of paricles, where curren paricles are he leaves. The paren of each node is he paricle of he previous ieraion before resampling. This ancesry ree is used o rerieve he exac lineage of a given paricle a any ime, and hen reconsruc he bes curren map. We have made a C++ adapaion of he algorihm described in [11], which is an improved version of a previous algorihm described in [10]. We implemened he algorihm wih a modificaion o he paricle filer for is use wih he humanoid by using a muli-laser approach. B. Muli-laser paricle filer The laser daa required o localize he robo is generaed by wo laser devices. Each laser is localized on one of he robo fee, providing a 181 lecure daa poins. Even if he posiion of he wo fee are direcly relaed by he mechanical srucure ha ies hem up, i is possible for each foo o have a differen posiion and orienaion. This means ha he relaive posiion of each laser respec o he oher is no mainained over ime, bu i changes as he robo moves. Since he generaion of a single robo map should accommodae he lecures of boh lasers, a special mehod o combine hem ino he same probabilisic process has been adoped. For he resoluion of his problem, we ake a muli-laser approach. We see he robo as a single holonomic wheeled robo, where paricles represen he posiion of he lef fee, as cenered in he robo cener. Righ fee laser posiion is obained by he combinaion of he curren relaive posiion o lef-laser and a gaussian disribuion of noise. By doing his, i is possible o esimae he poserior probabiliy over one robo rajecory (he one of he lef fee) and one single map. Observaions provided by each laser are inroduced ino he paricle filer as if hey where of a single robo, where
each observaion correspond o a deermined esimaed posiion of he relaed laser. Then, each paricle of he filer will conain as probabilisic values he pose of he lef-foo foo, and he join map x lef, m, w lef. Given an observaion uple (z lef, u lef, z righ, u righ he updae rule for one sep is for each paricle i: x l i = A(ulef 1, xlef 1 ) m i = M(z lef, x lef i xrigh i, z righ = A(u righ 1, xrigh 1 ), x righ ) + m i 1 i ) ω lef i = S(z lef, x lef i, m i 1 )S(z righ, x righ i, m i 1 )ω lef i 1 where z lef, x lef i, is an observaion uple for laser lef a ime, A is he acual model, M is he moion model, S is he sensor model, and ω i is he weigh for paricle i, and m i is he map guess of paricle i a ime. C. Moion model The moion model is in charge of describing he relaionship beween he odomery lecure and he acual pose of he robo, aking ino accoun he previous sep posiion of he robo. For our paricle filer, he moion model is based on odomery measuremens, and i is defined for an holonomic robo. This model is applied only o he lef laser in order o obain he nex sep posiion of ha foo. Laser righ posiion is hen calculaed direcly from he odomery relaed o he obained posiion of he lef foo, and an addiional gaussian noise. The moion of lef foo is decomposed ino wo principle componens. D will represen he movemen hrough he major movemen direcion, and C will represen he shif experimened by he foo in he orhogonal direcion o he major direcion. x = x 1 + Dcos(θ 1 + T 2 ) + Ccos(θ 1 + T +π 2 ) y = y 1 + Dsin(θ 1 + T 2 ) + Csin(θ 1 + T +π 2 ) θ = θ 1 + T mod 2π where θ is he facing angle of he robo foo a ime, and T is he acual urn performed. Hence, we approximae he major axis direcion a ime by (θ + T 2 ) and he minor by (θ + T +π 2 ). Parameers of he moion model have been uned using he algorihm described in [15], where he parameers of he model are obained by a learning mehod from recorded laser-odomery daa of he robo. Basically, i consiss of using an expecaion maximizaion algorihm (EM) o esimae he model parameers. The expecaion sep is provided by he execuion of he DP-SLAM algorihm on recorded daa, saring wih some random iniial parameers. The possible rajecories obained are hen used in he maximizaion phase o creae a se of parameers which bes describe he moions represened by hose rajecories. We refer he reader o Eliazar s paper for furher informaion abou how his opimizaion algorihm works. Parameers Value Num. paricles 1000 Grid size 3 cm TABLE I TABLE OF PARAMETERS USED FOR THE DP-SLAM ALGORITHM. D. Sensor model The sensor model is he same as described in [11]. I is a very complee model which akes ino accoun occlusions in he ray, and depends on he disance raveled hrough a grid square. As counerpar, he model uses far more CPU han he ypical rayracing algorihms. The model is based on he concep of opaciy of map grids. The opaciy erm basically describes he probabiliy ha a laser crosses he grid, and is calculaed from he number of rays ha reach he grid divided by he disance ravelled hrough ha grid. The opaciy of a grid defines he probabiliy ha a laser ray is inerruped on ha grid (P g), afer i ravels a disance x hrough ha grid of opaciy ρ. This behavior is modeled by an exponenial disribuion: P g(x, ρ) = 1 e x/ρ Then he probabiliy ha a laser sops a square j is calculaed as he probabiliy ha he laser ravels up o grid j-1, and hen sops a grid j: P (sop = j) = P g (x j, ρ j )(1 P g (x 1:j 1, ρ 1:j 1 )) The probabiliy of a measuremen is hen he sum, over all grid squares in he range of he laser, of he produc of he condiional probabiliy of he measuremen given ha he beam has sopped, and he probabiliy ha he beam sopped in each square. This sensor model allows for he raveling of non uniform disances over differen grids. E. Map generaor The generaion of he map is no performed using a simple ray-racing algorihm. I is based on he special compuaion performed by he sensor model described in secion III-D. For each grid, he algorihm mainains a compuaion of he oal disance ravelled hrough ha square by a laser beam, and he number of imes ha a laser ray has sopped in he square. The ρ esimaion is hen provided by he quoien beween he disances and he sops. ρ is used o indicae he opaciy of ha grid in he map consrucion. F. Resuls The maps generaed by our robo are shown in figure 4. They were generaed in realime while he robo was driven by a joysick. The qualiy of he maps is impressive even when he robo moves a is highes speed (1.5 km/h) and he updae rae o he paricle filer is produced only wice per second 1. Those good resuls may be explained by he fac ha wo lasers are used and when one laser is moving 1 Due o he limied compuaional power onboard he robo
small obsacles and a few people is around i, which were no included ino he original map. However, when larger obsacles are presen, he localizaion qualiy decreases rapidly. Is work for fuure o implemen on he robo some filers [16] ha improve localizaion under hose circumsances. Fig. 4. Some map examples creaed by Reem-B he oher one is saic. The saiciy of one laser over he same group of paricles may decrease he paricles diversiy, bu on he oher side, he low updae rae of he filer may increase he diversiy, creaing a kind of compensaion effec. This saemen will be confirmed in fuure work. IV. LOCALIZATION Once a map has been creaed by he SLAM algorihm, he robo can change is saus o localizaion mode, via a vocal or GUI command, and sar localizing on he generaed map wih no addiional (pos-processing) seps. Localizaion over ha map is performed using a ypical MoneCarlo paricle filer approach. A. Paricle filer descripion The localizaion module of Reem-B uses he same paricle filer and moion model as for mapping (described in secions III-B and III-C). However, in order o improve he speed of he algorihm wih more paricles, a simpler sensor model for localizaion has been used. A ypical sensor model based on a disance map is used (as described in [13]), which does no include he deecion of occlussions. The pracical resuls showed ha he model is good enough o localize he robo wih quie good sabiliy in ypical office seups. Paricles are creaed for one single fee and hen replicaed for he second one wih added gaussian noise. The paricle filer hen mainains only one single group of paricles as described in secion III-B. The simplificaion of he sensor model allows he increase of paricles used for localizaion, which is a very imporan facor specially when global localizaion or kidnapping are required. In he case ha he measuremen of he maching decreases below a cerain level, new gaussian noise is added o he pose esimaion. Whenever he diversiy of he paricles reaches a hreshold, paricles are randomly disribued over he whole map, and he filer sared again. This mechanism allows for a correc recuperaion of he robo localizaion when sars up or when i is kidnapped. B. Resuls In an office environmen, he robo localizes iself quie quickly and mainains is localizaion quie sable even when C. Improved global localizaion In order o improve he localizaion abiliies of he robo, an addiional localizaion mechanism has been implemened. I consiss of using visual informaion o re-localize he robo when he localizaion saus decreases below a cerain hreshold of confidence. During he map creaion phase, he robo is allowed o acquire visual landmarks whenever required. Whenever he process of acquisiion of visual landmark is acivaed, by a voice command, he map will include a snapsho of he curren visual field, and aach his snapsho o he posiion deeced by he robo. The disance of he snapsho is calculaed from laser informaion. Due o his issue, he snapsho feaure requires he robo o be sopped while capuring he landmark, and is limied o is use for landmarks on walls and wih free space from he fee o he wall. Whenever he localizaion confidence of he robo decreases a cerain level, hen i will auomaically sar a process o idenify on is visual field any of he snapshos ha are sored in he map. If one of he snapshos is idenified, hen he paricles are spread around he landmark wih an amoun of gaussian noise. Landmark disance o robo is calculaed by he lef-foo laser only during map creaion, since he laser is more precise and he environmen can be easily conrolled during map creaion. Insead, when he robo is lef alone, disance o landmarks has o be performed using vision, since i canno be assured ha no obsacle will inerfere he laser. Because of ha, he recogniion of landmarks is no affeced very much by movemen of he robo or he exisence of small obsacles beween he robo and he landmark. As drawbacks, we have a big difficuly recognizing objecs in ambiance wih bad ligh, and ha he disance calculaion from a sereo image is no very precise. During he experimens performed, we observed ha in indoor office-like environmens, he paricle filer based on laser was very sable and did no require he use of he vision add-on. Only in cases wih crowded rooms he localizaion confidence decreased considerably and he visual localizaion sysem acivaed. V. PATH PLANNING WITH OBSTACLE AVOIDANCE The pah planning module is inegraed wih he localizaion and mapping sysem. Using he maps creaed and he curren esimaed robo posiion, he pah planning module is able o calculae a safe pah o a requesed goal posiion in he map. Then he module commands he robo from curren posiion o he goal posiion following he pah planned while avoiding obsacles. Desinaion poins are requesed via voice (for predefined locaions) or hrough a conrol GUI which
Fig. 5. Sequence of global localizaion. The robo is swiched on a he lef mos picure. Then i is moved wih a joysick and localizaion improves wih i. Red dos represen he lef foo posiion guess, and green dos he righ one. Blue dos are he currenly sensed obsacles by he lasers. They mus mach he map srucure when correcly localized (as in he righ-mos picure). shows he curren map on an exernal compuer and allows he selecion of he desinaion poin. The auonomous navigaion abiliy of Reem-B is decomposed in hree differen pars [17]: moion planning, moion conrol and obsacle avoidance. A. Moion planning Moion planning is performed in hree phases: On he firs phase, he currenly sensed obsacles by he lasers are included on a emporal copy of he curren map, resuling in wha we call an updaed map (figure 6-lef). Second, a disances map is generaed from he updaed map (figure 6-cener). The disances map provides he places in he map ha are away enough from obsacles (safe places for he robo), given he acion radius of he robo. The rajecory is hen calculaed over he disances map. Only he free poins in he map are aken ino accoun on he calculaion of he rajecory. The rajecory is calculaed using he A* algorihm. The compued pah follows he poins in he map ha are safe enough for he robo, and provide he shores rajecory possible. The rajecory provided by he A* algorihm (raw rajecory) is very abrup which makes very difficul for he robo o follow (see pink line in he rajecory plo in figure 6-righ). The rajecory is hen ranslaed o a se of sraigh lines (filered rajecory) ha bes fi he raw rajecory (blue lines in figure 6-righ). The filered rajecory calculaes he sraigh lines ha bes fi in he raw rajecory. The final sep of he moion planning module calculaes from he filered rajecory he required seps for he robo o follow ha rajecory. Seps indicae in local coordinaes of he robo where he fee mus be in order o follow he rajecory. The calculaed seps are sen o he walking server, which ranslaes foo posiion ino he required angles for each moor, performing he acual walking movemen of he robo. B. Moion conrol Once he rajecory has been calculaed over he map, i has o be performed by he robo. This is a differen problem from moion planning which is called moion conrol. Imperfecions on localizaion, moion of he robo, and he map, Fig. 6. The lef mos figure shows he map used for navigaion. The cenered figure shows he calculaed disance map, over which, a rajecory will be calculaed. The righ mos figure shows he rajecory generaed by he pah planning algorihm from he robo (represened by wo dos in red and green), and some free poin in he map. The rajecory is composed of wo differen lines: firs one, in viole, is he raw rajecory calculaed by he A* algorihm. The blue line is he smoohed rajecory. Is his smoohed rajecory he one ha he robo will follow. generae differences beween he rajecory planned and he real rajecory followed by he robo. To deec hose errors, he robo is equiped wih a conrol of moion sysem which calculaes a every ime sep a measuremen of he difference beween he rajecory pah and he acual robo posiion. Difference is measured in erms of posiion and direcion of movemen. The direcion measuremen indicaes how differen is he curren orienaion of he robo relaed o he orienaion i should have o follow he calculaed line of moion. The posiion measuremen indicaes how far from he curren line of moion is he robo. Boh differences are obained hrough a direc connecion wih he localizaion module which indicaes where he robo acually is in he map. The moion conrol module modifies he angle of he seps in case of errors. Whenever he orienaion of he robo is above a minimal angle, seps are correced wihou replanning. If for any reason, he posiion of he robo goes ouside he roue planned by a cerain hreshold, a complee replanning is performed.
Fig. 7. Securiy zones of movemen of he robo for he fee. Figure shows hree differen zones per foo, one for fron movemens, one for diagonal movemens and anoher for laeral and backwards movemens. There is some overlap beween he fron zones of boh fee. C. Obsacle deecor wih replanning In order o avoid obsacles in he direcion of movemen, Reem-B is equipped wih a module ha deecs obsacles in he pah and replans if necessary. The robo has six securiy zones defined (see figure 7). Those zones are he secure movemen space for he robo. If an obsacle eners one of he zones, and he direcion of movemen of he robo is wihin ha zone, hen he obsacle deecor will signal his and sop he robo. Then a replan of he rajecory o goal is performed, aking ino accoun his new obsacle. The disance a which he robo deecs obsacles is uned by wo parameers ha specify he maximal fron and laeral disances a which he obsacles will be deeced. A presen, hose disances are se o 1.5 meers and 50 cm respecively. Obsacles deeced wihin he securiy zones will only invoque replanning when he direcion of movemen of he robo goes in he direcion of he obsacle. Fuure work includes he real ime modificaion of walking seps o dynamically avoid obsacles wihou replanning, by using a poenial field mehod. D. Resuls The ess performed allowed he robo o move auonomously in office ambiens. The es performed show ha he robo is capable o move coherenly in spaces big enough o allow he robo move freely. In smaller spaces he movemen becomes less fluid. The resuls show ha he robo does replan jus in a few cases, mosly because of errors in he movemen due o sleep movemens in sleepy grounds. Figure 8 shows one of hose cases. Figure 9 shows a change in rajecory when he obsacle deecor deecs an error. VI. DISCUSSION On he previous secion, he resuls presened show ha, when a map is available, Reem-B can auonomously and safely move inside and office environmen. However, we mus poin ha in fac here are wo modes of acuaion for Reem. On he firs one, he robo is joysick driven while generaing he map. On he second one, while moving hrough he map using rajecories generaed by he pah planning module, he robo is compleely auonomous. Fig. 9. Sequence of pah planning-2. A he lef-mos picure, he robo generaes a rajecory o a goal poin, ha is no looking a (he robo is oriened o he opposie side). Once he rajecory is planned, he robo sars moving (roaing) owards ha goal while avoiding obsacles. When roaion is compleed, he robo deecs an obsacle jus on is rajecory. Then a replan is made, aking ino accoun he newly deeced objec. The new rajecory compleely avoids he obsacle deeced. When in auonomous movemen mode, he robo achieves wih no problem o move around he obsacles and reach he desired posiion on he map. However, he movemen of he robo is no fluid a all and fluidiy decreases as far as he space becomes more cluered. This is a limiaion of he curren pah planning approach aken which does no define he rajecory as a coninuos one, bu as a se of sraigh lines ha he robo has o follow. A he end of each line, here is a urn required o face he direcion of he nex line. This urn, sops he robo from is curren forward moion. When he space is full of obsacles, he rajecory is composed of large groups of small lines, wha produces he effec of low fluidiy. This will be changed in he fuure, by planning he seps direcly hrough he rajecory provided by he A* algorihm. The mechanism presened can be improved in several poins, as has been indicaed along he ex, bu even on is presen sae, i is he firs ime a humanoid robo of is size uses classical laser-based echniques o guide a humansize humanoid robo in all he navigaion skills required for an auonomous robo. VII. CONCLUSIONS Complee navigaion abiliies for a human size humanoid robo have been shown 2. Even if wih some limiaions, his is he firs ime ha a humanoid robo of human size is able o generae a complee map of is environmen, localize on i and move auonomously on i. Those navigaion abiliies ogeher wih he characerisic ha he robo has long lasing baeries (more han wo hours), and all he compuaional resources required o conrol i are onboard, make of Reem- B one of he mos auonomous humanoids in he world of is size. REFERENCES [1] S. Thompson, S. Kagami, and K. Nishiwaki, Localisaion for auonomous humanoid navigaion, in IEEE-RAS Inernaional Conference on Humanoid Robos, 2006. [2] S. Thompson, Y. Kida, A. Miyazaki, and S. Kagami, Realime auonomous navigaion wih a 3d laser range sensor, in Proceedings of 3rd In. Conf. on Auonomous Robos and Agens, 2006. [3] K. Okada, M. Inaba, and H. Inoue, Inegraion of real-ime binocular sereo vision and whole body informaion for dynamic walking navigaion of humanoid robo, in Proceedings of In. Conf. on Mulisensor Fusion and Inegraion for Inelligen Sysems, 2003. 2 A video showing all he navigaion skills described in his paper can be found a www.pal-roboics.com/media.hml
Fig. 8. Sequence of pah planning-1. A he lef-mos picure, he robo is locaed a one corner of he room. Then i generaes a rajecory o he opposie corner. Once he rajecory is defined, he robo moves owards he goal poin while avoiding obsacles. A some poin in he rajecory, he robo moves oo far from he planned rajecory. Then a replan is made, and he robo follows he new rajecory unil he end goal posiion. [4] N. Karlsson, E. di Bernardo, J. Osrowski, L. Goncalves, P. Pirjanian, and M. Munich, The vslam algorihm for robus localizaion and mapping, in Proceedings of he 2005 IEEE Inernaional Conference on Roboics and Auomaion, 2005. [5] A. J. Davison, O. Sasse, and K. Yokoi, Vision based slam for a humanoid robo, in Inernaional Conference on Roboics and Auomaion, April 18-22 2005. [6] A. J. Davison, I. D. Reid, N. D. Molon, and O. Sasse, Monoslam: Real-ime single camera slam, IEEE Transacions on paern analysis and machine inelligence, vol. 29, no. 6, 2007. [7] J. Guman, M. Fukuchi, and M. Fujia, Real-ime pah planning for humanoid robo navigaion, in Proceedings of he Inernaional Join Conference on Arificial Inelligence, 2005, pp. 1232 1238. [8] T. Sugihara, Y. Nakamura, and H. Inoue, Realime humanoid moion generaion hrough zmp manipulaion based on invered pendulum conrol, in Proceedings of he IEEE Inernaional Conference on Roboics and Auomaion, 2002. [9] M. Toshihiro, H. Masanori, and Y. Shin Ichi, Developmen of laser area sensor by measuring laser pulse reflecing ime, Nippon Roboo Gakkai Gakujusu Koenkai Yokoshu, vol. 24, 2006. [10] A. Eliazar and R. Parr, Dp-slam: Fas, robus simulanious localizaion and mapping wihou predeermined landmarks, in Proceeding of he IJCAI 2003, 2003. [11], Dp-slam 2.0, 2006. [12] H. Durran-Whye and T. Bailey, Simulaneous localisaion and mapping (slam): Par i he essenial algorihms, Roboics and Auomaion Magazine, vol. June, 2006. [13] S. Thrun, D. Fox, W. Burgard, and F. Dellaer, Robus mone carlo localizaion for mobile robos, Arificial inelligence, 2001. [14] M. Kopicki, Mone carlo localisaion for mobile robos, Maser s hesis, Universiy of Birmingham, School of Compuer Science, 2004. [15] A. Eliazar and R. Parr, Learning probabilisic moion models for mobile robos, in proceedings of he Inernaional Conference on Machine Learning, 2004. [16] D. Fox, W. Burgard, and S. Thrun, Markov localizaion for mobile robos in dynamic environmens, Journal of arificial inelligence research, vol. 11, pp. 391 427, 1999. [17] H. Chose, K. M. Lynch, S. Huchinson, G. Kanor, W. Burgard, L. E. Kavraki, and S. Thrun, Principles of Robo Moion: Theory, Algorihms, and Implemenaions. The MIT Press, 2005.