基于粒子滤波的移动机器人SLAM算法

第40卷第1期2010年1月东南大学学报(自然科学版)JOURNALOFSOUTHEASTUNIVERSITY(NaturalScienceEdition)Vo.l40No.1Jan.2010doi:10.3969/.jissn.1001-0505.2010.01.022SLAM涂刚毅金世俊祝雪芬宋爱国(,210096):针对FastSLAM算法对传感器精度要求较高,不适用于方向性差的超声传感器问题,提出了一种基于超声概率栅格地图环境特征点提取匹配的移动机器人粒子滤波同时定位与地图创建(SLAM)算法.该算法可分解为机器人位姿估计和环境路标估计2个部分.基于蒙特卡罗定位原理利用粒子滤波算法对机器人运动轨迹进行估计;在建立全局超声概率栅格地图的基础上,利用概率栅格地图环境特征提取算法对环境路标坐标进行估计.实验证明,该算法较好地解决了超声测距传感器由于散射角大带来的特征点估计不准的问题,对环境路标和机器人轨迹的估计都比较准确.并对移动机器人累计误差进行了有效的补偿,减少了由于累积误差造成的移动机器人轨迹扭曲失真.:移动机器人;同步定位与地图创建;粒子滤波器;栅格地图:TP242:B:1001-0505(2010)01011706ParticlefilterSLAMmethodformobilerobotTuGangyiJinShijunZhuXuefenSongAiguo(SchoolofInstrumentScienceandEngineering,SoutheastUniversity,Nanjing210096,China)Abstract:FocusingonthelimitedapplicationofFastSLAMalgorithmduetoitshighaccuracyrequirmentforsensorandunfitnessforultrasonicsensorwithpoordirectivity,asimultaneouslocalizationandmapping(SLAM)algorithmutilizingparticlefilterisprovided,whichisbasedonultrasonicprobabilitygridmapfeaturepointsextractionandmatching.Thisalgorithmconsistsofrobotposeestimationandenvironmentlandmarkestimation.ParticlefiltersareappliedtoestimatetherobottrajectoryaccordingtoMonteCarlomethods.Basedonglobalultrasonicprobabilisticgridmap,theenvironmentlandmarkpositionisobservedbyenvironmentfeatureextractionalgorithm.Theeffectivenessoftheproposedalgorithmisvalidatedbyexperimentalresul.tTheestimationaccuracyoffeaturepointpositionaswellasenvironmentlandmarkareimproved.Thecumulativeerroroftherobotiscompensatedeffectivelyandthemobilerobottrajectorydistortionisreduced.Keywords:mobilerobo;tsimultaneouslocalizationandmapping;particlefilter;gridmap:20090617.:(1981),,;(),,,,[e-mail].t:(708045)!(06D031)(4022001004).:,,,.SLAM[J].:,2010,40(1):117122.[do:i10.3969/.jissn.1001-0505.2010.01.022].,.,,,,(simultaneouslocalizationandmapping,SLAM).SLAM.(particlefilter,PF),(sequentialMonteCarlomethods,SMC),,.,MontemerloRaoBlackwellized(RaoBlackwellizedparticlefilter,RBPF)FastSLAM[12].SLAM(extendedKalmanfilter,EKF)2.EKF,,.FastSLAM,.,,,.,[3][4],SLAM.,.4.∀.,.#..∃.PF,,,.%.,.1[56](MonteCarlolocalization,MCL)(Bayesianfilter,BF).,xk.2:zk={^k,^k}T,,^k,^k;uk={sk,!k}T,,sk,!k.zk,p(xkzk,uk)[7]:p(xkzk,uk)=∀p(zkxk)&∋p(xkuk,xk-1)p(xk-1zk-1,uk-1)dxk-1(1),∀;p(zkxk),;p(xkuk,xk-1)uk,xk,.2SLAM21SLAMSLAMxkskm,xk={sk,m}T,sk=(xk,yk,!k)xk,yk;m={m1,(,mN}N.PFSLAM:zk={z1,(,zk}uk={u1,(,uk},sk={s1,(,sk}p(skzk,uk,nk)p(sk,mzk,uk,nk).(1)[1]p(sk,mzk,uk,nk)=p(skzk,uk,nk)p(msk,zk,uk,nk)=p(skzk,uk,nk))Ni=1p(misk,zk,uk,nk)(2),nkk,nk={n1,n2,(,nk}.(2),SLAM2.22PF[7],PFSLAM.:s0Num.:.∀,Nums(i)0w(i)0s0={s(i)0,w(i)0}i=1,(,Num.#[89].k-1sk-1s(i)k-1p(s(i)kuk,s(i)k-1),ks(i)k.∃p(zks(i)k)s(i)k,m(i)k.118东南大学学报(自然科学版)第40卷%w(i)k,w(i)k=w(i)k-1p(zks(i)k)(3)∗w(i)k=w(i)k+Numi=1w(i)k(4),w(i)ksk[10],,,.−[2].p(misk,zk,uk,nk)mi.23sk,s(i)k.Sk,Sk={s(1)k,(,s(i)k}i=1,(,Num(5)s(i)k=(x(i)k,y(i)k,!(i)k)(6)sk=+Numi=1s(i)kw(i)k(7),i.,skuk.,s(i)kp(s(i)kuk,s(i)k-1).,NskNsk.,,Nx,Ny,N!.p(s(i)kuk,s(i)k-1):s(i)k=x(i)k-1y(i)k-1!(i)k-1+(sk+Nsk)cos!k(sk+Nsk)sin!k!k+N!k+NxNyN!(8)241,1^k^k=(x^L-x/k)2+(y^L-y/k)2arctany^L-y/kx^L-x/k-!k(9),(x^L,y^L);(x/k,y/k).,s(i)kp(zks(i)k).(x/(i)k,y/(i)k)(x(i)k,y(i)k)L,p(zks(i)k)z(i)L,k=x^(i)L,ky^(i)L,k=x/(i)k+^kcos(!(i)k+^k)y/(i)k+^ksin(!(i)k+^k)=x(i)k+Lcos!(i)k+^kcos(!(i)k+^k)y(i)k+Lsin!(i)k+^ksin(!(i)k+^k)(10),z(i)L,ks(i)k.25,,..[2,11].,R,;R.,..mi:mi=+Cxy.#(P(OCxy)PCxy)+Cxy.#P(OCxy)(11),#;PCxyCxy;P(OCxy).zk=(^k,^k),m.,(nearestneighbor,NN),.,,f(s(i)k,m)=minmi.m{d(mi,z(i)L,k)}(12),d(mi,z(i)L,k)s(i)kz(i)L,kmi;f(s(i)k,m)s(i)km.Th,f(s(i)k,m)Th,,.119第1期涂刚毅,等:基于粒子滤波的移动机器人SLAM算法26z(i)L,kmi,w(i)k,(2)mi((i)k,(i)k).zk={k,k}T.k=k-(i)k,k=k-(i)k.k!k,k0!k.!k,k~N(M!,∃2!).,k~N(M,∃2).k,k,p(zks(i)k)[12]:p(zks(i)k)=GM!,∃2!(k)GM,∃2(k)(13),G.(13)(3)zk.3!,SLAM.,8m18m.,,SLAM.!,.,,180211252,,.,,.2,,[2].2,1.1cm1(-117.5,44.7)(-115,48)4.142(11.3,219.5)(13,225)5.753(88,96)(91,101)5.834(163.2,288.4)(165,293)4.935(174,-70.6)(180,-73)6.466(268.6,122.3)(271,118)4.92,.3.,!3∃.3,.,,.120东南大学学报(自然科学版)第40卷,,,,,4.(a)X(b)Y(c)46,5(a)(b).(a)(b)55(a),,,.5(b)PFSLAM.,.,.6,.,.(a)X(b)Y(c)64,PFSLAM,,,.SLAM.(References)[1]MontemerloM,ThrunS.SimultaneouslocalizationandmappingwithunknowndataassociationusingFastSLAM[C]3ProcIEEEIntConfRoboticsandAutomation.Taipe,iChina:IEEEPress,2003:19851991.121第1期涂刚毅,等:基于粒子滤波的移动机器人SLAM算法[2],.FastSLAM2.0[J].,2009,31(1):2532.ZhouWu,ZhaoChunxia.AFastSLAM2.0algorithmbasedongeneticalgorithm[J].Robot,2009,31(1):2532.(inChinese)[3]PagacD,NebotEM,DurrantWhyteH.Anevidentialapproachtomapbuildingforautonomousvehicles[J].IEEETransactionsonRoboticsandAutomation,1998,14(4):623629.[4],,,.SLAM[J].,2007,19(16):37153718,3723.JuChunchun,HeBo,LiuBaolong,eta.lSimulationresearchonsimultaneousrobotlocalizationandmappingbasedonparticlefilter[J].JournalofSystemSimulation,2007,19(16):37153718,3723.(inChinese)[5],,.MonteCarlo[J].:,2007,37(1):4044.FangFang,MaXudong,DaiXianzhong.NewMonteCarloalgorithmformobilerobotselflocalization[J].JournalofSoutheastUniversity:NaturalScienceEdition,2007,37(1):4044.(inChinese)[6],,.MonteCarlo[J].,2008,30(3):210216.LiangZhiwe,iMaXudong,DaiXianzhong.An