·170. 智能系统学报 第8卷 展开可得 p()=p()dsd= 2(2()dsd (6) 第项 第项 式(6)中已经体现出递归式估计过程，其中积 p(st,t,Lul z",u")= 分中第2项代表了k-1时刻得出的联合后验概率 p(sp(Ip(I). 密度.为了得出最终的预测概率密度还需计算式 (6)中第1项p(s,z-1,W,k-1,在-1，Lw),该项代表 (9) 了机器人状态和目标状态的联合概率密度从k-1到k 也就是说该问题能够分解成为一一个对机器人状 时刻发生的变换，以下分2种情况进行讨论。 态的后验概率估计问题，一个对目标状态的后验概 1)当机器人状态和目标状态不相关的情况，此 率估计问题以及对m个标志柱位置的估计问题，该 时有 结论是本文所用方法的基础， p(s,4lz-,d,-1,,Lw）= 重写式(9)如式(10)所示： p(SI S-,u)p(tt,u). (7) p(s,t,Lyzu)= 从式(7)可知，机器人和目标状态的联合概率 p(slt,)4ls大，)Πp(Is大，) -N(wT(-》:Q级 密度变化为机器人状态变化和目标状态变化的乘 -N(Im:i-i.k-1) 积，两者之间并没有相互作用. (10) 2)当机器人状态和目标状态相关时有 即目标和标志柱服从Gaussian分布，因此可以 p(st,tyl z,u,st-,tt-,Ln)= 用EKF对它们进行估计，本文利用粒子滤波完成对 p(s4lt-,t,-1,4-,4e,Lw)p(elz-1, 机器人状态p(sz,)的估计.为了完成标志柱和 目标的估计，算法对每一个粒子所对应的标志柱 u,Sg-l,t-l,Lw）d止-l= p(1ms,z,)和目标状态p(tlst,z,)运用 p(sls-1,W,l)p(al1,t)d此…(8) EKF进行估计，其中s表示第i个离子在k时刻所 式(8)由全概率公式得出，表示了机器人状态变化 代表的机器人状态，因此假设离子总数为N,观测 受到了k-1时刻目标状态的影响，这种情况体现在现 到标志点个数为L,目标个数为1，则算法总共包含 实中为机器人不光要跟踪目标同时还需要尾随目标的 N,×(L+1)个EKF. 情况.下面介绍解决SLAMTO问题的具体实现方法， 首先，构造粒子群如下： 2基于Rao-Blackwellised粒子滤波器 =,i222-指 式中：S表示k时刻第i个粒子的位置其表示的机 的SLAMTO实现 器人的状态，u2和，2分别表示第i个粒 2.1问题的因式化表示 子k时刻已发现的1|个标志点位置的均值和方 如图1所示，如果知道机器人的路径s“= 差，和表示k时刻第i个粒子对应的目标状 [s:2…S],那么时间序列上的观测值z”= 态均值和方差.那么最终构造出的k时刻粒子群结 [z2… ,]将是相互独立的.那么式(1)能够进 构如图2所示 行因式化，如式(9)： 机器人状念 日标状态 路标1 路标L 粒子1： [xty:0:]T [4T LwE公 粒子2： [x贤]T Lu Lu L4421 粒子N: S [:]T LL [u L吃' 图2粒子结构示意 Fig.2 Structure of each particles