基于分散式EKF的多機器人協(xié)同定位

卓書芳，何用輝，吳燕峰

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

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

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

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

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

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

1.1　機器人的運動學(xué)方程

(1)

1.2　機器人的觀測方程

機器人裝備的外部傳感器能提供機器人之間的相對距離觀測量，故第i個機器人在k時刻的觀測方程為：

(2)

2　分散式EKF算法

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

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

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

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

2.2　時間更新

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

(3)

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

(4)

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

(5)

(6)

2.3　量測更新

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

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

(7)

假定在k時刻第i個機器人能同時接收到周圍l個機器人的量測信息，可得

(8)

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

(9)

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

(10)

(11)

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

(12)

同理，將上式也進行第i個機器人的分散式實現(xiàn)，即：

(13)

(14)

(15)

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

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

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

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

步驟1時間更新

(16)

步驟2量測更新

(17)

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

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

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

3.1　仿真條件設(shè)置

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

3.2　仿真實驗分析

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

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

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

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

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

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

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

4　結(jié)束語

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

參考文獻：

[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].自動化學(xué)報,2013,39(6):816-825.

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

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

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

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

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