A new terrain mapping method for mobile robots obstacle negotiation

Transcription

1 A ew errai mappig mehod for mobile robos obsacle egoiaio Cag Ye ad Joha Boresei Advaced Techologies Lab The Uiversiy of Michiga 0 Beal Ave, A Arbor, MI, USA ABSTRACT This paper iroduces a ew errai mappig mehod for mobile robos wih a -D laser ragefider. I he proposed mehod, a elevaio map ad a ceraiy map are buil ad used for he filerig of erroeous daa. The filer, called Ceraiy Assised Spaial (CAS) filer, firs employs he physical cosrais o moio coiuiy ad spaial coiuiy o disiguish corruped pixels (e.g., due o arifacs, radom oise, or he mixed pixels effec) ad missig daa from ucorruped pixels i a elevaio map. I he removes he corruped pixels ad missig daa, while missig daa is filled i by a Weighed Media filer. Ucorruped pixels are lef iac so as o reai edges of objecs. Our exesive idoor ad oudoor mappig experimes demosrae ha he CAS filer has beer performace i erroeous daa reducio ad map deail preservaio ha exisig filers. Keywords: laser ragefider, errai mappig, filerig, elevaio map, ceraiy map, mixed pixels.. INTRODUCTION To auoomously avigae o rugged errai (i.e., idoor eviromes wih debris o he floor or oudoor, off-road eviromes), a mobile robo requires he capabiliy o decide wheher a obsacle should be raversed or circumavigaed. The abiliy o make his decisio ad o apply he appropriae acio is called obsacle egoiaio (ON). A crucial issue ivolved i ON is errai mappig. Research effors o errai mappig have bee devoed o idoor eviromes [], oudoor, off-road errai [, 3, 4], as well as plaeary errai [5, 6, 7]. Mos of he exisig mehods employ sereovisio [, 3, 4, 7], which is sesiive o eviromeal codiio (e.g., ambie illumiaio) ad has low rage resoluio ad accuracy. As a aleraive or suppleme, 3-D Laser Ragefiders (LRFs) have bee employed sice he early ieies [5, 6, 8]. However, 3-D LRFs are very cosly, bulky, ad heavy. Therefore, hey are o suiable for small ad/or expedable robos. Furhermore, mos of hem are desiged for saioary use due o he slow sca speed i elevaio. A more feasible soluio for lower-cos robos is a -D LRF. Researchers a CMU [] used a Sick -D LRF lookig upward o perform idoor 3-D mappig. Due o he upward lookig maer, he 3D map is o suiable for ON sice he groud map is o available i his case. Herick ad Krokov [9] employed a Acuiy -D LRF o compleme sereovisio for obsacle ad hazard deecio. The scaer was poied o he groud a 45. Due o he small lookahead disace, he scaer was used for safey purposes oly, i.e., whe a obsacle is deeced, a emergecy commad is issued o sop he robo. More rece research a CMU [3] also used a -D LRF o compleme sereovisio i a errai mappig applicaio. I his work a Sick LMS 0 was moued o CMU s Nomad robo so as o look diagoally dowward ad forward. As he robo moves forward, he faig laser beam swep he groud ahead of he robo ad produced rage daa of he errai. Based o his daa, he Nomad produced a so called goodess map, which was creaed based o he curre sca daa ad he previous groud level (i.e., a leas square fi lie i he previous sca). The goodess map was he combied wih he map buil by sereovisio for pah plaig. I his work, he overall map was produced maily by sereovisio. This paper preses a ew errai mappig mehod usig a sigle sesor modaliy a -D LRF, ad a iovaive filerig mehod for map buildig. The paper is orgaized as follows: I Secio we iroduce our approach o errai mappig ad we discuss sources of map misrepreseaios. We also ivesigae he problems i applyig coveioal image filers o our errai-mappig problem ad describe he priciple of our filer. I Secio 3 we prese experimeal bechmark ess performed uder corolled codiios, followed by resuls of idoor ad oudoor experimes.

2 Fig.. Experimeal seup for errai mappig sudy: he liear moio able moves he LRF a velociy v (maximum: m/s) ad he roary able ils he LRF a a maximum rae of 37.5 /s i a rage of [-5, 5 ] for roll ad [-40, -5 ] for pich. Addiioal experimes wih he LRF moued o a real mobile robo were also coduced ad are icluded i Secio 3.. MAP BUILDING AND FILTERING ALGORITHM. Map buildig priciple As depiced i Fig., a Sick LMS 00 is moued o our Gorilla vehicle such ha he LRF looks diagoally dowward ad forward (a a pich agle of - ). While he vehicle is i moio, he faig laser beam sweeps he errai ahead of he vehicle ad produce coiuous rage measuremes of he errai. For he experimes described i his paper we se our LRF o a agular resoluio of ad a serial commuicaio speed of 500 Kbaud. Uder hese codiios he LRF produces 8 rage readigs (for is 80 field of view) every 3.3 ms (see [0] for deails). Our algorihm assumes ha real-ime, 6-degree-of-freedom (DOF) pose iformaio is available from our so-called Propriocepive Pose Esimaio (PPE) sysem, which is described i deail i []. Wih PPE available, he rage daa ca be rasformed io world coordiaes ad regisered io a errai map. The errai map cosiss of a elevaio map ad a ceraiy map. Boh are -D gridype maps. Each cell i he elevaio map holds a value ha represes he heigh of cell, while each cell i he ceraiy map holds a value ha expresses our ceraiy i he accuracy of he correspodig cell i he elevaio map. Fig.. Sick LMS 00 o he Gorilla vehicle I order o projec raw rage daa oo he -D elevaio map we use he Euler rasformaio. The forward kiemaic rasformaio is give by: v v l d = Tl d () v v T l T where d = ( x, y, z) ad d = ( xl, yl, zl ) are he rage measuremes i he world coordiae sysem ad i he LRF coordiae sysem, respecively. T l is he forward kiemaic rasformaio marix, which rasforms he coordiae value of a laser rage measureme i he LRF coordiae sysem o he coordiae value i he world coordiae. For cociseess, we omi he derivaio of he rasformaio marix here. To verify his idea ad sudy he mappig characerisics, we use he experimeal seup show i Fig.. The LRF is moued o a roary able, which produces roll ad pich roaio, ad he roary able is moued o a liear moio able. This experimeal seup effecively emulaes may of he moio-iduced disorios ecouered by a vehicle raversig rugged errai. The coordiae sysems for he experimeal seup are depiced i Fig. 3. The world coordiae sysem is deoed x y z. The coordiae sysem aached o he roary able is deoed x b y b z b wih he x b coordiae beig aliged wih he pich roaio axis. y b ad z b are parallel o y ad z, respecively whe he pich agle is zero. Aoher coordiae sysem, deoed x l y l z l, is aached o he roary able so ha y l is aliged wih he roll axis ad x l ad z l are parallel o x b ad z b whe he roll agle is zero. The LRF coordiae sysem is deoed x s y s z s wih z s aliged wih z l ad x s ad y s i parallel wih x l ad y l, respecively. I view of he -degree sca resoluio ad he 80-degree field of view of he LRF, he LRF ca be hough of as havig 8 discree laser beams i x s o 3 y s, wih beam # aliged wih coordiae x s, beam #9 aimed wih y s, ad beam #8 aliged wih -x s. The coordiae values of o 3 i x l y l z l, o i x b y b z b ad o i x y z are (0, 0, p), (, m, q) ad (u, v, w), respecively. The forward kiemaic rasformaio is give by x y z lcφcβ + psφ + + u = lcθsβ + lsθsφcβ qsθ + mcθ psθcφ + v lsθsβ lcθsφcβ + qcθ + msθ + pcθcφ + w ()

3 where sx sads for si(x ) ad cx sads for cos(x ). β is he agle bewee he k h laser beam (which akes rage z measureme l) ad he x s axis ad is give by z b z β = ( k ) π /80 (3) l m For every wo cosecuive measuremes a ime seps ad +, he o q z s φ y maximum chage of heigh value z for he measureme a (x, y ) is l x l o 3 calculaed by y s z θ x s laser z z β z max = ygθ + δl + θ + φ (4) o l θ φ x b y b w o y where y is he displaceme of he LRF alog he y axis, δ l is he u maximum measureme error of he LRF (3 mm accordig o [0]), x v z / l, z / θ ad z / φ are parial derivaives ad ca be derived Fig. 3. Coordiae sysems for he experime from Eq.. I his paper a grid size of 5 mm by 5 mm is used for boh he elevaio map ad he ceraiy map. A cell i he elevaio map is deoed h(i, while a cell i he ceraiy map is deoed c(i, which is a posiive ieger smaller ha 56. Usig he above rasformaio he coordiae values x ad y are compued for each rage measureme a ime ad mapped o he grid idex i ad j, respecively. The he pixel c(i, i he ceraiy map is updaed as follows: + c (i, + 3 if z h z or (, ) = 0 + max c i j c = (5) c oherwise where he icreme of 3 is used o icrease he ceraiy value faser; ad he pixel h(i, i he elevaio map is updaed by + + z if z > h + h = (6) h oherwise I every 3.3-millisecod ierval he 8 rage measuremes acquired from he LRF are mapped io boh maps.. Sources of mappig errors Ideally, a errai map buil accordig o he above-described algorihm should be sufficie for avigaig a mobile robo. However, some rage daa from he Sick LMS 00 are erroeous ad will cause mappig errors. The pheomeo of mixed pixels, missig daa, arifacs ad oise are he mai sources of erroeous measuremes:. Mixed pixels occur whe a laser beam his he very edge of a objec so ha par of he beam is refleced from ha edge of he objec ad par of he beam is refleced from he backgroud behid he objec. The resulig rage measureme lies somewhere bewee he disace o he objec ad he disace o he backgroud [0]. I researchig he mixed pixels pheomeo we foud ha if he disace D bewee he edge of he foregroud objec ad he backgroud is close o he laser pulse widh, L ( L m for he Sick LRF ), he a subsaial umber of mixed pixels is geeraed. However, if D > L, he he umber of mixed pixels drops sigificaly. This is because he Sick LRF is desiged o accep as valid readigs oly reflecios semmig from he same pulse. This smar desig feaure is very effecive i rejecig mos ambie oise, alhough i does o elimiae all. Base o he commuicaio wih Sick Germay, he LRF is a sigle sho measureme sysem. I seds ou a pulse wih a widh close o m ad deecs he reflecio wih he same pulse widh. A a corer sho his deecig scheme allows he receiver receive reflecio from he edge ad he backgroud (wihi he meer pulse widh legh). Therefore, resul i mixed pixels. I is obvious ha due o he shor pulse widh, mixed pixels problem i he SICK LRF is much less proouced ha LRFs based o phase shif measureme echique. Ierally, he LRF uses he eergy of he received sigal o correc he raw rage measureme. This correcive scheme may cause addiioal rage error o some daa alhough i improves he accuracy of some oher daa. Because he received sigal level is deermied by rage, icidece agle ad surface refleciviy hece ca be prediced.

4 . Missig daa occur whe he measured rage is ivalid []. For isace, a refleced sigal may be o weak for deecio due o a large icidece agle ad/or due o low diffuse refleciviy of he reflecig objec. This may cause SNR oo low error. Daa may also be missig if a laser beam is rapped ad hus o refleced back o he LRF. This codiio resuls i a operaioal overflow error i he Sick LRF. Direc exposure o he suligh or similar ligh source may lead o dazzlig ad cause ivalid readigs a cerai agles. For a complee lisig of Sick s error codiios see []. 3. Eviromeal ierfereces, such as srog ambie ligh, ad shock durig moio [3] may poeially creae oisy rage measuremes ad hece resul i oise i he elevaio map. Fig. 4 shows he effecs of hese errors. Such errors i he errai map deeriorae he ON performace of he vehicle. Fig. 4. Map misrepreseaio due o rage errors: Lef: Mixed pixels ca creae phaom objecs behid he edges of objecs. Missig daa creaes empy pixels i he upper surfaces of objecs. Righ: Eviromeal ierfereces ca cause arifacs/oise. Ambie ligh ad/or shock from moio ca creae radom oise. I he 6-meer measurig mode of he Sick LRF, we observed ha here are a lo of high spikes (labeled arifacs ) i he ceer of his elevaio map, which was buil o a cerai sidewalk ear our lab. Close examiaio revealed ha mos of hese arifacs resuled from very small rage measuremes (less ha he 0 millimeer a he exreme lef ad righ ed of he Sick s 8-degree sca rage i his experime.). We were uable o deermie wha causes hese misreadigs, bu we suspec udeeced overflow o be he problem. However, i is easy eough o delee rage readigs ha are smaller ha he 0-millimeer miimum rage before hey are used for map buildig. The oher high spikes are he resul of radom oise i rage daa, which produces rage errors a several meers ahead of he LRF. (i.e., o likely o be mixed pixels)..3 Coveioal image filerig algorihms A elevaio map ca be rasformed io a rage image [4] if he heigh value of each pixel is reaed as iesiy. This way coveioal image processig filers [5, 6, 7] ca be applied o he elevaio map i a aemp o elimiae he above-discussed erroeous daa. However, he applicaio of coveioal image filerig echiques is o wihou drawbacks. The foremos reaso is ha mos coveioal image filerig mehods are applied ucodiioally across he eire rage image ad may hus blur he image (i.e., aler he value of acually correc pixels). Addiioal drawbacks exis, ad i he followig we discus idividual image filers ad heir applicabiliy o elevaio maps. The Frequecy Filer [7] (e.g., Averagig Filer, Gaussia Filer [6]) are implemeed by he covoluio of a ipu image wih a covoluio kerel i he spaial domai, i.e., each pixel i he filered image is replaced by he weighed sum of he pixels i he filer widow. The effec is ha oise is suppressed bu he edges of feaures i he image are blurred a he same ime. The Wieer Filer [7] is more effecive i oise reducio, bu i requires ha he power specrum of he oise be kow i advace ad ha he oise be idepede of he sigal. However, his requireme is o me i he case of our elevaio map where oise is highly correlaed wih he sigal, for all hree error sources. Specifically, (a) mixed pixels are always locaed behid objecs edges; (b) missig daa occur mosly whe he laser beam illumiaes a surface wih low diffuse refleciviy or a a large icidece agle. Furhermore, he Wieer Filer also eds o blurr he image sice i is based o a Fourier rasform, which is implemeed as a covoluio i he spaial domai.

5 Media filers are well kow for heir capabiliy of removig impulse oise ad preservig image edges, ad are repored o have good performace i removig mixed pixels i laser rage images [5]. The sadard Media Filer ad is variaios (Weighed Media Filer [8] ad Ceer Weighed Media Filer [9]) ofe exhibi blurrig whe a large filer widow is used, or isufficie oise reducio for a small filer widow. For isace, hi edges (because hi objecs are prese or objecs are oo high such ha oly he fro edges are sesed) i a elevaio map may be compleely removed by a sadard Media filer. Adapive Media Filers [0, ] maiai a beer balace bewee deail preservaio ad oise reducio, ad hece achieve beer performace. However, all of hese media-ype filers affec all he pixels i a image icludig ucorruped oes. A umber of media ype filers [, 3] i he lieraure may poeially disiguish corruped pixels from ucorruped oes ad apply filerig oly o he corruped pixels. These filers uilize local ipu image characerisics (spaial iformaio oly) o ideify corruped pixels. However, a limiaio of his approach is he poeeial misclassificaio of hi edges as corruped pixels ad he subseque removal of hese pixels. For isace, a hi pole i a rage image produces a lie of high iesiy pixels, which may be misierpreed by he filer as impulse oise ad removed. This suggess ha i is isufficie o use oly spaial iformaio for he ideificaio of corruped pixels..4 The Ceraiy Assised Spaial (CAS) Filer I his paper we propose a ovel filer for elevaio maps, called Ceraiy Assised Spaial (CAS) filer. This filer uilizes o oly he spaial iformaio coaied i he ufilered elevaio map, bu also he ceraiy iformaio coaied i he ceraiy map, as explaied i Secio.. Due o he coiuiy of moio, every wo cosecuive measuremes o he edge of a real objec saisfy + z h zmax. Accordig o Eq. 5, edge pixels are assiged coiuous ceraiy icremes ad resul i large ceraiy values i he ceraiy map. Therefore, edge pixels i he elevaio map ca be ideified easily. They are preserved ad o filerig is applied. Mixed pixels may occur somewhere bewee he edges of objecs ad he backgroud. Their locaios are deermied by he perceage of laser spos hiig he backgroud [0]. Whe he LRF is i moio, as i ecessarily is for mappig, his perceage chages all he ime. Cosequely mixed pixels keep + chagig heir x ad y coordiae all he ime. This meas ha he codiio z h zmax is o saisfied. Hece, mixed pixels usually have small ceraiy value ad are spaially isolaed i boh he elevaio map ad he ceraiy map. They ca be ideified ad removed easily. Arifacs ad radom oise have he same characerisics ad ca also be removed by he same priciple. Fially, missig daa creae empy pixels i a elevaio map ad heir ceraiy values are zero. We foud ha applyig a Weighed Media Filer o hose pixels wih zero ceraiy values ca supply (ierpolae) he missig daa. The filerig process works as follows: Le H={h(i, i M, j N } ad C={c(i, i M, j N } deoe he elevaio map ad he ceraiy map of a errai map, respecively, where M is he legh ad N is he widh of he map. A filer wih a widow size of (k+) (k+) (where k is a oegaive umber; i his paper k= for a widow size of 5 5) is defied symmerically surroudig he curre pixel h(i, H. The curre pixel h(i, is updaed by he oupu of he filer which is give by 0 if c( i, A ad g s = 0 ad g c = 0 y = y wm else if g s = ad c( i, = 0 (7) h( i, oherwise ad A = 3 (i( s / v ) + ) (8) where i( ) meas roud o he eares ieger, s is he grid size, v is he maximum liear velociy of he LRF which is m/s i his paper ad =3.3 ms is a ime sep. g s ad g c are he spaial coiuiy idex of H ad C for he pixels i he filer widow, respecively. They are expressed by 0 if σ > B h gs = (9) oherwise

6 ad i+ k, j + k 0 if c(m,) < D or σc(i, > E gc = m= i-k, = j k (0) oherwise h( m, ) where σ ad σ h c are he variace of he ormalized local map H L = i k m i + k, j k j + k max( h( m, )) c( m, ) ad C L = i k m i + k, j k j + k respecively. We use 0 A for D ad 0.04 for E. To keep max( h( m, )) i k, j + + k he compuaioal cos low we use H < 7 o approximae he codiio σ B i Eq. 9. y wm is he L m= i k, = j k h > oupu of a Weighed Media (WM) filer ceered a pixels (i,. The ipu pixels o he WM filer are obaied by applyig a filer skeleo [8] (as depiced i Fig. 5) o he elevaio map. For j- j- j j+ j+ isace, h(i, has oe copy while h(i-, j-) has wo copies i he ipu pixels o i- he WM filer. The use of his skeleo is based o he followig heurisic: Sice i- we suspec h(i, o be a missig daum, he eares eighborig pixels h(i±, i j±) are also likely o be missig daa, while hose pixels farher from h(i, have i+ a greaer likelihood o o be missig daa. Noe, however, ha his weigh assigme for he WM filer poeially removes more missig daa o he upper i+ surface of a objec. Fig. 5. Filer skeleo for he WM filer Whe he CAS filer is applied o he raw elevaio map, mixed pixels, arifacs, ad radom oise are ideified by examiig he moio coiuiy ( c cosraied by Eq. 5) ad he spaial coiuiy i he elevaio map ad he ceraiy map ( g s ad g c ( i, ). Missig daa is disiguished by ispecig he curre pixel s ceraiy value c ad he spaial coiuiy g s i he elevaio map. Mixed pixels, arifacs, ad radom oise are herefore removed ad he missig daa are ierpolaed by he WM filer. 3. EXPERIMENTAL RESULTS I his paper we prese exesive experimeal resuls. These resuls ca be grouped io wo caegories: Bechmark ess performed uder highly corolled codiios, ad mobiliy ess i which he LRF was moued o a mobile plaform. I all of he experimes, we used a. GHz AMD Ahlo processor-based PC ruig RT-Liux for he realime daa collecio. 3. Bechmark Tess Based o he experimeal seup i Fig., we desiged he followig four bechmark ess: ) The LRF raslaes oly (deoed T ); ) he LRF raslaes ad roaes wih roll oly (deoed TR ); 3) he LRF raslaes ad roaes wih pich oly (deoed TP); ad 4) he LRF raslaes ad roaes wih roll ad pich (deoed TRP). For he experimes described here we buil 6 obsacle courses as lised i Table I. The performace of our CAS Filer is compared wih ha of he Wieer filer, he Averagig Filer, he Media Filer, ad he Ceer Weighed Media (CWM) Filer. Fig. 6 shows he obsacle course ad Fig. 6. Obsacle course for bechmark ess Fig.7 depics he raw ad filered elevaio maps based o bechmark es T wih differe filers. I ca be observed ha he CAS filer preserves more origial pixels, removes mos of he mixed pixels, ad fill almos all of he missig daa. The Wieer Filer reais almos all missig daa, ad some mixed pixels bu i oly smoohes some oher mixed pixels. The Media Filer ad he CWM filer over-filer he poles while he Averagig Filer disor he map sigificaly.

7 TABLE I FEATURES OF THE 6 OBSTACLE COURSES Obsacle Parallelepiped Cylider Poles layou 3 4 X X -5, 7, 9 X -7, 9 3-7, 9 4 X X X,, 4, 6 5 X X -4, 6 6 X X X -4, 6 7 X X X -7 8 X X X -9 9 X X -9 0 X X X -5, 7, 9 X X X -5, 7, 9 Dimesios of parallelepiped objecs (all uis i mm): #: , #: , #3: , #4: ; Dimesios of poles: #-#9 have diameers of 4,, 9, 5,, 9, 6, 4.5 ad.5, respecively ad a legh of 94; Dimesio of cylider: diameer=4, heigh= 459 (a) Origial (raw) elevaio map (b) Elevaio map afer applyig he CAS Filer (c) Elevaio map afer applyig he Wieer Filer (d) Elevaio map afer applyig he Averagig Filer (e) Elevaio map afer applyig he Media Filer (f) Elevaio map afer applyig he CWM filer Fig. 7. Origial elevaio map ad he maps afer applyig he five esed filers I order o compare he filer performace i a quaiaive way, we measured by had he exac locaio of each obsacle i each oe of he obsacle course ad covered hese measuremes io a groud ruh elevaio map. We he compared he accuracy of each filered map agais he groud ruh map by compuig a overall idex of performace (PI) accordig o PI N M ( hf h ) j= i= = N M ( hr h ) j= i= ()

8 where h r (i,, h (i, ad h f (i, are he elevaio values of pixel (i, i he raw elevaio map, he groud ruh map, ad he filered map respecively. We arbirarily seleced M=80 ad J=00 (which has full coverage of all obsacle courses) o evaluae he experimes here. Accordig o Eq., a smaller PI meas beer filer performace. For each of he obsacle courses we performed four differe ess, amely, he T, TR, TP, ad he TRP es, as described i he begiig of his secio. The resuls are summarized i Fig. 8. I is evide from hese resuls ha our proposed CAS Filer cosisely ouperformed he four coveioal filers. The relaively poor performace of he Media Filer ad he CWM Filer is due o heir ihere over-filerig. O he oher had, whe roll ad pich moio was added o he raslaory moio, he CAS Filer performed oly margially beer ha he Wieer Filer (ad he Averagig Filer i some isaces). This is due o he followig observaios: () Roll ad/or pich moio poeially icreases mappig errors. We oiced he followig source of mappig errors: (a) Roll caused vibraios i he roary able i some posiios of he liear moio able. (b) Boh roll ad pich chaged occlusios ad he sesed heighs of he poses. This resuled i error of he absolue elevaio of he pose i he map. However, i realiy we use he physical heighs (94 mm) ad he physical cross secio areas of he poses o build he rue map. The chages i occlusio ad i he sesed heigh icrease he mappig error. Accordig o Eq., hese addiioal errors add he same value o boh he umeraors ad he deomiaors of all filer PIs. Therefore, i moves he CAS filer s PI close o ha of he Wieer ad Averagig filer as i is appare ha he relaive differece of wo fracios is reduced by addig he same addiioal value o boh umeraors ad deomiaors. 3.5 Wieer Average Media CWM CAS 3.5 Wieer Average Media CWM CAS PI.5 PI Obsacle cour se Obsacle cour se (a) Traslaio oly (b) Traslaio wih pich PI Wieer Average Media CWM CAS Obsacle course (c) Traslaio wih roll.5.5 PI Wieer Average Media CWM CAS Obsacle course (d) Traslaio wih roll & pich Fig. 8. Compariso of filerig performace () Roll ad pich moio may icrease he amou of empy pixels o he objecs upper surfaces because hey icrease he LRF s sweepig velociy whe hey are i he same direcio wih he liear able s moio. I case ha he displaceme of he laser spo (hiig he objecs upper surface) i oe ime ierval exceed 5 mm (he size of oe cell), a empy pixel will resul. Also, roll ad pich moio may icrease he icidece agle of he laser beam ad hece resul i missig daa which causes empy pixels. Whe he umber of empy pixels i he filer widow is above a cerai value, he CAS filer removes he curre pixel while he Wieer Filer or he Averagig Filer replace he curre pixel wih a spaial covoluio value. I he TRP es for he 5 h obsacle course (see Fig. 9(d)), he performace of he CAS Filer is slighly worse ha ha of he Wieer Filer because oe parallelepiped objec is so far away from he liear moio able i laeral direcio ha i creaes a large amou of missig daa o he op surface. As a resul he CAS Filer compleely removes par of he

9 surface. However, we oiced ha his oly happes whe a objec is laerally far from he LRF s pah of moio. I pracice, laerally disa objecs have lile sigificaces i obsacle egoiaio (ON) asks. Oe impora feaure of he CAS Filer is ha i preserves he edges of objecs i he elevaio map, ad edges are he key feaure for ON. We should also meio i his discussio ha, despie heir relaively good PI performace, he Wieer Filer ad Averagig Filer are o very suiable for ON. This is because he Wieer Filer does o remove arifacs/oise i elevaio maps (see Secio 3.) while he Averagig Filer eds o lower he heigh of edges i he elevaio maps a highly udesirable rai i ON applicaios. 3. Experimeal resuls o a mobile robo We carried ou a umber of idoor ad oudoor map buildig experimes wih he LRF o our Gorilla vehicle (see Fig. ). However, hese experimes were limied o sraigh lie moio o fla, horizoal groud. More diverse errai would require real-ime pose esimaio, a feaure o ye implemeed i our mobile robo. Fig. 9 shows he obsacle courses ad he vehicle i our idoor ad oudoor mappig experimes. From he maps depiced i Fig. 0, i is appare ha he CAS Filer ouperforms he Wieer Filer because i almos compleely removes mixed pixels ad fills mos of he missig daa o he op surfaces of objecs (see Fig. 0(b)). I coras, he Wieer Filer leaves some oiceable mixed pixels ad mos of he missig daa i he map, as show i Fig. 0(c). I he oudoor mappig experime (see Fig. ), he CAS Filer compleely removed all arifacs ad radom oise while he Wieer Filer did o remove ay. I is worhwhile o meio ha arifacs ca be removed by a hreshold filer ha removes readigs smaller ha he Sick s miimum rage of 0 mm. However we le hem pass hrough o our filer because Fig. 9. Obsacle courses for mappig: lef: idoor, righ: oudoor (a) Origial (raw) elevaio map (b) Elevaio map afer applyig he CAS Filer (c) Elevaio map afer applyig he Wieer Filer Fig. 0. Idoor elevaio maps (a) Origial (raw) elevaio map (b) Elevaio map afer applyig he CAS Filer (c) Elevaio map afer applyig he Wieer Filer Fig.. Oudoor elevaio maps

10 we wa o es our filer s effeciveess uder uexpeced oudoor facors. I is well kow i image processig ha radom oises (e.g., Gaussia oise) are used o ieioally corrup images, which are he used o es a image filer s oise suppressio performace. I our case, his does work because we eed ceraiy value of each pixel, i.e., oly rue experime ca be used o es our filer. The arifacs ad oise serve for his purpose. 4. DISCUSSION We are aware ha here is a aleraive o filerig mixed pixels. Adams [4] proposed a efficie mixed pixels removal mehod for phase shif LRFs. The algorihm is based o he deecio of discoiuiy of he received sigal s ampliude. However, due o he differece of measurig priciples (ime-of-fligh i he case of he Sick LRF), his mehod mus be modified o sui he Sick LRF. Such modificaio requires kowledge of he ieral physics ad he hardware desig of he LRF, ad ca hus be implemeed effecively oly by he maufacurer, o he user. Specifically, Adam s discoiuiy deecio algorihm requires several samples of he received sigal (for compuig he secod derivaive of he square of he sigal ampliude wih respec o he area illumiaed by he laser o he edge) durig he ime he laser beam raverses he edge. However, a user level we ca oly ge a sigle measureme o he edge. This meas i is impracical o impleme his algorihm. Oe migh argue ha if he Sick LRF se ou boh refleciviy iformaio ad rage daa coiuously, he similar mehod could be implemeed ouside of he Sick LRF. However, i his case he 500 Kbaud badwidh limiaio of he Sick LRF would be exceeded. This, i ur, would be force elimiaio of some rage daa a udesirable opio i mappig applicaios. Fially, our mehod is based o he characerisics of he erroeous daa ad moio coiuiy cosrai. I is device idepede. Therefore, i is more geeral ad suiable for he ed users of LRFs. Differig from Adams algorihm, our approach removes o oly mixed pixels bu also uexpeced oudoor arifacs ad oise. 5. CONCLUSIONS I his paper we preseed a ovel filerig algorihm for errai mappig wih a -D LRF. The errai map cosiss of a elevaio map ad a ceraiy map. The value of each pixel i he ceraiy map is deermied as a fucio of a socalled moio coiuiy cosrai. Typical erroeous daa, such as mixed pixels, arifacs, ad radom oise do saisfy he moio coiuiy cosrai ad hus resul i a low ceraiy value. Examiaio of he appropriae pixels i he ceraiy map ad examiaio of he spaial coiuiy i he elevaio map allows our CAS Filer o ideify ad remove erroeous pixels. Missig daa, such as ha caused by large icidece agles ad low diffuse reflecio, ca be ideified because he affeced pixels are associaed wih ceraiy values of zero ad wih poor spaial coiuiy. Oce ideified, a WM filer ca fill i he missig daa. Edges of objecs are always associaed wih relaively large ceraiy values. As a resul our CAS Filer leaves pixels represeig objec edges iac. Overall, our proposed CAS Filer produces cosisely beer resuls ha coveioal filer algorihms because i makes use of he addiioal iformaio available i he Ceraiy Map. ACKNOWLEDGMENT This work was fuded by he U.S. Deparme of Eergy uder Award No. DE-FG04-86NE3796 ad uder a gra from The Uiversiy of Michiga s Auomoive Research Ceer (ARC), fuded by TACOM. The auhors would like o hak Mr. Weber, Sick Germay for his kid help ad isighful kowledge o he LRF. The auhors would also like o hak James Berry for his work o he desig of he roary able. REFERENCES S. Thru, W. Burgard ad D. Fox, A real-ime algorihm for mobile robo mappig wih applicaio o muli-robo ad 3D mappig, Proc. IEEE Ieraioal Coferece o Roboics ad Auomaio, 000, pp

11 S. Begé-Brezez, e al, Ucerai map makig i aural eviromes, Proc. IEEE Ieraioal Coferece o Roboics ad Auomaio, 996, pp D. S. Aposolopoulos, e al, Techology ad field demosraio of roboic search for Aarcic meeories, Ieraioal Joural of Roboics Research, vol. 9, o., pp , K. Fregee, R. Madhava ad L. E. Parker, Icremeal muliage roboic mappig of oudoor errai, Proc. IEEE Ieraioal Coferece o Roboics ad Auomaio, 00, pp I. S. Kweo ad Takeo Kaade, High-resoluio errai map from muliple sesor daa, IEEE Trasacios o Paer Aalysis ad Machie Ielligece, vol. 4, o., pp. 78-9, E. Krokov ad R. Hoffma, Terrai mappig for a walkig plaeary rover, IEEE Trasacios o Roboics ad Auomaio, vol. 0, o. 6, pp , R. Simmos, e al, Experiece wih rover avigaio for luar-like errais, Proc. IEEE/RSJ Ieraioal Coferece o Iellige Robos ad Sysem, 995, pp C. M. Shoemaker ad J. A. Borsei, The demo III UGV program: a esbed for auoomous avigaio research, Proc. IEEE ISIS/CIRA/ISAS Joi Coferece, 998, pp Herikse L. ad Krokov E., Naural Terrai Hazard Deecio wih a Laser Ragefider. Proc. IEEE I. Cof. O Roboics ad Auomaio, 997, pp C. Ye ad J. Boresei, Characerizaio of a -D laser scaer for mobile robo obsacle egoiaio, Proc. IEEE Ieraioal Coferece o Roboics ad Auomaio, 00, pp L. Ojeda ad J. Boresei, "FLEXav: Fuzzy Logic Exper Rule-based Posiio Esimaio for Mobile Robos o Rugged Terrai," Proc. IEEE Ieraioal Coferece o Roboics ad Auomaio, 00, pp Sick Ic., LMS/LMI Telegram Lisig, versio 05.00, page A. Berma, J. Daya ad B. Friedlad, Improved EKF mehod of esimaig locaios wih sudde high jumps i he measureme oise, Joural of Iellige ad Roboics Sysems, vol. 3, o. 4, pp , P. J. Besl, Surfaces i rage image udersadig, Spriger-Verlag, M. Heber ad E. Krokov, 3D measuremes from imagig laser radars: how good are hey? Image ad Visio Compuig, vol. 0, o. 3, pp ,99. 6 C. F. Olso, Adapive-scale filerig ad feaurig deecio usig rage daa, IEEE Trasacios o Paer Aalysis ad Machie Ielligece, vol., o. 9, A. K. Jai, Fudameals of Digial Image Processig, Preice Hall, D. R. K. Browrigg, The weighed media filer, Commuicaio of he ACM, vol. 7, o. 8, pp , S.-J. Ko ad S.-J. Lee, Ceer weighed media filers ad heir applicaios o image ehaceme, IEEE Trasacios o Circuis ad Sysems, vol. 38, o. 9, pp , H.-M. Li ad A. N. Willso, Jr., Media filers wih adapive legh, IEEE Trasacios o Circuis ad Sysems, vol. 35, o. 6, 988. T. Che ad H. R. Wu, Applicaio of pariio-based media ype filers for suppressig oise i images, IEEE Trasacios o Image Processig, vol. 0, o. 6, pp , 00. H.-L. Eg ad K.-K. Ma, Noise adapive sof-swichig media filer, IEEE Trasacios o image processig, vol. 0, o., pp. 4-5, T. Che, K.-K. Ma ad L.-H. Che, Tri-sae media filer for image deoisig, IEEE Trasacios o Image Processig, vol. 8, o., pp , M. D. Adams ad P. J. Prober, The ierpreaio of phase ad iesiy daa from AMCW ligh deecio sesors for reliable ragig, The Ieraioal Joural of Roboics Research, vol. 5, No. 5, pp , 996.