·927· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第5期 误差曲线 -··容差曲线 0.3 5 「X分量 0 50 100 150 200 250 300 0.4r Y分量 号02 50 100 150 200 250 0.2Z分量 ..300 0.1 50 100 150 200 250 300 1.0 位置估计 号0.5 50 100 150 200 250 300 s (b)机器人各分量及位置估计误差曲线 图14目标和机器人各分量及位置估计误差曲线 Fig.14 Estimation error curve of each components and position 从图14(a)可见，在0-85s期间，由于机器人 6,2.2准确性实验及分析 并未进行可观性机动，因此，目标各分量及位置 为了验证方法的准确性，假设目标起点与终 误差持续增加。在85~300s期间，机器人开始机 点均为0,15,1)，目标实际运行轨迹为圆形，线速 动运动，目标各分量及位置误差快速减小并始终 度为2m/s。机器人起点为(10,15,1)，朝向为(0,0，π)， 保持在较低水平。从图14(b)可见，机器人各分量 机器人与目标的平衡距离设为D=10m,机器人 及位置估计始终较为准确，从图11可见，环境特 最大机动速度设为vmas=2.5ms,仿真共进行了 征分布均匀且机器人在运动过程中有足够多的、 500次迭代，时间步进为0.05s。实验总图跟踪结 准确性高的特征保持在其视野中，这些保证了较 果如图16所示。 高的机器人定位精度。 25 目标和机器 图15为不同时刻，目标观测残差阵S对应的 20 88 人实际轨迹 不确定椭圆面积变化曲线。 15 M 目标和机器 人估计轨迹 10 120 目标起点 +环境特征实 100 与终点机器人 际位置 终点 80 0 环境特征估 60 -5 ?机器人实机器人估 计位置 际轨迹 计轨迹 目标估 40 -10 计轨迹 20 50 100150200250300 -15 目标实 -20 际轨迹 -25 图15 不同时刻目标观测值不确定椭圆面积变化曲线 -20 -10 0 10 20 25 Fig.15 Target observation uncertain elliptical area change X/m curve (a)实验俯视图 由图可见，在0~85s期间，观测不确定椭圆面 积变化很平稳，说明机器人对目标的前后时序观 测视差变化较小，用来进行深度估计的信息量较 少，因此，目标状态估计误差持续增大。在85s, 出现了一个较大跳变，此后机器人开始可观性机 动运动，在此期间，观测不确定椭圆面积变化较 为剧烈，这说明机器人在前后观测中存在较大视 差，能够为目标深度估计提供足够多的信息量， 图16实验总体结果 从而保证了目标跟踪的准确性。 Fig.16 Overall tracking resultst/s t/s 0 50 100 150 200 250 300 0 50 100 150 200 250 300 0 50 100 150 200 250 300 0 1.0 0.5 0.1 0.2 0.4 0.1 0.2 0.3 X 分量 误差曲线 容差曲线 X 分量 Y 分量 Y 分量 Z 分量 Z 分量 位置估计 位置估计 e/m e/m e/m e/m e/m e/m e/m e/m 0.2 50 100 150 200 250 300 (a) 目标各分量及位置估计误差曲线 (b) 机器人各分量及位置估计误差曲线 误差曲线 容差曲线 图 14 目标和机器人各分量及位置估计误差曲线 Fig. 14 Estimation error curve of each components and position 从图 14(a) 可见，在 0~85 s期间，由于机器人 并未进行可观性机动，因此，目标各分量及位置 误差持续增加。在 85~300 s 期间，机器人开始机 动运动，目标各分量及位置误差快速减小并始终 保持在较低水平。从图 14(b)可见，机器人各分量 及位置估计始终较为准确，从图 11 可见，环境特 征分布均匀且机器人在运动过程中有足够多的、 准确性高的特征保持在其视野中，这些保证了较 高的机器人定位精度。 S t 图 15 为不同时刻，目标观测残差阵 k对应的 不确定椭圆面积变化曲线。 0 50 100 150 200 250 300 t/s 20 40 60 80 100 120 S 图 15 不同时刻目标观测值不确定椭圆面积变化曲线 Fig. 15 Target observation uncertain elliptical area change curve 由图可见，在 0~85 s 期间，观测不确定椭圆面 积变化很平稳，说明机器人对目标的前后时序观 测视差变化较小，用来进行深度估计的信息量较 少，因此，目标状态估计误差持续增大。在 85 s， 出现了一个较大跳变，此后机器人开始可观性机 动运动，在此期间，观测不确定椭圆面积变化较 为剧烈，这说明机器人在前后观测中存在较大视 差，能够为目标深度估计提供足够多的信息量， 从而保证了目标跟踪的准确性。 6.2.2 准确性实验及分析 (0,15,1) 2 m/s (10,15,1) (0,0,π) D eq = 10 m vmax = 2.5 m/s 为了验证方法的准确性，假设目标起点与终 点均为 ，目标实际运行轨迹为圆形，线速 度为 。机器人起点为 ，朝向为 ， 机器人与目标的平衡距离设为 ，机器人 最大机动速度设为 ，仿真共进行了 500 次迭代，时间步进为 0.05 s。实验总图跟踪结 果如图 16 所示。 目标和机器 人实际轨迹 目标和机器 人估计轨迹 环境特征估 计位置 环境特征实 机器人 际位置 终点 机器人 起点 目标起点 与终点 机器人估 计轨迹 机器人实 际轨迹 目标估 计轨迹 目标实 际轨迹 25 20 15 10 5 0 −5 −10 −15 −20 −20 −10 0 (a) 实验俯视图 X/m Y/m 10 20 25 −25 图 16 实验总体结果 Fig. 16 Overall tracking results ·927· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第 5 期