無線傳感器環(huán)境下粒子群優(yōu)化的多機(jī)器人協(xié)同定位研究

程磊, 周明達(dá), 吳懷宇，李杰，王永驥

（1.武漢科技大學(xué) 冶金自動化與檢測技術(shù)教育部工程研究中心， 湖北 武漢 430081； 2.北京大學(xué) 工學(xué)院， 北京 100871； 3.華中科技大學(xué) 自動化學(xué)院， 湖北 武漢 430074）

?

無線傳感器環(huán)境下粒子群優(yōu)化的多機(jī)器人協(xié)同定位研究

程磊1,2, 周明達(dá)1, 吳懷宇1，李杰1，王永驥3

（1.武漢科技大學(xué) 冶金自動化與檢測技術(shù)教育部工程研究中心， 湖北 武漢 430081； 2.北京大學(xué) 工學(xué)院， 北京 100871； 3.華中科技大學(xué) 自動化學(xué)院， 湖北 武漢 430074）

協(xié)同定位是多機(jī)器人自主行為的一項(xiàng)重要技術(shù)，重點(diǎn)描述了無線傳感器網(wǎng)絡(luò)環(huán)境下結(jié)合粒子群優(yōu)化提出多機(jī)器人協(xié)同定位算法。該算法引入重采樣，解決了粒子耗盡問題，擴(kuò)大了解空間的范圍，保證了種群的多樣性，并且引入了慣性權(quán)重解決了粒子退化的問題。仿真結(jié)果表明，利用無線傳感器網(wǎng)絡(luò)進(jìn)行輔助導(dǎo)航，采用粒子群優(yōu)化算法，綜合無線傳感器網(wǎng)絡(luò)進(jìn)行輔助導(dǎo)航，融合各個機(jī)器人觀測信息，可以降低求解問題的空間維數(shù)，在高斯噪聲下能有效提高移動機(jī)器人定位精度。

粒子群優(yōu)化；多機(jī)器人；協(xié)同定位；無線傳感器網(wǎng)絡(luò)；重采樣；慣性權(quán)重；多信息融合；適應(yīng)度函數(shù)

協(xié)同定位是多機(jī)器人導(dǎo)航領(lǐng)域的一項(xiàng)關(guān)鍵技術(shù)，也是多機(jī)器人進(jìn)行其他行為的前提，它是指一群機(jī)器人在共同的環(huán)境下，通過共享環(huán)境信息，融合不同機(jī)器人的觀測信息，逐步消除累計誤差，實(shí)現(xiàn)在共同環(huán)境下確定各自機(jī)器人的位姿信息。與單個機(jī)器人定位相比，協(xié)同定位擁有更多的觀測信息，故具有更好的定位精度。文獻(xiàn)[1]運(yùn)用卡爾曼濾波器（Kalman filter, KF）及粒子濾波器（particle filter），通過增量評估機(jī)器人位姿和環(huán)境特征位置后驗(yàn)概率的方法使機(jī)器人在探索未知環(huán)境中，能夠遞增地建立地圖同時對路標(biāo)進(jìn)行定位，但此算法復(fù)雜性高，實(shí)時性不強(qiáng)。文中將無線傳感器網(wǎng)絡(luò)（wireless sensor network,WSN）應(yīng)用于多機(jī)器人協(xié)同定位系統(tǒng)，無線傳感器網(wǎng)絡(luò)即是計算機(jī)通信和傳感器多領(lǐng)域技術(shù)相結(jié)合的產(chǎn)物，也是將信息獲?。▊鞲校⑿畔鬟f與信息處理相結(jié)合的產(chǎn)物，它是將部署在監(jiān)測區(qū)域的大量具有有限計算能力的微型傳感器節(jié)點(diǎn)相互協(xié)作構(gòu)成的一個多跳自組織網(wǎng)絡(luò)，因其能在復(fù)雜環(huán)境下實(shí)時檢測和遠(yuǎn)程監(jiān)控而廣泛應(yīng)用于工業(yè)控制、安全監(jiān)測、倉庫管理和空間搜索等領(lǐng)域[4]。文中應(yīng)用無線傳感器網(wǎng)絡(luò)節(jié)點(diǎn)作為機(jī)器人定位的特定標(biāo)識，利用WSN環(huán)境下粒子群優(yōu)化的多機(jī)器人協(xié)同定位算法，使用帶有權(quán)重的的粒子多次迭代估計出機(jī)器人運(yùn)動的位置。

1 WSN環(huán)境粒子群優(yōu)化協(xié)同定位算法

1.1 粒子群優(yōu)化原理

粒子群優(yōu)化算法源自鳥群捕食行為原理，每個鳥可以看成是空間里面的一個不計體積和質(zhì)量的微粒，初始化的微粒位置為待優(yōu)化問題的潛在解，也就是待解決目標(biāo)函數(shù)的解，粒子在空間移動時能記錄個體最優(yōu)值pbest和全局最優(yōu)值gbest，每一次迭代后，粒子改變移動的位置和速度，從而更新全體及個體最優(yōu)值，最終全體最優(yōu)值為目標(biāo)函數(shù)的解。文中粒子全體最優(yōu)解為機(jī)器人真實(shí)位置。

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

式中：c1、c2為正的學(xué)習(xí)因子；r1、r2表示（0,1）之間均勻分布的隨機(jī)數(shù)；vid∈[-vmax,vmax],vmax是先前設(shè)定的最大移動速度；i為慣性權(quán)重，實(shí)驗(yàn)過程中線性地遞減w的值。

1.2 多傳感器信息融合及慣性權(quán)重的計算

實(shí)際實(shí)驗(yàn)中慣性權(quán)重值大的適用于全局粒子搜索，權(quán)重值小的適用于局部搜索。

圖1 機(jī)器人系統(tǒng)狀態(tài)

由此可得這n個節(jié)點(diǎn)對應(yīng)的似然度為

文中借鑒序貫蒙特卡洛方法[8]，利用一組帶有權(quán)重的采樣點(diǎn)來趨近狀態(tài)的后驗(yàn)概率密度函數(shù)。粒子的權(quán)重表示為

1.3 重采樣

1.4 適應(yīng)度函數(shù)

當(dāng)高權(quán)重粒子在多次迭代過程中逐步趨近高似然區(qū)域的時候，需要有一個函數(shù)來判斷趨近真實(shí)位置的程度，文中選用適應(yīng)度函數(shù)來判斷粒子群優(yōu)化算法對移動機(jī)器人預(yù)測位置的優(yōu)化程度[9]，適應(yīng)度函數(shù)如式（7）：

（7）

1.5 WSN下粒子群優(yōu)化的協(xié)同定位算法流程

無線傳感器環(huán)境下粒子群優(yōu)化的多機(jī)器人協(xié)同定位算法，通過多傳感器信息融合、更新粒子權(quán)重、重采樣、以及自適應(yīng)函數(shù)判斷最終位置來實(shí)現(xiàn)多機(jī)器人協(xié)同定位，其算法的流程圖如圖2所示。

2 實(shí)驗(yàn)分析

圖2 實(shí)驗(yàn)算法流程圖

圖3 系統(tǒng)產(chǎn)生的高斯噪聲及對應(yīng)的頻譜圖

三維環(huán)境的創(chuàng)建借鑒MATLAB7.0中改進(jìn)的peaks函數(shù)，應(yīng)用此函數(shù)便于仿真時在三維環(huán)境下直觀地觀測節(jié)點(diǎn)和機(jī)器人的位置。

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）

對于多機(jī)器人定位，只需得到單機(jī)器人位置，以同樣的方式就能得到其他機(jī)器人的位置。假定3個機(jī)器人位置（也是節(jié)點(diǎn)的位置）分別為（-0.1, 1.6, 6.5），（1.0, -0.1, 3.0），（0.21, 0.71, 0），地圖上放置6個無線傳感器節(jié)點(diǎn)（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，以第一個機(jī)器人的位置為例，為了讓粒子估計迅速收斂到真實(shí)的位置附近，需經(jīng)多次實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)采用的重要參數(shù)為：c1=2，c2=2，種群大小pop_size=40，最大迭代次數(shù)ma_gen=80，權(quán)系數(shù)最大值0.9,最小為0.4,如圖4所示，圖4是在迭代次數(shù)40及80次的時候粒子位姿狀態(tài)，圓表示機(jī)器人，星號表示粒子，菱形表示無線傳感器節(jié)點(diǎn)。

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

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

當(dāng)?shù)螖?shù)達(dá)到80時，粒子各自找到自己的最優(yōu)位姿并且集中收斂在一個位姿附近，假定80%的粒子處于一個最小空間球中（除開邊界的少數(shù)不合理的粒子），這80%粒子的三維坐標(biāo)值的均值就設(shè)定為團(tuán)體最優(yōu)值（即估計的機(jī)器人的位置）。經(jīng)過實(shí)驗(yàn)仿真計算，團(tuán)體最優(yōu)值為gbest=（-0.008 5, 1.582 3, 6.479 8），由圖5所示,使用MATLAB7.0，F(xiàn)igure-Tools-Date Cursor操作粗略估計機(jī)器人的位置為（-0.1, 1.6, 6.5）。下面介紹一個機(jī)器人在Z、X坐標(biāo)不變的時候（另外2個機(jī)器人不變），Y坐標(biāo)動態(tài)發(fā)生變化的情況，實(shí)驗(yàn)進(jìn)行了50次，如圖6所示。橫坐標(biāo)為運(yùn)算的次數(shù)，縱坐標(biāo)為Y軸方向的機(jī)器人真實(shí)位置（黑色小點(diǎn)表示）和估計位置（黑色星號表示），實(shí)驗(yàn)中選定一個位置（24,2.021），發(fā)現(xiàn)對應(yīng)的預(yù)估計的位置為（24，0.627 3），Y軸誤差為1.393 7 m，其他的點(diǎn)可由圖形上觀測，可見WSN環(huán)境下移動機(jī)器人的節(jié)點(diǎn)定位誤差在機(jī)器人移動時存在且不穩(wěn)定，但相對誤差較好，誤差程度較平穩(wěn)，系統(tǒng)冗余度、可靠性較好，基本上解決了粒子重采樣時出現(xiàn)的粒子退化現(xiàn)象。

圖5 迭代80次粒子估計的位置

圖6 多移動機(jī)器人協(xié)同定位誤差

多機(jī)器人協(xié)同定位算法實(shí)驗(yàn)誤差分析如表1。

表1 多機(jī)器人協(xié)同定位算法實(shí)驗(yàn)誤差分析

Table 1 Experimental analysis on Multi-robot cooperative localization algorithm

粒子數(shù)迭代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）

為進(jìn)一步驗(yàn)證算法的實(shí)時性能以及定位精度，將此算法在機(jī)器人位置固定不變時，以不同粒子數(shù)和迭代次數(shù)進(jìn)行實(shí)驗(yàn)，對應(yīng)求出算法運(yùn)行時間（s）和定位精度（m）如表1所示。由表1可知,迭代次數(shù)越大、粒子數(shù)越多，相應(yīng)的定位精度也越準(zhǔn)確，可是相應(yīng)的時間也增大，算法的時效性不夠，為滿足不同的定位要求，可適當(dāng)調(diào)整相應(yīng)的參數(shù)以滿足實(shí)驗(yàn)要求。

4 結(jié) 論

文中將無線傳感器系統(tǒng)與粒子群優(yōu)化算法相結(jié)合以實(shí)現(xiàn)多機(jī)器人協(xié)同定位。無線傳感器網(wǎng)絡(luò)輔助機(jī)器人定位，實(shí)現(xiàn)了節(jié)點(diǎn)間的信息融合，在定位中引入重采樣，多次迭代更新粒子狀態(tài)，減少了冗余節(jié)點(diǎn)的影響。仿真結(jié)果表明，本方法能夠有效地進(jìn)行定位，滿足了定位精度要求，今后將研究算法的時效性問題以滿足快速定位的要求。

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

[2]海丹, 李勇, 張輝,等. 無線傳感器網(wǎng)絡(luò)環(huán)境下基于粒子濾波器的移動機(jī)器人SLAM算法[J].智能系統(tǒng)學(xué)報， 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]李會榮, 高岳林. 粒子群優(yōu)化的速度方程改進(jìn)與自適應(yīng)變異策略[J]. 計算機(jī)工程與應(yīng)用, 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]劉利枚, 蔡自興. 基于改進(jìn)的粒子群優(yōu)化的FastSLAM方法[J]. 高技術(shù)通訊， 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生，副教授, 中國人工智能學(xué)會青年工作委員會常務(wù)委員，中國人工智能學(xué)會智能機(jī)器人專業(yè)委員會委員，湖北省人工智能學(xué)會理事，主要研究方向?yàn)闄C(jī)器人及復(fù)雜系統(tǒng)。主持包括國家自然科學(xué)基金項(xiàng)目在內(nèi)的省部級以上項(xiàng)目9項(xiàng)，獲國家教學(xué)成果二等獎1項(xiàng)，獲學(xué)術(shù)論文獎6項(xiàng)，發(fā)表學(xué)術(shù)論文40余篇，編著教材2部。

周明達(dá)，男，1989生，主要研究方向?yàn)闄C(jī)器人基礎(chǔ)運(yùn)動控制及其人工智能控制。

吳懷宇，男，1961生，教授，博士生導(dǎo)師，主要研究方向?yàn)榉?wù)機(jī)器人及其控制。國家及省部級基金7項(xiàng)，國際合作項(xiàng)目3項(xiàng)以及橫向課題8項(xiàng)，獲省部級科技進(jìn)步一等獎2項(xiàng)；教育部科技成果鑒定2項(xiàng)，獲湖北省自然科學(xué)優(yōu)秀學(xué)術(shù)論文一等獎1項(xiàng)，國家發(fā)明專利2項(xiàng)，實(shí)用新型專利2項(xiàng)，發(fā)表學(xué)術(shù)論文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.

國家自然科學(xué)基金資助項(xiàng)目（60705035，61075087，61203331）；湖北省重點(diǎn)實(shí)驗(yàn)室開放基金重點(diǎn)資助項(xiàng)目（Z201102）；河南省高等學(xué)?？刂乒こ讨攸c(diǎn)學(xué)科開放基金資助項(xiàng)目（KG2011-01）；湖北省教育廳科研計劃重點(diǎn)資助項(xiàng)目（D20131105）；湖北省科技計劃自然科學(xué)基金重點(diǎn)資助項(xiàng)目（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

程磊, 周明達(dá), 吳懷宇，等. 無線傳感器環(huán)境下粒子群優(yōu)化的多機(jī)器人協(xié)同定位研究[J]. 智能系統(tǒng)學(xué)報， 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.