粒子群優(yōu)化的多機(jī)器人協(xié)作定位方法

劉利枚 ，蔡自興

(1. 中南大學(xué) 信息科學(xué)與工程學(xué)院，湖南 長沙，410083；2. 湖南商學(xué)院 計算機(jī)與電子工程學(xué)院，湖南 長沙，410205)

機(jī)器人定位是機(jī)器人研究領(lǐng)域中的熱點(diǎn)問題之一。為了實現(xiàn)移動機(jī)器人路徑規(guī)劃、導(dǎo)航、決策和執(zhí)行任務(wù)，要求機(jī)器人必須先感知環(huán)境，對自己進(jìn)行準(zhǔn)確定位。而與單個機(jī)器人相比，多機(jī)器人協(xié)作定位[1-5]對于多機(jī)器人系統(tǒng)協(xié)作運(yùn)行具有高效、高精度、高容錯性、高魯棒性等優(yōu)點(diǎn)，因而更適用于各種復(fù)雜的任務(wù)。目前，一些研究者提出了很多協(xié)作定位的方法，如：Roumeliotis等[6]通過卡爾曼濾波(Kalman fliter, KF)估計多機(jī)器人中每個機(jī)器人的位置信息(包括相對坐標(biāo)、相對方位)，然后，把集中的KF分成n個單獨(dú)的KF，進(jìn)行分布式處理。該方法成功地應(yīng)用于室內(nèi)環(huán)境。針對室外非結(jié)構(gòu)化環(huán)境，Raj等[7]利用外圍傳感器全球定位系統(tǒng)(Globe positioning system, GPS)提出基于擴(kuò)展卡爾曼濾波(Extend Kalman filter, EKF)的分布式協(xié)作定位方法。該方法提高了定位的精度，但實現(xiàn)比較復(fù)雜。Dieter等[8]應(yīng)用馬爾科夫定位方法提出了當(dāng)一個機(jī)器人觀測到另一個機(jī)器人時，融合機(jī)器人的運(yùn)動信息、環(huán)境感知信息和相互觀測模型，更新每個機(jī)器人的位置，從而實現(xiàn)多機(jī)器人的相互定位，但彼此探測時忽略了探測信息的相互依賴，有可能導(dǎo)致過分樂觀的位置估計。Andrew等[9-10]利用粒子濾波器(Particle filter, PF)對機(jī)器人隊伍進(jìn)行相對定位。每個機(jī)器人以自身的坐標(biāo)系為基準(zhǔn)，通過網(wǎng)絡(luò)中信息傳播及坐標(biāo)變換來確定隊列中其他每個機(jī)器人相對自身的位置，即使有些機(jī)器人沒有被直接觀測到，它也能夠確定其相對位置。由于不考慮絕對位置，這種方法比較適合于未知的、非靜態(tài)的環(huán)境。在該方法中，每個機(jī)器人都維護(hù)n-1個粒子濾波器(n為機(jī)器人個數(shù))。但是，隨著機(jī)器人個數(shù)的增加，每個機(jī)器人所要維護(hù)的粒子濾波器數(shù)量也不斷增加，其計算復(fù)雜度以指數(shù)級增長，影響了定位的實時性和實用性。王玲等[11-13]將粒子濾波和擴(kuò)展卡爾曼濾波相結(jié)合提出了一種多機(jī)器人合作定位的方法。該方法結(jié)合了粒子濾波器的魯棒性、適應(yīng)性與EKF的高效率和實時性，使機(jī)器人群中各成員可共享全局定位信息, 從而能有效地在未知環(huán)境中確定自身的位置，但使用GPS進(jìn)行全局定位，實現(xiàn)成本高。已經(jīng)應(yīng)用的各種定位算法中大致分為卡爾曼濾波和粒子濾波2類。在此，本文作者在粒子濾波的基礎(chǔ)上，充分利用異質(zhì)多機(jī)器人中某一機(jī)器人的精確定位能力，將粒子群優(yōu)化算法引入到粒子濾波方法中，得出一種粒子群優(yōu)化的粒子濾波算法，以實現(xiàn)多機(jī)器人的相對定位。與傳統(tǒng)的方法相比，該方法融合相對觀測量對粒子濾波預(yù)測采樣過程和粒子的權(quán)重進(jìn)行優(yōu)化，加速了粒子集收斂，從而增強(qiáng)了位置預(yù)測的準(zhǔn)確性。

1 相對觀測信息的獲取

為了實現(xiàn)異質(zhì)機(jī)器人的相互定位，進(jìn)行以下假設(shè)：

(1) 每個機(jī)器人都配有運(yùn)動感知傳感器，每個機(jī)器人之間都能進(jìn)行信息交換。

(2) 異質(zhì)多機(jī)器人中存在能力較強(qiáng)的高配機(jī)器人，該機(jī)器人裝配有能夠測量其他機(jī)器人相對距離和方向的外部傳感器，且該機(jī)器人能夠進(jìn)行精確自定位。低配的機(jī)器人裝配有能測量相對位置的外部傳感器。

(3) 每個機(jī)器人能檢測到其他機(jī)器人，并且每個機(jī)器人的身份識別總是準(zhǔn)確無誤的。

觀測量的獲取分為3個階段：首先，每個機(jī)器人根據(jù)運(yùn)動模型，由內(nèi)部傳感器感知它的運(yùn)動狀態(tài)，得到機(jī)器人移動的距離和方向變化；然后，高配的機(jī)器人直接測量附近可探測到的機(jī)器人的相對位置，將其他機(jī)器人的位置轉(zhuǎn)換到自己的局部坐標(biāo)中，計算相對觀測量；最后，高配機(jī)器人把自己觀測到的相對位置信息廣播給其他機(jī)器人，其他機(jī)器人通過處理這些信息得到所有觀測到和觀測不到的機(jī)器人相對于自己的距離和方向。

假設(shè)若干個機(jī)器人組成的隊列在環(huán)境中探索，其中能精確定位機(jī)器人至少1個。用 R =(x,y,θ)T表示機(jī)器人在某一時刻的位置，則所有機(jī)器人在某時刻的位置可以表示為 ( RA, RB1,… ,RBn)T。機(jī)器人之間的相對位置如圖1所示。

圖1 2個機(jī)器人之間的相對觀測量Fig.1 Relative observation between two robots

其中：RA和 RBn分別為觀測者(高配機(jī)器人)和被觀測者(低配機(jī)器人)在某一時刻的位置；為機(jī)器人RBn相對于RA的距離；θRA和θRBn分別為RA和RBn的方向角；為 RBn相對于 RA的方向角；為 RA相對于RBn的方向角。

2 融合相對觀測量的粒子群優(yōu)化協(xié)作定位算法

基于粒子濾波的同時定位與建圖方法，機(jī)器人的位置估計是從運(yùn)動模型中采樣獲得的，因此，無法根據(jù)最新的觀測對機(jī)器人位置進(jìn)行更新，導(dǎo)致機(jī)器人位置偏離了實際位置，為此，提出一種融合相對觀測量的粒子群優(yōu)化(Particle swarm optimization, PSO)協(xié)作定位算法。具有高配的每個機(jī)器人都維護(hù)一個粒子集合。機(jī)器人通過傳感器獲取相對觀測量，利用粒子群優(yōu)化的粒子濾波方法，估計機(jī)器人路徑軌跡的后驗概率分布，生成預(yù)測粒子集，計算粒子權(quán)重，估計機(jī)器人組中每個機(jī)器人的位置，從而提高了多機(jī)器人定位的精度和效率。

粒子群優(yōu)化算法(PSO)[14-15]是一種基于群體智能的全局優(yōu)化算法，它利用m個粒子組成的粒子群在n維目標(biāo)搜索空間中搜索最優(yōu)解。粒子i具有2個屬性：位置xi=(xi1, xi2, …, xin)T與速度vi=(vi1, vi2, …, vin)T，其中xi為所求問題的1個潛在解。粒子xi記錄它經(jīng)歷過的最好位置(個體最優(yōu)位置)ppbest=(pi1, pi2, …, pin)T和它的所有領(lǐng)域個體經(jīng)歷過的最好位置(全局歷史最優(yōu)位置) pgbest=(pg1, pg2, …, pgn)T。每次迭代中，粒子i第d維的速度和位置按下列表達(dá)式更新：

其中：d=1, 2, …, n，為搜索空間維數(shù)；i=1, 2, …, m，為粒子總數(shù)；k為當(dāng)前進(jìn)化代數(shù)；r1和r2為分布于[0, 1]之間的隨機(jī)數(shù)；c1和c2為加速常數(shù)；ω為慣性權(quán)重。

在基本 PF算法中，機(jī)器人下一時刻的位置預(yù)測是將t-1時刻的第i粒子的位置信息通過控制量ut對機(jī)器人的新位置進(jìn)行采樣獲得，并把新的位置寫入t時刻的粒子中，如下式所示：

從式(5)發(fā)現(xiàn)：這一預(yù)測過程沒有考慮到t時刻的最新觀測信息，因而無法根據(jù)最新的觀測信息對機(jī)器人預(yù)測位置進(jìn)行及時更新。本文通過觀測值對預(yù)測采樣過程進(jìn)行優(yōu)化，提出一種融合相對觀測量的粒子群優(yōu)化協(xié)作定位算法。

首先，使用粒子群算法中的速度與位置方程對機(jī)器人的預(yù)測位置進(jìn)行更新。更新方程如下：

然后，引用適應(yīng)度函數(shù)來判斷機(jī)器人預(yù)測位置的優(yōu)化程度。該適應(yīng)度函數(shù)定義如下：

其中：Rk表示機(jī)器人的測量噪聲協(xié)方差矩陣；c3為常數(shù)；zt表示 t時刻的觀測信息；ztpred表示 t 時刻機(jī)器人對被觀測者的預(yù)測信息。ztpred可通過預(yù)測位置值()*和已經(jīng)觀測到的位置li計算得到：

設(shè)定1個閾值δ，當(dāng)ηFitness≥δ時，表明該預(yù)測值)*已分布在機(jī)器人真實位置附近；反之，則表明該預(yù)測值 ()*偏離了機(jī)器人真實位置。通過以上優(yōu)化過程使得粒子集趨向于高似然區(qū)。

根據(jù)上述分析，完整的粒子群優(yōu)化多機(jī)器人協(xié)作定位算法如表1所示。

表1 融合相對觀測量的粒子群優(yōu)化多機(jī)器人協(xié)作定位算法Table1 Particle swarm optimized multi-robot cooperative localization obtained by fusing relative observation

3 實驗及分析

3.1 協(xié)作定位實驗結(jié)果

為了驗證融合相對觀測量的多機(jī)器人 PSOPF定位方法的有效性，實驗分別采用里程計估計位置方法、PF單機(jī)器人定位方法、多機(jī)器人PF定位方法與本文提出的PSOPF方法，在5 m×5 m的實驗環(huán)境中運(yùn)行。機(jī)器人B采用自行改造的MORCSⅡ，該移動機(jī)器人配備有2個傳感器和8個聲納。機(jī)器人A采用自行設(shè)計并研制的MORCS-1，系統(tǒng)裝配了1個360脈沖/轉(zhuǎn)增量式光電編碼器構(gòu)成的里程計、E-Core RD1100系列的數(shù)字光纖陀螺儀、CXTILT02E/02EC 傾角傳感器等內(nèi)部傳感器測量機(jī)器人的位姿，利用SICK公司生產(chǎn)的LMS291激光雷達(dá)、攝像頭作為外部傳感器感知機(jī)器人之間的距離和相對方向信息，該感知系統(tǒng)能夠?qū)崿F(xiàn)俯仰與水平的二維運(yùn)動。實驗采用的算法重要參數(shù)為：c1=1.5，c2=1.2，c3=35，ω=0.7，m=100。

在運(yùn)動過程中注意保持與機(jī)器人A的相對距離不要過大，以便能準(zhǔn)確識別其他機(jī)器人并獲得相對觀測量。首先機(jī)器人A與2個機(jī)器人B1和B2做勻速同向直線運(yùn)動，且初始朝向角為0°，使用本文的方法，實際軌跡和參考軌跡如圖2(a)所示(其中，實線為參考軌跡即機(jī)器人的真實運(yùn)動軌跡，虛線為利用本文方法協(xié)作定位后的實際估計軌跡)。以機(jī)器人 B1為例，機(jī)器人定位誤差見圖 3(a)。圖 3(a)中里程計所估計的最大誤差為0.53 m；由PF算法所得的單機(jī)器人定位誤差為0.27 m；由PF多機(jī)器人協(xié)作定位誤差為0.16 m；由PSOPF多機(jī)器人協(xié)作定位誤差為0.12 m。然后，機(jī)器人做勻速折線運(yùn)動，實際估計軌跡和參考軌跡如圖2(b)所示，B1的定位誤差如圖 3(b)所示。圖 3(b)中機(jī)器人B1里程計所估計的最大距離誤差為0.65 m；由單機(jī)器人 PF算法所得最大距離誤差為 0.45 m；由 PF多機(jī)器人協(xié)作定位所得最大距離誤差為 0.32 m；由PSOPF多機(jī)器人協(xié)作定位算法所得最大距離誤差為0.29 m。

為進(jìn)一步驗證算法的性能，分別采用3種算法計算20次。表2所示分別為4種算法在直線和折線運(yùn)動下的定位平均誤差和方差。

3.2 實驗結(jié)果分析

從圖2可以看出：高配機(jī)器人A定位精度比機(jī)器人B定位精度高；當(dāng)機(jī)器人朝向角發(fā)生變化時，實際估計軌跡會突然偏離參考軌跡，位置估計誤差會增大；但隨著方向角不變，很快又收斂到機(jī)器人的真實位置，位置估計誤差逐漸減小?？傮w來說，使用本文方法所得實際估計軌跡和參考軌跡基本一致。

在機(jī)器人運(yùn)動過程中，位姿估計主要受里程計和機(jī)器人速度的影響。從圖3和表2可以看出：當(dāng)機(jī)器人B方向角不變且勻速運(yùn)動時，其里程計測量誤差較小且基本保持恒定，因此，根據(jù)里程計測量值計算得到的位姿估計誤差隨時間增加而逐漸增大。采用本文所提出的方法，機(jī)器人B的初始階段定位誤差比較大，隨著高配機(jī)器人利用觀測信息進(jìn)行協(xié)作定位后，機(jī)器人B逐漸向真實位置靠近，誤差逐漸減小。當(dāng)機(jī)器人在折線勻速運(yùn)動中方向角變化時，系統(tǒng)誤差或者位置誤差突然增大；當(dāng)方向角不變時，機(jī)器人逐漸穩(wěn)定，位置誤差變化較小。

從以上實驗結(jié)果可以看出：利用相對觀測量的PSOPF方法，與單機(jī)器人PF和多機(jī)器人PF算法相比，其定位精度有很大提高。

圖2 機(jī)器人在xOy坐標(biāo)軸中的實際軌跡和參考軌跡Fig.2 Motion real trajectory and reference trajectory in xOy coordinate

圖3 定位誤差結(jié)果Fig.3 Localization error

表2 2種不同運(yùn)動方式的距離誤差比較Table2 Comparison of distance errors for two different running modes m

4 結(jié)論

(1) 基于粒子濾波的同時定位與建圖方法中，針對機(jī)器人的位置估計沒有根據(jù)最新的觀測對機(jī)器人位置進(jìn)行更新問題，融合較精確定位的機(jī)器人實時觀測與其他成員的相對觀測量，利用粒子群優(yōu)化的粒子濾波器，完成了多機(jī)器人的相對定位，形成了一個精度高的定位系統(tǒng)。最后通過實驗和實時數(shù)據(jù)，驗證了該方法有效、可行。

(2) 該方法適用于由多個小型低配機(jī)器人和能力特別強(qiáng)的高配機(jī)器人組成的異質(zhì)多機(jī)器人系統(tǒng)。

[1] Zhang L, Rene Z, Pascal l. Self-adaptive Monte Carlo for single-robot and multi-robot Localization[C]//Proceedings of the IEEE International Conference on Automation and Logistics.Shengyang, China, 2009: 1926-1953.

[2] Luca C, Miguel K N, Jing J D, et al. Rao-blackwellized particle filter multi-robot SLAM with unknown initial correspondences and limited communication[C]//2010 IEEE International Conference on Robotics and Automation. Alaska, USA, 2010:243-250.

[3] CHEN Hao-yao, SUN Dong, YANG Jie. Localization strategies for indoor multi-robot formations[C]//2009 IEEE International Conference on Advanced Intelligent Mechatronics. Singapore,2009: 1218-1224.

[4] Nikolas T, Stergios I, Roumeliotis, et al. Cooperative multi-robot localization under communication constraints[C]// IEEE International Conference on Robotics and Automation. Kobe,Japan, 2009: 4394-4401.

[5] 蔡自興, 陳白帆, 王璐, 等. 異質(zhì)多移動機(jī)器人協(xié)同技術(shù)研究的進(jìn)展[J]. 智能系統(tǒng)學(xué)報, 2007, 2(3): 1-7.

CAI Zi-xing, CHEN Bai-fan, WANG Lu, et al. The progress of cooperative technology for heterogeneous multiple mobile robots[J]. CAAI Transaction on Intelligent System, 2007, 2(3):1-7.

[6] Roumeliotis S I, Bekey G A. Distributed multirobot localization[J]. IEEE Transactions on Robotics and Automation,2002, 18(5): 781-795.

[7] Raj M, Kingsley F, Lynne E P. Distributed cooperative outdoor multirobot localization and mapping[J]. Autonomous Robots,2004, 17(1): 23-29.

[8] Dieter F, Wolfram B, Hannes K, et al. A probabilistic approach to collaborative multi-robot localization[J]. Autonomous Robots,2000, 8(3): 325-344.

[9] Andrew H, Maja J M, Gaurav S S. Putting the ‘I’ in ‘Team’: An ego-centric approach to cooperative localization[C]//Proceedings of the IEEE International Conference on Robots and Automation.Taipei, 2003: 868-874.

[10] Andrew H. Multi-robot simultaneous localization and mapping using particle filters[J]. International Journal of Robotics Research, 2006, 25(12): 1243-1256.

[11] 王玲, 萬建偉, 劉云輝, 等. 基于 PF-EKF的多機(jī)器人合作定位方法[J]. 中國科學(xué) E輯: 信息科學(xué), 2007, 37(12):1544-1555.

WANG Lin, WAN Jian-wei, LIU Yun-hui, et al. Multi-robot cooperative localization based on PF-EKF[J]. Science in China Series E: Information Science, 2007, 37(12): 1544-1555.

[12] 王玲, 邵金鑫, 萬建偉, 等. 多機(jī)器人定位中基于熵的分布觀測量選擇方法[J]. 電子學(xué)報, 2007, 35(2): 333-337.

WANG Lin, SHAO Jin-xin, WAN Jian-wei, et al. Distributed entropy based relative observation selection for multi-robot localization[J]. Acta Electronic Sinica, 2007, 35(2): 333-337.

[13] 王玲, 劉云輝, 萬建偉, 等. 基于相對方位的多機(jī)器人合作定位算法[J]. 傳感技術(shù)學(xué)報, 2007, 20(4): 794-800.

WANG Lin, LIU Yun-hui, WAN Jian-wei, et al. Multi-robot cooperative localization based on relative bearing[J]. Chinese Journal of Sensors and Actuators, 2007, 20(4): 794-800.

[14] Kennedy J, Eberhart R. Particle swarm optimization[C]//Proceedings of the IEEE International Conference on Neural Networks. Perth Western, Australia: IEEE Service Center, 1995:1941-1948.

[15] Riccardo P, James K, Tim B. Particle swarm optimization an overview[J]. Swarm Intelligence, 2007, 1(1): 33-57.