Robust mapping and localization in indoor environm

RobustMappingandLocalizationinIndoorEnvironmentsusingSonarData¤JuanD.Tard´os,Jos´eNeiraDept.Inform´aticaeIngenier´ıadeSistemas,UniversidaddeZaragozaMar´ıadeLuna3,E-50015Zaragoza,[e-mail],[e-mail],JohnJ.LeonardMITDept.ofOceanEngineering77MassachusettsAv.,Cambridge,[e-mail],[e-mail]5,2001AbstractInthispaperwedescribeanewtechniqueforthecreationoffeature-basedstochasticmapsusingstandardPolaroidsonarsensors.Thefun-damentalcontributionsofourproposalare:(1)aperceptualgroupingprocessthatpermitstherobustidentificationandlocalizationofenviron-mentalfeatures,suchasstraightsegmentsandcorners,fromthesparseandnoisysonardata;(2)amapjoiningtechniquethatallowsthesys-temtobuildasequenceofindependentconstant-sizestochasticmapsandjointheminagloballyconsistentandoptimalway;(3)arobustmecha-nismtodeterminewhichfeaturesinastochasticmapcorrespondtothesameenvironmentfeature,allowingthesystemtoupdatethestochasticmapaccordingly,andperformtaskssuchasrevisitingandloopclosing.Wedemonstratethepracticalityofthisapproachbybuildingageometricmapofamediumsize,realindoorenvironment,withseveralpeoplemov-ingaroundtherobot.Mapsbuiltfromlaserdataforthesameexperimentareprovidedforcomparison.¤SubmittedtoInternationalJournalofRoboticsResearchonDecember5,200111IntroductionTheproblemofconcurrentmappingandlocalization(CML)foranautonomousmobilerobotisstatedasfollows:startingfromaninitialposition,amobilerobottravelsthroughasequenceofpositionsandobtainsasetofsensormea-surementsateachposition.Thegoalisforthemobilerobottoprocessthesensordatatoproduceanestimateofitspositionwhileconcurrentlybuildingamapoftheenvironment.TheproblemofCML,alsoreferredtoasSLAM(si-multaneouslocalizationandmapbuilding),presentsanumberofdifficultissues,including(1)efficientmappingoflarge-scaleenvironments,(2)correctassocia-tionofmeasurements,and(3)robustestimationofmapandvehicletrajectoryinformation.Thepaperpresentscontributionstoeachofthesethreeareas.1.1ChoiceofRepresentationAswithmanyproblemsinroboticsandartificialintelligence,theissueofchoos-ingarepresentationisperhapsthekeystepindevelopinganeffectiveCMLsolution.Acentralrequirementistheabilitytorepresentuncertainty(Brooks1984,Lozano-P´erez1989).Popularchoicesforthemaprepresentationin-cludegrid-based(Elfes1987,ShultzandAdams1998),topological(Kuipers2000,ChosetandNagatani2001),feature-basedmodels(MoutarlierandChatila1989,AyacheandFaugeras1989),andsequentialMonteCarlomethods(Thrun2001,Doucet,deFreitasandGordan2001).Thispaperadoptsafeature-basedapproachtoCML,inwhichtheloca-tionsofgeometricfeaturesintheenvironmentandthepositionofthevehiclearejointlyestimatedinastochasticframework(Smith,SelfandCheeseman1988,MoutarlierandChatila1989).CMLiscastasavariable-dimensionstateestimationprobleminwhichthesizeofthestatespaceisincreasedordecreasedasfeaturesareaddedorremovedfromthemap.Astherobotmovesthroughitsenvironment,itusesnewsensormeasurementstoperformtwobasicoperations:(1)addingnewfeaturestoitsstatevector,and(2)updatingconcurrentlyitsestimateofitsownstateandthelocationsofpreviouslyobservedfeaturesintheenvironment.Relatedrecentresearchbyourselvesandothersthatadoptsasimilarper-spectiveontheproblemcanbefoundinFeder,LeonardandSmith(1999),CastellanosandTard´os(1999),Dissanayake,Newman,Durrant-Whyte,ClarkandCsorba(2001)andGuivantandNebot(2001).AlternativeapproachesincludetheworkofLuandMilios(1997a),Thrun(2001)andGutmannandKonolige(1999).Thesemethodsdonotneedtoexplicitlyassociateindividualmeasurementswithfeaturesintheenvironment,butseemtorelyonthehighqualityoflaserscannerdata.Thrun(2001)writes“Itisunclearhowtheperfor-manceofourapproachdegradeswithinaccuracyofthesensors.Forexample,itisunclearifsonarsensorsaresufficientlyaccuratetoyieldgoodresults”.Inthispaper,weaimtoshowthatafeature-based,geometricapproachtoCMLcanbeachievedwithsparseandnoisyairsonardata.2−4−20246810−4−20246810LaserscanSonarreturnsFigure1:Sensorinformationobtainedfromasinglerobotpositioninatypicalenvironmentusinga180degreeSICKlaserscannerandaringof24Polaroidsonarsensors.Thetrueenvironmentmap,obtainedbyhand,isshownindottedlines.1.2TheSonarMappingProblemMostwouldagreethatmobilerobotnavigationandmappinginindoorenvi-ronmentsismuchmoredifficulttoperformwithsonarthanwithlaserdata.Figure1providesacomparisonofdatafromaSICKlaserscannerandaringof24sonarsensorstakenatasinglepositioninatypicalenvironment.Thelackofinformationinthesonardataincomparisontothelaserdataisevident:onlyhalfofthePolaroidsensorsobtainareturnwithahighproportionofoutliers.Asaresult,theunderlyingstructureofthesceneislessvisuallyapparenttoahumanobserver.Despitetheincreaseddifficultyofsonarinterpretation,wefeelthatitisinterestingtoperformresearchwithsonarforavarietyofrea-sons.Fromtheperspectiveofcost,laserscannersaremuchmoreexpensivethansonarsensors.Fromtheperspectiveofbasicscience,questionssuchasthebasicmechanismsofbatanddolphinecholocationarehighlyimportant(Au1993).Finally,thefundamentalcharacteristicsofhighrangeaccuracyandwidebeamwidtharesharedwithmanytypesofso