卡爾曼濾波框架下基于最大相關(guān)熵的移動機(jī)器人位姿估計(jì)

李志鵬，程 蘭，王志飛，閻高偉

(太原理工大學(xué) 電氣與動力工程學(xué)院，太原 030024)

同時(shí)定位和建圖(simultaneous localization and mapping，SLAM)是機(jī)器人實(shí)現(xiàn)環(huán)境感知、確定自身位置的關(guān)鍵技術(shù)[1-3]。而機(jī)器人的位姿估計(jì)精度直接決定機(jī)器人的定位精度，進(jìn)而影響建圖精度。因此，提高位姿估計(jì)的精度一直是機(jī)器人領(lǐng)域研究者不斷追求的目標(biāo)。

基于卡爾曼濾波的狀態(tài)估計(jì)算法是實(shí)現(xiàn)機(jī)器人位姿估計(jì)最常用的技術(shù)手段。文獻(xiàn)[4]實(shí)現(xiàn)了基于擴(kuò)展卡爾曼濾波(extended kalman filter，EKF)的移動機(jī)器人位姿估計(jì)算法，仿真結(jié)果表明EKF有良好的估計(jì)性能。文獻(xiàn)[5]提出了迭代擴(kuò)展卡爾曼濾波(iterated extended kalman filter，IEKF)，利用最新的估計(jì)結(jié)果對非線性測量函數(shù)進(jìn)行線性化迭代，有效地提高了EKF的位姿估計(jì)精度。此外，文獻(xiàn)[6]利用麥夸爾特法推導(dǎo)了新的濾波器LMEKF，利用迭代更新的方式改進(jìn)了EKF的更新步驟，進(jìn)一步提高了IEKF的位姿估計(jì)精度。但以上濾波方法都要求非線性函數(shù)可微，進(jìn)行線性化時(shí)往往會產(chǎn)生一定的偏差，且需要計(jì)算雅可比矩陣。

為了避免線性化帶來的位姿估計(jì)偏差，研究者將UKF用于位姿估計(jì)，通過對一些Sigma點(diǎn)的無跡變換來估計(jì)位姿，得到了基于UKF的位姿估計(jì)算法[7]。然而，基于EKF和UKF的位姿估計(jì)算法都假設(shè)狀態(tài)噪聲和測量噪聲服從高斯分布，并不適用于非高斯噪聲。然而在機(jī)器人定位與建圖過程中，非高斯噪聲確是客觀存在的，如移動汽車的噪聲、由于電磁干擾以及通信系統(tǒng)故障和缺陷而導(dǎo)致的脈沖噪聲等。

針對EKF和UKF的局限性，研究者提出了適用于非高斯噪聲的位姿估計(jì)算法，如基于粒子濾波(particle filter，PF)[8]的位姿估計(jì)算法、基于高斯和濾波(gaussian sum filter，GSF)[9]的位姿估計(jì)算法等。這些算法利用大量的樣本來逼近位姿的后驗(yàn)概率密度函數(shù)，雖然在估計(jì)精度上得到了改善，但存在計(jì)算量大的問題。

近年來，研究者開始將相關(guān)熵用于處理非高斯噪聲下的位姿估計(jì)問題[10-11]。相關(guān)熵是對兩個(gè)隨機(jī)變量之間相似性的一種度量。文獻(xiàn)[12]介紹了最大相關(guān)熵(maximum correntropy，MC)在非高斯噪聲環(huán)境中的應(yīng)用，基于MC的位姿估計(jì)算法能夠明顯提高位姿估計(jì)精度。之后又有學(xué)者將MC應(yīng)用于卡爾曼濾波理論中，如相關(guān)卡爾曼濾波器(C-KF)[13]、最大相關(guān)熵卡爾曼濾波器(MCKF)[14-15]、最大相關(guān)熵?cái)U(kuò)展卡爾曼濾波器(MCEKF)[16-18]以及最大相關(guān)熵?zé)o跡卡爾曼濾波器(MCUKF)[19]等。MC在基于卡爾曼濾波器中的應(yīng)用證明了將MC和基于卡爾曼濾波算法相結(jié)合實(shí)現(xiàn)非高斯噪聲下的位姿估計(jì)的可行性。但現(xiàn)有的基于MC和卡爾曼濾波算法的移動機(jī)器人位姿估計(jì)算法只利用了位姿估計(jì)結(jié)果的先驗(yàn)信息，沒有充分利用最新的位姿估計(jì)結(jié)果去更新預(yù)測的測量結(jié)果。

本文針對上述問題，提出了在UKF框架下基于MC的移動機(jī)器人位姿估計(jì)算法。利用MC在處理非高斯噪聲中的優(yōu)勢，代替最小方差標(biāo)準(zhǔn)來構(gòu)造新的代價(jià)函數(shù)，通過麥夸爾特法對該函數(shù)的優(yōu)化實(shí)現(xiàn)在UKF框架下移動機(jī)器人的位姿估計(jì)。

1 問題描述

所采用的移動機(jī)器人模型如圖1所示[20]。該模型可以表示二維平面上的自主移動機(jī)器人的一般運(yùn)動模型。機(jī)器人的狀態(tài)根據(jù)運(yùn)動學(xué)模型定義為表示機(jī)器人的位置坐標(biāo)，表示轉(zhuǎn)向角，控制輸入量為舵角和速度。

圖1 移動機(jī)器人模型Fig.1 Model of mobile robot

1.1 移動機(jī)器人運(yùn)動模型

為研究方便，可將圖1中的移動機(jī)器人模型轉(zhuǎn)換為一長為L的連桿，前端兩側(cè)車輪的速度為v，如圖2所示。

圖2 移動機(jī)器人簡化模型Fig.2 Simplified model of mobile robot

根據(jù)圖2，對移動機(jī)器人的運(yùn)動學(xué)模型進(jìn)行建模，可得機(jī)器人運(yùn)動方程：

(1)

轉(zhuǎn)化為矩陣形式：

(2)

式中：ω(i)表示狀態(tài)噪聲。

1.2 移動機(jī)器人觀測模型

(3)

式中：(xk,yk)表示路標(biāo)點(diǎn)的位置坐標(biāo)，rk(i)表示觀測噪聲。

綜上，式(2)和(3)構(gòu)成系統(tǒng)的狀態(tài)方程和觀測方程。本文通過對狀態(tài)向量xv的估計(jì)來實(shí)現(xiàn)對移動機(jī)器人位姿的估計(jì)。

2 相關(guān)工作

本文算法是在MCUKF[19]的基礎(chǔ)上進(jìn)行改進(jìn)的，為了說明本文算法與MCUKF的不同，首先對相關(guān)熵和MCUKF進(jìn)行簡要介紹。

2.1 相關(guān)熵理論

由于MCUKF引入了相關(guān)熵，為確保內(nèi)容的完整性，本節(jié)首先對相關(guān)熵進(jìn)行介紹。相關(guān)熵是一種相似性度量方法，給定兩個(gè)隨機(jī)變量X和Y，則其相關(guān)熵為[13]：

(4)

式中：E[·]為期望算子，F(xiàn)X,Y(x,y)為X,Y的聯(lián)合分布，φ(x,y)表示Mercer核函數(shù)，如常用的高斯核函數(shù)Gδ(e)=exp(-e2/2δ2)，這里e=x-y.實(shí)際中很難直接計(jì)算相關(guān)熵，因此通常采用數(shù)據(jù)驅(qū)動的方式對其進(jìn)行估計(jì)。即假設(shè)采集到的關(guān)于x和y的樣本為{x(i),y(i)}，i=1,2,…,n，則X,Y的相關(guān)熵可通過下式進(jìn)行估計(jì)：

(5)

(6)

高斯相關(guān)熵包含了所有由核帶寬δ加權(quán)的X和Y的偶矩。選擇合適的δ，將高階矩納入到信號處理算法中，可以更加準(zhǔn)確地描述非高斯分布的高階特性，這是相關(guān)熵處理非高斯噪聲的主要優(yōu)點(diǎn)。

2.2 MCUKF算法

MCUKF考慮的狀態(tài)空間模型為：

(7)

式中：xi和zi分別為第i時(shí)刻的系統(tǒng)狀態(tài)和測量值，非線性函數(shù)fi(g)和hi(g)已知，過程噪聲ωi和測量噪聲vi不相關(guān)，其均值為零，協(xié)方差為Qi和Ri.初始狀態(tài)x0的均值和協(xié)方差P0為0.在MCUKF中，假設(shè)過程噪聲ωi服從高斯分布，而測量噪聲vi服從非高斯分布。

MCUKF的預(yù)測步驟與傳統(tǒng)UKF相同：

(8)

(9)

(10)

對于濾波更新步驟，MCUKF利用最小方差標(biāo)準(zhǔn)來處理高斯過程噪聲，利用MC來處理非高斯測量噪聲，其所采用的代價(jià)函數(shù)為[19]：

(11)

(12)

(13)

測量矩陣Hi表示為：

(14)

式中：

(15)

則更新的測量噪聲矩陣Ri表示為：

(16)

式中，

(17)

(18)

式中，

(19)

(20)

協(xié)方差估計(jì)結(jié)果為：

(21)

MCUKF引入權(quán)重Li來調(diào)優(yōu)增益Ki，以避免非高斯噪聲對測量值的干擾。但MCUKF只考慮了過程噪聲ωi為高斯分布的情況，且在濾波步中只是根據(jù)狀態(tài)的預(yù)測結(jié)果來更新預(yù)測的測量值，導(dǎo)致最終的估計(jì)結(jié)果精度不高。針對此問題，本文借鑒IEKF中迭代估計(jì)的思路，提出了迭代的MCUKF，即MCIUKF，并利用麥夸爾特法推導(dǎo)了濾波更新方程。

3 位姿估計(jì)算法(MCIUKF)

基于上一節(jié)的描述，MCUKF存在的問題有：1) 過程噪聲沒有考慮非高斯分布的情況，且沒有充分利用最新的估計(jì)結(jié)果；2) 文獻(xiàn)[19]對MCUKF的研究只進(jìn)行了數(shù)值仿真，并沒有用于解決實(shí)際問題。

針對這些問題，本文提出MCIUKF算法，利用MC來處理非高斯過程噪聲和非高斯測量噪聲，用MC代替最小方差標(biāo)準(zhǔn)設(shè)計(jì)新的代價(jià)函數(shù)，并在測量矩陣中納入最新的位姿估計(jì)結(jié)果，以提高在非高斯噪聲環(huán)境下的估計(jì)性能。最后將該算法應(yīng)用于移動機(jī)器人的位姿估計(jì)。

(22)

然后通過求解以下優(yōu)化問題進(jìn)行濾波更新：

(23)

由于上述優(yōu)化很難得到解析解，考慮一種基于梯度的迭代方法來求解式(23)，即麥夸爾特法。在迭代濾波算法中，高斯牛頓法被廣泛應(yīng)用于解決非線性優(yōu)化問題，而麥夸爾特法可以進(jìn)一步提高高斯牛頓法的收斂性，獲得更好的濾波性能[5-6]。

利用麥夸爾特法迭代求解如下：

(24)

(25)

(26)

(27)

(28)

式中，

(29)

(30)

Φ(xv(i))表示為：

(31)

將式(25)和式(31)代入式(24)可得狀態(tài)估計(jì)：

(32)

式中，

(33)

(34)

(35)

(36)

協(xié)方差估計(jì)為：

(37)

4 算法實(shí)現(xiàn)

本文當(dāng)前研究假設(shè)機(jī)器人總是可以觀測到足夠數(shù)量的路標(biāo)點(diǎn)以實(shí)現(xiàn)位姿估計(jì)。本節(jié)將所提出的算法用于移動機(jī)器人的位姿估計(jì)，而實(shí)現(xiàn)機(jī)器人位姿估計(jì)的前提是數(shù)據(jù)關(guān)聯(lián)。下面將主要介紹數(shù)據(jù)關(guān)聯(lián)、狀態(tài)向量增廣及算法的偽代碼，為位姿估計(jì)算法的實(shí)現(xiàn)做準(zhǔn)備。

4.1 數(shù)據(jù)關(guān)聯(lián)

在SLAM的濾波更新階段，需要對觀測到的路標(biāo)和狀態(tài)向量中的路標(biāo)進(jìn)行匹配，這一過程被稱為數(shù)據(jù)關(guān)聯(lián)。在路標(biāo)數(shù)目不多、存儲資源富裕的情況下，可以直接維護(hù)一個(gè)表達(dá)所有路標(biāo)在狀態(tài)向量中位置的列表，table[id]代表編號為id的路標(biāo)在狀態(tài)向量中的位置，table[id] = 0代表該路標(biāo)未在狀態(tài)向量中出現(xiàn)。當(dāng)機(jī)器人觀測到新路標(biāo)時(shí)，需要將新路標(biāo)添加到待估向量中，即需要進(jìn)行狀態(tài)向量增廣。

4.2 狀態(tài)向量增廣

狀態(tài)向量增廣是將新觀測到的路標(biāo)信息添加到狀態(tài)向量及協(xié)方差矩陣中，同時(shí)改變狀態(tài)向量及協(xié)方差矩陣的維度。若一個(gè)新觀測到的路標(biāo)點(diǎn)為z，其對應(yīng)的全局坐標(biāo)為m=(mx,my)T，此時(shí)機(jī)器人位姿為xv=[x,y,φ]T，則根據(jù)觀測模型可得z到m的轉(zhuǎn)換為：

(38)

將m添加到狀態(tài)向量中：

(39)

4.3 程序偽代碼

本文整體的算法流程如下。

其中，步驟3，4表示濾波預(yù)測過程，步驟5表示數(shù)據(jù)關(guān)聯(lián)，步驟7-16表示濾波更新過程，步驟18表示狀態(tài)向量增廣。

5 仿真分析

5.1 仿真環(huán)境設(shè)置

為了分析算法的性能，用Matlab模擬了移動機(jī)器人采用激光雷達(dá)觀測數(shù)據(jù)并進(jìn)行位姿估計(jì)的仿真環(huán)境。機(jī)器人的的仿真參數(shù)設(shè)置為：輪距L=4 m，速度vvad=8 m/s，最大舵角xmax=30°，最大轉(zhuǎn)向速度θ=20°/s，最大觀測距離d=30 m，控制周期為0.025 s，調(diào)整權(quán)重δ=8，阻尼參數(shù)μ=0.001.為更接近真實(shí)的應(yīng)用場景，令傳感器的觀測范圍僅為車前半徑為d的半圓內(nèi)的路標(biāo)點(diǎn)，并對速度、最大舵角及轉(zhuǎn)向速度進(jìn)行限制，即v=vrat，|γ|

所考慮的非高斯噪聲分別為脈沖噪聲和有色噪聲，本節(jié)針對這兩種噪聲分析所提出算法的性能。利用混合高斯噪聲分布來表示脈沖噪聲，其分布函數(shù)為：

(40)

考慮的有色噪聲分布函數(shù)為[21]：

fc(k)=fw(k)+c1gfw(k-1)-c2gfw(k-2) .

(41)

移動機(jī)器人運(yùn)行的實(shí)際軌跡和所觀測的路標(biāo)點(diǎn)如圖3所示。其中藍(lán)色星號代表路標(biāo)點(diǎn)，紅色圓點(diǎn)代表路徑上的關(guān)鍵點(diǎn)，外圍有藍(lán)色圓圈的為機(jī)器人的下一目標(biāo)點(diǎn)，黑色虛線代表機(jī)器人的真實(shí)軌跡，黑色圓代表機(jī)器人，藍(lán)色圓點(diǎn)為當(dāng)前機(jī)器人位置，藍(lán)色圓點(diǎn)與黑色圓邊上點(diǎn)的連線代表機(jī)器人目前航向，有與藍(lán)色點(diǎn)連線的路標(biāo)代表其被機(jī)器人傳感器觀測到，紅色圓圈代表傳感器的觀測半徑，傳感器僅觀測機(jī)器人正前方180°范圍內(nèi)的路標(biāo)點(diǎn)。

圖3 機(jī)器人運(yùn)行軌跡Fig.3 Robot trajectory

5.2 仿真結(jié)果分析

5.2.1誤差與穩(wěn)定性分析

本文采用蒙特卡羅仿真，連續(xù)運(yùn)行50次程序，并利用均方根誤差(RMSE)來衡量算法的性能。同時(shí)將仿真結(jié)果與EKF[4]，UKF[7]，MCUKF[19]進(jìn)行了對比分析。

第i次迭代的均方根誤差表示為：

(42)

第i次迭代的狀態(tài)估計(jì)誤差表示為：

(43)

(44)

(45)

圖4 脈沖噪聲下4種算法的估計(jì)軌跡Fig.4 Estimated trajectories of four algorithms under impulse noise

圖5 脈沖噪聲下4種算法位姿估計(jì)結(jié)果的均方根誤差Fig.5 RMSEs of pose estimation results of four algorithms under impulse noise

圖6 脈沖噪聲下4種算法的狀態(tài)估計(jì)誤差Fig.6 State estimation errors of four algorithms under impulse noise

圖7 有色噪聲下4種算法的估計(jì)軌跡Fig.7 Estimated trajectories of four algorithms under colored noise

在不同噪聲環(huán)境下各算法的平均誤差對比結(jié)果如表1、表2所示。

由圖4-圖6以及表1可以直觀地看出，在脈沖噪聲環(huán)境下，本文算法在精度上明顯優(yōu)于其它算法，并且與MCUKF相比，本文算法的均方根誤差平均降低了0.112 7 m；由圖7-圖9以及表2可以看出，在有色噪聲環(huán)境下，本文算法相對于其它算法有更好的位姿估計(jì)精度，并且與MCUKF相比，本文算法的均方根誤差平均降低了0.279 4 m.同時(shí)本文算法具有較好的數(shù)值穩(wěn)定性，這主要是由于采用了基于MC的代價(jià)函數(shù)，并在測量矩陣中納入了最新估計(jì)，提高了位姿估計(jì)精度。而EKF，UKF只是在高斯噪聲下有較好的估計(jì)性能；MCUKF則是由于更新過程中測量矩陣?yán)玫氖窍闰?yàn)估計(jì)信息，導(dǎo)致其估計(jì)精度降低。仿真結(jié)果證明了將MCIUKF用于機(jī)器人位姿估計(jì)的有效性。

圖8 有色噪聲下4種算法位姿估計(jì)結(jié)果的均方根誤差Fig.8 RMSEs of pose estimation results of four algorithms under colored noise

圖9 有色噪聲下4種算法的狀態(tài)估計(jì)誤差Fig.9 State estimation errors of four algorithms under colored noise

表1 脈沖噪聲下4種算法的平均誤差對比Table 1 Average error comparison of four algorithms under impulse noise

表2 有色噪聲下4種算法的平均誤差對比Table 2 Average error comparison of four algorithms under colored noise

5.2.2時(shí)間復(fù)雜度分析

本文使用的計(jì)算機(jī)系統(tǒng)型號為戴爾Inspiron 3576，運(yùn)行內(nèi)存為8 G，顯示內(nèi)存為128 MB，各算法單次迭代時(shí)間復(fù)雜度對比如表3所示。

表3 時(shí)間復(fù)雜度對比Table 3 Time complexity comparison

由表3可知，本文算法在時(shí)間復(fù)雜度方面稍遜于其它算法，這是由于算法引入了最大相關(guān)熵，對熵的計(jì)算導(dǎo)致了計(jì)算復(fù)雜度的增加；另外，在算法的更新環(huán)節(jié)利用麥夸爾特法進(jìn)行迭代求解也增加了時(shí)間復(fù)雜度，進(jìn)而使算法的實(shí)時(shí)性相對較低。

6 結(jié)束語

本文提出了一種適用于非高斯噪聲環(huán)境的基于MC和IUKF相結(jié)合的位姿估計(jì)算法，提高了算法在非高斯噪聲下的位姿估計(jì)精度。本文的主要貢獻(xiàn)：1) 在濾波更新步驟定義了一個(gè)基于MC的代價(jià)函數(shù)，并利用麥夸爾特法推導(dǎo)出對應(yīng)的濾波更新方程，得到了MCIUKF濾波器；2) 將所提出的MCIUKF算法應(yīng)用于移動機(jī)器人的位姿估計(jì)。在非高斯噪聲環(huán)境下，與對比算法相比，本文算法能夠明顯提高位姿估計(jì)的精度。但所提出的算法由于采用了最大相關(guān)熵，導(dǎo)致其實(shí)時(shí)性相對較低，這也是未來工作的研究方向。