第1页 / 共8页
第2页 / 共8页
第3页 / 共8页
第4页 / 共8页
第5页 / 共8页
第6页 / 共8页
第7页 / 共8页
第8页 / 共8页
apollo传感器融合论文.pdf
RobustandPreciseVehicleLocalizationbasedonMulti-sensorFusioninDiverseCityScenesGuoweiWan,XiaolongYang,RenlanCai,HaoLi,YaoZhou,HaoWang,ShiyuSong1Abstract—Wepresentarobustandpreciselocalizationsystemthatachievescentimeter-levellocalizationaccuracyindisparatecityscenes.OursystemadaptivelyusesinformationfromcomplementarysensorssuchasGNSS,LiDAR,andIMUtoachievehighlocalizationaccuracyandresilienceinchallengingscenes,suchasurbandowntown,highways,andtunnels.RatherthanrelyingonlyonLiDARintensityor3Dgeometry,wemakeinnovativeuseofLiDARintensityandaltitudecuestosignificantlyimprovelocalizationsystemaccuracyandrobustness.OurGNSSRTKmoduleutilizesthehelpofthemulti-sensorfusionframeworkandachievesabetterambiguityresolutionsuccessrate.Anerror-stateKalmanfilterisappliedtofusethelocalizationmeasurementsfromdifferentsourceswithnoveluncertaintyestimation.Wevalidate,indetail,theeffectivenessofourapproaches,achieving5-10cmRMSaccuracyandoutperformingpreviousstate-of-the-artsystems.Importantly,oursystem,whiledeployedinalargeautonomousdrivingfleet,madeourvehiclesfullyautonomousincrowdedcitystreetsdespiteroadconstructionthatoccurredfromtimetotime.Adatasetincludingmorethan60kmrealtrafficdrivinginvariousurbanroadsisusedtocomprehensivelytestoursystem.I.INTRODUCTIONVehiclelocalizationisoneofthefundamentaltasksinautonomousdriving.Thesingle-pointpositioningaccuracyoftheglobalnavigationsatellitesystem(GNSS)isabout10mduetosatelliteorbitandclockerrors,togetherwithtroposphericandionosphericdelays.Theseerrorscanbecalibratedoutwithobservationsfromasurveyedreferencestation.Thecarrier-phasebaseddifferentialGNSStech-nique,knownasRealTimeKinematic(RTK),canprovidecentimeterpositioningaccuracy[1].ThemostsignificantadvantageofRTKisthatitprovidesalmostall-weatheravailability.However,itsdisadvantageisequallyobviousthatit’shighlyvulnerabletosignalblockage,multi-pathbecauseitreliesontheprecisioncarrier-phasepositioningtechniques.Intuitively,LiDARisapromisingsensorforpreciselocalization.FailureduringharshweatherconditionsandroadconstructionstillisanimportantissueofLiDAR-basedmethods,althoughrelatedworkshaveshowngoodprogressinsolvingtheseproblems,forexample,lightrain[2]andsnow[3].Furthermore,LiDARandRTKaretwosensorsthosearecomplementaryintermsofapplicablescenes.LiDARworkswellwhentheenvironmentisfullof3Dor*ThisworkissupportedbyBaiduAutonomousDrivingBusinessUnitinconjunctionwiththeApolloProject.TheauthorsarewithBaiduAutonomousDrivingBusinessUnit,{wanguowei,yangxiaolong02,cairenlan,lihao30,zhouyao,wanghao29,songshiyu}@baidu.com.1Authortowhomcorrespondenceshouldbeaddressed,E-mail:songshiyu@baidu.comFig.1:OurautonomousvehicleisequippedwithaVelodyneLiDARHDL-64E.Anintegratednavigationsystem,NovAtelProPak6plusNovAtelIMU-IGM-A1,isinstalledforrawsensordatacollection,suchasGNSSpseudorangeandcarrierwave,IMUspecificforceandrotationrate.Thebuilt-intightlyintegratedinertialandsatellitenavigationsolutionwasnotused.AcomputingplatformequippedwithDualXeonE5-2658v312cores,andaXilinxKU115FPGAchipwith55%utilizationforLiDARlocalization.texturefeatures,whileRTKperformsexcellentlyinopenspace.Aninertialmeasurementunit(IMU),includingthegyroscopesandtheaccelerometers,continuouslycalculatetheposition,orientation,andvelocityviathetechnologythatiscommonlyreferredtoasdeadreckoning.It’sself-containednavigationmethod,thatisimmunetojamminganddeception.Butitsuffersbadlyfromintegrationdrift.Thus,eachsensorhasitsownuniquecharacteristicsanditsworkingconditions.Here,weproposearobustandpreciselocalizationsystemusingmulti-sensorfusiondesignedforautonomousvehiclesdrivingincomplexurbanandhighwayscenes.Moreprecisely,weadaptivelyfusedifferentlocal-izationmethodsbasedonsensorssuchasLiDAR,RTK,andIMU.ThesensorconfigurationofoursystemisshowninFigure1.Oursystemprovidesstable,resilientandpreciselo-calizationservicetoothermodulesinanautonomousvehicle,whichhasthecapabilityofdrivinginseveralcomplexscenes,suchasdowntown,tunnels,tree-linedroads,parkinggarages,andhighways.Wedemonstratelarge-scalelocalizationusingover60kmofdataindynamicurbanandhighwayscenes.InFigure2,weshowthearchitectureofourmulti-sensorfusionframework.Tosummarize,ourmaincontributionsare:•Ajointframeworkforvehiclelocalizationthatadap-tivelyfusesdifferentsensorsincludingLiDAR,RTK,andIMU.Iteffectivelyleveragestheiradvantagesandshieldsoursystemfromtheirfailureinvariousscenes
Vehicle or RoverVelodyne LiDAR IMU Reference StationGNSS LocalizationSINS The error-state Kalman filter (estimated states including errors of position, velocity, attitude, accelerometers bias, gyroscopes bias)Raw GNSS observationsPredicted prior PVA with VCVObserved posterior position/heading with VCV3D point cloudsPrior LiDAR mapRaw GNSS observationsLiDAR Localization Predicted prior PVA with VCVObserved posterior position/velocity with VCVOnline localization estimates with position/velocity/attitude (PVA) with variance and co-variance (VCV)Pre-built Map DatabaseCorrectionsIntegrated PVAFusion frameworkSpecific force and rotation rateFig.2:Overviewofthearchitectureofoursystemthatestimatestheoptimalposition,velocity,attitude(PVA)oftheautonomousvehiclebycombiningsensorinput(purple)withpre-builtLiDARmap(yellow).GNSSandLiDARestimatethePVAusedbyanerror-stateKalmanfilterasthemeasurements,whiletheKalmanfilterprovidesthepredictedpriorPVA.Thestrap-downinertialnavigationsystem(SINS)isusedasapredictionmodelintheKalmanfilterpropagationphasebyintegratingthespecificforcefbmeasuredbytheaccelerometerandtherotationrateωbibmeasuredbythegyroscope.Thecorrectionsincludingthebiasofaccelerometerandgyroscope,theerrorsofPVA,etcestimatedbytheKalmanfilterarefedtotheSINS.byhavingeffectiveuncertaintyestimation.•ALiDARlocalizationmethodthatadaptivelycombinesintensityandaltitudecuestoachieverobustandaccurateresultsespeciallyduringchallengingsituationslikeroadconstruction,outperformingpreviousworks.•Avehiclelocalizationsystemthathasbeenrigorouslytesteddailyincrowdedurbanstreets,makingourve-hiclesfullyautonomousinvariouschallengingscenesincludingurbandowntown,highways,andtunnels.II.RELATEDWORKMulti-sensorfusionisnotabrandnewidea.However,fus-ingmultiplesensorsandmakingthewholesystemaccurate,robustandapplicableforvariousscenesisaverychallengingtask.A.Soloviev.[4]andY.Gaoetal.[5]implementedintegratedGNSS/LiDAR/IMUnavigationsystemsusinga2DlaserscannerplusGNSSandIMU.TheapplicablescenesarelimitedduetotheLiDARlocalizationmodulethatreliesonparticularfeaturessuchasbuildingwalls.LiDARaidedbyinertialsensorscanalsolocalizetheautonomousvehicle.Itprovidesthelocalizationmeasurements,whileinertialsen-sorstypicallyareusedtopredicttheincrementalmovementbetweenscanstoimprovethepointcloudmatching.Theseworks[6],[7],[8]areamongthem.TheirmethodsrelyonLiDARsolely,butRTKisaperfectcomplementarylocalizationmethodtoLiDAR.RTKplaysanimportantrole,especiallyinopenspacesorstreetswithroadconstruction.LiDAR-basedvehiclelocalizationhasbeenverypopularinrecentyears.R.K¨ummerleandW.Burgard[9]developedanautonomoussystemthatutilizesmulti-levelsurfacemapsofcorrespondingenvironmentstolocalizeitselfbasedontheparticlefilter.K.YonedaandS.Mita[10]localizedtheirautonomousvehiclebyusingIterativeClosestPoint(ICP)method[11]toalignthereal-timescanningpointcloudtothegeo-referencedmap.However,itisknownthatmethodslikeICPareverysensitivetotheinitialguess.Theycanfailinsceneswithoutabundant3Dfeatures,suchashighwaysorotheropenspaces.TheworksthatareclosesttooursarefromJ.LevinsonandS.Thrun[2],[12].TheyproposeaLiDARintensity-basedlocalizationmethod.LiDARintensityprovidesmoretextureinformationoftheenvironmentasvaluableadditionalcuescomparedtothelocalizationsystemthatisbasedsolelyonthe3Dgeometryofthepointcloud.Weimprovedthemethodologyforvariousaspects,includinganewimagealignmentsteptorefinetheheadingangleestimation,ajointcostfunctioninvolvingboththeintensityandthealtitudetoachieverobustresults,andanewsteptoestimatethecovariancematricesoftheresult.MethodsmentionedthusfararedesignedformultiplelayersLiDAR,suchasVelodyneHDL-64E,VelodyneHDL-32EorevenVelodyneVLP-16.ItisexpectedthattheretailpriceoftheseLiDARscannerswillfallquicklybecausetherearemorethan40manufacturerscompetinginthisrisingfieldaswearewritingthisarticle.Ratherthanusingmulti-layerLiDARs,works[13],[14],[15],[16],[17],[18],[19]attempttoaccomplishthesimilartaskwith2Dorlow-endLiDARs.III.LIDARMAPGENERATIONOurLiDARbasedlocalizationmodulereliesonapre-generatedmap.Ourgoalhereistoobtainagrid-cellrepre-sentationoftheenvironment.Eachcellstoresthestatisticsoflaserreflectionintensityandaltitude.In[2]and[12],eachcellisrepresentedbyasingleGaussiandistributionmodelwhichmaintainstheaverageandthevarianceoftheintensity.In[20],thisisextendedtoaGaussianmixturemodel(GMM)maintainingboththeintensityandthealtitude.Inourwork,westillusesingleGaussiandistributiontomodeltheenviron-mentbutinvolveboththeintensityandthealtitudeasshowninFigure3.Wefoundthatcombiningboththeintensityandthealtitudemeasurementsinthecostfunctionthroughanadaptiveweightingmethodcansignificantlyimprovethelocalizationaccuracyandrobustness.ThiswillbediscussedindetailsinSectionIV-BandVII-C.IV.LIDARBASEDLOCALIZATIONOurlocalizationsystemestimatestheposition,velocityandattitude(PVA)jointly.However,weseekanoptimalsolutionthatincludesonlythe3-dimensionalpositionandtheheading,(x,y,a,h)intheLiDARlocalizationmodule.Here(x,y)isa2DCartesiancoordinatefromaconformalpro-jection,suchastheUniversalTransverseMercator(UTM).Ourpre-builtmapincludesthestatisticsofthealtitudeoftheroadsurface.Wecouldobtainthealtitudeestimationabyassumingthevehiclerunsonthesurfaceoftheroad.[20]
(a)(b)(c)Fig.3:Anexampleofapre-builtLiDARmapincludingthestatisticsoflaserintensityandaltitude.(a)Apre-builtmapcomposedcovering3.3km×3.1kmarearenderedinlaserintensityvalues.(b)Azoomed-inviewof(a).(c)Thesamezoomed-inviewof(a),butrenderedinaltitudea.and[21]makeexhaustivesearchingover(x,y,h),whichcausesthecomputationalcomplexityofO(n3).Forbetterefficiency,wehaveaseparatehestimationstepbasedontheLucas-Kanadealgorithm[22]andahistogramfilterforthehorizontal(x,y)estimation.PleaserefertoAlgorithm1foracompletestep-by-stepoverview.Algorithm1LiDAR-basedlocalizationInput:Priormapm,onlinepointcloudz,roughtransformationT0=(x0,y0,a0,φ0,θ0,h0)andsearchspaceX,Y.Output:Bestregistration(ˆx,ˆy,ˆa,ˆh),andcovariancematrixCxy.1:ˆh←headingangleestimationIV-A2:ˆa0←m(x0,y0)Getaltitudefrommap3:Transformzwiththetransformation(x0,y0,ˆa0,φ0,θ0,ˆh)4:forxi,yi∈{x0+X,y0+Y}do5:Pr←SSDr(xi,yi,z,m)Equ.686:Pa←SSDa(xi,yi,z,m)Equ.787:P(z|xi,yi,m)←(Pr)γ·(Pa)1−γEqu.59108:P(xi,yi)←P(z|xi,yi,m)·(¯P(xi,yi))1/κEqu.49:endfor10:(ˆx,ˆy)←{P(xi,yi)}Equ.1111:Cxy←{P(xi,yi)}Equ.1212:ˆa←m(ˆx,ˆy)Getaltitudefrommap13:return(ˆx,ˆy,ˆa,ˆh,Cxy)A.HeadingAngleEstimationSimilartotheproceduresinthemapgenerationsection,foragivenoffset(x,y,h),weprojecttheonlinepointcloudontothegroundplane,calculatetheaverageintensityandaltitudeineachcell,andbuildanonlinemap.Tomaketheheadingestimationefficientenough,theonlinepointcloudisdown-sampledusingalargergridcellinthemap.Seethesupplementalmaterialfordetails.Thenwecancalculatethesumoftheerrorsoftheintensityandaltitudebetweentheonlinemapandthepre-builtone.Ourgoalistofindanoptimaloffsetthatcanminimizebothintensityandaltitudeerrorterms.TheLucas-Kanadealgorithm[22]isapplied,whichisatechniquethatsearchesforthebestmatchbetweentwoimagesusingthegradientdescent.Moreprecisely,atypeofLucas-Kanadealgorithm,theforwardsadditivealgorithm[23],isapplied.Fortheintensityterm,theHubernormisusedastheregularizertopenalizethepotentialintensityoutlier,andagradient-dependentweight[24]isusedtodown-weightthepointsthoseprojectontoasub-pixelwithhighgradient.Forthealtitudeterm,wetreatitasanotherindependentchannelofintensityandsimplysumthesetwoerrortermswithafixedweight.TheenergyfunctionissolvedefficientlybyaGauss-Newtonoptimizationalgorithminacoarse-to-finestrategywiththeSIMDparallelizedimplementation.Weverifiedthattheproposedmethodcanconvergetosolutionswithsub-degreeaccuracy.AtacticalgradeMEMSIMUcannotprovidesufficientlyaccurateheadingangleestimationbyitself.TheeffectivenessofthisstepisshowninTableIIinSectionVII-C.AlthoughtheLucas-Kanadealgorithmcanproducethehorizontaltranslationestimation,wefoundthatitisnotaccurateandrobustenoughinpractice.Therefore,weuseonlytherotationcomponentastheheadingangleestimation,whichisusedinthenextstep,thehorizontallocalization.B.HorizontalLocalizationThehistogramfilterisanonparametricmethod.Itap-proximatestheposteriorsbydecomposingthestatespaceintofinitelymanyregions,andrepresentingthecumulativeposteriorforeachregionbyasingleprobabilityvalue.Forcompleteness,wequotetheequationsofthepredictionandupdatephaseofthehistogramfilterfrom[25]:¯Pk,t=iP(Xt=xk|ut,Xt−1=xi)·Pi,t−1,(1)Pk,t=η·P(zt|Xt=xk)·¯Pk,t,(2)wherePk,trepresentsthebeliefofeachstatexkattimet,utisthecontrolinput,andztisthemeasurementvector.Weapplythehistogramfiltertothehorizontallocalization.Thestatecontains(x,y).Thereasonweusethehistogramfilteristhatthecumulativeprobabilitiesoftheposteriorden-sityarecalculatedinadiscreteway.Thisexhaustivesearchensurestheoptimalsolution.Equations1and2correspondtothepredictionandupdatestep,andthecorrespondingequationsofourmethodareEquations3and4,respectively.1)ThePredictionStep:Thepredictionstepistopredictonthenewbeliefofthestatewiththehistoricaldistribu-tionofthefilterandthecontrolvariablesorthemotionmodel.HerewedirectlyupdatethehistogramfiltercenterusingthemotionpredictionfromtheSINS,andupdatethebeliefdistributionwitharandomwalkwithGaussiannoise.Previousworks[2],[12]relyonapre-fusedGNSS/IMUsolution.Acompletemulti-sensorfusionframeworkallowsoursystemtoworkundervariedchallengingcircumstances.Thus,thepredictionstepupdatestheprobabilityofeachcellasfollows:¯P(x,y)=η·i,jP(i,j)·exp(−(i−x)2+(j−y)22σ2),(3)
where¯P(x,y)isthepredictedprobability,P(i,j)istheprobabilityafterthemotionupdatefromtheSINS.Hereσistheparameterdescribingtherateofdriftbetweentwoframes.2)TheUpdateStep:Thesecondstepofthehistogramfilteristhemeasurementupdatestep,inwhichtheposteriorbeliefofeachstateisestimatedbythefollowingequation:P(x,y|z,m)=η·P(z|x,y,m)·(¯P(x,y))1/κ,(4)wherezistheonlinemapbuiltwiththeonlinelaserscansinthewayidenticaltothemappingprocedureinSectionIII,Nzisthenumberofthecellsoftheonlinemapwithvaliddata.misthepre-builtLiDARmap,(x,y)isthemotionpose,ηisthenormalizingconstant,andκistheKullback-Leiblerdivergence[26]betweenthedistributionsofxyP(z|x,y,m)andxy¯P(x,y).TheKLdivergencedepictsthediscriminationinformationandisusedtobalancetheinfluenceoftheprediction.NotethatwediscardtheuncertaintytermoftheGNSS/IMUposeintheposteriorbeliefmodelingusedby[12],becausetheKalmanfilterisin-troducedinthisworktoincorporatetheGNSSmeasurement.WehaveamorecomprehensivefusionframeworkfusingtheinputfromvarioussourcesincludingGNSS.ThelikelihoodP(z|x,y,m)iscalculatedbymatchingtheonlinepointcloudwiththepre-builtmap.Weadaptivelyfusetheintensityandthealtitudemeasurementbybuildingacostfunctionwithadynamicweightingparameterγ:P(z|x,y,m)=η·P(zr|x,y,m)γ·P(za|x,y,m)1−γ,(5)wherezrandzarepresenttheintensityandaltitudemea-surementoftheonlinesensorinput.Theintensitypartisformalizedasbelow:SSDr=i,j(rm(i−x,j−y)−rz(i,j))2(σ2m(i−x,j−y)+σ2z(i,j))σ2m(i−x,j−y)σ2z(i,j),(6)whererm,andrzdenotetheaverageintensityvalueinthepre-builtmapandtheonlinemap,respectively.σmandσzrepresentthestandarddeviationoftheintensityvalue.SSD(SumofSquaredDifferences)isusedtoevaluatethesimilaritybetweenthesensorinputandthemap.Theimpactoftheenvironmentchangeisimplicitlydiminishedbythevarianceterm.Twosignificantlydifferentintensityvalueswithbothlowvariancevaluesimplytheenvironmentchange.Similarly,theSSDofaltitudeistakenintoaccountasbelow:SSDa=i,j(am(i−x,j−y)−az(i,j))2,(7)whereamandazrepresenttheaveragealtitudeofthepre-builtmapandtheonlinemap,respectively.WeremovethevariancetermhereinSSDabecausethealtitudeisvariableverticallybynature.ThelikehoodofP(zr|x,y,m)andP(za|x,y,m)aredefinedas:P(zr|x,y,m)=η·α−SSDr2·Nz,P(za|x,y,m)=η·α−λ·SSDa2·Nz,(8)whentheparameterα=e,theyareGaussian-likeprobabilitydistribution.Wechangeαtoadjustitssmoothness.Theadaptiveweightparameterγplaysanimportantroleduringtheintensityandaltitudefusion.Weletitdeterminedbythevariancesof[P(zr|x,y,m)]xyand[P(za|x,y,m)]xy.Thevariancesaredefinedbythefollowingequations:σ2x=xyP(x,y)β(x−¯x)2xyP(x,y)β,σ2y=xyP(x,y)β(y−¯y)2xyP(x,y)β,(9)where¯xand¯yarethecenterofmassofdistribution.Therefore,thexandyvariancesofintensityandaltitudearerepresentedasσ2x(r),σ2y(r),σ2x(a),andσ2y(a).Theweightγiscomputedas:γ=σ2x(a)σ2y(a)σ2x(a)σ2y(a)+σ2x(r)σ2y(r).(10)3)OptimalOffset:Theoptimaloffsetisestimatedfromtheposteriordistributionofthehistogramfilter.Insteadofusingallthestatesofthehistogramfiltertocalculatetheoptimaloffset,weuseonlyasmallsquaredareaaroundthestateofthelargestorthesecondlargestposteriorbelief.Ifthevalueofthesecondlargestposteriorbeliefachievesagivenratioofthelargestoneandisclosertothecenterofthehistogramfilter,wetakethestateofthesecondlargestposteriorbeliefasthecenterofthesmallsquaredarea.Otherwise,wetakethestateofthelargestposteriorbelief.AssumingthesmallareaisZ,theoptimaloffset(ˆx,ˆy)iscalculatedby:ˆx=(x,y)∈ZP(x,y)β·x(x,y)∈ZP(x,y)β,ˆy=(x,y)∈ZP(x,y)β·y(x,y)∈ZP(x,y)β.(11)4)UncertaintyEstimation:ThelocalizationresultisusedtoupdatetheKalmanfilterinsectionVI.Thekeyissueistheevaluationoftheuncertaintyassociatedwiththestateestimates.Weletthevectortd=(ˆx,ˆy)Tandtx,y=(x,y)Tbetheoptimizedoffsetandtheoffsetofthe(x,y)cell,respectively.TheresultingcovariancematrixCxycanbecomputedas:Cxy=1x,yP(x,y)β·xyP(x,y)β·(tx,y−td)(tx,y−td)T.(12)InFigure6fromtheexperimentalsection,weshowsamplesofthefilterdistribution,theestimatedcovariancematrixtogetherwiththestateestimates.Weobservethatthees-timatedcovariancematrixisconsistentwiththeobservederrorcomparedtothegroundtruth.V.GNSSBASEDLOCALIZATIONTheRTKalgorithmisimplementedtofullyutilizethepropertiesofothersensors.HerewepresenthowtheRTK
moduleisaidedbyoursensorfusionframework,butnotthedetailsoftheimplementationofRTKitself.Toimprovetheambiguityresolutionsuccessrate,weuseallGPS,BeiDouandGLONASSobservationscurrentlyavailable,andtheGLONASSinter-frequencybiasisesti-mated[27].Withtheleastsquares,wecangetthesingle-difference(SD)floatambiguitiesandtheircovariance,thenapplyatransformationmatrixtoconvertthemintothedouble-differenced(DD)ambiguitieswiththeintegernature,afterwhich,MLAMBDA[28]isutilizedtoresolvetheambiguities.Althoughtheambiguityresolved(AR)RTKsolutionispreferred,thereareindeedmanysituationswithseveremultipathandsignalblockagewhenitisdifficulttoresolvetheambiguities,forexample,underurbanbuildingsorinforests,whereonlyafloatedambiguityRTKsolutionorcode-baseddifferentialGNSSareavailablewithsub-meteraccuracy.Overallinourframework,theGNSSpositioningresult,eitherAR-RTKorfloatRTK,isusedtoupdatetheKalmanfilterwiththecorrespondinguncertaintycomputedfromtheweightedsumofthepseudo-rangesandphasesresiduals.A.INS-aidedAmbiguityResolutionWithouttheaidofothersensors,thesuccessrateofambiguityresolutionwouldheavilydependonpseudo-rangeprecisionandunderurbanbuildingswithservemulti-path,theratemaydegradegreatly.Howeverinourwork,INSconstrainedbyLiDAR(insectionIV)and/orGNSS,canprovideapromisingpredictiontonarrowdowntheambiguitysearchspace.Forexample,whenpassingthroughatunnelwithoutGNSSsignals,theframeworkcouldcontinuetoworkwithLiDARconstrainingINSerrorsandprovideanaccuratepositionpredictiontohelpresolveambiguitiesuntilGNSSsignalsarereacquired.Currently,ourworkonlylooselycouplesthesensorobservations(tightlycouplingisplannedforthefuture).B.PhaseCycleSlipDetectingWhentheGNSSreceiverlosesitslockonsignaltracking,asuddenjumpofcarrierphasemeasurements,calledcycleslip,canhappen,whichthenforcesdiscontinuityofintegerambiguityandworsensthepositioning[29].Furthermore,underurbanenvironmentswhereGNSSsignalsarecom-monlyobstructedandreflected,thecycleslipscanoccurmuchmorefrequentlythaninstaticopenconditionsandshouldbedetectedandrepairedtoimproveRTKperfor-manceforamobilevehicle.Hereweestimatetheincrementaloffsetoftherover’spositionandthereceiverclockdriftbetweentwoconsecutiveepochsat5Hzbasedontheconsiderationofthesatellitegeometrywiththetroposphericandionosphericdelaysre-mainingunchanged.VI.SENSORFUSIONInourfusionframework,anerror-stateKalmanfilterisappliedtofusethelocalizationmeasurements,discussedintheabovesection,withIMU.ThefusionframeworkcanoptimallycombinetheorientationrateandaccelerometerinformationfromIMU,forimprovedaccuracy.IMUissuf-ficientlyaccuratetoproviderobuststateestimatesbetweenLiDARandRTKmeasurements.A.SINSKinematicsEquationandErrorEquationAStrap-downInertialNavigationSystem(SINS)estimatestheposition,velocityandattitudebyintegratingtheIMUdata.Inthispaper,wechooseeast-north-up(ENU)asthenavigationreferenceframe(n),andright-forward-up(RFU)asthebodyframe(b)[5],andwealsousetheearthframe(e)andtheinertialframe(i)[30].Primarily,thedifferentialequation[5][30][1]innframeofSINSiswellknownas:˙vn=Cnb(fb−ba)−(2ωnie+ωnen)×vn+gn˙r=Rcvn˙qnb=12∗(ωbnb×)⊗qnb,(13)wherer=(λ,L,a)Tisthevehicleposition;vnisthevehiclevelocity;qnbistheattitudequaternionfrombframetonframe;Cnbisthedirectioncosinematrixfrombframetonframe;gnisthegravity;bgisthegyroscopesbiases;baistheaccelerometersbiases;ωbib,fbaretheIMUgyroscopesandaccelerometersoutputrespectively;ωγαβistheangularrateofβframewithrespecttoαframe,resolvedinγframe;⊗isthequaternionmultiplicationoperator;Rctransformstheintegrationofvelocitytolongitudeλ,latitudeLandaltitudea,andRc=diag(1(RN+a)cos(L),1RM+a,1),whereRN,RMarethetransverseradiusandthemeridianradius,respectively.ForthetacticalgradeMEMSIMU,theIMUbiasescanbemodeledasaconstantvaluemodel.WhenwecombinetheSINSwithothersupplementarysensors,theRTKorLiDAR,usinganerror-stateKalmanfilter,aSINSerrormodelisnecessary.Theψanglemodelerrorequationforthevelocity,positionandattitudeerrorcanbeexpressedasfollowsinthenavigationframe[31]:δ˙vn=Cnbfb×δψ−(2ωnie+ωnen)×δvn+Cnbδfbδ˙r=−ωnen×δr+Rcδvnδ˙ψ=−ωnin×δψ−Cnbδωbib,(14)whereδvn,δr,δfb,δωbibaretheerrorofvn,r,fb,ωbibrespectively;δψistheerrorofattitudeangle.B.FilterStateEquationBecausetheSINSerrorgrowsovertime,togetaprecisePVA,weuseanerror-stateKalmanfiltertoestimatetheerroroftheSINSandusetheestimatederror-statetocorrecttheSINS.WechoosethestatevariablesasX=rvnqnbbabgT,andthestatevariables’errorasδX=δrδvnδψδbaδbgT.FromtheSINSerrorEquation(14)andtheIMUmodel,wecanobtainthestateequationoftheKalmanfilterasfollows:δ˙X=F(X)δX+G(X)W,(15)whereW=wawgwbawbgTisthesystemnoise,whichcomprisestheIMUoutputnoiseandtheIMUbiasnoise.
C.FilterMeasurementUpdateEquationThemeasurementupdatecomprisestheLiDARandGNSSparts.TheLiDAR-basedlocalizationoutputsthepositionandheadingangleofthevehicleasthefiltermeasurement.TheGNSSlocalizationoutputsonlytheposition.Inourframework,allthemeasurementsareusedtoupdatetheKalmanfilterwiththecorrespondinguncertaintycomputedinSectionIV-B.4andV.TherestofthetimeormeasurementupdatejustfollowsthestandardKalmanfilter.Seethesupplementalmaterialformoredetails.D.DelayHandlingDuetothetransmissionandcomputationdelay,themea-surementdelayanddisordermustbetakenintoconsidera-tion.Thesolutionisthatwemaintaintwofiltersandafixedlengthbufferofthefilterstates,thefiltermeasurementsandtheIMUdata,inchronologicalorder.Thefilter-1computesthereal-timePVAanditscovariancebyperformingthetimeupdateandintegratingtheIMUdatainstantlywhennewIMUdataisreceived.Thefilter-2processesthedelayedmeasurement.Whenanewmeasurementatt1isreceived,weexecutethefollowingactions:1)Obtainthefilterstatesatt1fromthebuffer.Updatethefilterstatesinfilter-2.2)Executethemeasurementupdateatt1infilter-2.3)Executethetimeupdateinfilter-2usingtheIMUdatainthebufferuntilitreachesthecurrenttime.Orwestopatt2,ifanothermeasurementisfoundatt2inthebuffer,wheret2islaterthant1.Thismeasurementsatt1andt2arereceivedinthewrongorder.4)Executethemeasurementupdateatt2,ifthereisanothermeasurementatt2.ThenrepeatStep3andfindmoremeasurementsreceivedinthewrongorder.5)Whenwefinishthetimeupdateandreachthecurrenttime,thefilterstatesareinthebufferandthestatesofthefilter-1areupdatedaccordingtothenewresultsstartingfromt1tothecurrenttime.Figure4isusedtoillustratethisprocedure.tatbtcfilter-2filter-2filter-1IMU datalidar localizationgnss localizationt1t2Fig.4:Delayanddisordermeasurementprocessing.ta,tbandtcrepresenttheIMUtimeseries,themeasurementoccurredtimeseries,andthemeasurementreceivedtimeseries,respectively.Therectangleandthestarrepresenttwomeasurementsthatreceivedinthewrongorder.VII.EXPERIMENTALRESULTSOurtestingplatformisshowninFig1.Thestoragesizeofthemapandthelocalizationaccuracyaretwofactorsthoseshouldbebalancedcarefullyinthesystem.OurmaphasasingleGaussiandistributionandtheresolutionofagridcellis12.5cm.Thisallowsustostore1kmofurbanmapsusingabout5MBofthediskspacewithrequisitegeneralpurposelosslesscompressiontechniques.Ground-truthvehiclemotiontrajectoriesaregeneratedusingofflinemethodsforquantitativeanalysis.InopenspaceswithgoodGNSSsignalreception,theGNSS/INSsolutionbasedonpost-processingalgorithms,suchastheNovAtelInertialExplorer,isabletoproduceenoughaccuratevehiclemotiontrajectories.InweakGNSSsignalscenarios,suchascomplexurbanroads,wetreatitasaclassicmapreconstructionproblemcombiningseveraltechniquesincludingNovAtelIEpost-processing,LiDARSLAM,loopclosure,andtheglobalpose-graphoptimization.WeonlyshowqualitativeresultsindemovideoclipsforGNSS-deniedscenes,suchastunnelorundergroundgarage.Thus,weclassifyourtestingdatasets(60kmintotal)intothreegeneralcategories:1)48.1kmregularroads:YF-1,YF-2,YF-3,YF-4,YF-5coveringcommonroadconditions,suchasurban,countryside,andatrafficjam.2)10.4kmweakGNSSsignalroads:HBY-1,DS-1coveringnarrowroadslinedwithtallbuildingsortrees.3)2.1kmGNSS-deniedroads:DT-1coveringtunnels.A.QuantitativeAnalysisOursystemhasbeenextensivelytestedinreal-worlddrivingscenarios.Wecompareourlocalizationperfor-manceagainstthestate-of-the-artintensity-basedlocalizationmethodproposedbyLevinsonetal.[2],[12]andthebuilt-intightly-coupledGNSS/IMUintegratedsolutioninthecom-mercialproduct.NotethattheimplementationofLevinsonetal.[2],[12]alsoreliesonapre-fusedGNSS/IMUinput.Forthebestperformanceof[12]andtomakethecomparisonconvincing,thebuilt-insolutioninthecommercialproductisusedastheinputto[12].Inordertoexplicitlydemonstratethecontributionofdifferentsensors,thetestresultsofourproposedsystemareshownintwomodes:1)2-Systems:LiDAR+IMU2)3-Systems:LiDAR+GNSS+IMU.InTableI,weshowthequantitativeresultsinbothregularorweakGNSSroads.Noteourvastperformanceimprovementover[12]andtherobustandaccuratelocalizationresultsinbothregularandweakGNSSscenarioswithcentimeterlevelaccuracy.Thatboththe2-Systemsand3-Systemsworkwelldemonstratesthatoursystemdoesnotrelyonasinglesensorbutfusesthesensorsinputusingresilientandadaptivemeth-ods.Onregularroads,theobservationthatthe3-Systemsworksbetterthanthe2-SystemsprovesahelpfulaidfromtheGNSSRTKmodule.However,boththesystemshavealmostsimilarperformancesontheweakGNSSroads.ItisapowerfulproofthatouradaptiveKalmanfilterframeworksuccessfullyrejectsthenoisyoutliersproducedbytheGNSSRTKmoduleinthesechallengingscenarios.WedisplaythelateralandlongitudinalerrorovertimeinFig.5.Asexhibited,ourproposedsolutionisabletoachievebetterperformanceover[12]consistentlyovertime.B.QualitativeAnalysisForGNSS-deniedroads,wedonotpresentquantitativecomparisonsduetothelackofgroundtruth.Inouradditionalvideoclips,weshowthequalitativecomparisonbetweenourresultsandNovAtel’sGNSSRTK/IMUposes.Thereasonwe
didnotshowtheresultof[12]isthatitfailswhenNovAtel’sRTK/IMUposesarenotstableandsmoothenough.InFigure6,wegiveperformanceanalysisofeachmoduleandfunctioninoursystemindetail.(a)and(b)demonstratethegoodperformanceofoursystemincrowdedsceneswithpeopleorcars.(c)showsaveryinterestingcasewithanewlypavedroadandarecentlybuiltwall.TheLiDARcannothandlesuchsignificantenvironmentalchangesbasedonlyontheintensitycues.Itgivesgoodresultswhenweadaptivelyfusetheadditionalaltitudecues.LogsMethodHoriz.RMSHoriz.MaxLong.RMSLat.RMS<0.3mPct.RegularRoadsInteg0.0250.3620.0130.01899.98%[12]0.2091.9340.0970.16182.25%2-Sys0.0750.5600.0500.04599.42%3-Sys0.0540.5510.0320.03699.54%Weak-GNSSRoadsInteg0.2591.2870.1520.16672.81%[12]0.1430.7370.0880.09395.02%2-Sys0.0700.3150.0500.03999.99%3-Sys0.0730.2580.0530.041100.0%TABLEI:Quantitativecomparisonwith[12]andthebuilt-intightly-coupledGNSS/IMUintegratedsolutioninNovAtelProPak6.Theperfor-manceoftwomodesofoursystemisshown:1)2-Systems:LiDAR+IMU;2)3-Systems:LiDAR+GNSS+IMU.ThebenefitsofGNSSinregularroadsareclearlyvisible.Ourlocalizationerrorisfarlowerthan[12].TheGNSS/IMUsolutiondoesnotperformwellinweakGNSSsignalscenarios.C.DetailedAnalysisofLiDARTodemonstratetheeffectivenessofeachoftheabovecon-tributionsinSectionIV,weshowthelocalizationaccuracywithdifferentmethodsinTableIIusingthe2-Systems.Wealsointroduceaspecialdatalog(YF-6),whichincludesadistrictwheretheroadwasnewlypavedafterthemapdatacollection.Intensitydenotesthebaselinemethodwhereonlyintensitycuesareused.InHeading,weaddtheheadingestimationstep,whichisespeciallyhelpfulforthelow-gradeIMU.InFixedAlt,weincorporatethealtitudecuesduringtheLiDARmatching.Wesettheweightforaltitudecuesto0.5(γinEqu.5).Notethelargeimprovementthatclearlydemonstratestheeffectivenessofthealtitudecues.Further,inAdaptAlt,weincorporatethealtitudecueswithadaptiveweights.FromtheresultofYF-4intableII,weobservethatourheadingangleoptimizationstepiscrucialtoLiDARlocalization.TheresultofYF-6intableIIindicatesthatouradaptiveweightingstrategymakesthesystemmorerobusttoenvironmentalchanges,suchasroadconstruction,andseasonalvariations.Demonstrably,thisgivesusthelowestlocalizationerrorsinallmetrics.D.Run-timeAnalysisOursystemprimarilycontainsthreemodules:LiDAR,GNSS,andSINS.LiDAR,GNSS,andSINSworkat10hz,5hz,and200hz,respectively.Therearetwoimplementationsdesignedfordifferentapplications.OneoccupiesonlyasingleCPUcore.TheotherusesasingleCPUcoreplusaFPGA.Inthesinglecoreversion,wereducethecomputationloadbyusingasmallerhistogramfiltersizeanddownsam-plingdataduringtheheadingangleevaluation.BothversionsLogsMethodHoriz.RMSHoriz.MaxLong.RMSLat.RMS<0.3mPct.YF-4Intensity0.1531.0900.1070.08594.41%Heading0.0810.4610.0600.04299.63%FixedAlt0.0740.3410.0540.04099.97%AdaptAlt0.0680.3070.0480.03999.98%YF-6Intensity0.50811.9280.3680.29392.72%Heading0.48512.7710.3520.28295.68%FixedAlt0.2168.0160.1510.12995.89%AdaptAlt0.0570.3190.0380.03599.99%TABLEII:Comparisonoflocalizationerrorsofvariousmethodsusedinoursystem.Thebenefitsofeachofheadinganglerefinement,altitudecuesandadaptiveweightsareclearlyvisible.providesimilarlocalizationresultsintermsofaccuracy,butalargerhistogramfiltersizecanpotentiallyincreasethepossibilityoftheconvergencewhenthefilterdriftsawayinanabnormalevent.GNSSandSINSmodulesonlytakeabout0.2CPUcore.VIII.CONCLUSIONANDFUTUREWORKWehavepresentedacompletelocalizationsystem,de-signedforfullyautonomousdrivingapplications.Itadap-tivelyfusestheinputfromcomplementarysensors,suchasGNSS,LiDARandIMU,toachievegoodlocalizationaccuracyinvariouschallengingscenes,includingurbandowntown,highwaysorexpressways,andtunnels.Oursys-temachieves5-10cmRMSaccuracybothlongitudinallyandlaterallyandisreadyforindustrialusebyhavingtwoversionswithdifferentcomputinghardwarerequirements.Oursystem,deployedinalargeautonomousdrivingfleet,makesourvehiclesfullyautonomousincrowdedcitystreetseveryday.Thegeneralityofourfusionframeworkandalgorithmdesignmeansitcanbeusedtoreadilyfusemoresensorsatvariouscostlevels,facingdifferentapplications.Actually,wehavebeguntestingoursystemwithconsumergradeMEMSIMUandVelodyneVLP-16LiDAR.ACKNOWLEDGMENTWewouldliketothankourcolleaguesfortheirkindhelpandsupportthroughouttheproject.WeixinLuhelpedwiththevehiclepreparation.ChengWanghelpedwiththedemovideoproduction.ShichunYi,LiYuandChengWanggeneratedtheLiDARmap.NadyaBoschhelpedwiththetextediting.REFERENCES[1]P.D.Groves,PrinciplesofGNSS,inertial,andmultisensorintegratednavigationsystems.Artechhouse,2013.[2]J.Levinson,M.Montemerlo,andS.Thrun,“Map-basedprecisionvehiclelocalizationinurbanenvironments.”inRSS,vol.4,2007,p.1.[3]R.W.WolcottandR.M.Eustice,“FastLIDARlocalizationusingmultiresolutionGaussianmixturemaps,”inICRA,2015.[4]A.Soloviev,“TightcouplingofGPS,laserscanner,andinertialmeasurementsfornavigationinurbanenvironments,”inION,2008.[5]Y.Gao,S.Liu,M.M.Atia,andA.Noureldin,“INS/GPS/LiDARintegratednavigationsystemforurbanandindoorenvironmentsusinghybridscanmatchingalgorithm,”Sensors,vol.15,no.9,2015.[6]A.Soloviev,D.Bates,andF.GRAAS,“Tightcouplingoflaserscannerandinertialmeasurementsforafullyautonomousrelativenavigationsolution,”Navigation,vol.54,no.3,pp.189–205,2007.
0100200300400500600700800900100000.20.40.60.8time / slong. error / m Levinson et alOur 2−SystemsOur 3−Systems(a)LongitudinalRMSofYF-10100200300400500600700800900100000.20.40.60.8time / slat. error / m Levinson et alOur 2−SystemsOur 3−Systems(b)LateralRMSofYF-1Fig.5:Quantitativecomparisonwith[12].Ourproposedsolutionachievesbetterperformanceover[12]consistentlyovertime.(a)(b)(c)Fig.6:Performanceanalysisofeachindividualfunction.Thefirstrowisthefrontcameraimageofthescene.Thefiguresinthesecondrowindicatetheperformanceofoursystem.Green,blueandredellipsesrepresenttheuncertaintyofthelocalizationestimationofeachmethod:Fusion,LiDARandRTK,respectively.ThesmallbluefigureinthecornerrepresentstheposteriordensityinthehistogramfilteroftheLiDARmodule.(a)TheRTKgivesunstableresultsduetothesignalblockage.(a)-(b)Thesystemperformswellwithcrowdsorcarsaround.(c)showsthattheLiDARfailswhenonlytheintensitycueisusedbutsucceedswhenwemakeuseofboththeintensityandaltitudecuesonanewlypavedroadwitharecentlybuiltwall(thefigureswithintheorangepolygon).TheLiDARmapisbuiltwiththedatacollectedinthescenedepictedintheupper-leftfigure.[7]J.Tang,Y.Chen,X.Niu,L.Wang,L.Chen,J.Liu,C.Shi,andJ.Hyypp¨a,“LiDARscanmatchingaidedinertialnavigationsysteminGNSS-deniedenvironments,”Sensors,vol.15,2015.[8]G.Hemann,S.Singh,andM.Kaess,“Long-rangeGPS-deniedaerialinertialnavigationwithLIDARlocalization,”inIROS,2016.[9]R.Kummerle,D.Hahnel,D.Dolgov,S.Thrun,andW.Burgard,“Autonomousdrivinginamulti-levelparkingstructure,”inICRA,2009,pp.3395–3400.[10]K.Yoneda,H.Tehrani,T.Ogawa,N.Hukuyama,andS.Mita,“LiDARscanfeatureforlocalizationwithhighlyprecise3Dmap,”inIV,2014.[11]P.J.Besl,N.D.McKayetal.,“Amethodforregistrationof3-dshapes,”T-PAMI,vol.14,no.2,pp.239–256,1992.[12]J.LevinsonandS.Thrun,“Robustvehiclelocalizationinurbanenvironmentsusingprobabilisticmaps.”inICRA,2010.[13]M.Adams,S.Zhang,andL.Xie,“Particlefilterbasedoutdoorrobotlocalizationusingnaturalfeaturesextractedfromlaserscanners,”inICRA,vol.2,2004,pp.1493–1498.[14]R.K¨ummerle,R.Triebel,P.Pfaff,andW.Burgard,“Montecarlolocalizationinoutdoorterrainsusingmultilevelsurfacemaps,”JournalofFieldRobotics,vol.25,no.6-7,pp.346–359,2008.[15]I.BaldwinandP.Newman,“Roadvehiclelocalizationwith2DPush-broomLIDARand3Dpriors,”inICRA,2012,pp.2611–2617.[16]——,“Laser-onlyroad-vehiclelocalizationwithdual2DPush-broomLiDARsand3Dpriors,”inIROS,2012,pp.2490–2497.[17]M.Sheehan,A.Harrison,andP.Newman,“Continuousvehiclelocalisationusingsparse3Dsensing,kernelisedr´enyidistanceandfastgausstransforms,”inIROS,2013,pp.398–405.[18]Z.Chong,B.Qin,T.Bandyopadhyay,M.H.Ang,E.Frazzoli,andD.Rus,“Synthetic2DLiDARforprecisevehiclelocalizationin3Durbanenvironment,”inICRA,2013,pp.1554–1559.[19]W.Maddern,G.Pascoe,andP.Newman,“Leveragingexperienceforlarge-scaleLIDARlocalisationinchangingcities,”inICRA,2015.[20]R.W.WolcottandR.M.Eustice,“RobustLIDARlocalizationusingmultiresolutiongaussianmixturemapsforautonomousdriving,”TheInternationalJournalofRoboticsResearch,vol.36,2017.[21]E.B.Olson,“Real-timecorrelativescanmatching,”inICRA,May2009,pp.4387–4393.[22]B.D.LucasandT.Kanade,“Aniterativeimageregistrationtechniquewithanapplicationtostereovision,”inIJCAI,ser.IJCAI,SanFrancisco,CA,USA,1981,pp.674–679.[23]S.BakerandI.Matthews,“Lucas-kanade20yearson:Aunifyingframework,”IJCV,vol.56,no.3,pp.221–255,2004.[24]J.Engel,V.Koltun,andD.Cremers,“Directsparseodometry,”T-PAMI,vol.40,no.3,pp.611–625,2018.[25]S.Thrun,W.Burgard,andD.Fox,Probabilisticrobotics.MITpress,2005.[26]S.KullbackandR.A.Leibler,“Oninformationandsufficiency,”Ann.Math.Statist.,vol.22,no.1,pp.79–86,031951.[27]L.Wanninger,“Carrier-phaseinter-frequencybiasesofGLONASSreceivers,”JournalofGeodesy,vol.86,no.2,pp.139–148,2012.[28]T.Z.X.-W.Chang,X.Yang,“MLAMBDA:amodifiedLAMBDAmethodforintegerleast-squaresestimation,”JournalofGeodesy,2005.[29]T.TakasuandA.Yasuda,“CycleslipdetectionandfixingbyMEMS-IMU/GPSintegrationformobileenvironmentRTK-GPS,”inION,2008,pp.64–71.[30]P.G.Savage,“Strapdowninertialnavigationintegrationalgorithmdesignpart2:Velocityandpositionalgorithms,”JournalofGuidanceControlandDynamics,vol.21,no.2,pp.208–221,1998.[31]D.O.Benson,“Acomparisonoftwoapproachestopure-inertialandDoppler-inertialerroranalysis,”T-AES,no.4,pp.447–455,1975.
相关推荐
2023年江西萍乡中考道德与法治真题及答案.doc
2012年重庆南川中考生物真题及答案.doc
2013年江西师范大学地理学综合及文艺理论基础考研真题.doc
2020年四川甘孜小升初语文真题及答案I卷.doc
2020年注册岩土工程师专业基础考试真题及答案.doc
2023-2024学年福建省厦门市九年级上学期数学月考试题及答案.doc
2021-2022学年辽宁省沈阳市大东区九年级上学期语文期末试题及答案.doc
2022-2023学年北京东城区初三第一学期物理期末试卷及答案.doc
2018上半年江西教师资格初中地理学科知识与教学能力真题及答案.doc
2012年河北国家公务员申论考试真题及答案-省级.doc
2020-2021学年江苏省扬州市江都区邵樊片九年级上学期数学第一次质量检测试题及答案.doc
2022下半年黑龙江教师资格证中学综合素质真题及答案.doc
