無線傳感器環境下粒子群優化的多機器人協同定位研究

程磊, 周明達, 吳懷宇，李杰，王永驥

（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.