IEKF濾波在移動機器人定位中的應用

強敏利，張萬緒

（西北大學 信息科學與技術學院，陜西 西安 710127）

隨著電子技術和計算機技術的快速發(fā)展及機器人定位精度的不斷提高，智能移動機器人的應用越來越廣泛?，F(xiàn)已廣泛應用于工業(yè)生產、?？仗剿?、軍事、家庭和一些服務行業(yè)。

精確的定位是移動機器人安全、有效地完成任務的關鍵和前提[1]?，F(xiàn)有的大多數定位方法是通過機器自身攜帶的傳感器實時感知自身所處環(huán)境的位置和周圍信息，并不斷修正自身狀態(tài)，然后在有障礙物的環(huán)境中有效地完成任務。

卡爾曼濾波[2]是由一系列遞歸數學公式描述的。它們提供了一種高效可計算的方法來估計過程的狀態(tài)，并使估計均方誤差最小，應用廣泛且功能強大。但如果模型或系統(tǒng)噪聲特性估計與實際不符，則會降低濾波器的精度甚至導致濾波器發(fā)散。為了解決此問題，本文結合外部環(huán)境特征信息并在測量更新階段多次迭代濾波估計值，減小定位誤差，提高系統(tǒng)穩(wěn)定性。

1 系統(tǒng)模型

本文的研究對象是一種三輪智能移動機器人，該機器人的其中一輪為萬向輪，另外兩輪為驅動輪。兩個驅動輪上裝有光電碼盤。利用驅動輪的碼盤數據，可以計算機器人的動態(tài)位置信息。機器人的正前方裝有激光雷達測距傳感器，可以采集已知環(huán)境中的路標信息。準確的系統(tǒng)模型直接影響著機器人的定位精度。因此，本文首先根據機器人信息建立了坐標系統(tǒng)模型、機器人運動模型和傳感器觀測模型。

1.1 坐標系統(tǒng)

定位是對機器人的位姿進行估計，用三個變量[xx(k),xy(k),xθ(k)]T表示，其中(xx(k),xy(k))表示機器人的位置，xθ(k)表示機器人車體的方向。為了確定機器人在平面中的位置，建立了二維平面全局坐標系，如圖1所示。坐標原點為O，取在機器人正前方測距傳感器的中心位置，并把原點作為機器人的起始位置。X軸的正方向作為機器人開始運動時的正前方，即以 X軸作為 0°，順時針方向為負，逆時針方向為正，夾角范圍是-180°～180°。假設機器人從O點運動到P點，相對初始位置的偏移角度為α，則機器人在二維平面中的位姿可描述為[x,y,α]T,其中(x,y)是 P點的坐標，表示機器人在 P點的位置，α是機器人車體相對全局坐標X軸的夾角,表示機器人在P點的航向角。

圖1 機器人的坐標模型

1.2 運動模型

機器人的運動模型即位姿估計模型，是根據內部傳感器里程計初步估計出機器人當前的位姿。理想條件下，機器人的運動模型應該準確地描述機器人的運動，得到機器人的狀態(tài)變化過程。然而，用有限的參數進行系統(tǒng)建模不可能完全表達系統(tǒng)的動態(tài)變化過程，同時傳感器數據和車體的運動都帶有噪聲，給機器人的運動模型帶來了不確定性。因此，要完整地描述機器人的運動，必須采用一個高度復雜的非線性函數，這為定位算法的實現(xiàn)帶來了難度。可以用一個離散時間差分方程[3]表示：

其中，f(x(k-1))是系統(tǒng)的狀態(tài)轉移函數，一般是非線性的，u(k)是輸入控制指令，q(k)是過程噪聲，用來表示機器人運動過程中，傳感器的誤差漂移、輪子的滑動和系統(tǒng)建模等的誤差。實際應用中，通常采用一個簡化的運動模型來近似，如式(2):

其中，x(k-1)表示機器人位姿；△T為傳感器采樣時間；v為機器人運動速度；△θ為轉角(gamma)；WB為兩驅動輪的軸距，x(k)是機器人的新位姿。

1.3 觀測模型

根據機器人運動模型估計的位姿，只是實際位姿的一個粗略的估計，由于輪子打滑等原因存在一定誤差，而且隨著機器人移動時間的增加，誤差將越來越大，最終移動機器人將偏離自己的軌跡。為了修正誤差，移動機器人需要利用激光雷達傳感器，對路標進行觀測，修正自身的位姿。

本文利用激光雷達測距傳感器的觀測信息來修正機器人自身的位置。機器人在運動過程中，利用測距傳感器掃描周圍環(huán)境，獲得的觀測量z(k)是周圍環(huán)境路標相對于傳感器的距離和方向，然后根據先驗的地圖信息確定自身的實際位置。在定位問題的狀態(tài)空間描述中可以表示為：

其中，h(·)是測量函數，r(k)是觀測噪聲，表示運動和觀測中的不確定性。

圖 2表示移動機器人在位置[xx(k),xy(k),xθ(k)]T時，用激光雷達觀測到第i個路(xi,yi)時機器人與路標的位置信息。其中，d表示路標與機器人的相對距離,α-xθ(k)表示移動機器人在[xx(k),xy(k),xθ(k)]T時，路標距離機器人方向的角度。

圖2 機器人與路標位置對應關系

激光雷達返回的路標觀測信息為機器人與路標的相對位置信息(d,g)，d為路標相對機器人的距離，g為路標相對機器人的夾角，則觀測模型可以表示為：

式(4)中的觀測模型是由第i個路標的位置坐標(xi,yi)和機器人位姿[xx(k),xy(k),xθ(k)]T計算得到第 i個路標特征相對機器人的距離和機器人前進方向的夾角。

2 IEKF濾波定位算法

機器人從已知位置出發(fā)，利用里程計及慣性導航儀可以保持對自身運動的跟蹤。由于里程計存在誤差，移動一段距離后，機器人對其位置會變得不確定。機器人應用迭代擴展卡爾曼濾波定位算法可以實現(xiàn)相對環(huán)境地圖定位，消除不確定性，否則，將會偏離預計路徑。迭代擴展卡爾曼濾波[4](IEKF)是非線性系統(tǒng)進行參數估計和數據融合[5]常用的算法，用泰勒級數將非線性方程進行線性化，然后再遵循經典的卡爾曼濾波算法對系統(tǒng)狀態(tài)進行估計。具體的融合定位結構框圖如圖3所示。

圖3 多信息融合定位結構圖

IEKF算法是以EKF算法為基礎，可以歸納為狀態(tài)預測、觀測與匹配、迭代與更新三個步驟。

首先，進行狀態(tài)預測。機器人初始時刻的位姿已知，假設k-1時刻獲得的狀態(tài)估計值為,利用式(2)系統(tǒng)運動模型獲得k時刻的狀態(tài)估計值(k)。然后分別計算系統(tǒng)模型對狀態(tài)和過程噪聲的雅克比矩陣，預測誤差的協(xié)方差P(k)-，完成狀態(tài)的預測過程。

最后，利用觀測值迭代更新預測值。先分別計算觀測模型的雅克比矩陣 Hi(k)，卡爾曼增益 Wi(k)，再利用Hi(k)和Wi(k)更新誤差協(xié)方差矩陣。其次計算觀測變量的殘余vi(k)，更新狀態(tài)。

綜上所述，IEKF濾波定位的具體步驟可以表示如下：

(1)給定已知的環(huán)境路標與實際機器人路徑，建立數據庫。

(2)初始化機器人位姿與協(xié)方差矩陣。假設機器人從原點開始運動，則：

根據式(2)得具體位姿預測形式如下：

誤差協(xié)方差預測：

其中，Gv是f對x偏導的雅克比矩陣：

Gu是f對q的偏導的雅克比矩陣：

Q為過程噪聲的協(xié)方差矩陣。

(4)獲取觀測信息。根據觀測方程(3)得到環(huán)境信息。首先根據預測的狀態(tài)信息x^-(k)以及已知的地圖路標信息得到預測的觀測值z^(k)，然后利用激光雷達觀測環(huán)境中的路標得到實際的觀測值z(k)并進行特征匹配。

(5)狀態(tài)更新。根據觀測值修正預測的機器人位姿。具體步驟如下：

②For i=1:N

計算觀測模型的雅克比矩陣：

計算卡爾曼增益：

更新誤差協(xié)方差矩陣：

計算觀測變量的殘余：

更新狀態(tài)：

③ 將迭代結果作為k時刻的狀態(tài)更新值。

其中：“^”代表估計，“-”代表先驗，“+”代表后驗，N 為迭代次數。

3 仿真實驗及結果分析

這一部分對IEKF算法進行實驗驗證，并與傳統(tǒng)的EKF算法進行比較。

實驗所用的環(huán)境區(qū)域是100 m×100 m的室內環(huán)境(假設障礙物靜止不動)。機器人運行路徑的關鍵點(waypoint)和路標 (landmark)均人為指定，實驗中隨機設定了7個路徑關鍵點和8個路標，如圖3所示。移動機器人的運動速度v=2.0 m/s,角速度 rate g=20×π/180 rad/s。激光測距傳感器的最大觀測距離為30 m,觀測角度范圍為-30°～30°。 控制噪聲為 σv=0.6 m/s,σg=1.0×π/180 rad，觀測噪聲為 σR=0.1 m/s,σB=1.0×π/180 rad， 機器人兩個驅動輪之間的間距WB=0.5 m，傳感器采樣時間△T=0.025 s。

假設機器人從原點[0,0,0]T開始運動,分別應用EKF和IEKF兩種算法進行實驗，結果如圖4所示。

圖4 機器人定位結果

圖中分別為機器人實際的路徑、EKF算法和IEKF算法估計路徑，“*”為設定的路標 (landmark)，“+”代表 EKF 算法和IEKF算法估計的路標，“o”代表路徑關鍵點 (waypoint)。

從圖4可以看出，在剛開始的一段時間，兩種算法的狀態(tài)估計和特征點估計較理想，但隨著時間的推移，IEKF所估計的狀態(tài)能較好地與真實狀態(tài)吻合，效果最為理想。表1給出了兩種不同算法分別在X軸方向、Y軸方向和方位角的均方誤差?？梢悦黠@地看出，IEKF算法的定位精度優(yōu)于EKF算法。

表1 兩種濾波算法在各個方向的均方誤差比較

IEKF算法與傳統(tǒng)的EKF算法不同之處在于狀態(tài)更新階段，在狀態(tài)更新階段IEKF算法采用多步迭代，多次對觀測方程線性化，重復整個濾波過程，最終達到最優(yōu)的狀態(tài)估計。 圖5為改變更新階段的迭代次數，其他參數設置不變產生的定位曲線與真實曲線的對比。

圖5 不同迭代次數產生的定位曲線

可以看到，總體上不同迭代次數的IEKF算法定位曲線與真實曲線基本吻合，定位結果比較理想。隨著迭代次數的增加，定位精度更加精確，具體反映在表2中。

表2 不同迭代次數在各個方向的均方誤差比較

眾所周知，迭代次數越多，計算量越大，時間越長。因此，實際機器人定位中，針對實際硬件條件，可以選取合適的迭代次數提高算法的定位精度，增強算法的收斂穩(wěn)定性。

由兩次實驗結果可以看出，IEKF算法估計的路標特征和機器人路徑與實際基本相符。從實驗所得的誤差數據可以看出，相對于機器人自身的空間大小，誤差在可接受的范圍之內，表明該定位系統(tǒng)定位精度較高，具有較高的可靠性。

本文針對機器人定位精度問題，在傳統(tǒng)卡爾曼濾波的基礎上，提出一種迭代擴展卡爾曼濾波算法。該算法相對于傳統(tǒng)卡爾曼濾波算法，在狀態(tài)更新階段采用多次迭代更新，減少了系統(tǒng)線性化時所帶來的誤差，并將該算法與多傳感器信息融合技術結合應用于有路標的機器人定位。模型簡單、存儲量小。實驗結果表明該算法在保證實時性的同時較大地提高了定位精度，能夠滿足機器人高精度及可靠性等方面的要求。

[1]丁偉，孫華，曾建輝.基于多傳感器信息融合的移動機器人導航綜述[J].傳感器與微系統(tǒng)，2006,25(7):1-3.

[2]石杏喜，趙春霞.基于概率的移動機器人SLAM算法框架[J].計算機工程，2010,36(2)：31-32.

[3]曾健平,王保同，謝海情.自主移動機器人定位系統(tǒng)Kalman濾波算法改進[J].計算機應用研究，2011,28(5):1710-1712.

[4]李良群，姬紅兵，羅軍輝.迭代擴展卡爾曼粒子濾波器[J].西安電子科技大學學報，2007,34(2)：233-238.

[5]陳小寧，黃玉清，楊佳.多傳感器信息融合在移動機器人定位中的應用[J].傳感器與微系統(tǒng)，2008，27(6)：110-113.

[6]鄒智榮，蔡自興，陳白帆.移動機器人SLAM中一種混合數據關聯(lián)方法[J].小型微型計算機系統(tǒng)，2011,32(7)：1341-1343.