基于改進MCKF的UWB/IMU室內組合定位算法*

廖天睿， 吳向東， 趙林惠， 金曉明， 賈之陽， 戴亞平

(1.北京聯合大學 機器人學院，北京 100101；2.北京理工大學 自動化學院，北京 100081)

0 引 言

隨著移動終端與移動互聯網業務在國內的普及，越來越多的個性化需求應運而生，其中室內定位的需求越來越強烈[1,2]。為實現高精度室內定位，Xu Y等人[3]利用卡爾曼濾波(Kalman filtering,KF)結合了超帶寬(ultra wideband,UWB)與慣性測量單元(inertial measurement unit,IMU)，實現了室內環境下的高精度定位。Xu Y等人[4]進一步采用聯邦卡爾曼濾波進行融合定位，使得定位系統精度進一步提升。Shi Q等人[5]在定位過程中引入了回環檢測，抑制了組合定位系統靜止時的漂移問題。

目前,基于KF的組合定位算法已具有較高的精度，但在復雜室內環境下，UWB傳感器易受復雜室內環境的干擾且IMU存在誤差累積，導致觀測量上存在較大的非高斯噪聲[6～9]。采用KF作為框架的融合定位算法難以處理觀測量上的非高斯噪聲，導致定位精度嚴重下降[10]。近年來，Chen B等人[7]提出了最大相關熵卡爾曼濾波(maximum correntropy Kalman filtering,MCKF)算法，可有效處理觀測量上的非高斯噪聲。但MCKF在噪聲類型不單一時，濾波精度以及魯棒性會大幅下降，難以應用于噪聲類型多變的室內定位系統。

本文針對MCKF存在的上述問題進行了改進，提出了改進最大相關熵卡爾曼濾波(improved MCKF,I-MCKF),利用新息對MCKF的觀測噪聲協方差矩陣進行先驗修正，提高了MCKF在噪聲類型不單一時的濾波精度及魯棒性。將I-MKCF應用于室內組合定位系統中，提出了基于I-MCKF的UWB/IMU室內組合定位算法。并在實際室內環境下，進行動態實驗，以驗證基于I-MCKF的UWB與IMU的組合定位算法的精度與可行性。

1 基于UWB與IMU的定位算法

1.1 基于飛行時間定位算法

UWB定位坐標利用基于飛行時間(time of flight,TOF)定位算法進行解算。TOF定位算法利用雙面雙向測距法測量標簽到各個基站的距離[11],使用三角定位法計算出標簽的坐標[6]。r1,r2,r3為測量出的標簽與基站的距離，基站坐標已知，其橫坐標分別為xA,xB,xC，縱坐標分別為yA,yB,yC，則標簽xD,yD的坐標可通過解方程組(1)得到，將第k次采樣獲得的標簽坐標記為(xUWB(k),yUWB(k))

(1)

1.2 慣導解算

將IMU載體靜置，參數初始化后根據測得的加速度、角速度與姿態矩陣即可計算出速度[7]，進而計算出第k次采樣時IMU定位坐標(xIMU(k),yIMU(k))。

雖然利用上述定位方法獲得的(xUWB(k),yUWB(k))與(xIMU(k),yIMU(k))通過卡爾曼濾波即可得到傳感器融合定位后的坐標。但在實際應用中，由于UWB易受復雜室內環境的影響以及IMU受誤差累積等影響，卡爾曼濾波難以對噪聲準確建模，使得系統的定位精度大幅下降。采用MCKF算法對測量出的坐標進行濾波處理可一定程度降低上述噪聲對定位精度的影響，提升定位精度。

2 MCKF

MCKF將最大熵準則與卡爾曼濾波結合，具有更高的魯棒性，可較好地處理觀測量上的非高斯噪聲[12]。MCKF在傳感器的每一次采樣后利用不動點迭代至最大熵點，其具體計算過程如下所示：

1)確定最大熵的核函數Gσ以及核函數帶寬σ，通常核函數選為高斯核函數。同時設定初始狀態估計值(0|0)以及初始誤差協方差矩陣P(0|0)，取采樣次數k=1。

2)更新估計狀態以及誤差協方差矩陣，并對先驗誤差協方差矩陣P(k|k-1)與過程噪聲協方差矩陣R(k)進行Cholesky分解得到Bp(k|k-1)與Br(k)，取迭代次數t=1，并且取(k|k)0=(k|k-1)。根據系統模型，構造相關熵模型的先驗值D(k)、后驗值W(k)以及D(k)與W(k)(k|k)的殘差E(k)，需計算D(k)與W(k)(k|k)相關熵最大時的(k|k)t。

3)使用不動點迭代算法，根據式(2)～式(8)計算出相關熵最大時的(k|k)t，不動點迭代算法如下所示

(2)

(3)

(4)

(5)

(6)

(7)

H(k)(k|k-1)]

(8)

4)當第t次迭代后(k|k)t的變化小于誤差限ε，則停止迭代并輸出(k|k)t，否則重復步驟(3)。

5)更新誤差協方差矩陣

P(k)=[I-(k)H(k)]P(k|k-1)[I-(k)H(k)]T

(9)

3 基于I-MCKF的組合定位算法

3.1 I-MCKF

本文提出了I-MCKF，利用新息對觀測噪聲協方差矩陣進行動態修正，從而提升MCKF在噪聲類型不單一時的濾波精度。

I-MCKF在MCKF的步驟(2)后加入觀測噪聲協方差矩陣修正環節。首先計算新息z(k)及其協方差估計值(k)，通常取遺忘因子λ=0.95，則

z(k)=y(k)-H(k)X(k|k-1)

(10)

(11)

(12)

但當噪聲類型不單一時，單一估值的觀測噪聲協方差矩陣R(k)并不能使式(12)成立，故須校正R(k)，使其滿足式(12)。校正R(k)時，首先構造中間變量N(k)

N(k)=(k)-H(k)P(k|k-1)HT(k)

(13)

(14)

β1|kβ2|kr21β2|kr22…β2|kβn|kr2n

???

β1|kβn|krn1β2|kβn|krn2…βn|krnm)

(15)

式中nij與rij分別為矩陣N(k)與(k)的第i行第j列元素。將式(10)～式(15)插入式(3)、式(4)之間，即可代到I-MCKF。I-MCKF將修正后的(k)用于MCKF的剩余步驟，即可提升MCKF的濾波精度，保證其在噪聲類型不單一時的性能。

3.2 基于I-MCKF室內組合定位算法

(16)

(17)

式中μ為過程噪聲，η為觀測噪聲。

圖1 基于I-MCKF的組合定位算法

建立狀態方程與觀測方程后，對噪聲協方差矩陣初始化，保證在使用最大熵原理進行定點迭代時可以收斂，并加快定點迭代的收斂速度。噪聲協方差矩陣的值可利用經驗進行估計，也可通過實驗測量得出。

根據計算出的噪聲協方差矩陣構造相關熵模型為

(18)

D(k)=B(k)-1[(k|k-1)Y(k)]

(19)

(20)

式中Y(k)為第k次采樣時的采樣值，chol(x)為Cholesky算子，表示對x進行Cholesky分解。將式(16)～式(20)代入I-MKCF算法中，利用不動點迭代算法計算出熵最大時標簽坐標的估計值(x(k),y(k))，即融合定位結果。

4 室內動態定位實驗與結果分析

本文的實驗場景設計如圖2所示，其中三個基站的坐標分別為(0,0)，(4.45,7.40)，(8.90,0)，運動軌跡選擇繞室內行進一周。

圖2 實驗場景示意

初始化IMU參數后，手持標簽沿著上述路徑移動，并將無KF組合定位算法、MCKF組合定位算法與I-MCKF組合定位算法的輸出進行對比，結果如圖3、圖4以及表1所示。

圖3 三種組合定位算法定位結果

圖4 三種定位算法誤差對比結果

實驗結果表明:KF組合定位算法的均方根誤差(root mean square error,RMSE)最大，為0.091 m，最大誤差為0.433 m；MCKF組合定位算法的RMSE為0.062 m，相較于KF組合定位算法降低了31.9 %；I-MCKF組合定位算法的RMSE為0.023 m，相較于MCKF組合定位算法進一步降低了62.9 %。

表1 三種定位方法在室內環境下的誤差

5 結 論

實驗結果表明：KF組合定位算法受復雜室內環境的干擾以及誤差累積的影響較大，故定位精度較低。MCKF雖然可以一定程度上抑制復雜室內環境的干擾以及誤差累積的影響，但由于室內環境中觀測噪聲多變，使得MCKF的濾波精度降低。I-MCKF通過對MCKF的觀測噪聲協方差矩陣進行實時修正，顯著提升了MCKF在噪聲類型不單一時的性能，故I-MCKF組合定位算法具有最高的定位精度，可實現高精度室內定位。此外，I-MCKF組合定位算法的更新頻率可達100 Hz，滿足室內定位系統實時定位需求。