Improving 2D Reactive Navigators with Kinect

Transcription

1 Improving 2D Reacive Navigaors wih Kinec Javier Gonzalez-Jimenez, J.R. Ruiz-Sarmieno, and Cipriano Galindo Sysem Enginnering and Auomaion Dp., Malaga, Spain {javiergonzalez, joaraul, Keywords: Absrac: Kinec, Reacive Navigaion, Obsacle Deecion, Mobile Robo. Mos successful mobile robos rely on 2D radial laser scanners for perceiving he environmen. The use of hese sensors for reacive navigaion has a serious limiaion: he robo can only deec obsacles in he plane scanned by he sensor, wih he consequen risk of collision wih objecs ou of his plane. The recen commercializaion of RGB-D cameras, like Kinec, opens new possibiliies in his respec. In his paper we address he maer of adding he 3D informaion provided by hese cameras o a reacive navigaor designed o work wih radial laser scanners. We experimenally analyze he suiabiliy of Kinec o deec small objecs and propose a simple bu effecive mehod o combine readings from boh ype of sensors as well as o overcome some of he drawbacks ha Kinec presens. Experimens wih a real robo and a paricular reacive algorihm have been conduced, proving a significan upgrade in performance. INTRODUCTION Mos successful mobile robos rely on 2D radial laser scanners for perceiving he environmen and on a hybrid navigaion approach (Fiorini and Shiller, 988), including a reacive algorihm o deal wih obsacle avoidance (Blanco e al., 2008). The usage of 2D scanners for reacive navigaion bears a serious limiaion: he robo can only deec obsacles on he plane scanned by he sensor, ypically a plane parallel o he floor. Obviously, his enails a collision risk wih objecs a a differen heigh or wih salien pars. Such a limiaion can only be ackled by gahering 3D informaion of he robo s surrounding from a hree-dimensional fieldof-view. Pas soluions for obaining 3D daa includes he so-called acuaed laser range finders (alrf), which are expensive and no quick enough for mobile robo navigaion purposes (Holz e al., 2008, Marder-Eppsein e al., 200). Nowadays, his hurdle can be overcome by RGB-D cameras, like Kinec (Kinec, 203). Kinec has been developed by Microsof as a naural inerface for videogames. Addiionally, i presens cerain feaures ha urn i ino an aracive device for mobile roboics: I is a compac and lighweigh sensor which provides boh RGB and range images. I is fas, working a a frequency of 30 Hz. The operaion range is accepable for indoor applicaions: from 0.5 o 3.5m. I is cheap: around 50 nowadays. Wih he aim of improving he reacive navigaion capabiliies of a mobile robo, in his work we address he problem of replacing (or enhancing) a radial laser scanner which feeds a reacive navigaor wih a Kinec sensor. I is imporan o remark ha our ineres is no in he modificaion of he reacive algorihm, bu in adaping he Kinec deph image in order o provide he navigaion algorihm wih a virual 2D scan, encapsulaing he 3D world informaion. For such a goal, wo major drawbacks need o be overcome. On he one hand, he deph image of Kinec, which has a resoluion of 640x480 pixels, needs o be effecively condensed in a 2D scan forma, losing as less meaningful informaion as possible for he moion algorihm. On he oher hand, Kinec has a large blind zone, boh in angle (i.e. narrow field-of-view) and for shor disances. For a soluion o hese problems we propose a wo-seps posprocesing of he Kinec daa:

2 Figure. Kinec sensor and coordinae sysem (aken from (Garcia and Zalevsky, 2008)). i) The range image can be seen as he oupu of 480 radial scanners heading o differen il angles. We simplify ha informaion by projecing he perceived obsacles around he robo on a virual horizonal scanning plane. ii) A shor-erm memory of sensed obsacles is implemened such ha unobserved obsacles lying in he blind zone of he sensor are incorporaed ino he virual scan. Alhough here are some works ha use Kinec as an inpu sensor for reacive navigaion (see for example (Cunha e al., 20, Biswas and Veloso, 20)), o he bes knowledge of he auhors he commened drawbacks have no been explicily addressed. In order o es he suiabiliy of he proposed approach, a se of experimens has been conduced wihin cluered scenarios. The experimens have validaed he improvemen in performance of a paricular reacive navigaor (Blanco e al., 2008), hough he soluion presened here can be applied o any oher reacive algorihm ha uilizes a 2D represenaion of he space, i.e. only relying on 2D laser scanners. Nex secion gives a descripion of he Kinec device. Secion III presens he proposed soluion for he usage of Kinec as an addiional inpu sensor for any reacive navigaion algorihm based on a 2D obsacle represenaion. In secion IV resuls from he conduced experimens are presened and discussed. Finally, some conclusions are oulined. 2 THE KINECT SENSOR 2. Descripion The Kinec device (see figure ) is equipped wih an RGB camera, a deph sensor, a marix of microphones, a moorized base which endows he sensor wih a il movemen of 27º, and a 3-axis acceleromeer. Focusing on he deph sensor, also called range camera, i is composed of an infrared ligh projecor combined wih a monochrome CMOS sensor. I has a VGA resoluion (640x480 pixels) wih -bi resoluion deph, and a daa refresh raio of 30 Hz. Is nominal field of view is 58º in he horizonal plane and 45º in he verical one. The operaional range repored by he manufacurer is from.2 m. o 3.5, hough in our experimens we have esed ha he sensor is able o deec objecs placed a 0.5 m. (see secion 2.3). 2.2 Working Principle of he Range Camera The range camera of Kinec consiss of wo devices: an infrared (IR) ligh projecor which cass a paern of poins ono he scene and a sandard CMOS monochrome sensor (IR camera) (Freedman e al., 200). Boh are aligned along he X axis, wih parallel opical axis separaed a disance (baseline) of 75mm (see figure ). This configuraion eases he compuaion of he range image, which is performed hrough he riangulaion beween he IR rays and heir corresponding do projecions ono he image. The mehod o compue he correspondence beween rays and pixels relies on innovaive echnique called Ligh Coding (Garcia and Zalevsky, 2008), paened by PrimeSense (PrimeSense, 203), which enails a very paricular facory calibraion. In his calibraion, a se of images of he poin paern, as projeced on a planar surface a differen known disances, are sored in he sensor. These are he socalled reference images. Kinec works like a correlaion-based sereo sysem wih an ideal configuraion (i.e. idenical cameras, aligned axes separaed along he X axis) where he IR rays are virually replaced by line-ofsigh of he poins in he reference images. As in sereo, deph a each image poin is derived from is dispariy along he X axis, which is compued by correlaing a small window over he reference image. Furher informaion abou his calculaion can be found in (Khoshelham, 20). Regarding he accuracy of his mehod, he disance errors are lower han 2 cm. for close objecs (up o 2m.), linearly increasing unil an average error of 0 cm. a 4m.

3 Legend Objec deeced Objec no deeced No measuremens available Cenral measures Cenral measures Figure 2. Resuls for he small obsacle deecion experimen. The verical axis represens he 6 cenral columns of he range image. The horizonal axis is he disance, discreized ino inervals of 5 cm., from Kinec o he sick horizonally placed in fron of he robo. Each cell encodes wheher he sick was deeced a any of he image rows considered. Figure 3. Experimen seup o es he visual acuiy of Kinec for deecing small obsacle. In his paricular case, he obsacle is a horizonal sick wih a hickness of 4 cm. 2.3 Kinec Reliabiliy for Deecing Thin Obsacles To assess how much reliable Kinec is o feed a robo reacive navigaion sysem, i is of ineres o analyze is capaciy o deec surrounding obsacles, paricularly hose ha are hardly deecable by oher sensors, eiher because of heir small size or because of heir posiion in he scene, e.g. he salien board of a able, he legs of a chairs, coa sands, ec. We have performed a number of experimens where sicks of differen hickness (, 2, and 4 cm.) were horizonally placed in fron of he sensor, a a cerain heigh, verifying wheher hey are deeced a differen disances. The experimens were conduced in a corridor wih he Kinec sensor mouned on a mobile robo (see figure 3). To exrac he meaningful daa for his analysis we only focused on a recangular window of he range image, concreely he 6 cenral columns of a small number of consecuive image rows a he heigh where he sick is expeced o be. We mus accoun for an inerval of rows since here is no guaranee ha he sick is observed in a single, fixed row during all he robo run. Noice ha, hough he sick is roughly horizonal, is projecion on he range image may cover differen rows and also may change from one posiion o anoher. Saring a a disance of 4 meers from he obsacle, he robo gradually moves owards i a discree incremens, while recording range images as well as he robo odomery a each posiion. This experimen was repeaed 5 imes for each hickness (, 2, and 4 cm.). Figure 2 displays he resuls of wo of hese robo runs, for he and 2cm. sick. I is ineresing o see

4 how he 2cm.-hick sick was almos always observed over he full operaional inerval (from 0.5 o 3.5 m.). The plo for he 4cm.-wide sick has been omied because i was always deeced. These resuls reveal he Kinec s poenial for deecing small obsacles up o an accepable disance. Noe how in he case of he obsacle of cm is deecion is no sable: hough i sars o be deeced a 2.3 m, i laer disappears because of he discree spaial sampling of he sensor. 3 KINECT AS INPUT SENSOR OF A 2D REACTIVE NAVIGATOR In general, he use of Kinec o feed a reacive navigaor based on a 2D space represenaion presens wo difficulies: i) he huge amoun of daa i provides, and ii) he exisence of a blind zone boh a shor disance and because of he narrow horizonal field-of-view (in comparison o laser radial scanners). Our soluion o overcome hese issues consiss of a pos-processing sage o convenienly adap he daa o he specific needs of he reacive navigaor, which may also receive sensorial informaion from oher sources, e.g. a 2D laser scanner (see figure 4). The firs sep of such a pos-processing sage aims a reducing he huge amoun of daa while keeping he relevan 3D sensorial informaion abou obsacles. A second phase srives for coping wih he problem of he blind zone by creaing a shor-erm memory which emporally recalls he perceived obsacles around he robo. This memory is hen ransformed o a scan forma suiable o be exploied by he reacive navigaor. Nex, hese processes are described. 3. Projecion of he Poin Cloud o 2D The Kinec sensor provides more han measures per frame a 30Hz. This inense flow of daa provides he robo wih very valuable informaion abou he surrounding bu enails wo problems for a reacive navigaor: Firs, i becomes a significan compuaional burden ha could hamper he robo o concurrenly execue oher asks, e.g. robo localizaion, mapping, ec. Second, hese daa canno be direcly exploied by convenional robo reacive algorihms designed o be fed wih 2D scans, such as Virual Force Field (Borensein and Koren, 989), Nearness diagram (Minguez and Monano, 2004), PTG-based navigaor (Blanco e al., 2008), ec. Our approach o overcome hese wo problems consiss in condensing he 3D Kinec daa ino a 2D scan by selecing he minimum measured disance from each column of he range image. This is a simple and efficien procedure whose resuls is a virual scan of 640 ranges (he number of columns in he image) ha capures he closes obsacle poin for all he differen heighs (image rows), as shown figure 5. Please, noice ha, by exracing a virual scan for differen heigh inervals, his soluion could cope wih robos wih varying polygonal secions. The soluion implemened in his work, hereby, is a paricular case of his general approach. 3.2 Shor Term Memory A serious limiaion of Kinec is is inabiliy o deec close objecs, boh below is minimum operaional range and ou of he field-of-view (FOV). The minimum range depends on he surface color and maerial, and is ypically beween 0.5 and m., while he horizonal and verical FOV are 58 and 45 degrees, respecively. This blind zone becomes a serious drawback for a reacive navigaor, since when he robo approaches an obsacle (boh wih roaional or ranslaional movemens) i suddenly disappears and he space becomes obsacle-free, causing he robo o crash ino i. To overcome his problem we propose a shor-erm memory around he robo locaion which mainains previous measuremens emporally. Such a memory has been implemened hrough a grid, similarly o an occupancy map (see figure 6), which is formalized as follows. Kinec Laser scanner 3D obsacle projecion in 2D Shor erm memory Virual scan Scanner simulaion Obsacle Grid Map Robo Localizaion Reacive Navigaor Figure 4. General diagram of he proposed sysem o combine daa from a laser scanner and a Kinec sensor o feed a reacive navigaor.

5 Z Y Z X Figure 5. Lef, image of he scene capured by he RGB camera. Middle, op view of he scene perceived by he range camera. Righ, laeral view of he same scene. The green poins correspond o discarded measuremens, and he blue ones are hose considered for he virual scan Memory A shor-erm memory M is defined as an n x m marix, which discreizes a cerain bi-dimensional area around he robo. Le M(j) represen he probabiliy of he cell c j o be occupied by an obsacle, based on he observaions, o,, o k, of he robo, ha is: Robo M (, i j) p( c o,..., o ) () j For he calculaion of such a probabiliy i is convenien o use he so-called log-odds (refer o (Thrun, 2003) for furher deail), which requires he compuaion, for each c j a ime, of he expression: pc ( o,..., o) j k ( j) log pc ( jo,..., ok ) l c The memory cells are iniialized wih he value: l pc ( ) 0 j ( c j) log p ( c j ) k (2) (3) where p(c j ) has been se o 0.5. Given l i is possible o rerieve he probabiliy of each cell as: pc ( o,..., o) e j k l ( c i, j ) (4) and from ha, we creae an obsacle map by considering ha a cell c j is occupied if is probabiliy is above a given hreshold. To feed he reacive navigaor, a scanner simulaor convers his obsacle grid map ino a scan forma. Sensor blind zone Robo Observaion in k Figure 6. Shor-erm memory a ime k. The memory sores a se of previous sensor measuremens ino a grid. Righ, he obsacles considered by he reacive navigaor are formed by he inegraion of poins from he blind zone (in red) and he curren virual scan (in blue), which is obained from he Kinec daa (in green) Memory updae Robo Obsacles aken ino accoun by he reacive navigaion algorihm in k Le o be a virual scan observaion derived from Kinec a ime. Le c k be he cell where he k-h measuremen of he scan lies, given he curren robo pose. Using he Bayes rule and log-odds, he updaing of he value l (c j ) as a consequence of a new scan o is performed hrough he following expression (see (Thrun, 2003)): cij ck, pc ( j o) pc (, ) i j l ( c j) l ( c j) log log pc ( o) pc ( ) j j (5)

6 Figure 7. Mobile robo SANCHO, equipped wih 2 Hokuyo laser scanners and a Kinec sensor for obsacle avoidance. where he a poseriori probabiliy pc ( j o ) is calculaed wih he Bayes rule using he following sensor model: 2 ( o d j) 2 po ( M, c j ) e 2 2 (6) where d,j is he disance o he cener of he cell c j. pc (, ) i j The erm log is a consan which pc ( j) depends on he seleced value of he a priori probabiliy pc ( j). Therefore, considering pc ( j) 0.5, he calculaion of he memory updae is simplified o: pc ( j o) ij k, ( j ) ( j ) log p ( c j o ) c c l c l c (7) Noice ha, according o his updae mechanism a cell requires persisen observaions o change from occupied o free, and viceversa. Also, noe ha non-observed cells are no updaed, i.e., hey keep heir values. 4 IMPLEMENTATION AND DISCUSSION The proposed approach has been implemened in he mobile robo SANCHO (Gonzalez e al, 2009) (see figure 7). The size of he shor-erm memory was se o 00 x 00 cells, for a 2 x 2m. area around he robo, i.e., wih a cell size of 2 cm. SANCHO, buil upon a commercial Pioneer 3DX base, is equipped wih wo Hokuyo laser scanners (Hokuyo, 203) placed a 30 cm. over he floor (scanning he fron and he rear of he robo) and a Kinec sensor in is fron upper par (see figure 7). The reacive navigaion sysem of SANCHO, called PTG-based navigaor, ransforms he 2D local obsacles in he 3D Configuraion Space of he robo ino he so-called Trajecory Parameer Spaces (TP-Spaces). A TP-Space is a 2D represenaion of all he poses he robo could reach if i moved according o a cerain pah model. Since several pah models are considered by he reacive sysem, several TP-Spaces are buil. The mahemaical ransformaion beween he C-Space and he TP- Spaces is done by Parameerized Trajecory Generaors or PTGs, each one represening a pah model (e.g. circular, urn&move-sraigh, ec.) ha fulfills cerain geomerical and opological properies. As a resul of his ransformaion he robo in he TP-space becomes a free-flying poin whose moion migh be solved by any holonomic obsacle avoidance, such as Virual Force Field (Borensein and Koren, 989) or Nearess Hisogram (Minguez and Monano, 2004). Please, refer o (Blanco e al., 2008) for a more deailed explanaion of he PTG-based navigaor. An ineresed feaure of he PTG-based navigaor is ha i can deal wih several sensors ha provide surrounding informaion simulaneously. When using only he wo radial laser scanners, SANCHO is prone o crash wih many unobserved objecs ha i may encouner in a ypical environmen, e.g. ables, chairs, boxes, plans, coa sands, shelves, ec. Afer incorporaed he Kinec sensor his problem has been drasically reduced, hough no compleely eliminaed. Since a precise quanificaion of he improvemen level of he proposed approach is no possible we have ried o validae he mehod wih wo differen ess. In he firs, we have repeaed a number of robo local navigaions, i.e. go from A o B, wih he wo sensing configuraions (wih and wihou Kinec) and varying he obsacles along he pah. Figure 8 shows one of hese seups in our lab, wih obsacles a differen heighs, including a papers box, he base of anoher robo, he board of a able, and a coa sand wih jackes. Some of he rajecories followed by he robo are shown in figure 9. The red one

7 5 CONCLUSIONS Figure 8. Scenario where some of he experimens were conduced. 4 3 In his paper we have presened mehods o convenienly adap he 3D informaion provided by a Kinec device o work wih a reacive navigaor designed o cope wih radial laser scanners. I has been proposed soluions for wo of he major Kinec drawbacks: he huge amoun of daa provided by he sensor, more han measures per frame a 30Hz, and is large blind zone due o boh is narrow field-of-view and he lower limi of is operaional range (from 0.5 o m., depending on he surface characerisics). We have experimenally demonsraed he Kinec poenial o deec small obsacles, a key aspec for safey during a reacive navigaion. Finally, a number of ess have been conduced, validaing he suiabiliy of he proposed mehods Figure 9. Pahs followed by he robo during four differen navigaions. The colored circles represen he iniial posiion of he robo, and he crosses correspond o he arge locaions. Black poins are obsacles deeced by he laser range finders. The recangles and circles have been added manually, and represen he obsacles undeeced by he laser scanners. Axis unis are meers. corresponds o he navigaion performed using only he radial laser scanners, which ends up wih he robo bumping ino he cardboard box. On he conrary, when using Kinec (he oher hree pahs), SANCHO manages o negoiae all he obsacles, so reaching he goal successfully. A second experimen has consised of robo SANCHO moving randomly for more han 20 hours (in sessions of around 2 hours) in he environmen represened in he map of figure 0. The reacive navigaion in his case was running as he lower par of a hybrid navigaional sysem wih a opological navigaion on op (Fernández-Madrigal e al., 2004). During all hese sessions SANCHO suffered 5 collisions: hree when passing doorways and 2 when making rapid urns. ACKNOWLEDGEMENTS This work has been suppored by wo projecs: GiraffPlus, funded by EU under conrac FP7 - ICT - #28873, and TAROTH: New developmens oward a robo a home, funded by he Spanish Governmen and he European Regional Developmen Fund ERDF under conrac DPI REFERENCES Biswas, J., Veloso, M., 20. Deph Camera based Localizaion and Navigaion for Indoor Mobile Robos. In RGB-D Workshop a RSS 20. Blanco, J.L., Gonzalez, J., Fernandez-Madrigal, J.A., Exending Obsacle Avoidance Mehods hrough Muliple Parameer-Space Transformaions. In Auonomous Robos, vol. 24 (). Borensein, J., Koren, Y., 989. Real-ime Obsacle Avoidance for Fas Mobile Robos. In IEEE Transacions on Sysems, Man, and Cyberneics, vol. 9, no. 5. Cunha, J., Pedrosa, E., Cruz, C., Neves, A., Lau, N., 20. Using a Deph Camera for Indoor Robo Localizaion and Navigaion. In RGB-D Workshop a RSS 20. Fernández-Madrigal, J.A., Galindo, C., González, J., Assisive navigaion of a roboic wheelchair using a mulihierarchical model of he environmen. In Inegraed Compuer-Aided Engineering vol., no. 4.

8 Figure 0. Topological and meric maps used in he second experimen mapping 6 rooms conneced by a large corridor. Red nodes represen opological locaions and blue lines he possible pahs beween hem. Orders o he robo were o navigae randomly beween nodes during periods of 2 hours. The meric map was used for localizaion purposes. Fiorin P., Shiller, Z., 988. Moion Planning in Dynamic Environmens using Velociy Obsacles. In The Inernaional Journal of Roboics Research, vol. 7, no. 7. Freedman, B., Shpun, A., Machline, M., Ariel Y., 200. Deph Mapping using Projeced Paerns. Paen No.: US 200/0823 A. Garcia, J., Zalevsky, Z., Range Mapping Using Speckle Decorrelaion. Paen No.: US 7,433,024 B2. Gonzalez, J., Galindo, C., Blanco, J.L., Fernandez- Madrigal, J.A., Arevalo, V., Moreno, F.A., SANCHO, a Fair Hos Robo. A Descripion. In IEEE Inernaional Conference on Mecharonics (ICM), Malaga, Spain. Holz, D., Lörken, C., Surmann H., Coninuous 3D Sensing for Navigaion and SLAM in Cluered and Dynamic Environmens. In Inernaional Conference on Informaion Fusion of he (FUSION). Cologne, Germany. Hokuyo: hp:// Kinec: hp:// Khoshelham, K., 20. Accuracy analysis of kinec deph daa. In ISPRS Workshop Laser Scanning, Calgary, Canada. Marder-Eppsein, E., Berger, E., Fooe, T., Gerkey, B., Konolige, K., 200. The Office Marahon: Robus navigaion in an indoor office environmen. In IEEE Inernaional Conference on Roboics and Auomaion (ICRA). Minguez, J., Monano, L., Nearness Diagram (ND) Navigaion: Collision Avoidance in Troublesome Scenarios. In IEEE Transacions on Roboics and Auomaion, vol.20, no.. PrimeSense: hp:// Thrun, S., Learning occupancy Grid Maps wih Forward Sensor models. Auonomous Robos, n. 5.