基于EKF和PF的多機器人協(xié)同定位技術(shù)

摘 要： 無論對于單機器人還是多機器人系統(tǒng)，定位能力是其完成各項任務(wù)的前提條件。針對常用定位技術(shù)難以滿足移動機器人群體協(xié)同定位精度高、實時性強等要求，提出了一種基于擴展卡爾曼濾波（EKF）和粒子濾波（PF）的混合定位技術(shù)。首先，對機器人進行運動建模，然后分別討論基于擴展卡爾曼濾波和粒子濾波的機器人協(xié)同定位的基本原理、優(yōu)缺點，在此基礎(chǔ)上提出基于擴展卡爾曼濾波和粒子濾波相結(jié)合的協(xié)同定位方法。結(jié)果表明，該算法在滿足一定條件下可有效解決定位精度與運算量之間的矛盾，可實現(xiàn)在初始條件未知或存在較大誤差情況下多機器人快速、精確地協(xié)同定位。

關(guān)鍵詞： 多機器人系統(tǒng)； 協(xié)同定位； 擴展卡爾曼濾波； 粒子濾波

中圖分類號： TN710?34； TP332.3 文獻標識碼： A 文章編號： 1004?373X（2013）23?0095?04

Multi?robots co?localization technique based on EKF and PF

TIAN Hong?bing1， FAN Guang?nan2， SONG Long2

（1. College of Computer and Communication， Lanzhou University of Technology， Lanzhou 730050， China；

2. College of Electrical and Information Engineering， Lanzhou University of Technology， Lanzhou 730050， China）

Abstract： The positioning capacity is a prerequisite to complete the tasks， no matter for single?robot or multi?robots systems. It is difficult for common location technique to meet the requirements of mobile robot groups in high co?localization accuracy and strong real?time. A hybrid location technique based on Extended Kalman Filter （EKF） and Particle Filter （PF） is proposed. The multi?robots are motion modeling， then the basic principle of robot co?localization based on EKF or PF and their advantages and disadvantages are discussed respectively. On this basis， another co?localization based on the combination of EKF and PF is proposed. The experiment result shows that this method is effectively in solving the contradiction between positioning accuracy and calculation under some condition. And when the initial condition is unknown or the errors is large， the multi?robots co?localization can also be rapidly and precisely.

Keywords： multi?robot system； co?operative localization； EKF； PF

0 引 言

隨著人工智能、計算機、傳感器等技術(shù)的不斷發(fā)展，移動機器人的研究及應(yīng)用得到了前所未有的進步。多機器人的群體協(xié)作由于具有比單一機器人系統(tǒng)更高的工作效率、魯棒性、定位精度等優(yōu)點而得到了廣泛的關(guān)注，其中多機器人的協(xié)同定位問題成為近年來機器人研究領(lǐng)域的一大熱點及難點。目前，機器人協(xié)同定位技術(shù)主要有：卡爾曼濾波、粒子濾波、最大似然估計、模糊邏輯、人工神經(jīng)網(wǎng)絡(luò)[1]。這些定位方法的基本思想是：如何融合內(nèi)部傳感器的測量信息和外部傳感器的觀測信息以減少過程誤差和傳感器的測量誤差，得到更精確的機器人位置信息[2]。它們都有一定的適應(yīng)范圍，單一方法很難滿足多機器人系統(tǒng)的復(fù)雜工作環(huán)境。針對初始狀態(tài)未知或存在較大測量誤差的情況，本文提出EKF和PF相結(jié)合的定位方法，即先用PF使初始條件收斂到EKF的初始誤差允許范圍，再用EKF進行迭代濾波，以達到快速、精確協(xié)同定位的目的。

1 多機器人系統(tǒng)協(xié)同定位技術(shù)框架

多機器人系統(tǒng)的協(xié)同定位是指多機器人群體利用相互之間的觀測信息，在未知環(huán)境中互為路標，并通過信息交換，共享各個機器人獲得的自身和環(huán)境測量信息，得到比單個機器人自身定位更精確的位姿估計[3]，其技術(shù)框架如圖1所示。

單個機器人通常會配備多個內(nèi)部和外部傳感器以獲取自身參數(shù)信息和周圍環(huán)境信息。從圖1可以看出，機器人的傳感器信息經(jīng)過濾波等預(yù)處理后，得到兩種不同性質(zhì)的定位信息：自身位姿信息和相對觀測信息。然后運用EKF、PF等技術(shù)進行第二級的融合，最終實現(xiàn)協(xié)同定位。

圖1 多機器人系統(tǒng)協(xié)同定位技術(shù)框架

2 機器人系統(tǒng)運動模型和觀測模型

為進行機器人協(xié)同定位，首先需要建立機器人的運動模型和傳感器的觀測模型[3]。機器人的運動模型因其機動性設(shè)計的變化而有所不同。不失一般性，下面以二維平面內(nèi)兩輪機器人為例，介紹機器人運動模型和觀測模型，并利用該模型進行各算法分析。

2.1 運動模型

如圖2，圖3所示，假設(shè)機器人在圖中[A]點的位姿[（xk，yk，φk）]，運動到[B]點后的位姿變?yōu)閇（xk+1，yk+1，φk+1），][φ]是機器人前進方向與[X]軸的夾角，[s1k，][s2k]是本次采樣期間機器人左右輪運動的距離，[d]為兩輪間距。移動機器人的運動模型建立過程如下：

圖2 機器人運動的幾何模型

圖3 單個采樣周期內(nèi)機器人的運動形式

Step 1：以[A]點建立坐標系，求[B]點位姿[（Δx，Δy，Δφ）]，有：

[Δx=-0.5（s1k+s2k）sin（Δφ2）Δy=0.5（s1k+s2k）cos（Δφ2）Δφ=2arcsin[（s1k-s2k）4d]] （1）

Step 2：將上一步得到的位姿變換到圖3坐標系中，有：

[xk+1yk+1=xkyk+sinφksinφk-cosφksinφkΔxΔyφk+1=φk+Δφ≈φk+（s2k-s1k）2d] （2）

Step 3：將式（1）代入式（2），可得到機器人的運動方程如下：

[xk+1=xk+0.5（s1k+s2k）cos（φk+（s2k-s1k）4d）yk+1=yk+0.5（s1k+s2k）sin（φk+（s2k-s1k）4d）φk+1=φk+（s2k-s1k）2d] （3）

Step 4：令狀態(tài)變量[Xk=（xk，yk，φk）T，][Uk=（s1k，s2k）T，]則上述式（3）方程組可化為如下非線性函數(shù)形式：

[Xk+1=f（Xk，Uk）] （4）

2.2 觀測模型

圖4所示為某時刻機器人[i，j]進行相對觀測的結(jié)果。其中[Dij]是相對距離，[α]是機器人[j]相對[i]的方向角，[β]是[i]相對[j]的方位角，統(tǒng)稱為觀測信息。相對觀測信息與機器人自身位姿信息的數(shù)量關(guān)系稱為觀測模型，其具體關(guān)系如下：

[Zi=h（Xi，Yj）?Dij=（xj-xi）2+（yj-yi）2α=arctan（yj-yi）（xj-xi）-φi] （5）

其中觀測變量[Zi]=[（Dij，α）T，]同理可得[Zj。]

圖4 相對觀測模型

3 擴展卡爾曼濾波

3.1 基本原理

對于線性系統(tǒng)而言， 卡爾曼濾波器是一個統(tǒng)計意義上最優(yōu)的狀態(tài)估計。機器人的運動和觀測形式都是非線性的，所以可用擴展卡爾曼濾波[4?5]進行自身位姿信息和觀測信息的融合。EKF求解過程如下：

Step 1：線性化即求解各方程的雅克比矩陣。

[Φk=?f（Xk，Uk）?Xk] （6）

[Rk=?f（Xk，Uk）?Uk] （7）

[Hik=?h（Xik，Xjk）?Xik] （8）

Step 2：預(yù)測。

狀態(tài)預(yù)測：

[Xk+1=f（Xk|k，Uk）] （9）

方差預(yù)測：

[Pk+1|k=Φk?Pk|k?ΦTk+Rk?Q?ΦTk] （10）

觀測量預(yù)測：

[Z=h（X（i）k+1|k，X（j）k+1|k）] （11）

Step 3：更新濾波參數(shù)。

卡爾曼增益：

[K=Pk+1|k?Hik?（HTik?Pk+1|k?Hik+ρ）-1] （12）

狀態(tài)更新：

[Xk+1|k+1=Xk+1|k+K?（Z-Z）] （13）

方差更新：

[Pk+1|k+1=（1-K?HTik）?Pk+1|k] （14）

重復(fù)Step 2和Step 3直至估計值與真實值十分接近或相等。其中，[Q]和[ρ]分別為碼盤誤差和觀測誤差的方差，且初始狀態(tài)和協(xié)方差已知。

3.2 算法的優(yōu)缺點

擴展卡爾曼濾波是一個不斷預(yù)測、修正的遞推過程，其在求解時不需存儲大量的觀測數(shù)據(jù)，只是利用新的觀測數(shù)據(jù)隨時更新濾波參數(shù)，因此其便于實時處理觀測結(jié)果。

由式（5）所示的觀測模型方程知，當[ΔyΔx=][（yj-yi）（xj-xi）→±∞]時，對應(yīng)[arctan（yj-yi）（xj-xi）→][±π2。]這意味著盡管兩個機器人之間距離[Δx]非常小，由于定位誤差的影響，當前時刻的[Δx]與前一時刻的[Δx]可能會發(fā)生符號的改變， 而[Δy]則較大或保持在一定的范圍，其符號不變，從而導(dǎo)致了相對方向角[α]的跳變， 使相對方位的預(yù)測出現(xiàn)較大的誤差，濾波出現(xiàn)發(fā)散情況，影響了定位的魯棒性和可靠性。

4 粒子濾波

4.1 基本原理

粒子濾波器[6?8]的主要思想是利用一組帶有權(quán)重的采樣來表示系統(tǒng)狀態(tài)的后驗概率分布。粒子濾波器的結(jié)構(gòu)示意圖如圖5所示。

圖5 粒子濾波器PF的結(jié)構(gòu)示意圖

圖5中的重采樣是為了解決標準粒子濾波出現(xiàn)的粒子退化問題。重要性采樣所需重要性密度函數(shù)的選擇對粒子濾波至關(guān)重要。選取不同的重要性密度函數(shù)會產(chǎn)生不同的粒子濾波器如標準粒子濾波，高斯?斯密特、輔助、正則化等改進粒子濾波算法。

4.2 算法的優(yōu)缺點

PF可以很好地表示非線性、非高斯模型，是解決及時定位和地圖構(gòu)建的有效手段，被認為是實現(xiàn)真正全自主移動機器人的關(guān)鍵 。

但是該算法仍然存在著一些問題，其中最主要的問題是依賴大量的樣本數(shù)據(jù)才能很好地近似系統(tǒng)的后驗概率密度，計算量大、效率不高。

5 EKF?PF混合定位

5.1 理論準備

綜上可知，基于EKF和PF的協(xié)同定位方法有各自優(yōu)勢，但也各有不足。因此提出了基于EKF和PF相結(jié)合的協(xié)同定位方法。該混合定位方法步驟如下：

Step 1：初始化及預(yù)處理。初始化工作主要有完成PF、EKF所需的參數(shù)設(shè)置，然后作濾波去噪、放縮、限幅等預(yù)處理操作。

Step 2：使用PF算法直至初始條件收斂到滿足EKF算法的初始誤差范圍。其中PF算法可根據(jù)情況選擇一定的改進型粒子濾波算法。

Step 3：當初始誤差滿足EKF要求后，啟用EKF濾波進行狀態(tài)估計及更新，調(diào)整各機器人位姿，最終實現(xiàn)多機器人系統(tǒng)的協(xié)同定位。其中，EKF和PF的具體設(shè)計參照第3、4節(jié)。

理論上，上述混合定位方法可獲得EKF和PF兩者的優(yōu)勢，同時克服了兩者的不足，在精確定位的同時使運算量減少，從而提高定位效率。

5.2 仿真結(jié)果與分析

為驗證上述混合定位方法的優(yōu)越性，進行以下仿真實驗。選用了兩個雙輪機器人小車模型，其內(nèi)外部傳感器的數(shù)據(jù)采樣頻率為10 Hz和1 Hz，機器人小車寬0.7 m，以1 m/s的速度分別向東和向北運動，初始位姿分別為（5，0，0）、（0，5，[π2]）。分別采用上述三種算法進行協(xié)同定位狀態(tài)估計。以單次實驗數(shù)據(jù)為例，運用Matlab軟件作出不同情況下兩機器人小車的運動軌跡及協(xié)同定位誤差[ρ]（偏離真實位置的距離）如圖6，圖7所示。

由仿真結(jié)果可以看出，這三種協(xié)同定位方法均能在一定程度上改善定位性能。單獨EKF算法在某些情況下會產(chǎn)生濾波發(fā)散；單獨PF算法雖最終能使估計值收斂到真實值，但所花時間較長，計算量較大；而基于EKF和PF的混合定位算法能更快、更接近機器人的真實運動，更好地實現(xiàn)協(xié)同定位。

圖6 兩個機器人的運動軌跡估計

圖7 機器人定位誤差對比

6 結(jié) 論

實驗結(jié)果表明，基于EKF和PF的混合協(xié)同定位能得到比常用定位方法更精確的機器人定位信息，且速度更快。下一步將把上述算法運用到實體機器人上，以測試其在多機器人協(xié)同定位中的實際效果。該技術(shù)對未來的多機器人環(huán)境檢測、編隊協(xié)作等研究有重要參考價值。

參考文獻

[1] 郭戈，胡征峰，董江輝.移動機器人導(dǎo)航與定位技術(shù)[J].微計算機信息，2003（8）：10?11.

[2] 邵金鑫.基于多傳感器信息融合的多機器人協(xié)同定位技術(shù)研究[D].長沙：國防科技大學(xué)，2006.

[3] 劉顯榮.傳感器融合在多機器人合作定位中的應(yīng)用[J].科技通報，2005（2）：277?281.

[4] 付夢印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京：科學(xué)出版社，2003.

[5] TESLI? L， ?KRJANC I， KLAN?AR G. EKF?based localization of a wheeled mobile robot in structured environment [J]. Journal of Intelligent and Robotic Systems， 2011， 62（2）： 187?203.

[6] 周翟和，劉建業(yè)，賴際舟.粒子濾波的改進優(yōu)化算法在組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報，2010（6）：711?715.

[7] 張淼，胡建旺，周云峰，等.改進粒子濾波算法的比較[J].電光與控制，2009（2）：30?32.

[8] 張弦.基于改進粒子濾波算法的移動機器人定位[D].北京：北京郵電大學(xué)，2010.

[9] 張詩桂，朱立新，趙義正.粒子濾波算法研究進展與展望[J].自動化技術(shù)與應(yīng)用，2010（6）：1?9.

[10] 王炎慶.基于卡爾曼濾波器的神經(jīng)元干細胞序列圖像中活躍細胞的追蹤[D].哈爾濱：哈爾濱工程大學(xué)，2010.

作者簡介：田紅兵 男，1976年出生，湖北孝感人，碩士，工程師。主要研究方向為嵌入式系統(tǒng)及計算機軟件應(yīng)用。

樊光南 男，1987年出生，江西吉安人，碩士研究生。主要研究方向為智能控制系統(tǒng)與技術(shù)、射頻識別技術(shù)等。

宋 龍 男，1976年出生，甘肅靜寧人，碩士，工程師。主要研究方向為計算機自動控制技術(shù)、電力電子技術(shù)等。