基于EKF和PF的多機器人協同定位技術

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

關鍵詞： 多機器人系統； 協同定位； 擴展卡爾曼濾波； 粒子濾波

中圖分類號： 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 引 言

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

1 多機器人系統協同定位技術框架

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

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

圖1 多機器人系統協同定位技術框架

2 機器人系統運動模型和觀測模型

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

2.1 運動模型

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

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

圖3 單個采樣周期內機器人的運動形式

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：令狀態變量[Xk=（xk，yk，φk）T，][Uk=（s1k，s2k）T，]則上述式（3）方程組可化為如下非線性函數形式：

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

2.2 觀測模型

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

[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 基本原理

對于線性系統而言， 卡爾曼濾波器是一個統計意義上最優的狀態估計。機器人的運動和觀測形式都是非線性的，所以可用擴展卡爾曼濾波[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：預測。

狀態預測：

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

方差預測：

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

觀測量預測：

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

Step 3：更新濾波參數。

卡爾曼增益：

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

狀態更新：

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

方差更新：

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

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

3.2 算法的優缺點

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

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

4 粒子濾波

4.1 基本原理

粒子濾波器[6?8]的主要思想是利用一組帶有權重的采樣來表示系統狀態的后驗概率分布。粒子濾波器的結構示意圖如圖5所示。

圖5 粒子濾波器PF的結構示意圖

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

4.2 算法的優缺點

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

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

5 EKF?PF混合定位

5.1 理論準備

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

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

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

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

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

5.2 仿真結果與分析

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

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

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

圖7 機器人定位誤差對比

6 結 論

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

參考文獻

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

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

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

[4] 付夢印，鄧志紅，張繼偉.Kalman濾波理論及其在導航系統中的應用[M].北京：科學出版社，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] 周翟和，劉建業，賴際舟.粒子濾波的改進優化算法在組合導航系統中的應用[J].中國慣性技術學報，2010（6）：711?715.

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

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

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

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

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

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

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