劉利枚 ，蔡自興

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

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

1 相對(duì)觀測(cè)信息的獲取

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

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

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

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

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

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

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

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

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

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

粒子群優(yōu)化算法(PSO)[14-15]是一種基于群體智能的全局優(yōu)化算法，它利用m個(gè)粒子組成的粒子群在n維目標(biāo)搜索空間中搜索最優(yōu)解。粒子i具有2個(gè)屬性：位置xi=(xi1, xi2, …, xin)T與速度vi=(vi1, vi2, …, vin)T，其中xi為所求問(wèn)題的1個(gè)潛在解。粒子xi記錄它經(jīng)歷過(guò)的最好位置(個(gè)體最優(yōu)位置)ppbest=(pi1, pi2, …, pin)T和它的所有領(lǐng)域個(gè)體經(jīng)歷過(guò)的最好位置(全局歷史最優(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ī)器人下一時(shí)刻的位置預(yù)測(cè)是將t-1時(shí)刻的第i粒子的位置信息通過(guò)控制量ut對(duì)機(jī)器人的新位置進(jìn)行采樣獲得，并把新的位置寫入t時(shí)刻的粒子中，如下式所示：

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

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

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

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

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

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

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

3 實(shí)驗(yàn)及分析

3.1 協(xié)作定位實(shí)驗(yàn)結(jié)果

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

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

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

3.2 實(shí)驗(yàn)結(jié)果分析

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

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

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

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

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

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

4 結(jié)論

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

(2) 該方法適用于由多個(gè)小型低配機(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ì)多移動(dòng)機(jī)器人協(xié)同技術(shù)研究的進(jìn)展[J]. 智能系統(tǒng)學(xué)報(bào), 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] 王玲, 萬(wàn)建偉, 劉云輝, 等. 基于 PF-EKF的多機(jī)器人合作定位方法[J]. 中國(guó)科學(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] 王玲, 邵金鑫, 萬(wàn)建偉, 等. 多機(jī)器人定位中基于熵的分布觀測(cè)量選擇方法[J]. 電子學(xué)報(bào), 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] 王玲, 劉云輝, 萬(wàn)建偉, 等. 基于相對(duì)方位的多機(jī)器人合作定位算法[J]. 傳感技術(shù)學(xué)報(bào), 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.