迭代卡爾曼濾波在機器人定位中的應用

摘 要:定位是移動機器人最基本的問題之一。應用了迭代卡爾曼濾波(IEKF)集成航位推算和全局觀測信息，解決機器人的定位問題。該方法在卡爾曼濾波測量更新階段，多次迭代計算估計狀態，直到誤差小于一定的閾值。減少了由于泰勒展開的截斷帶來的定位誤差，使得算法的收斂穩定性增強。最后通過仿真實驗與EKF方法比較。結果表明，IEKF在移動機器人定位中是一種有效的方法。 關鍵詞:迭代卡爾曼濾波; 誤差分析; 移動機器人; 機器人定位

中圖分類號:TP919-34; TP391 文獻標識碼:B

文章編號:1004-373X(2010)22-0123-03

Application of Iterated Kalman Filtering in Robot Localization

LONG Hui1， HU Li2， ZHOU Yan-yu3

(1.Hunan Biological and Electromechanical Polytechnic， Changsha 410126， China;2.Hunan Intelligent Control System Company， Changsha 410001， China;

3.College of Information Engineering， Central South University， Changsha 410075， China)

Abstract: Localization is one of the most fundamental problem in mobile robots. The localization problem was solved by iterated extended Kalman filtering (IEKF) by combining the reckon reference and global observation information. In the measurement update phase of Kalman filtering， the state estimation is iterated many times until estimation error is lower than the threshold and localization error aroused from Taylor expand is reduced effectively. The convergent stability was improved by the method. Finally， IEKF and EKF methods are compared. the simulation result shows that IEKF is an effective method in localization of mobile robots.Keywords: iterated Kalman filtering; error analysis; mobile robots; robot localization

0 引 言

定位是自主機器人導航當中最基本的問題之一[1]，對于一個自主機器人系統，精確的空間定位是其實現自主導航的前提。Leonard和 Durrant-Whyte 把導航問題歸結為3個子問題[2]，包括回答“我在哪里?”，“我要去哪里?” 和“我怎樣到達那里?”。其中第一個問題就是定位問題。如果機器人沒有獲取自身的位置，就無法進行后續的目標識別和路徑規劃?，F有大多數定位方法的關鍵是通過融合內部傳感器的測量信息與外部傳感器的測量信息，以減少過程噪聲和傳感器的測量誤差，得到更精確的機器人位置信息。

Kalman濾波[3]是一種線性無偏最小方差估計，采用時間遞推得到的，模型簡單，數據存儲量小，特別適用于計算機應用，已被廣泛應用于航天、航空、航海、系統工程、通信等多種領域狀態的估計和控制中。

1 問題描述

在二維空間，移動機器人的位置可以用3個變量表示(x，y，θ)[4]，其中x，y和θ分別表示機器人的位置坐標和方位角。機器人具有的運動模型如下:

X(k+1)=f[X(k)，u(k)，w(k)]

=x(k)y(k)θ(k)+ΔD(k)cos[θ(k)+Δθ(k)/2]

ΔD(k)sin[θ(k)+Δθ(k)/2]

Δθ(k)(1)

z(k)=h[x(k)，v(k)](2)

式中:x(k)為系統在k 時刻狀態; z(k)為k 時刻狀態變量的觀測量; u(k)=ΔD(k)

Δθ(k)為系統的控制輸入;ΔD和Δθ為k到k+1時刻機器人的相對位移和運動方向;w(k)和v(k)分別為高斯加性過程噪聲和測量噪聲，且具有如下的統計特性:

E[v(k)]=E[w(k)]=0;

E[v(k)vT(j)]=Q(k)δ(k); j=k

E[w(k)wT(j)]=R(k)δ(k); j=k

E[v(k)wT(j)]=0(3)

機器人運動過程和觀測過程均受到隨機高斯白噪聲干擾，在此的目的是根據系統方程和觀測方程，并結合噪聲統計特性及給定的初始位置估計機器人的位置，機器人位置如圖1所示。

圖1 機器人位置示意圖

2 迭代擴展卡爾曼濾波定位

2.1 擴展卡爾曼濾波

卡爾曼濾波是由R.E. Kalman于1960 年提出的，它是一種簡單有效的線性最優遞推估計算法，可估計帶噪聲的線型動態系統狀態。然而，假設卡爾曼濾波器是線性系統，但實際上機器人的運動模型與觀測模型是非線性的。因此通常采用擴展卡爾曼濾波器(extended kalman filter，EKF)，并通過一階泰勒展開來近似非線性模型。依據Curran [5]綜合航位推算、超聲和紅外傳感器數據，提出了基于EKF的定位和運動控制算法。為了簡化定位算法，Jensfelt和Christensen[6]應用加權霍夫變換算法提取了環境特征，采用最小化環境特征模型和激光雷達數據，建立了基于EKF的位姿跟蹤算法?；贓KF的濾波算法分為時間更新和測量更新2個階段，具體步驟如下:

(1) 初始化。給定機器人初始位置和協方差矩陣:

+0=[x(0)，y(0)，θ(0)]T;P(0)+

(2) 時間更新。

當k≥1，則:

(k)-= f[(k-1)+，u(k)]

P(k)- =AxP(k-1)ATx+BuP(k-1)BTu + Q(k)

Ax=fxx=x(k)+，Bx=fuu=u(k)

(3) 測量更新:

(k)+=(k)-+K(k)[z(k)-(k)]

=h[x(k)]

P(k)+=[I-K(k)H(k)]P(k)-

其中:卡爾曼增益:

K(k)=P(k)-H(k)T[H(k)P(k)-H(k)T+

R(k))-1H(k)=h(x)xx=x(k)+

(4) 重復第(2)和第(3)步。

算法中采用矩陣A，B，H系統運動方程和測量方程對狀態和輸入求偏導數的雅克比矩陣。

2.2 迭代擴展卡爾曼濾波定位

由于EKF舍去了高階無窮小，在非線性誤差比較嚴重時，會帶來估計的不準確。針對EKF線性化存在的誤差，很多學者做了大量研究，如迭代EKF算法[7]、基于Uscent變換的Uscent Kalman Filter(UKF)算法、中心差分卡爾曼濾波算法、粒子濾波和UPF濾波以及相應的改進算法[8]等。

其中，迭代EKF[9]算法在原有EKF基礎上，對量測更新的狀態估計值采用多步迭代，從而減少由于泰勒展開帶來的截斷誤差。在此應用IEKF方法實現移動機器人運行狀態的位置。基于上述思想，在任意時刻計算k迭代估計值的步驟如下[10]:

(1) 將x(k)的一步預測狀態向量(k)-作為迭代的初始值x(0)(k)，依據觀測方程線性化，計算濾波值與擴展卡爾曼濾波相同，并把結果(k)作為x(1)(k)。

(2) 在x(1)(k)處重新將觀測方程線性化，整個濾波過程重復一遍，新估計值(k)+作為x(2)(k)。

(3) 迭代過程中，當兩步狀態差值滿足式(4):

|i+1j(k)-ij(k)|≤ε， j = 1，2，…，n(4)

迭代終止，否則返回步驟(2)。其中ε是預先給定的迭代閾值。

3 仿真分析

假設機器人按正方形軌跡運動，起點位置(100，100)，則機器人首先沿著x軸正方向運動，每步10 cm，逆時針方向作圓周運動。為了真實地反映出Kalman濾波的效果，采用Monte-Carlo方法，以及多次實驗取均值的方法進行研究，可以計算出估計的誤差均值和方差，其表達式為:

x(k)=1M∑Mi=1[xi(k)-i(k|k)](5)

式中:M為Monte-Carlo仿真的次數;k為取樣點數。仿真的次數越多，實驗的效果越接近于實際，但計算的速度明顯變慢，因此在仿真時，需要根據實際適當選取。在本程序中，取M=50。

測量噪聲v(k)為高斯白噪聲(記為WGN)，v(k)～N(0，10-5 )。 圖2為機器人原始運行軌跡和EKF方法、IEKF估計值軌跡。從圖2中可以看出，IEKF運行軌跡更接近真實軌跡。圖3是采用EKF和IEKF方法y沿軸方向估計誤差值的均值隨著采樣點數的增多而發生變化的情況。

由仿真結果可看出，執行IEKF算法相對于EKF算法效果更好。 這主要歸功于狀態估計的多步迭代，在一定程度上修正了由于泰勒展開引起的截斷誤差，使得算法收斂的穩定性得到增強。

圖2 IEKF機器人的原始軌跡、觀測數據和估計值軌跡

圖3 目標跟蹤誤差曲線

4 結 語

在此，針對擴展卡爾曼濾波算法精度較低的缺點，提出了對算法測量更新階段的狀態估計值，并采用多步迭代更新，使其在滿足一定閾值時，迭代結束。該算法一定程度上減小了局部線性化帶來的截斷誤差。仿真結果與EKF算法進行了對比，結果表明該算法的濾波精度有明顯提高。

參考文獻

[1]王志文，郭戈.移動機器人導航技術現狀與展望[J].機器人，2003，25(5):470-474.

[2]LEONARD J， DURRANT-WHYTE H.Mobile robot localizaiton by tracking deometric beacons[J]. IEEE Transactions on Robotics and Automation， 1991，7(3): 376-382.

[3]NEGENBORN Rudy.Robot localization and Kalman filters on finding your position in a noisy world[D]. Holland: Utrecht University， 2003.

[4]SEYR Martin， JAKUBED Stefan.Mobile robot predictive trajectory tracking[C]. ICINCO. [S.l.]: ICINCO， 2005: 112-119.

[5]CURRANT A， KYRIAKOPOULOS K J.Sensor-based self localization forwheeled mobile robots[J].Journal of Robotic Systems，1995， 12 (3): 163-176.

[6]JENSFELT P， CHRISTENSEN H I.Pose tracking using laser scanning and minimalistic environmental models [J].IEEE Transaction on Robotics and Automation，2001， 17(2): 138-147.

[7]SIBLEY G， SUKHATME G， MATTHIES L.The iterated Sigma point Kalman filter with applications to long range stereo[C]// Proceedings of Robotics Science and Systems.Philadelphia， USA: MIT Press， 2006: 263-270.

[8]WANG X， ZHANG H.A UPF-UKF framework for SLAM[C]// Proceedings of IEEE International Conference on Robotics and Automation.Roma， Italy: IEEE， 2007: 1664-1669.

[9]郭杭.迭代擴展卡爾曼濾波用于實時GPS數據處理[J].武漢測繪科技大學學報，1999，24(2):112-114，123.

[10]高為廣，潘娜娜，張曉東.改進的迭代EKF算法在偽衛星定位中的應用[J].測繪科學，2008，33(4):73-75.