第2期 伍明，等：基于粒子滤波的未知环境下机器人同时定位、地图构建与目标跟踪 ·171· 2.2粒子权值的计算 阵，为式(12)对t的雅克比阵在s和t处的值，即 下面介绍粒子权值的计算，粒子权值相当于该 粒子对应观测值的相似度，即 w=p(zk|S2,k,乙g-1,4)= (-2+-y.-3-y 0 -x0 p(24乙n|52,4%,2k-1,4s)= 类似，可以计算出p(Is) p(1S,zk-1,)p(z|S,4,3k-1,) 2.3目标和标志柱均值和方差的计算 Mankoy p(zn,1S,乙-1，us)= 接下来介绍目标点状态的估计，基于单个粒子 p(zI s)p(zI si,t)p(zm I s) 目标点状态估计可表示为 p(I s)p(z). (11) p(41s,)g 式中：，之之，分别代表对已知标志柱、目标和新标 ap()p() 志柱的观测值，‘为预测阶段根据EKF得到的目标 p(as14,)p(1s,,w） 状态（参见下2.3节内容），而p(zm,Is)认为是均匀 p(a1,4)p(,l5克，，wm 分布的，因此最终权值可表示为标志柱观测值相似 度和目标观测值相似度之间的乘积.以下具体介绍 p(zs1si,4)p(e1t-1,s,z-l, 目标观测值相似度p(z,Is,)的计算， dp(4l,2,)1 假设机器人的观测值为距离和角度信息，则机 2(zs)p(1,4)p(l1之，)d出- 器人对目标的观测模型： 更新阶设 预碳阶没 √(x-x)2+(y-y)7 (15) z4=h(s,)= argtan( 式中：α为归一化系数，s为k时刻第i个粒子状态 -0 √(x-x)2 式(15)可以应用EKF进行估计，EKF分为预测和更 (12) 新2个阶段，分别介绍如下 式中：s=[xRyR0]T表示机器人的状态量，t= 2.3.1EKF预测阶段 [xy8]T表示目标的状态量，d和0分别为机器 本文假设目标以恒速u=[vw]T运动并且u 人的观测值. 上存在协方差为Q”的误差（这里假设目标的运动 假设观测值相似度服从正态分布p(z)~N(, 为单模态形式，多模态运动形式将是今后研究的目 ),则相似度函数p(z,Is,)(为了简洁表示省去上 标).并且目标运行遵循差动驱动模型，即非完整性 标和下标)如式(13)： 约束模型： p(zI s,t)= ti=g(ti,uk)= 2mp(-2g-i))g-p》 -1 k·△t·co8(0k-1+△t·wx)7 yi-1 g·△t·sin(0k-l+△t·ws） (16) (13) 0k1 △t·①k 式中：为根据该粒子表示的机器人状态s和预测目 式中：△t为输人的间隔时间.从式(16)可见，目标在 标状态t由式(12)得到的预测观测值，乙：为实际的观测 k时刻的不确定性由2部分组成，1)是由k-1时刻 值，号为观测值的协方差矩阵，下面介绍的计算. 的不确定性传播而来，记作-1；2)是由输人量的 协方差阵反映对目标观测值的误差情况，这 不确定性Q传播而来，记作.则根据误差传播公式 种误差由三方面造成，首先是传感器本身的误差，设 可以得到k时刻的预测误差协方差阵为 其值为R“;其次是由目标状态不确定性引起的误 残=+”= 差，设其值为R;最后为机器人状态不确定性引起 H-1(H)T+HQ(). (17) 的误差，由于这里用粒子群来代表这种不确定性，因 式中：H"为式(16)对于输人4的雅克比阵在4s和 此对于单个粒子来说，认为机器人状态不存在误差。 -1处的值，H为式(16)对于机器人状态t的雅克 最终表示如式(14)： 比阵在k和1-1处的值，即 =R+Rm=H()T+Rm.(14)》 H=08 式中：为预测阶段由EKF得到的目标状态协方差 d让，4-1三