无线传感器环境下粒子群优化的多机器人协同定位研究

程磊, 周明达, 吴怀宇，李杰，王永骥

（1.武汉科技大学 冶金自动化与检测技术教育部工程研究中心， 湖北 武汉 430081； 2.北京大学 工学院， 北京 100871； 3.华中科技大学 自动化学院， 湖北 武汉 430074）



无线传感器环境下粒子群优化的多机器人协同定位研究

程磊1,2, 周明达1, 吴怀宇1，李杰1，王永骥3

（1.武汉科技大学 冶金自动化与检测技术教育部工程研究中心， 湖北 武汉 430081； 2.北京大学 工学院， 北京 100871； 3.华中科技大学 自动化学院， 湖北 武汉 430074）

协同定位是多机器人自主行为的一项重要技术，重点描述了无线传感器网络环境下结合粒子群优化提出多机器人协同定位算法。该算法引入重采样，解决了粒子耗尽问题，扩大了解空间的范围，保证了种群的多样性，并且引入了惯性权重解决了粒子退化的问题。仿真结果表明，利用无线传感器网络进行辅助导航，采用粒子群优化算法，综合无线传感器网络进行辅助导航，融合各个机器人观测信息，可以降低求解问题的空间维数，在高斯噪声下能有效提高移动机器人定位精度。

粒子群优化；多机器人；协同定位；无线传感器网络；重采样；惯性权重；多信息融合；适应度函数

协同定位是多机器人导航领域的一项关键技术，也是多机器人进行其他行为的前提，它是指一群机器人在共同的环境下，通过共享环境信息，融合不同机器人的观测信息，逐步消除累计误差，实现在共同环境下确定各自机器人的位姿信息。与单个机器人定位相比，协同定位拥有更多的观测信息，故具有更好的定位精度。文献[1]运用卡尔曼滤波器（Kalman filter, KF）及粒子滤波器（particle filter），通过增量评估机器人位姿和环境特征位置后验概率的方法使机器人在探索未知环境中，能够递增地建立地图同时对路标进行定位，但此算法复杂性高，实时性不强。文中将无线传感器网络（wireless sensor network,WSN）应用于多机器人协同定位系统，无线传感器网络即是计算机通信和传感器多领域技术相结合的产物，也是将信息获取（传感）、信息传递与信息处理相结合的产物，它是将部署在监测区域的大量具有有限计算能力的微型传感器节点相互协作构成的一个多跳自组织网络，因其能在复杂环境下实时检测和远程监控而广泛应用于工业控制、安全监测、仓库管理和空间搜索等领域[4]。文中应用无线传感器网络节点作为机器人定位的特定标识，利用WSN环境下粒子群优化的多机器人协同定位算法，使用带有权重的的粒子多次迭代估计出机器人运动的位置。

1 WSN环境粒子群优化协同定位算法

1.1 粒子群优化原理

粒子群优化算法源自鸟群捕食行为原理，每个鸟可以看成是空间里面的一个不计体积和质量的微粒，初始化的微粒位置为待优化问题的潜在解，也就是待解决目标函数的解，粒子在空间移动时能记录个体最优值pbest和全局最优值gbest，每一次迭代后，粒子改变移动的位置和速度，从而更新全体及个体最优值，最终全体最优值为目标函数的解。文中粒子全体最优解为机器人真实位置。

vid（t+1）=wvid（t）+c1r1（pid-xid（t））+c2r2（pgd-xid（t））

式中：c1、c2为正的学习因子；r1、r2表示（0,1）之间均匀分布的随机数；vid∈[-vmax,vmax],vmax是先前设定的最大移动速度；i为惯性权重，实验过程中线性地递减w的值。

1.2 多传感器信息融合及惯性权重的计算

实际实验中惯性权重值大的适用于全局粒子搜索，权重值小的适用于局部搜索。

图1 机器人系统状态

由此可得这n个节点对应的似然度为

文中借鉴序贯蒙特卡洛方法[8]，利用一组带有权重的采样点来趋近状态的后验概率密度函数。粒子的权重表示为

1.3 重采样

1.4 适应度函数

当高权重粒子在多次迭代过程中逐步趋近高似然区域的时候，需要有一个函数来判断趋近真实位置的程度，文中选用适应度函数来判断粒子群优化算法对移动机器人预测位置的优化程度[9]，适应度函数如式（7）：

（7）

1.5 WSN下粒子群优化的协同定位算法流程

无线传感器环境下粒子群优化的多机器人协同定位算法，通过多传感器信息融合、更新粒子权重、重采样、以及自适应函数判断最终位置来实现多机器人协同定位，其算法的流程图如图2所示。

2 实验分析

图2 实验算法流程图

图3 系统产生的高斯噪声及对应的频谱图

三维环境的创建借鉴MATLAB7.0中改进的peaks函数，应用此函数便于仿真时在三维环境下直观地观测节点和机器人的位置。

z=（1-x）3·e（-（x3）-（y+14）3）-10·（（x/3）-x5-y5）·

e（-x3-y2）…+（1/7）·e（-（x+10）6-y2）·e（-x3-y2）-3e（-（x2））

（8）

对于多机器人定位，只需得到单机器人位置，以同样的方式就能得到其他机器人的位置。假定3个机器人位置（也是节点的位置）分别为（-0.1, 1.6, 6.5），（1.0, -0.1, 3.0），（0.21, 0.71, 0），地图上放置6个无线传感器节点（0, 0, -15），（-3.5, 6, 0.5），（-3.5, -6, 0.5），（3.5, -6, 0.5） ，（3.5, 6, 0.5） ，（0, 0, 10）。其通信距离为4 m，以第一个机器人的位置为例，为了让粒子估计迅速收敛到真实的位置附近，需经多次实验验证，实验采用的重要参数为：c1=2，c2=2，种群大小pop_size=40，最大迭代次数ma_gen=80，权系数最大值0.9,最小为0.4,如图4所示，图4是在迭代次数40及80次的时候粒子位姿状态，圆表示机器人，星号表示粒子，菱形表示无线传感器节点。

（a）迭代40次时粒子分布

（b）迭代80次时粒子分布

当迭代次数达到80时，粒子各自找到自己的最优位姿并且集中收敛在一个位姿附近，假定80%的粒子处于一个最小空间球中（除开边界的少数不合理的粒子），这80%粒子的三维坐标值的均值就设定为团体最优值（即估计的机器人的位置）。经过实验仿真计算，团体最优值为gbest=（-0.008 5, 1.582 3, 6.479 8），由图5所示,使用MATLAB7.0，Figure-Tools-Date Cursor操作粗略估计机器人的位置为（-0.1, 1.6, 6.5）。下面介绍一个机器人在Z、X坐标不变的时候（另外2个机器人不变），Y坐标动态发生变化的情况，实验进行了50次，如图6所示。横坐标为运算的次数，纵坐标为Y轴方向的机器人真实位置（黑色小点表示）和估计位置（黑色星号表示），实验中选定一个位置（24,2.021），发现对应的预估计的位置为（24，0.627 3），Y轴误差为1.393 7 m，其他的点可由图形上观测，可见WSN环境下移动机器人的节点定位误差在机器人移动时存在且不稳定，但相对误差较好，误差程度较平稳，系统冗余度、可靠性较好，基本上解决了粒子重采样时出现的粒子退化现象。

图5 迭代80次粒子估计的位置

图6 多移动机器人协同定位误差

多机器人协同定位算法实验误差分析如表1。

表1 多机器人协同定位算法实验误差分析

Table 1 Experimental analysis on Multi-robot cooperative localization algorithm

粒子数迭代50次迭代55次迭代60次迭代70次200.378m（9.34s）0.314m（9.45s）0.316m（10.34s）0.318m（11.32s）300.314m（10.23s）0.232m（11.65s）0.226m（12.43s）0.216m（13.23s）400.317m（12.34s）0.243m（13.45s）0.221m（14.76s）0.207m（15.01s）500.327m（14.56s）0.234m（15.67s）0.216m（16.78s）0.206m（17.69s）

为进一步验证算法的实时性能以及定位精度，将此算法在机器人位置固定不变时，以不同粒子数和迭代次数进行实验，对应求出算法运行时间（s）和定位精度（m）如表1所示。由表1可知,迭代次数越大、粒子数越多，相应的定位精度也越准确，可是相应的时间也增大，算法的时效性不够，为满足不同的定位要求，可适当调整相应的参数以满足实验要求。

4 结 论

文中将无线传感器系统与粒子群优化算法相结合以实现多机器人协同定位。无线传感器网络辅助机器人定位，实现了节点间的信息融合，在定位中引入重采样，多次迭代更新粒子状态，减少了冗余节点的影响。仿真结果表明，本方法能够有效地进行定位，满足了定位精度要求，今后将研究算法的时效性问题以满足快速定位的要求。

[1]SAYED H, TARIGHAT A, KHAJEHNOUR N. New work based wireless location [J]. IEEE Signal Processing Magazine, 2005, 22（4）: 24-40.

[2]海丹, 李勇, 张辉,等. 无线传感器网络环境下基于粒子滤波器的移动机器人SLAM算法[J].智能系统学报， 2010, 5（5）: 1673-4785. HAN Dan, LI Yong, ZHANG Hui, et al. Wireless sensor network environment of mobile robot SLAM algorithm based on particle filter[J]. Journal of Intelligent Systems， 2010, 5（5）: 1673-4785.

[3]李会荣, 高岳林. 粒子群优化的速度方程改进与自适应变异策略[J]. 计算机工程与应用, 2010（13）, 13: 47-50. LI Huiyong, GAO Yuelin. Particle swarm optimization improved velocity and adaptive mutation strategy[J]. Computer Engineering and Application, 2010（13）: 47-50.

[4]刘利枚, 蔡自兴. 基于改进的粒子群优化的FastSLAM方法[J]. 高技术通讯， 2011, 21（4）: 422-427. LIU Limei, CAI Zixin. FastSLAM method based on improved particle swarm optimization[J]. High Technology Communication, 2011, 21（4）: 422-427.

[5]SUN G L, CHEN J, GUO W. Signal processing techniques in network-aided position[J]. IEEE Signal Processing Magazine, 2005, 22（2）: 12-23.

[6]ROUMELIOTIS S, BEKEY G. Distributed multi-robot localization[J]. IEEE Transactions on Robotics and Automation, 2002, 18（5）: 781-795.

[7]LI Z, GHOSH B. Line segment based map building and localization using 2D laser Range finder[C]// IEEE Conference on Robotics and Automation, Karlsruhe,Germany, 2000: 2538-2543.

[8]VLASSIS N, MOTOMURA Y, KROSE B. Supervised linear feature extration for mobile robot Localization[C]// IEEE Conference on Robotics and Automation. Karlsruhe,Germany, 2000: 2979-2984.

[9]MOMESNO L, GASPAR J, VICTOR J. Cooperative localization by fusing vision-based bearing measurements and motion[C]// IEEE/RSJ International Conference on Intelligent Robots and Systems. Edmonton, Canada, 2005: 19-24.

[10]BERTALMIO M. Filling in by joint interpolation of vector fields and grey levels[J]. IEEE Transactions on Image Processing, 2001, 10（8）: 1200-1210.

[11]ALAMI R, FLEURY S, HERRB M, et al. Multi-robots cooperation in the MARTHA project[J]. IEEE Robotics and Automation, 1998, 5（l）: 36-47.

[12]LUO R C, LI J X, CHEN C T. Indoor localization using line based map for autonomous mobile robot[C]// IEEE Workshop on Advanced Roboties and its Social im Pacts, 2008: 1-6.

[13]XIA Y M, YANG Y M. A new particle fllter and its applieation in mobile robot localization[C]// International Conference on Fuzzy Systems and Knowledge Diseovery. Jinan, China, 2008: 522-525.

[14]GUIVANT J, NEBOT E. Optimization of the simultaneous localization and map building algorithm for real time imPlementation[J]. IEEE Transactions on Robotics and Automation, 2001, 17（3）: 242-257.

[15]KIM C, SAKTHIVEL R, CHUNG W K. Unscented FastSLAM: a robust and efficient solution to the SLAM Problem[J]. IEEE Transactions on Robotics, 2008, 24（4）: 808-820.

[16]FOX D, KO J. Distributed multirobot exploration and mapping[J]. Proceeedings of the IEEE, 2006, 94（7）: 1325-1339.

[17]BARSHAN B, DURRANT W H. Inertial navigation systems for mobile robots[J]. IEEE Trans Robot Autom, 1995, 11（3）: 328-42.

[18]DISSANAYAKE M, NEWMAN P, CLARK S, et al. A solution to the simultaneous localization and map building （SLAM） problem[J]. IEEE Trans Robot Autom, 2001, 17（3）: 22-41.

[19]DURRANT W H, BAILEY T. Simultaneous localization and mapping （SLAM）: part I[J]. IEEE Robot Autom Mag, 2006, 13（2）: 99-110.

[20]ARULAMPALAM M, MASKELL S, GORDON N, et al. A tutorial on particle filters for online nonlinear Bayesian tracking[J]. IEEE Trans Signal Process, 2002, 50（2）: 174-88.

程磊，男，1976生，副教授, 中国人工智能学会青年工作委员会常务委员，中国人工智能学会智能机器人专业委员会委员，湖北省人工智能学会理事，主要研究方向为机器人及复杂系统。主持包括国家自然科学基金项目在内的省部级以上项目9项，获国家教学成果二等奖1项，获学术论文奖6项，发表学术论文40余篇，编著教材2部。

周明达，男，1989生，主要研究方向为机器人基础运动控制及其人工智能控制。

吴怀宇，男，1961生，教授，博士生导师，主要研究方向为服务机器人及其控制。国家及省部级基金7项，国际合作项目3项以及横向课题8项，获省部级科技进步一等奖2项；教育部科技成果鉴定2项，获湖北省自然科学优秀学术论文一等奖1项，国家发明专利2项，实用新型专利2项，发表学术论文50余篇，出版专著1部，主编教材3部。

Cooperative multi-robot localization based on particle swarm optimization in the environment of wireless sensor

CHENG Lei1,2, ZHOU Mingda1, WU Huaiyu1, LI Jie1, WANG Yongji3

（1. Engineering Research Center of Metallurgical Automation and Measurement Technology of Ministry of Education, Wuhan University of Science and Technology,Wuhan 430081, China； 2. College of Engineering, Peking University, Beijing 100871, China；3. School of Automation, Huazhong University of Science and Technology, Wuhan 430074, China）

Cooperative localization is an important technique of multi-robot’s autonomous behavior. In this paper, the multi-robot cooperative localization algorithm based on the optimization of particle swarm optimization under wireless sensor network environment is described. The resampling algorithm is introduced to solve the problem of particle depletion, enlarge the scope of solution space and guarantee the diversity of population. The introduction of inertia weight provides a solution for the particle degradation. Simulation results showed that by using the particle swarm optimization algorithm, which is supported by wireless sensor network to assist navigation and integrating robots’ observation information, the spatial dimensions of the problem can be reduced. In addition, the accuracy of robot localization can be improved effectively under the background of Gaussian noise.

particle swarm optimization; multi-robot; cooperative localization; wireless sensor network; resampling; inertia weight; multiple information fusion; fitness function

2013-10-24.

日期：2015-01-13.

国家自然科学基金资助项目（60705035，61075087，61203331）；湖北省重点实验室开放基金重点资助项目（Z201102）；河南省高等学校控制工程重点学科开放基金资助项目（KG2011-01）；湖北省教育厅科研计划重点资助项目（D20131105）；湖北省科技计划自然科学基金重点资助项目（2010CDA005）.

程磊. E-mail:chenglei@wust.edu.cn.

10.3969/j.issn.1673-4785.201310067

http://www.cnki.net/kcms/doi/10.3969/j.issn.1673-4785.201310067.html

TP31

A

1673-4785（2015）01-0138-05

程磊, 周明达, 吴怀宇，等. 无线传感器环境下粒子群优化的多机器人协同定位研究[J]. 智能系统学报， 2015, 10（1）: 138-142.

英文引用格式：CHENG Lei, ZHOU Mingda, WU Huaiyu, Li Jie, Wang Yong-ji. Cooperative multi-robot localization based on particle swarm optimization in the environment of wireless sensor[J]. CAAI Transactions on Intelligent Systems, 2015, 10（1）: 138-142.