·172· 智能系统学报 第8卷 「·cos(04+·)-·△2·sin(04+△·)门 13)F5.≠0 d·sin(0+·）4··cos(0:+·4) 14)ww =p(nIs)p()() 0 15)iJ=EKFCi =g. 16)ENDIF 17)0=0·0w T10-vk·△t·sin(0-1+△t·we)1 18)Fzm,≠☑ 01tk·△t·cos(0-4+△·） L00 1 I9）[i2-,'元2，.」=ADD_LANKMARK 2.3.2EKF更新阶段 ([iaa2i24]) 在每一次迭代过程中若机器人没有发现目标， 20)ENDIF 即目标的观测值为空时，那么EKF只进行预测，其 21ELSE 结果是目标的不确定性将不断增大.当机器人发现 22)[ui2-n2i2-a,]=[n2h12i2hn1 目标时，EKF将利用观测值对目标的均值和方差进 23)ENDELSE 行修正，而减少目标的不确定性， 24)ENDFOR 假设由预测阶段已经得到目标均值和方差为 25)ENDIF 和，而实际观测值为乙，则EKF对均值和方差 26)IF Resample_condition_satisfy 的更新为 27)[{,,2-,2a]=RESA- i=ui+Kkaiman (z Hui), MPLE[ 花=(I-Kkaim H') (18) 28)ENDIF 式中：Kkdman为卡尔曼系数其值，Kamn= 其中1)~5)是预测阶段(prediction step),首先 ()T(H()T+Rm)1,Rm为传感器误差 根据机器人状态转移方程对粒子进行预测撒点（粒子 协方差阵。 滤波形式)，之后对该粒子中对应的目标状态均值和 单个粒子标志柱位置估计也同样能够用相似的 方差利用目标的状态转移方程进行预测(EKF预测 方式进行描述，不再赘述， 阶段式(16)和(17)，此时的预测结果是存在误差的， 2.4 SLAMTO算法 算法主要思想是对整个粒子群应用粒子滤波进 当有观测值出现，算法进入修正阶段，即6)~24)，主 行重要性采样及重采样，而在每个粒子中又应用 要包括：根据式(11)进行的粒子权值的更新，目标状 EKF对标志柱及目标的均值和方差进行预测和校 态和标志柱位置的更新.具体来讲，第8)行运用数据 正操作，具体算法如下。 关联方法将观测值2区分为江，2，（分别表示对 [{i,ui2-a,}]= 标志柱、目标、新标志柱的观测值)，而Table则包含 PFSLAMT[-12山2a马a--格] ,和已知标志柱的对应关系，需要注意的是每个粒子 i均拥有自己的k时刻环境地图1,2，…，1m,并根据 1)F0Ri=1:N, 2)s是~p(sxls话-1，x), 该地图进行数据关联.9)~12)行利用z4,和式(11)对 3)=g(,4), 粒子权值进行更新，并根据式(18)用EKF对目标的 4)=-1(H)「+酽Q(H), 均值和方差进行修正，类似地13)~16)行利用对 5)ENDFOR 粒子权值进行更新，并用EKF对已知标志柱均值和 6)IFz%≠☑ 方差进行修正，需要注意的是当观测到多个已知标志 7)FOR i=1:N, 柱时粒子权值等于其对应的每一个相似度(likehood) 8)[Table =Data-association 的乘积.17)行根据式(11)生成最终的粒子权值.18) [n.2-h2i2.mh]） ~23)行完成对于标志柱地图的更新.26)~27)行利 用文献[13]介绍的方法对粒子群进行重采样。 9)Fz,≠0 10)wi=p() 3实验结果 11)[u]=EKFUPDATE() 下面首先通过仿真实验从定位准确性、粒子数 12)ENDIF 量和标志柱数量对定位的影响三方面来验证算法的