卓書芳，何用輝，吳燕峰

(福建信息職業(yè)技術(shù)學(xué)院 自動(dòng)化工程系， 福州　350003)

多機(jī)器人協(xié)同定位的相關(guān)研究工作始于20世紀(jì)。多機(jī)器人協(xié)同定位算法是通過一定的信息交換，實(shí)現(xiàn)機(jī)器人之間的導(dǎo)航資源共享，并采用數(shù)據(jù)融合技術(shù)估計(jì)出系統(tǒng)中所有機(jī)器人的位置狀態(tài)[1-3]。目前，對(duì)于多機(jī)器人協(xié)同定位技術(shù)的常用方法是結(jié)合具體的導(dǎo)航傳感器采用各種改進(jìn)的卡爾曼濾波(kalman filter,KF)算法進(jìn)行數(shù)據(jù)融合，其中擴(kuò)展卡爾曼濾波(extended kalman filter,EKF)算法最為普遍[4-5]。

文獻(xiàn)[5-6]提出了一種集中式數(shù)據(jù)結(jié)構(gòu)的EKF算法，以各個(gè)機(jī)器人為處理節(jié)點(diǎn)，利用濾波估計(jì)方法將各個(gè)節(jié)點(diǎn)的導(dǎo)航信息融合到某個(gè)主節(jié)點(diǎn)上，在提高主節(jié)點(diǎn)導(dǎo)航精度的同時(shí)，可通過反饋主節(jié)點(diǎn)高精度信息到各個(gè)節(jié)點(diǎn)從而提高這個(gè)協(xié)同定位系統(tǒng)的導(dǎo)航能力。但是，集中式EKF算法容錯(cuò)性不佳，處理中心的損壞會(huì)導(dǎo)致整個(gè)系統(tǒng)的癱瘓。

分散式數(shù)據(jù)結(jié)構(gòu)是一種無中心、無依賴的算法結(jié)構(gòu)，每個(gè)節(jié)點(diǎn)都可以自行處理本地信息，且節(jié)點(diǎn)之間可以自由通信，這種數(shù)據(jù)結(jié)構(gòu)相較于集中式數(shù)據(jù)結(jié)構(gòu)的優(yōu)勢(shì)在于不依賴單獨(dú)的處理中心，避免了單一節(jié)點(diǎn)被摧毀而導(dǎo)致整個(gè)機(jī)器系統(tǒng)工作癱瘓，從而增強(qiáng)系統(tǒng)的生存能力，具有很強(qiáng)的軍事意義[7-11]。

針對(duì)此，本文提出一種分散式EKF的多機(jī)器人協(xié)同定位算法。該算法利用EKF融合內(nèi)部傳感器感知機(jī)器人自身的運(yùn)動(dòng)信息，以及外部傳感器感知的機(jī)器人之間相對(duì)觀測信息，并根據(jù)分散式數(shù)據(jù)結(jié)構(gòu)特點(diǎn)，將這些信息拆分到各個(gè)機(jī)器人平臺(tái)節(jié)點(diǎn)上以提高整個(gè)系統(tǒng)的容錯(cuò)性。通過仿真和理論分析，基于分散EKF的多機(jī)器人協(xié)同定位算法能獲得較高的估計(jì)精度。

1　多機(jī)器人協(xié)同定位系統(tǒng)的建模

1.1　機(jī)器人的運(yùn)動(dòng)學(xué)方程

(1)

1.2　機(jī)器人的觀測方程

機(jī)器人裝備的外部傳感器能提供機(jī)器人之間的相對(duì)距離觀測量，故第i個(gè)機(jī)器人在k時(shí)刻的觀測方程為：

(2)

2　分散式EKF算法

2.1　分散式數(shù)據(jù)融合

不同于集中式數(shù)據(jù)融合算法將所有不經(jīng)過處理的原始數(shù)據(jù)傳遞到中心運(yùn)動(dòng)平臺(tái)上進(jìn)行綜合處理，分散式數(shù)據(jù)融合算法以各個(gè)運(yùn)動(dòng)平臺(tái)為數(shù)據(jù)處理節(jié)點(diǎn)，節(jié)點(diǎn)自主處理本地信息且相互之間自由通信，是一種無處理中心、無依賴的算法結(jié)構(gòu)，如圖1所示。這種數(shù)據(jù)結(jié)構(gòu)避免了由于部分運(yùn)動(dòng)單元故障而導(dǎo)致整個(gè)系統(tǒng)工作癱瘓的問題，增強(qiáng)了算法的適用性和靈活性。

圖1　分散式數(shù)據(jù)融合結(jié)構(gòu)

鑒于上述分散式數(shù)據(jù)融合算法性能的優(yōu)越性，本文將分散式數(shù)據(jù)融合的思想用于多機(jī)器人協(xié)同定位濾波算法設(shè)計(jì)中，設(shè)計(jì)了一種分散式EKF算法對(duì)多機(jī)器人的位置狀態(tài)進(jìn)行估計(jì)。

2.2　時(shí)間更新

為實(shí)現(xiàn)濾波算法的分散式設(shè)計(jì)，將每個(gè)單機(jī)器人看作是一個(gè)數(shù)據(jù)融合中心，利用內(nèi)部傳感器得到的數(shù)據(jù)進(jìn)行狀態(tài)的時(shí)間更新過程。

(3)

一步預(yù)測協(xié)方差矩陣為：

(4)

由于第i個(gè)機(jī)器人能利用外部傳感器接收到第j個(gè)機(jī)器人的信息，因此時(shí)間更新步驟中還需要計(jì)算機(jī)器人之間的一步協(xié)方差矩陣，即：

(5)

(6)

2.3　量測更新

濾波量測更新的分散化實(shí)現(xiàn)，就是期望單機(jī)器人能在不依賴其他機(jī)器人節(jié)點(diǎn)的基礎(chǔ)上，利用外部傳感器測得的機(jī)器人之間相對(duì)距離信息來求得狀態(tài)估計(jì)值。本文的思想是：將量測信息拆分到各個(gè)機(jī)器人平臺(tái)節(jié)點(diǎn)上，每個(gè)節(jié)點(diǎn)作為一個(gè)獨(dú)立的處理中心，不依賴其他機(jī)器人節(jié)點(diǎn)完成量測更新過程，從而估計(jì)出整個(gè)協(xié)同定位狀態(tài)。

考慮N個(gè)機(jī)器人組成的系統(tǒng)，其全局狀態(tài)矢量可以表示為

(7)

假定在k時(shí)刻第i個(gè)機(jī)器人能同時(shí)接收到周圍l個(gè)機(jī)器人的量測信息，可得

(8)

根據(jù)Kalman濾波知識(shí)，可知

(9)

為實(shí)現(xiàn)對(duì)第i個(gè)機(jī)器人的分散式實(shí)現(xiàn)，將上式參數(shù)進(jìn)行拆分，即：

(10)

(11)

同時(shí)，要實(shí)現(xiàn)濾波算法的迭代運(yùn)算，還需要求取當(dāng)前時(shí)刻的協(xié)方差矩陣，即

(12)

同理，將上式也進(jìn)行第i個(gè)機(jī)器人的分散式實(shí)現(xiàn)，即：

(13)

(14)

(15)

其中i=1,2,…,N。

2.4　分散式EKF算法適用性分析

當(dāng)?shù)趓個(gè)機(jī)器人發(fā)生故障時(shí)，第i個(gè)機(jī)器人仍然可以利用內(nèi)部傳感器實(shí)現(xiàn)時(shí)間更新過程。更重要的是，第i個(gè)機(jī)器人仍然可以根據(jù)與其他機(jī)器人之間的相對(duì)距離觀測實(shí)現(xiàn)量測更新過程，從而估算出其位置信息。

根據(jù)上式分析，此時(shí)基于分散式EKF的多機(jī)器人協(xié)同定位算法分可為兩個(gè)步驟：時(shí)間更新和量測更新。

步驟1時(shí)間更新

(16)

步驟2量測更新

(17)

根據(jù)式(16)和(17)進(jìn)一步分析可知：第r個(gè)機(jī)器人的故障不會(huì)影響其他機(jī)器人的狀態(tài)估計(jì)，即不會(huì)導(dǎo)致整個(gè)機(jī)器人系統(tǒng)的故障，故分散式EKF算法比集中式數(shù)據(jù)融合算法的適用性更強(qiáng)，算法性能更優(yōu)越。

3　仿真結(jié)果與性能分析

根據(jù)理論研究和實(shí)驗(yàn)仿真分析，本節(jié)將比較分析集中式EKF濾波和分散式EKF濾波的性能。

3.1　仿真條件設(shè)置

利用標(biāo)識(shí)為a、b、c的機(jī)器人搭建協(xié)同定位系統(tǒng)，對(duì)集中式EKF濾波和分散式EKF濾波進(jìn)行仿真驗(yàn)證。仿真實(shí)驗(yàn)中，每個(gè)機(jī)器人都裝備里程計(jì)和一定的通信設(shè)備，機(jī)器人a裝備GPS接收機(jī)，可進(jìn)行GPS觀測；機(jī)器人速度和航向角由里程計(jì)給出，設(shè)定機(jī)器人移動(dòng)速度為4 m/s，測量速度標(biāo)準(zhǔn)差為0.5 m/s，航向角標(biāo)準(zhǔn)差為1 rad/s，同時(shí)仿真中引入的測距噪聲標(biāo)準(zhǔn)差為1 m，仿真時(shí)間設(shè)置為20 s，仿真中系統(tǒng)的采樣時(shí)間置為T=0.1 s。

3.2　仿真實(shí)驗(yàn)分析

首先設(shè)定每個(gè)機(jī)器人在整個(gè)仿真階段都可進(jìn)行相關(guān)觀測和通信。

圖2、3給出了一次實(shí)驗(yàn)中分別基于分散式EKF和集中式EKF的協(xié)同定位結(jié)果。由圖可知：相較于集中式EKF來說，基于分散式EKF的機(jī)器人協(xié)同定位能有效地抑制系統(tǒng)噪聲影響，具有較好的定位精度。

圖2　基于分散式EKF的多機(jī)器人協(xié)同導(dǎo)航軌跡

圖4給出了機(jī)器人b各狀態(tài)量的誤差曲線圖，從圖中可以看出：由于系統(tǒng)噪聲影響，基于集中式EKF的b機(jī)器人位置和航向角的誤差曲線均存在較為明顯的抖動(dòng)，但基于分散式EKF的協(xié)同定位方案能有效抑制這些系統(tǒng)噪聲對(duì)定位精度的影響，且其各狀態(tài)量誤差都近乎收斂于零，達(dá)到快速定位的要求。

為進(jìn)一步分析分散式EKF算法的性能，設(shè)定b機(jī)器人發(fā)生故障，即它無法與其他機(jī)器人進(jìn)行相互觀測，但b機(jī)器人可通過水聲通信得到其他機(jī)器人的狀態(tài)信息，仿真時(shí)間設(shè)置為10 s。

圖5給出了b機(jī)器人發(fā)生故障時(shí)分別基于分散式EKF和集中式EKF的協(xié)同定位結(jié)果。由圖可知：基于集中式EKF算法不能給出準(zhǔn)確的估計(jì)，而基于分散式EKF算法具有良好的容錯(cuò)性，并能保持較高的定位精度。

圖4　b機(jī)器人在各狀態(tài)的誤差曲線

4　結(jié)束語

本文研究了多機(jī)器人協(xié)同定位的應(yīng)用，在無中心、無依賴分散式數(shù)據(jù)結(jié)構(gòu)的基礎(chǔ)上提出一種基于分散式EKF的多機(jī)器人協(xié)同定位算法。通過仿真實(shí)驗(yàn)對(duì)比分析可知：該算法能保證有效的定位精度，且具有一定的容錯(cuò)性，為推廣多機(jī)器人協(xié)同定位的濾波算法應(yīng)用提供了較好的理論依據(jù)和解決思路。

參考文獻(xiàn)：

[1]PFOTZER L,BOHN S,OBERLAERDER J,et al.Coop-erativemulti-robot localization with radio-based sensors[C]//Nature Inspired Mobile Robotics:Proceedings of the16th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines,CLA-WAR.Australia:[s.n.],2013:781-788.

[2]TAO T,HUANG Y L,YUAN J,et al.Multi-robot active simultaneous localization and mapping based on cooperative correction approach[J].Pattern Recognition and Artificial Intelligence,2012,25(3):534-543.

[3]CHARROW B,MICHAEL N,KUMAR V.Cooperative multir-obotestimation and control for radio source localization[C]//Experimental Robotics.USA:Springer International Publishing,2013:337-351.

[4]CAI Y F,TANG Z M,ZHAO C X.Multi-EKF Localization Algorithm for Multi-Robots Formation Navigation[J].AdvanceMaterials Research,2012,383:5648-5655.

[5]CHEN L,HU H,MCDONALD M K.EKF based Mobile Robot Localization[C]//Emerging Security Technologies(EST),2012 Third International Conference on.USA:IEEE,2012:149-154.

[6]葛泉波,李文斌,孫若愚,等.基于EKF的集中式融合估計(jì)研究[J].自動(dòng)化學(xué)報(bào),2013,39(6):816-825.

[7]穆華.多運(yùn)動(dòng)平臺(tái)協(xié)同導(dǎo)航的分散式算法研究[D].長沙:國防科技大學(xué),2010.

[8]盧潔瑩,黎楚熙,蘇為洲.雙機(jī)器人編隊(duì)的協(xié)同姿態(tài)估計(jì)[J].控制工程,2016,23(5):670-675.

[9]奔粵陽,郭妍,李敬春,等.基于聯(lián)合分布狀態(tài)信息濾波的機(jī)器人協(xié)同定位[J].系統(tǒng)工程與電子技術(shù),2015,37(2):385-393.

[10] 程磊,周明達(dá),吳懷宇,等.無線傳感器環(huán)境下粒子群優(yōu)化的多機(jī)器人協(xié)同定位研究[J].智能系統(tǒng)學(xué)報(bào),2015,10(1):138-142.

[11] 向勇,田威,洪鵬,等.雙機(jī)器人鉆鉚系統(tǒng)協(xié)同控制與基坐標(biāo)系標(biāo)定技術(shù)[J].航空制造技術(shù),2016(16):87-92.