卡爾曼濾波框架下基于最大相關熵的移動機器人位姿估計

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

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

同時定位和建圖(simultaneous localization and mapping，SLAM)是機器人實現環境感知、確定自身位置的關鍵技術[1-3]。而機器人的位姿估計精度直接決定機器人的定位精度，進而影響建圖精度。因此，提高位姿估計的精度一直是機器人領域研究者不斷追求的目標。

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

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

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

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

本文針對上述問題，提出了在UKF框架下基于MC的移動機器人位姿估計算法。利用MC在處理非高斯噪聲中的優勢，代替最小方差標準來構造新的代價函數，通過麥夸爾特法對該函數的優化實現在UKF框架下移動機器人的位姿估計。

1 問題描述

所采用的移動機器人模型如圖1所示[20]。該模型可以表示二維平面上的自主移動機器人的一般運動模型。機器人的狀態根據運動學模型定義為表示機器人的位置坐標，表示轉向角，控制輸入量為舵角和速度。

圖1 移動機器人模型Fig.1 Model of mobile robot

1.1 移動機器人運動模型

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

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

根據圖2，對移動機器人的運動學模型進行建模，可得機器人運動方程：

(1)

轉化為矩陣形式：

(2)

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

1.2 移動機器人觀測模型

(3)

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

綜上，式(2)和(3)構成系統的狀態方程和觀測方程。本文通過對狀態向量xv的估計來實現對移動機器人位姿的估計。

2 相關工作

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

2.1 相關熵理論

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

(4)

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

(5)

(6)

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

2.2 MCUKF算法

MCUKF考慮的狀態空間模型為：

(7)

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

MCUKF的預測步驟與傳統UKF相同：

(8)

(9)

(10)

對于濾波更新步驟，MCUKF利用最小方差標準來處理高斯過程噪聲，利用MC來處理非高斯測量噪聲，其所采用的代價函數為[19]：

(11)

(12)

(13)

測量矩陣Hi表示為：

(14)

式中：

(15)

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

(16)

式中，

(17)

(18)

式中，

(19)

(20)

協方差估計結果為：

(21)

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

3 位姿估計算法(MCIUKF)

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

針對這些問題，本文提出MCIUKF算法，利用MC來處理非高斯過程噪聲和非高斯測量噪聲，用MC代替最小方差標準設計新的代價函數，并在測量矩陣中納入最新的位姿估計結果，以提高在非高斯噪聲環境下的估計性能。最后將該算法應用于移動機器人的位姿估計。

(22)

然后通過求解以下優化問題進行濾波更新：

(23)

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

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

(24)

(25)

(26)

(27)

(28)

式中，

(29)

(30)

Φ(xv(i))表示為：

(31)

將式(25)和式(31)代入式(24)可得狀態估計：

(32)

式中，

(33)

(34)

(35)

(36)

協方差估計為：

(37)

4 算法實現

本文當前研究假設機器人總是可以觀測到足夠數量的路標點以實現位姿估計。本節將所提出的算法用于移動機器人的位姿估計，而實現機器人位姿估計的前提是數據關聯。下面將主要介紹數據關聯、狀態向量增廣及算法的偽代碼，為位姿估計算法的實現做準備。

4.1 數據關聯

在SLAM的濾波更新階段，需要對觀測到的路標和狀態向量中的路標進行匹配，這一過程被稱為數據關聯。在路標數目不多、存儲資源富裕的情況下，可以直接維護一個表達所有路標在狀態向量中位置的列表，table[id]代表編號為id的路標在狀態向量中的位置，table[id] = 0代表該路標未在狀態向量中出現。當機器人觀測到新路標時，需要將新路標添加到待估向量中，即需要進行狀態向量增廣。

4.2 狀態向量增廣

狀態向量增廣是將新觀測到的路標信息添加到狀態向量及協方差矩陣中，同時改變狀態向量及協方差矩陣的維度。若一個新觀測到的路標點為z，其對應的全局坐標為m=(mx,my)T，此時機器人位姿為xv=[x,y,φ]T，則根據觀測模型可得z到m的轉換為：

(38)

將m添加到狀態向量中：

(39)

4.3 程序偽代碼

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

其中，步驟3，4表示濾波預測過程，步驟5表示數據關聯，步驟7-16表示濾波更新過程，步驟18表示狀態向量增廣。

5 仿真分析

5.1 仿真環境設置

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

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

(40)

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

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

(41)

移動機器人運行的實際軌跡和所觀測的路標點如圖3所示。其中藍色星號代表路標點，紅色圓點代表路徑上的關鍵點，外圍有藍色圓圈的為機器人的下一目標點，黑色虛線代表機器人的真實軌跡，黑色圓代表機器人，藍色圓點為當前機器人位置，藍色圓點與黑色圓邊上點的連線代表機器人目前航向，有與藍色點連線的路標代表其被機器人傳感器觀測到，紅色圓圈代表傳感器的觀測半徑，傳感器僅觀測機器人正前方180°范圍內的路標點。

圖3 機器人運行軌跡Fig.3 Robot trajectory

5.2 仿真結果分析

5.2.1誤差與穩定性分析

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

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

(42)

第i次迭代的狀態估計誤差表示為：

(43)

(44)

(45)

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

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

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

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

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

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

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

圖9 有色噪聲下4種算法的狀態估計誤差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時間復雜度分析

本文使用的計算機系統型號為戴爾Inspiron 3576，運行內存為8 G，顯示內存為128 MB，各算法單次迭代時間復雜度對比如表3所示。

表3 時間復雜度對比Table 3 Time complexity comparison

由表3可知，本文算法在時間復雜度方面稍遜于其它算法，這是由于算法引入了最大相關熵，對熵的計算導致了計算復雜度的增加；另外，在算法的更新環節利用麥夸爾特法進行迭代求解也增加了時間復雜度，進而使算法的實時性相對較低。

6 結束語

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