第2期 伍明，等，一种机器人未知环境下动态目标跟踪交互多模滤波算法 ·135· Xg=∑X, (48) 行即可 P=∑{P+[X-g]×[X-g]T}u, 3仿真实验结果 (49) 下面将通过仿真实验来验证该方法的有效性， 2)不同模态状态预测阶段.对于目标运动不同 实验是在Matlab7.5下进行.为了能够体现机器人 模态T∈M,根据式(27)、(35)完成各模态对应系 机动性的特点，这里设计控制律使得机器人能够跟 统状态向量和协方差阵的预测： 随目标运动.具体来说，首先计算机器人位置和目标 X=StatePredict(X,u,At,A), 位置的角度，以此作为机器人角度控制量，之后计算 (50) 机器人位置和目标位置的距离，以此作为机器人速 Pux=CovariancePredict 度控制量，这里设置了几个速度标准并根据距离的 (P,Xg,u袋k,2,△t,A24,Quk）.(51) 远近来选择不同的速度标准 若有观测值出现继续，否则转到6). 目标在不同时段分别采用定速度模型和定加速 3)不同模态状态更新阶段.利用式(46)、(47) 度模型2种不同运动模式，该实验首先在长度为 完成不同模态系统状态和协方差阵的更新 -250~250m,宽度为-250~250m的正方形区域 +山+1=区+k+K区+11， (52) 内均匀产生800个特征点.为了更准确地分析结果， P++1=P+k-K+IS2+(K+1)T.(53) 假设运动开始时机器人就发现了目标，机器人的运 4)模式概率更新.不同于传统的MM,此处 动控制误差阵为Q=diag(12m,0.252rad),机器人 的取值只考虑目标观测值可能性并不考虑特征观测 观测误差为R·=diag(0.12m,0.052rad)机器人的 值可能性，因此=N((1:2);0,IS(1:2,1: 观测视野为前向，观测最大距离为100m.目标在定 2)),(1:2)表示取的前2项（对应目标观测值 速度模型下的噪声变量gcM=0.1,在定加速度模 差异)，IS(1:2,1:2)表示取IS的1~2行和1~ 型下的噪声变量g心“=1.8，每一轮迭代的间隔时 2列子阵（对应目标观测协方差子阵）.之后对于各 间为△t=0.158,仿真共进行200次迭代.所得到的 模式计算本次更新后的模式可能性，即 结果如图3所示.图3显示了实验的总体情况，其中 4=(1/c)∑p1=(1/c)9(54) 虚线和实线分别表示目标的实际和估计轨迹；虚线 和实线分别表示机器人的实际和估计轨迹；带星号 式中：c为归一化因数. 5)系统扩充.当存在对于新特征的观测值时， 三角代表已发现标志的概率分布，其中星号代表分 根据式(11)完成状态向量的扩充，式(18)完成协方 布均值，三角代表不确定性三角；星号代表特征的实 差阵的扩充， 际位置.机器人和目标的起始位置为(0,0)，机器人 6)系统状态估计，协方差阵组合产生.最终通 的终点为(97,2.65)，目标的终点为 过加权得到本次迭代的系统状态估计和协方差阵， (101.1,-3.68).从图中看出机器人为了追赶目标 计算方法如下. 出现了几次环型运动. 一机器人估计运动轨迹 X41=∑4lk+4, (55) 40 ··一一机器人实标运动轨迹 20: 机器人。“ 日标估计运动轨迹 日标实际运动轨迹 Pe=∑{P+k+l+ 0 入特征估计分布 -20 自标 ·特征实际位置 [Xnk41-Xe]×[及+k+1-X]T{A.(56) -40 以上算法过程假设目标已经出现，如果目标没 -60 020020406080100 有出现，那么系统将按照传统EKF_SLAM的方法运 X/m 行，当发现目标后首先利用式(10)、(12)对系统状 图3仿真总体图 态向量和协方差阵进行扩充，之后按照上述方法运 Fig.3 Simulation results of SLAMTO