A Novel Filter for Terrain Mapping With Laser Rangefinders

Transcription

1 Published i IEEE Trasacios o Roboics ad Auomaio, Vol., No. 5, Oc. 4, pp A Novel Filer for Terrai Mappig Wih Laser Ragefiders Cag Ye, Member, IEEE ad Joha Borese Member, IEEE Absrac This paper iroduces a ovel filer for errai mappig wih a -D laser ragefider. The filer, called Ceraiy Assised Spaial (CAS) filer, uses he physical cosrais o moio coiuiy ad spaial coiuiy o ideify corruped pixels ad missig daa i a elevaio map. The filer removes he corruped pixels, fills i he missig daa ad leaves he ucorruped pixels iac so as o preserve he deails of a errai map. Our exesive idoor ad oudoor mappig experimes show he CAS Filer s superior performace i erroeous daa reducio ad map deail preservaio over coveioal filers. Idex Terms laser ragefider, errai mappig, filerig, elevaio map, ceraiy map, mixed pixels. I. INTRODUCTION Auoomous avigaio of mobile robos o rugged errai requires he capabiliy o decide wheher a obsacle should be raversed or circumavigaed. The abiliy o make his decisio ad o acually execue i 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, 7], which is sesiive o eviromeal codiios (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 usually 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 frame rae. 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. The 3-D map is o suiable for ON sice o groud map is produced. Herikse ad Krokov [9] employed a Acuiy -D LRF o compleme sereovisio for obsacle ad hazard deecio. Due o he small look-ahead disace, he scaer was used for safey purposes oly (o sop he robo i case of a emergecy). More rece research a CMU [3] used a SICK LMS (wih a rage up o 3 m ad a field of view of ) o he Nomad robo. The LRF looks diagoally dowward ad forward. As he robo moves forward, he faig laser beam swep he groud ahead of he robo ad produced errai daa. Based o he daa, a so called goodess map, was creaed by comparig he curre sca daa wih he previous groud level (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 II, we iroduce our approach o errai mappig. I Secio III, we prese experimeal resuls ad discuss he compuaioal cos of he filer. I Secio IV, we compare our filer o oher relaed mixed pixel removal mehods ad discuss he real-ime implemeaio issues. II. MAP BUILDING AND FILTERING ALGORITHM A. Map buildig priciple As depiced i Fig., a SICK LMS is moued o our Gorilla vehicle such ha he LRF looks diagoally forward ad dowward a a pich agle of -. While he vehicle is i moio, he faig laser beam sweeps he errai ahead of he vehicle ad produces 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 5 Kbaud. Uder hese codiios, he LRF produces 8 rage readigs for is 8- degree field of view every 3.3 ms (see [] for deails). Our algorihm assumes ha real-ime, 6-degree-of-freedom pose iformaio is available from our so-called Propriocepive Pose Esimaio (PPE) sysem, which is described i deail i []. Wih he pose iformaio 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 grid-ype maps. Each cell i he elevaio map holds a value ha represes he heigh of he objec a ha Fig.. SICK LMS o he Gorilla vehicle 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. 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. For cociseess, we omi he derivaio of he rasformaio marix here. To sudy he mappig characerisics uder well-corolled codiios, we use he experimeal seup show i he lef of Fig.. The LRF is moued o a roary able, which produces roll ad pich roaio. The liear moio able x z o x b v θ q w o z b u m x l x s z l o r z s φ y l o3 y s laser β y b y Fig.. Experimeal seup ad he coordiae sysems for errai mappig sudy: he liear moio able moves he LRF a velociy v (max.: 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 [-4, -5 ] for pich. produces liear moio for he roary able. This experimeal seup emulaes o some exe he moio of a mobile robo o rugged errai. The coordiae sysems for he experimeal seup are depiced i he righ of Fig.. They cosiss of he world coordiae sysem x y z, he coordiae sysems aached o he roary able x b y b z b ad x l y l z,, ad he LRF coordiae sysem x s y s z 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 (,, p), (, m, q), ad (u, v, w), respecively. The forward kiemaic rasformaio is give by x y z lcφcβ + psφ + r + u = lcθsβ + lsθsφcβ qsθ + mcθ psθcφ + v lsθsβ lcθsφcβ + qcθ + msθ + pcθcφ + w where sx sads for si(x) ad cx sads for cos(x). β is he agle bewee he k h laser beam (which produces rage measureme l) ad he x s -axis ad is give by β = ( k ) π /8 (3) For every wo cosecuive measuremes a ime seps ad, he maximum chage of heigh value z for he measureme a (x, y ) is calculaed by z z z zmax = y aθ + δl + θ + φ (4) l θ φ ()

2 where y is he displaceme of he LRF alog he y axis, δ l is he maximum measureme error of he LRF (3 mm accordig o []), z / l, z / θ, ad z / φ are parial derivaives ha ca be derived from Eq.. I his paper, a grid cell size of 5 5 mm is used such ha each cell may be illumiaed oe ime eve if he vehicle moves a he maximum speed of m/s. A cell i he elevaio map is deoed h(, while a cell i he ceraiy map is deoed c(. Usig he above rasformaio he coordiae values x ad y are compued for each rage measureme a ime ad mapped o he grid idices i ad j, respecively. The, pixel c( i he ceraiy map is updaed as follows: c ( + a if z h ( z or (, ) + max c i j = = c ( (5) c ( oherwise where a is he icreme of he ceraiy value ad i ca be ay posiive ieger (we use 3 i his sudy). The pixel h( 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. B. 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 are erroeous ad will cause mappig errors. The pheomeo of mixed pixels, missig daa, ad arifacs/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 []. 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 legh of he laser pulse, L ( L m for he SICK LRF ), a subsaial umber of mixed pixels are geeraed. However, if D > L, he umber of mixed pixels drops sigificaly (see Fig. 3). This is because he SICK LRF is desiged o accep oly reflecios semmig from he same pulse as valid daa. This smar desig is effecive i rejecig ambie oise. Fig. 3 shows ha mixed pixels exis eve if D is as big as.6 meer. Aoher characerisic of mixed pixels is ha a small chage i a edge s laeral posiio wih referece o he laser beam may iduce a large chage i he measured rage. () Missig daa occur whe he measured rage is ivalid []. For isace, o reur or oo weak a reured sigal may resul i missig daa; direc exposure o he suligh may lead o dazzlig ad cause ivalid readigs. For a complee lis of he SICK LRF s error codiios see []. (3) Eviromeal ierfereces, such as ambie ligh ad shock durig moio, may poeially creae oisy rage measuremes ad hece resul i oise i he elevaio map. The aalysis of LRF oise is reaed i [3]. Fig. 4 shows he effecs of hese errors. Such errors i he errai map deeriorae he ON performace of he vehicle. C. Coveioal image filerig algorihms A elevaio map ca be rasformed io a rage image, i which he heigh value of each pixel is represeed by iesiy. This way coveioal image processig filers [4, 5, 6] 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 ucodiioally applied o he eire rage image ad may hus blur he image. Covoluio-based frequecy filers, such as he Averagig Filer, Gaussia Filer [5]), or Wieer Filer [6], are implemeed Base o our commuicaio wih SICK i Germay, he LRF is a sigle sho measureme sysem. I seds ou a pulse wih a widh close o m ad expecs a refleced sigal wih he same pulse widh; received sigals of oher pulse widhs are rejeced. A a corer sho, he LRF may receive reflecio from boh he edge ad he backgroud if he disace bewee hem is wihi he pulse widh. This resuls i mixed pixels. by he covoluio of a ipu image wih a covoluio kerel i he spaial domai. The effec is ha oise is suppressed bu he objecs i he image are Fig. 3. Characerisics of mixed pixels: 4 rage measuremes were ake a each poi. A measureme is deermied as a mixed pixel if he differece bewee he measured rage ad he foregroud/backgroud is bigger ha 6 mm. Large umbers of mixed pixels are geeraed whe he disace D bewee he edge of he foregroud objec ad he backgroud is close o he legh of a pulse of he laser, L. However, if D > L, he he umber of mixed pixels drops sigificaly. (a) Mixed pixels ca creae phaom objecs behid he edges of objecs. Missig daa creaes empy pixels i he upper surfaces of objecs. (b) Eviromeal ierfereces ca cause arifacs/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 errai map, which was buil o a fla sidewalk ear our lab. Close examiaio revealed ha mos of hese arifacs resuled from very small rage measuremes, which may be removed by hreshold. We suspec ha udeeced overflow is he cause. The oher high spikes are he resul of radom oise i rage daa, which produces rage errors a several meers ahead of he LRF. Fig. 4. Map misrepreseaio due o rage errors blurred a he same ime. Filers based o Aisoropic Diffusio [7] diffuse more i smooh areas ad less aroud large iesiy rasiios, hus small variaios i image iesiy (i.e., hose ha are caused by oise) are smoohed ad edges are preserved. However, mixed pixels may creae large phaom objecs, which look like edges raher ha like oise, alog he edges of objecs. I may be difficul o smooh hem based o he gradie of iesiy wihou diffusig he edges. Media Filers have he advaage of o iroducig ay ew pixel io he images, hereby preservig deails. Media Filers have bee used o remove mixed pixels i laser rage images [4]. 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

3 3 isufficie oise reducio for a small filer widow. Adapive Media Filers [, ] 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 i he scieific lieraure [, 3] may poeially disiguish corruped pixels from ucorruped oes ad apply filerig oly o he corruped pixels. These filers uilize local ipu image characerisics o ideify corruped pixels. However, a limiaio of his approach is he poeial 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 may herefore be removed. Based o his discussio, we summarily coclude ha i is isufficie o use oly spaial iformaio for reliable ideificaio ad removal of corruped pixels. max D. The Ceraiy Assised Spaial (CAS) Filer I his paper, we propose a ovel filer for elevaio map buildig. The filer is 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 II. A. 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 []. 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-coordiaes all he ime. This meas ha he codiio z h ( z 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 M, j N} ad C={c( i M, j N} deoe he elevaio map ad he ceraiy map, respecively, where M is he legh ad N is he widh of he map. A filer wih a widow size of (k+) (k+) (i his paper we use k=) is defied symmerically surroudig he curre pixel h(i, H. The curre pixel h( is updaed by he oupu of he filer, which is give by if c( A ad gs( = ad gc( = y( = ywm ( else if gs( = ad c( = (7) h( oherwise ad A = a (i( s / v ) + ) (8) where i( ) meas roud o he eares ieger, s is he grid cell size, v is he liear velociy of he LRF (he maximum is m/s i his paper) ad =3.3 ms is a ime sep. I his equaio, i( s / v ) is he esimae o he umber of imes a grid is illumiaed. I order o filer he mixed pixels a he viciiy of he edges, a cosa value of is added. 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 if σ ( > B h g s ( = (9) oherwise ad i+ k, j < > = + k if c(m,) G or σc( E g c ( () m= i-k, = j k oherwise where σ ad σ h c are he variace of he ormalized local maps H L ad C L ormalized by he maximum elevaio ad ceraiy values, respecively. We use A for G ad.4 for E. To keep he compuaioal cos low we i k, j use + + k H L < 7 o approximae he codiio σ ( > B i Eq. 9. y ( h wm is he i- j- j- j j+ j+ m= i k, = j k i- i oupu of a Weighed Media (WM) filer i+ ceered a pixels (. The ipu pixels o he WM filer are obaied by applyig a filer i+ skeleo [8] (as depiced i Fig. 5) o he Fig. 5. Filer skeleo for he elevaio map. For isace, h( has oe proposed WM filer copy, while h(i-, j-) has wo copies i he ipu pixels o he WM filer. The use of his skeleo is based o he followig heurisic: Sice we suspec h( o be a missig daum, he eares eighborig pixels h(i±, j±) are also likely o be missig daa, while hose pixels farher from h( have 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 surface of a objec. Whe he CAS Filer is applied o he raw elevaio map, mixed pixels, arifacs, ad radom oise are ideified by examiig he moio coiuiy (i.e., 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 filled by he WM filer. III. EXPERIMENTAL RESULTS I his secio, 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 real-ime daa collecio. A. 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 (e.g., obsacle course #6 comprises parallelepiped &, he cylider, pole -4 ad pole 6). 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 4 h obsacle course, while 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. Examiaio o all oher ess reveals ha he CAS filer preserved all poles (as small as.5 mm i diameer) while successfully removig he mixed pixels. Fig. 6. Oe of he 6 idoor obsacle courses.

4 4 Table I: Feaures of he 6 obsacle 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 X X X -5, 7, 9 X X X -5, 7, 9 X X -5, 7, 9 3 X X -7, 9 4 X X X 5 X X X 6 X X 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 For each of he 6 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 icreased mappig errors because hey caused vibraios of he roary able ad chages i occlusios. Accordig o Eq., hese addiioal errors move he CAS Filer s PI close o ha of he Wieer ad Averagig filer. () Roll ad pich moio may icrease he amou of empy pixels o he objecs upper surfaces. 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. However, he deerioraio of he CAS Filer s PI does o meas ha he Wieer Filer ad Averagig Filer are more suiable for ON. This is because he Wieer Filer does o remove arifacs/oise i elevaio maps (see Secio III. B), while he Averagig Filer eds o lower he heigh of edges i he elevaio maps a highly udesirable rai. I order o compare he filer performace i a quaiaive way, we measured by had he exac locaio of each obsacle i each obsacle course ad covered hese measuremes io a groud ruh elevaio map. We he evaluae he performace of filer by compuig a overall idex of performace (PI) accordig o N M j= i= PI = N M ( h ( h ( ) ( h ) r ( h ( j= i= f () where h r (, h ( ad h f ( are he elevaio values of pixel ( i he raw elevaio map, he groud ruh map, ad he filered map, respecively. We arbirarily seleced M=8 ad N= (which has full coverage of all obsacle courses) o evaluae he experimes here. Accordig o Eq., a smaller PI meas beer filer performace. Noe: We foud ha he iclusio of σ c ( >. 4 i he bechmark ess did o improve he filer s performace sigificaly. Therefore, we dropped his erm o save compuaio i ON applicaios. B. 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 daa from our PPE sysem, which we have o ye iegraed wih he ON applicaio described here. Fig. 9 shows he obsacle courses ad he vehicle i our idoor ad oudoor mappig experimes. From he maps depiced i Fig., i is appare ha he CAS Filer ouperforms he Wieer Filer because i almos compleely removes mixed pixels (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: he liear moio able moved he LRF alog he direcio v

5 5 3.5 PI.5.5 Wieer Average Media.5.5 PI.5 W ieer Average Media PI (a) Traslaio oly (c) Traslaio wih roll Fig. 8. Compariso of filer performace W ieer Average Media Fig. 9. s for mappig: lef: idoor, righ: oudoor ad fills mos of he missig daa o he op surfaces of objecs (see Fig. (b))..5.5 PI (b) Traslaio wih pich (d) Traslaio wih roll & pich W ieer Average Media I coras, he Wieer Filer leaves some oiceable mixed pixels ad mos of he missig daa i he map, as show i Fig. (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. C. Preservaio of deails ad compuaioal cos The preservaio of deails ad compuaioal cos of he CAS Filer depeds o each case ad we are uable o provide a geeral aalyical aalysis. However, we ca give a prelimiary esimae based o he bechmark ess. Table II shows he perceage of pixels ideified by he CAS Filer as mixed pixels ad missig daa i bechmark es T. For he wors case (he 3rd obsacle course) i he 6 obsacle courses, 74.8% of pixels were lef iac. This meas ha he CAS Filer preserved a sigifica amou of deails, (a) Origial (raw) elevaio map (b) Elevaio map afer applyig he CAS Filer (c) Elevaio map afer applyig he Wieer Filer Fig.. Idoor elevaio maps: he vehicle moved alog direcio v (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: he vehicle moved alog direcio v

6 6 TABLE II PERCENTAGE OF PIXELS IDENTIFIED AS CORRUPTED IN BENCHMARK TEST T s Mixed pixels Missig daa No filerig Noe: The oal umber of pixels i he maps is, of which oly pixels are processed by he CAS Filer. which is also he mai moive for he developme of he CAS Filer. For a esimae o compuaioal cos, we compared he CAS Filer wih he sadard Media Filer. From Equaios (7)-(), we ca deermie ha i order o process he pixels i a row (K=96), he CAS Filer requires: P+K- addiios, K- subracios, 3K muliplicaios ad K divisios. The algorihm furher icurs complexiy.76 KQ lgq i he WM filer ad K(P-) i compariso (o fid he maximum). P=5 ad Q=45 are he umber of pixels i he filer widows ad he umber of pixels i he WM filer skeleo (Fig. 5), respecively. The Media Filer has a complexiy of KPlg P. For ieger operaio i he PC used i he experime, we foud: () he four fudameal operaios of arihmeic ad he sorig operaio have roughly he same compuaioal cos,.7 s; ad () a compariso operaio has a cos of.85 s. Cosiderig he worse case (he 3rd obsacle course), he esimaed average compuig ime of he CAS Filer is. ms, while ha of he Media Filer is 38.7 ms. This meas ha he CAS Filer is less expesive i compuaio ha he Media Filer. Alhough i requires addiioal compuaio i buildig he ceraiy map (Eq. 4 & 5), his is o sigifica. Thus we believe ha he CAS Filer is compuaioally more efficie ha he Media Filer. A examiaio of he oher bechmark ess revealed similar resuls, which we omi here because of space limiaios. IV. DISCUSSION A. Compariso o oher mixed pixels removal mehods 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 (imeof-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 ha i is impracical o impleme his algorihm. Fially, our mehod is based o he characerisics of he erroeous daa ad he moio coiuiy cosrai ad i is hus device idepede. Therefore, i is more geeral ad suiable for he ed users of LRFs. I coras o Adams algorihm, our approach removes o oly mixed pixels bu also uexpeced oudoor arifacs ad oise. B. Real-ime implemeaio issues The CAS Filer ca be used for mappig where off-lie processig is sufficie. For real-ime obsacle egoiaio, he CAS Filer ca be implemeed as follows: wihi he compuer s world model, a so-called acive widow of size 4 pixels (rows colums) ravels wih he vehicle. The LRF is locaed a he ceer of he firs row. I each LRF samplig period, oly he pixels (i boh elevaio & ceraiy map) from he (-m) h row o he h row ahead of he vehicle are updaed. The he pixels i row -m is filered ad regisered io aoher elevaio map, which is used for obsacle egoiaio. m is chose i such a way ha he vehicle ca acquire sufficie obsacle/errai iformaio i elevaio. The grid cell size of he elevaio map for obsacle egoiaio may be bigger ha 5 5 mm subjec o applicaio. Our errai mappig mehod requires real-ime vehicle pose iformaio, which will be obaied by our PPE sysem. Therefore, i is o ecessary o perform simulaeous localizaio ad mappig i our mehod. The PPE sysem was proposed i [] ad he researchers a our lab have already buil his sysem. We will iegrae his sysem io our mappig mehod i he immediae fuure. V. CONCLUSIONS I his paper, we preseed a ovel filer 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 boh maps allows our CAS Filer o ideify mixed ad missig pixels. This disicio, i ur, allows he CAS Filer o apply differe reames o hese differe codiios. Ucorruped pixels are lef iac o preserve he map deails. Overall, our proposed CAS Filer produces cosisely beer resuls ha coveioal filerig 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-FG4-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 advise 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 I. Cof. Roboics ad Auomaio,, pp S. Begé-Brezez, e al, Ucerai map makig i aural eviromes, Proc. IEEE I. Cof. Roboics ad Auomaio, 996, pp D. S. Aposolopoulos, e al, Techology ad field demosraio of roboic search for Aarcic meeories, I. J. Roboics Research, vol. 9, o., pp. 5-3,. 4 K. Fregee, R. Madhava ad L. E. Parker, Icremeal muli-age roboic mappig of oudoor errai, Proc. IEEE I. Cof. Roboics ad Auomaio,, pp I. S. Kweo ad Takeo Kaade, High-resoluio errai map from muliple sesor daa, IEEE Tras. Paer Aal. Machie Iell., vol. 4, o., pp. 78-9, E. Krokov ad R. Hoffma, Terrai mappig for a walkig plaeary rover, IEEE Tras. Robo. Auoma., vol., o. 6, pp , R. Simmos, e al, Experiece wih rover avigaio for luar-like errais, Proc. IEEE/RSJ I. Cof. Iellige Robos ad Sysem, 995, pp C. M. Shoemaker ad J. A. Borse The demo III UGV program: a esbed for auoomous avigaio research, Proc. IEEE ISIS/CIRA/ISAS Joi Coferece, 998, pp L. Herikse ad E. Krokov, Naural Terrai Hazard Deecio wih a Laser Ragefider, Proc. IEEE I. Cof. Roboics ad Auomaio, 997, pp C. Ye ad J. Borese Characerizaio of a -D laser scaer for mobile robo obsacle egoiaio, Proc. IEEE I. Cof. Roboics ad Auomaio,, pp L. Ojeda ad J. Borese "FLEXav: Fuzzy Logic Exper Rule-based Posiio Esimaio for Mobile Robos o Rugged Terra" Proc. IEEE I. Cof. Roboics ad Auomaio,, pp SICK Ic., LMS/LMI Telegram Lisig, versio 5., page D. Niza, e al, The measureme ad use of regisered reflecace ad rage daa i scee aalysis, Proceedigs of he IEEE, vol. 65, o., M. Heber ad E. Krokov, 3D measuremes from imagig laser radars: how good are hey? Image ad Visio Compuig, vol., o. 3, pp. 7-78,99. 5 C. F. Olso, Adapive-scale filerig ad feaurig deecio usig rage daa, IEEE Tras. Paer Aal. Machie Iell., vol., o. 9,. 6 A. K. Ja Fudameals of Digial Image Processig, Preice Hall, P. Peroa ad J. Malik, Scale-space ad edge deecio usig aisoropic diffusio, IEEE Tras. Paer Aal. Machie Iell., vol., o. 7, D. R. K. Browrigg, The weighed Media Filer, Commuicaio of he ACM, vol. 7, o. 8, pp , 984.

7 9 S. Ko ad S. Lee, Ceer weighed Media Filers ad heir applicaios o image ehaceme, IEEE Tras. Circuis ad Sysems, vol. 38, o. 9, pp , 99. H. Li ad A. N. Willso, Jr., Media Filers wih adapive legh, IEEE Tras. 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 Tras. Image Processig, vol., o. 6, pp ,. H. Eg ad K. Ma, Noise adapive sof-swichig Media Filer, IEEE Tras. Image Processig, vol., o., pp. 4-5,. 3 T. Che, K. Ma ad L. Che, Tri-sae Media Filer for image deoisig, IEEE Tras. 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, I. J. Robo. Res., vol. 5, No. 5, pp ,