基于特征地图的移动机器人EKF-SLAM和FastSLAM算法自主导航研究

2010624280JournalofBeijingUnionUniversityNaturalSciencesJun.2010Vol.24No.2SumNo.802010－02－0150675161607050351985－、1961－、。EKF-SLAMFastSLAM430081SLAM。2DPythonPythonNumPy、PyLabEKF-SLAMFastSLAM。。TP242.3A1005-0310201002-0018-07TheAutonomousNavigationResearchofMobileRobotEKF-SLAMandFastSLAMAlgorithmBasedonFeatureMapTAOhuiWUHuai-yuCHENGLeiYANGShengEngineeringResearchCenterforMetallurgicalAutomationandDetectingTechnologyMinistryofEducationWuhan430081ChinaAbstractMobilerobotautonomousnavigationandSLAMSimultaneousLocalizationandMappinginanun-knownenvironmentarediscussed.Amethodtobuildtwo-dimensionalvisuallandmarkfeaturemapsisdesignedandbasedonmonocularvisionsensorandodometeroftherobustperceptualmodelathree-dimensionalinforma-tionlandmarkdatabaseintheworldcoordinatesystemisbuiltandfeaturemapisgained.AmethodtoanalyzetheautonomousnavigationofamobilerobotbasedonPythonbymeansofputtingintoextendedmoduleNumPyandPyLabisputforward.OnPythonasimulationplatformisconstructedandthenavigationprocessisanalyzedbyEKFExtendedKalmanFilter-SLAMandFastSLAMalgorithmunderdifferentnoiseenvironments.Resultsshowthatthecompositionprogramandsimulationplatformaresuitable.KeywordsmobilerobotsimultaneouslocalizationandmappingvisuallandmarksfeaturemapextendedKalmanfilter0。242EKF-SLAMFastSLAM1、2。。SLAMSimultaneousLo-calizationandMapping3-45-7。SLAMCsorba、SLAMThrun、BurgardEKFEx-tendedKalmanFilterSLAMEKF-SLAMMontemerloFastSLAMRBPFRao-BlackwellizedParticleFilter3-48。SLAM、“”。3～89、10、11。SLAM、、。、、12SLAM13SLAM14SLAM15。15SLAM。SLAMPythonEKF-SLAMFastSLAM、。1SLAM1.1SLAMSLAM。SLAM1。1SLAM1xkkzkikiukk－1xkmii。SLAMk－1Pxk－1m|zk－1uk－1x0ukzk。16-17Pxkm|zk－1ukx0=∫Pxk|xk－1ukPxk－1m|zk－1uk－1x0dxk－11Pxkm|zkukx0=Pzk|xkmPxkm|zk－1ukx0Pzk|zk－1uk21、2Pxkm|zkukx0。1.2RBPF18。91201061SIFT234EKFRBPF。2aSIFT2b、2c。2bA、B、C、D42D———3。41。12bA1.87－1.056.90B－1.67－2.199.77C－0.820.4816.20D－0.120.612.733、。EKFRBPF。。2a2b2c02242EKF-SLAMFastSLAM31.3xyθxyθ。XRPYR、X1OY1419。44xk=xk－1+Δlkcosθk－1+αkyk=yk－1+Δlksinθk－1+αkθk=θk－1+atanΔlksinαkx2k－1+y2k－槡1+Δlkcosα()k3xkykkθkkΔlkk－1kαkk。xiGk=xiGk－1yiGk=yiGk－14xiGkyiGki=1…nixiRkyiRkixiGkyiG[]k=RxiRkyiR[]k+xky[]kR=cosαk－sinαksinαkcosα[]k5R。2Python。。。－140X2V、1V。PythonEKF-SLAMFastSLAM。2.1PythonPythonNumPy。NumPynndarray、、FortranC++1220106。PyLab。PyLab5。52.2EKF-SLAMEKF-SLAM20。Kalman6。6Kalman6k^xk^x－kk、zkkukkHkVkkPkAkWkkQkRkk。2.3FastSLAMFastSLAMEKFSLAM。21。FastSLAMRBPFEKF、。Kalman。Xt－1utztmk－1Rao-BlackwellisedFastSLAMX′t=Xt=formin1mdoxmt=pxt|ut－1xmt－1wmt=pzt|xmtX′t=X′t+〈xmtwmt〉formin1mdomxitXtreturnXt2.4EKF-SLAMFastSLAM+7a、7b+7c、7d。7、、2。7c10.6－54.110.6－144.01m。。22242EKF-SLAMFastSLAM7EKF-SLAMFastSLAM。EKF-SLAM、FastSLAM、。Python。2a2.71－143.641.451.54519b1.50－1433.4－11.20.2809d0.721－144.59－8.720.5282133.1－20.30.6303。。PythonFastSLAMEKF-SLAM。、SLAM、、。32201061KimYWonCPakJ.FuzzyadaptiveparticlefilterforlocalizationofamobilerobotC∥LectureNotesinComputerSci-ence200741－48.2SasiadekJZHartanaP.Sensorfusionfordead-reckoningmobilerobotnavigationC∥Proceedingofthe6thIFACSympo-siumonRobotControlSYROCO2000531－536.3Durrant-WhyteHBaileyT.SimultaneouslocalizationandmappingSLAMpartIessentialalgorithmsJ.IEEERoboticsandAutomationMagazine200613299－108.4Durrant-WhyteHBaileyT.SimultaneouslocalizationandmappingSLAMpartIIstateoftheartJ.IEEERoboticsandAutomationMagazine2006133108－117.5.SLAMJ.200741121982－1986.6BaileyT.ConsistencyoftheEKF-SLAMalgorithmC∥InternationalConferenceonIntelligentRobotsandSystems.Bei-jing20063562－3568.7PazLMJensfeltPTard′osJDetal.EKFSLAMupdatesinOnwithdivideandconquerSLAMC∥ProceedingIEEEInternationalConferenceonRoboticsandAutomationItalyRome20081657－1663.8MontemerloMThrunSKollerDetal.FastSLAMAFactoredSolutiontotheSimultaneousLocalizationandMappingProblemC∥ProceedingsoftheAAAINationalConferenceonArtificialIntelligence200451－59.9LeonardJJDurrantWhyteF.SimultaneousmapbuildingandlocalizationforanautonomousmobilerobotC.ProceedingsoftheIEEEInternationalworkshoponIntelligentRobotsandSystemsJapanOsak19971442－1447.10LeonardJJFederHJS.AcomputationallyefficientmethodforlargescaleconcurrentmappingandlocalizationC∥Pro-ceedingsoftheNinthInternationalSymposiumonRoboticsResearchLondonSpringerVerlag1999316－321.11GuivantJNebotEBaikerS.AutonomousnavigationandmapbuildingusinglaserrangesensorsinoutdoorapplicationsJ.JournalofRoboticSystems20061710565－583.12.J.2004365566－569.13.Line-feature-basedSLAMAlgorithmJ.200632143－46.14.J.2005316925－933.15.J.20094533245－248.16.M.1965.17.M.1987.18.Rao-BlackwellizedJ.2007372402－406.19SiegwartRNourbakhshIR.M..2006.20WelchGBishopG.AnIntroductiontotheKalmanFilterR.USUniversityofNorthCarolinaatChapelHill200624－30.21.ANovelMethodforMobileRobotSimultaneousLocalizationandMappingJ.A200676937－944.42