第1期 程磊，等：无线传感器环境下基于粒子群优化的多机器人协同定位研究 ·141. 器人的位置为例，为了让粒子估计迅速收敛到真实 的位置附近，需经多次实验验证，实验采用的重要参 X:-0.1 网格 Y:1.6 粒子 数为：c1=2,c2=2,种群大小pop_size=40,最大迭 10 Z6.5 。机器人1 代次数ma_gen=80,权系数最大值0.9，最小为0.4， S/ 机器人2 如图4所示，图4是在迭代次数40及80次的时候 0 机器人3 。节点1 粒子位姿状态，圆表示机器人，星号表示粒子，菱形 5 。节点2 表示无线传感器节点。 -10 节点3 -15 。节点4 10 。节点5 5 0 cm 0 。节点6 2 0 x/cm -10 图5迭代80次粒子估计的位置 15 Fig.5 80 iterative estimation of the particle position 0 -2 S环境下基于粒子群优化的多机器人协同定位算法误差仿真图 x/em 20m ·算法估计位置 ·机器人真实位置 (a)迭代40次时粒子分布 15A 4 106 24 ￥2021 107 X:24 -5 5 Y:0.6273 -10 -15 -204 -10 -25 -15 0 10 30 0 50 2 第几次算法的运行 0 0 y/cm 2 x/cm 图6多移动机器人协同定位误差 Fig.6 Multi-robot cooperative localization error (b)迭代80次时粒子分布 图4迭代时的粒子分布 多机器人协同定位算法实验误差分析如表1。 Fig.4 The particle distribution of iteration 表1多机器人协同定位算法实验误差分析 Table 1 Experimental analysis on Multi-robot cooper- 当迭代次数达到80时，粒子各自找到自己的最 ative localization algorithm 优位姿并且集中收敛在一个位姿附近，假定80%的 粒子数迭代50次迭代55次迭代60次迭代70次 粒子处于一个最小空间球中（除开边界的少数不合 理的粒子)，这80%粒子的三维坐标值的均值就设 0.378m 0.314m 0.316m 0.318m 定为团体最优值（即估计的机器人的位置）。经过 (9.34s) (9.45s) (10.34s)(11.32s) 实验仿真计算，团体最优值为g=(-0.0085, 0.314m 0.232m 0.226m 0.216m 1.5823,6.4798),由图5所示，使用MATLAB7.0, 30 (10.23s)(11.658)(12.43s) (13.23s) Figure-Tools-Date Cursor操作粗略估计机器人的位 0.243m 0.221m 0.207m 置为(-0.1,1.6,6.5)。下面介绍一个机器人在Z、 40 0.317m (12.34s)(13.45s)(14.76s) (15.01s) X坐标不变的时候（另外2个机器人不变），Y坐标动 态发生变化的情况，实验进行了50次，如图6所示。 0.234m 0.216m 0.206m 50 0.327m 横坐标为运算的次数，纵坐标为Y轴方向的机器人 (14.568)(15.67s)(16.78s)(17.69s) 真实位置（黑色小点表示）和估计位置（黑色星号表 为进一步验证算法的实时性能以及定位精度， 示)，实验中选定一个位置(24,2.021)，发现对应的 将此算法在机器人位置固定不变时，以不同粒子数 预估计的位置为(24,0.6273)，Y轴误差为 和迭代次数进行实验，对应求出算法运行时间(s) 1.3937m,其他的点可由图形上观测，可见WSN环 和定位精度(m)如表1所示。由表1可知，迭代次 境下移动机器人的节点定位误差在机器人移动时存 数越大、粒子数越多，相应的定位精度也越准确，可 在且不稳定，但相对误差较好，误差程度较平稳，系 是相应的时间也增大，算法的时效性不够，为满足不 统冗余度、可靠性较好，基本上解决了粒子重采样时 同的定位要求，可适当调整相应的参数以满足实验 出现的粒子退化现象。 要求。器人的位置为例袁为了让粒子估计迅速收敛到真实 的位置附近袁需经多次实验验证袁实验采用的重要参 数为 院糟员 越 圆袁糟圆 越 圆袁 种群大小 责燥责赃泽蚤扎藻 越 源园袁最大迭 代次数 皂葬赃早藻灶 越 愿园袁权系数最大值 园援怨袁最小为 园援源袁 如图 源 所示袁图 源 是在迭代次数 源园 及 愿园 次的时候 粒子位姿状态袁圆表示机器人袁星号表示粒子袁菱形 表示无线传感器节点遥 渊葬冤迭代 源园 次时粒子分布 渊遭冤迭代 愿园 次时粒子分布 图 源摇 迭代时的粒子分布 云蚤早援源摇 栽澡藻 责葬则贼蚤糟造藻 凿蚤泽贼则蚤遭怎贼蚤燥灶 燥枣 蚤贼藻则葬贼蚤燥灶 当迭代次数达到 愿园 时袁粒子各自找到自己的最 优位姿并且集中收敛在一个位姿附近袁假定 愿园豫的 粒子处于一个最小空间球中渊除开边界的少数不合 理的粒子冤 袁这 愿园豫粒子的三维坐标值的均值就设 定为团体最优值渊即估计的机器人的位置冤 遥 经过 实验仿真计算袁团体最优值为 早遭藻泽贼 越 渊 原 园援 园园愿 缘袁 员援缘愿圆 猿袁远援源苑怨 愿冤 袁由图 缘 所示袁使用 酝粤栽蕴粤月苑援 园袁 云蚤早怎则藻鄄栽燥燥造泽鄄阅葬贼藻 悦怎则泽燥则 操作粗略估计机器人的位 置为渊原园援员袁 员援远袁 远援缘冤遥 下面介绍一个机器人在 在尧 载 坐标不变的时候渊另外圆 个机器人不变冤 袁再 坐标动 态发生变化的情况袁实验进行了缘园次袁如图远所示遥 横坐标为运算的次数袁纵坐标为 再 轴方向的机器人 真实位置渊黑色小点表示冤 和估计位置渊黑色星号表 示冤 袁实验中选定一个位置渊圆源袁圆援园圆员冤 袁发现对应的 预估计的位置为 渊圆源袁园援远圆苑 猿冤袁 再 轴误差为 员援猿怨猿 苑 皂袁其他的点可由图形上观测袁可见 宰杂晕 环 境下移动机器人的节点定位误差在机器人移动时存 在且不稳定袁但相对误差较好袁误差程度较平稳袁系 统冗余度尧可靠性较好袁基本上解决了粒子重采样时 出现的粒子退化现象遥 图 缘摇 迭代 愿园 次粒子估计的位置 云蚤早援缘摇 愿园 蚤贼藻则葬贼蚤增藻 藻泽贼蚤皂葬贼蚤燥灶 燥枣 贼澡藻 责葬则贼蚤糟造藻 责燥泽蚤贼蚤燥灶 图 远摇 多移动机器人协同定位误差 云蚤早援 远摇 酝怎造贼蚤鄄则燥遭燥贼 糟燥燥责藻则葬贼蚤增藻 造燥糟葬造蚤扎葬贼蚤燥灶 藻则则燥则 摇 摇 多机器人协同定位算法实验误差分析如表 员遥 表 员摇 多机器人协同定位算法实验误差分析 栽葬遭造藻 员摇 耘曾责藻则蚤皂藻灶贼葬造 葬灶葬造赠泽蚤泽 燥灶 酝怎造贼蚤鄄则燥遭燥贼 糟燥燥责藻则鄄 葬贼蚤增藻 造燥糟葬造蚤扎葬贼蚤燥灶 葬造早燥则蚤贼澡皂 粒子数 迭代 缘园 次 迭代 缘缘 次 迭代 远园 次 迭代 苑园 次 圆园 园援猿苑愿 皂 渊怨援猿源 泽冤 园援猿员源 皂 渊怨援源缘 泽冤 园援猿员远 皂 渊员园援猿源 泽冤 园援猿员愿 皂 渊员员援猿圆 泽冤 猿园 园援猿员源 皂 渊员园援圆猿 泽冤 园援圆猿圆 皂 渊员员援远缘 泽冤 园援圆圆远 皂 渊员圆援源猿 泽冤 园援圆员远 皂 渊员猿援圆猿 泽冤 源园 园援猿员苑 皂 渊员圆援猿源 泽冤 园援圆源猿 皂 渊员猿援源缘 泽冤 园援圆圆员 皂 渊员源援苑远 泽冤 园援圆园苑 皂 渊员缘援园员 泽冤 缘园 园援猿圆苑 皂 渊员源援缘远 泽冤 园援圆猿源 皂 渊员缘援远苑 泽冤 园援圆员远 皂 渊员远援苑愿 泽冤 园援圆园远 皂 渊员苑援远怨 泽冤 摇 摇 为进一步验证算法的实时性能以及定位精度袁 将此算法在机器人位置固定不变时袁以不同粒子数 和迭代次数进行实验袁对应求出算法运行时间渊泽冤 和定位精度渊皂冤如表 员 所示遥 由表 员 可知袁迭代次 数越大尧粒子数越多袁相应的定位精度也越准确袁可 是相应的时间也增大袁算法的时效性不够袁为满足不 同的定位要求袁可适当调整相应的参数以满足实验 要求遥 第 员 期摇摇摇摇摇摇摇摇摇摇 程磊袁等院无线传感器环境下基于粒子群优化的多机器人协同定位研究 窑员源员窑