摘 要:定位是移動(dòng)機(jī)器人最基本的問題之一。應(yīng)用了迭代卡爾曼濾波(IEKF)集成航位推算和全局觀測(cè)信息，解決機(jī)器人的定位問題。該方法在卡爾曼濾波測(cè)量更新階段，多次迭代計(jì)算估計(jì)狀態(tài)，直到誤差小于一定的閾值。減少了由于泰勒展開的截?cái)鄮淼亩ㄎ徽`差，使得算法的收斂穩(wěn)定性增強(qiáng)。最后通過仿真實(shí)驗(yàn)與EKF方法比較。結(jié)果表明，IEKF在移動(dòng)機(jī)器人定位中是一種有效的方法。 關(guān)鍵詞:迭代卡爾曼濾波; 誤差分析; 移動(dòng)機(jī)器人; 機(jī)器人定位

中圖分類號(hào):TP919-34; TP391 文獻(xiàn)標(biāo)識(shí)碼:B

文章編號(hào):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 引 言

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

Kalman濾波[3]是一種線性無偏最小方差估計(jì)，采用時(shí)間遞推得到的，模型簡(jiǎn)單，數(shù)據(jù)存儲(chǔ)量小，特別適用于計(jì)算機(jī)應(yīng)用，已被廣泛應(yīng)用于航天、航空、航海、系統(tǒng)工程、通信等多種領(lǐng)域狀態(tài)的估計(jì)和控制中。

1 問題描述

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

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)為系統(tǒng)在k 時(shí)刻狀態(tài); z(k)為k 時(shí)刻狀態(tài)變量的觀測(cè)量; u(k)=ΔD(k)

Δθ(k)為系統(tǒng)的控制輸入;ΔD和Δθ為k到k+1時(shí)刻機(jī)器人的相對(duì)位移和運(yùn)動(dòng)方向;w(k)和v(k)分別為高斯加性過程噪聲和測(cè)量噪聲，且具有如下的統(tǒng)計(jì)特性:

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)

機(jī)器人運(yùn)動(dòng)過程和觀測(cè)過程均受到隨機(jī)高斯白噪聲干擾，在此的目的是根據(jù)系統(tǒng)方程和觀測(cè)方程，并結(jié)合噪聲統(tǒng)計(jì)特性及給定的初始位置估計(jì)機(jī)器人的位置，機(jī)器人位置如圖1所示。

圖1 機(jī)器人位置示意圖

2 迭代擴(kuò)展卡爾曼濾波定位

2.1 擴(kuò)展卡爾曼濾波

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

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

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

(2) 時(shí)間更新。

當(dāng)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) 測(cè)量更新:

(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) 重復(fù)第(2)和第(3)步。

算法中采用矩陣A，B，H系統(tǒng)運(yùn)動(dòng)方程和測(cè)量方程對(duì)狀態(tài)和輸入求偏導(dǎo)數(shù)的雅克比矩陣。

2.2 迭代擴(kuò)展卡爾曼濾波定位

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

其中，迭代EKF[9]算法在原有EKF基礎(chǔ)上，對(duì)量測(cè)更新的狀態(tài)估計(jì)值采用多步迭代，從而減少由于泰勒展開帶來的截?cái)嗾`差。在此應(yīng)用IEKF方法實(shí)現(xiàn)移動(dòng)機(jī)器人運(yùn)行狀態(tài)的位置?；谏鲜鏊枷?，在任意時(shí)刻計(jì)算k迭代估計(jì)值的步驟如下[10]:

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

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

(3) 迭代過程中，當(dāng)兩步狀態(tài)差值滿足式(4):

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

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

3 仿真分析

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

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

式中:M為Monte-Carlo仿真的次數(shù);k為取樣點(diǎn)數(shù)。仿真的次數(shù)越多，實(shí)驗(yàn)的效果越接近于實(shí)際，但計(jì)算的速度明顯變慢，因此在仿真時(shí)，需要根據(jù)實(shí)際適當(dāng)選取。在本程序中，取M=50。

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

由仿真結(jié)果可看出，執(zhí)行IEKF算法相對(duì)于EKF算法效果更好。 這主要?dú)w功于狀態(tài)估計(jì)的多步迭代，在一定程度上修正了由于泰勒展開引起的截?cái)嗾`差，使得算法收斂的穩(wěn)定性得到增強(qiáng)。

圖2 IEKF機(jī)器人的原始軌跡、觀測(cè)數(shù)據(jù)和估計(jì)值軌跡

圖3 目標(biāo)跟蹤誤差曲線

4 結(jié) 語

在此，針對(duì)擴(kuò)展卡爾曼濾波算法精度較低的缺點(diǎn)，提出了對(duì)算法測(cè)量更新階段的狀態(tài)估計(jì)值，并采用多步迭代更新，使其在滿足一定閾值時(shí)，迭代結(jié)束。該算法一定程度上減小了局部線性化帶來的截?cái)嗾`差。仿真結(jié)果與EKF算法進(jìn)行了對(duì)比，結(jié)果表明該算法的濾波精度有明顯提高。

參考文獻(xiàn)

[1]王志文，郭戈.移動(dòng)機(jī)器人導(dǎo)航技術(shù)現(xiàn)狀與展望[J].機(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]郭杭.迭代擴(kuò)展卡爾曼濾波用于實(shí)時(shí)GPS數(shù)據(jù)處理[J].武漢測(cè)繪科技大學(xué)學(xué)報(bào)，1999，24(2):112-114，123.

[10]高為廣，潘娜娜，張曉東.改進(jìn)的迭代EKF算法在偽衛(wèi)星定位中的應(yīng)用[J].測(cè)繪科學(xué)，2008，33(4):73-75.