第2期 伍明，等：基于粒子滤波的未知环境下机器人同时定位、地图构建与目标跟踪 ·173· 有效性，仿真是在Matlab7.0环境下进行的，假设环 651 境中包含若于个标志柱、一个机器人以及一个目标. 650 为了减少问题的复杂性，假设环境中不存在障碍物， 6.49 机器人能够感知一定范围内的目标和标志柱，并且 获得以方向和深度为表征的观测值.另外，机器人会 =648 根据目标位置调整输入使其能够追踪目标，而目标 647 速度及角速度控制量为恒定值， 6.46 3.1实验1 假设环境为10×10m2,其中均匀分布着20个 645249咖5咖6927922妙909892 标志柱，算法采用的粒子数为200，并假设机器人速 (d)区域3标志柱粒子 度和角度控制噪声方差阵为diag(0.4m, 4.95m 0.0872rad).目标速度和角度控制噪声方差阵为： 494 diag(0.8m,0.6rad),观测的误差协方差阵为：diag 493 (0.12m,0.0082rad,仿真实验迭代320次并且假设 4.92 8次迭代发生1次观测，这样的实验共进行了20 4.91 次，其中一次实验结果场景如图3所示 4.90 7 区域 489 48 6 且标轨迹 6.806.82 6.846.866.886.90 ,父域1 域4 (e)区域4目标位置 图3.SLAMTO仿真结果 2日、标志柱 Fig.3 Simulation results of SLAMTO 机器人轨迹 图3中，星点代表标志点的实际位置，以下结论 1 区域2 是在多次仿真基础上得出的，运用一次仿真结论只 2 3 45678910 是为了说明方便.由于观测频率相对于环境更迭频 率较慢(1：8)，运动噪声较大以及机器人定位误差 (a)仿其结果 的传播导致目标定位误差大于机器人定位的误差 5.35 (误差均值0.0393，误差均方差0.0136)，尽管如 5.30 此，对目标的定位仍比较准确（误差均值0.0639， =.5.25 误差均方差0.0104).图3(b)~(e)是特定区域的 5.20 放大图，其中图3(b)、(e)中粒子群代表对机器人 5.18 8.2 8.3 8.4 8.58.6 和目标的位置估计，图3(c)、(d)中粒子群代表对 应标志柱的估计值，要注意的是图3(b)、(e)中和 (b)区域1机器人位置 图3(c)、(d)中的粒子是对的，即图3(b)、(e)中粒 0.125r 子是s,中对应的S和u分量，而图3(c)、(d)中 粒子是3：中对应第3个和第12个标志柱的均值分 0.1204 量.图3(b)、(e)中由于重采样的作用代表机器人 和目标位置的粒子群分布集中并且多数粒子分布重 .0.115 合，因此粒子群分布能够代表机器人和目标的位置， 对于图3(d)来说所有粒子均重合，这同样是重采样 0.1104 的结果，因为对于该标志点的观测早已发生，在跌代 的过程中误差较大的粒子逐步被误差小的粒子取 0.105 1.555 1.560 1.5651.5701.5751.580 代，最终只有早期单个离子对应的该标志点位置估 计保留下来.而图3(c)中对应标志点的粒子群较为 (c)区域2标志柱粒子 分散，这是因为该标志点刚刚发现，重采样过程还没