基于中心差分粒子滤波的SLAM算法

1、362Vol.36,No.2February,201020102ACTAAUTOMATICASINICASLAM1111Rao-Blackwellized,.(Simultaneouslocalizationandmapping,SLAM),Sterling,;,SLAM.FastSLAMSLAM,.;.,Rao-Blackwellized,ASLAMAlgorithmBasedonCentralDierenceParticleFilterZHUJi-Hua1ZHENGNan-Ning1YUANZe-Jian1ZHANGQiang1,.802,1,34.SLAM56,.,DoucetRao-B

2、lackwellizedSLAM(Simultaneous7localizationandmapping).内容版权归作者所有:36Pamt1(3)内容版权归作者所有2502:SLAM251(c)1h,wi=24h2,w(c2)i=h113.(3):,:aimt1=imuimTt1,t(5),f(·)(6):¯im=f(imumuimtt1,t+t)(6),:xmt|w(c)imt1=2Li¯t(7)i=0Pmt|t1=w(c1)(¯imi+Lmiti=1¯t)2+w(c2L)(¯imit+¯i+Lmt2¯0mt)

3、2(8),(7)(8).,6.,:xmt|t1xnmnmPmt|t10t|t1=,Pt|t1=Rtnmmt|t1=xnxnmt|t1t|t1±hw(c)nm11Pt|t1,P(1:L,:)PL().Kmt:Kmmmt=t(St)1:x¯mmt=xmt1+Kt(ztnmt)Pm=Pmmmmtt1KtSt(Kt)T.,(9)(17).,(16)(17)(7)(8).,xmtN(x¯mmt,Pt)1(15)(16)(17):(18)内容版权归作者所有252Rt(19)h1(·):M¯imt=h1(xmt,im)(20):µmnt,t=2lw

4、(c)iM¯imt(21)i=0mnt,t= lw(c1)i(im+i=1i+lm)2w(c2)i(im+i+lm20m)2(22),lL,(4).,:m=µmnt,t1µmnt,t1±h内容版权归作者所有36w(c)m11nt,t1 Z¯1:lml+1:2lmtZ¯tT(27)¯mKt:K¯m=¯mtt(S¯mt)1(28):µmmnt,t=µnt,t1+K¯m(ztzmtt)(29)mmnt,t=nt,t1K¯mmtS¯t(K¯mt

5、)T(30),Sterling,.,.,13.2.3:mmwmwt|xt1,uut)p(ztt=|m,xxmmp(xt)t12:SLAM253(wm)2iM(32)=1Ne(M/2,M),.,17,.2.4,.,9“”.,.SLAM5,:,t,.,.T,K,;.,.,.2.5CDFastSLAM:1):Xmt1,2.1,(18),Xmt2);:(31);3)Ne:,(32);4):Ne:,(Xmwmt,t)(Xjt,1/N);5):,.6):,2.2;7),.M,N,n,p.CDFastSLAM:,M,.O(Mp).O(M).O(M),O(Mn).,Ne,O(M).,O(MN),9,O(Mlog(N).,CDFastSLAM.,CDFastSLAMFastSLAM2.0.3,9FastSLAM2.0,12UFastSLAM,CDFastSLAM