基于改進(jìn)MCKF的UWB/IMU室內(nèi)組合定位算法*

廖天睿， 吳向東， 趙林惠， 金曉明， 賈之陽(yáng)， 戴亞平

(1.北京聯(lián)合大學(xué) 機(jī)器人學(xué)院，北京 100101；2.北京理工大學(xué) 自動(dòng)化學(xué)院，北京 100081)

0 引 言

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

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

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

1 基于UWB與IMU的定位算法

1.1 基于飛行時(shí)間定位算法

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

(1)

1.2 慣導(dǎo)解算

將IMU載體靜置，參數(shù)初始化后根據(jù)測(cè)得的加速度、角速度與姿態(tài)矩陣即可計(jì)算出速度[7]，進(jìn)而計(jì)算出第k次采樣時(shí)IMU定位坐標(biāo)(xIMU(k),yIMU(k))。

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

2 MCKF

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

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

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

3)使用不動(dòng)點(diǎn)迭代算法，根據(jù)式(2)～式(8)計(jì)算出相關(guān)熵最大時(shí)的(k|k)t，不動(dòng)點(diǎn)迭代算法如下所示

(2)

(3)

(4)

(5)

(6)

(7)

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

(8)

4)當(dāng)?shù)趖次迭代后(k|k)t的變化小于誤差限ε，則停止迭代并輸出(k|k)t，否則重復(fù)步驟(3)。

5)更新誤差協(xié)方差矩陣

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，利用新息對(duì)觀測(cè)噪聲協(xié)方差矩陣進(jìn)行動(dòng)態(tài)修正，從而提升MCKF在噪聲類型不單一時(shí)的濾波精度。

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

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

(10)

(11)

(12)

但當(dāng)噪聲類型不單一時(shí)，單一估值的觀測(cè)噪聲協(xié)方差矩陣R(k)并不能使式(12)成立，故須校正R(k)，使其滿足式(12)。校正R(k)時(shí)，首先構(gòu)造中間變量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的濾波精度，保證其在噪聲類型不單一時(shí)的性能。

3.2 基于I-MCKF室內(nèi)組合定位算法

(16)

(17)

式中μ為過(guò)程噪聲，η為觀測(cè)噪聲。

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

建立狀態(tài)方程與觀測(cè)方程后，對(duì)噪聲協(xié)方差矩陣初始化，保證在使用最大熵原理進(jìn)行定點(diǎn)迭代時(shí)可以收斂，并加快定點(diǎn)迭代的收斂速度。噪聲協(xié)方差矩陣的值可利用經(jīng)驗(yàn)進(jìn)行估計(jì)，也可通過(guò)實(shí)驗(yàn)測(cè)量得出。

根據(jù)計(jì)算出的噪聲協(xié)方差矩陣構(gòu)造相關(guān)熵模型為

(18)

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

(19)

(20)

式中Y(k)為第k次采樣時(shí)的采樣值，chol(x)為Cholesky算子，表示對(duì)x進(jìn)行Cholesky分解。將式(16)～式(20)代入I-MKCF算法中，利用不動(dòng)點(diǎn)迭代算法計(jì)算出熵最大時(shí)標(biāo)簽坐標(biāo)的估計(jì)值(x(k),y(k))，即融合定位結(jié)果。

4 室內(nèi)動(dòng)態(tài)定位實(shí)驗(yàn)與結(jié)果分析

本文的實(shí)驗(yàn)場(chǎng)景設(shè)計(jì)如圖2所示，其中三個(gè)基站的坐標(biāo)分別為(0,0)，(4.45,7.40)，(8.90,0)，運(yùn)動(dòng)軌跡選擇繞室內(nèi)行進(jìn)一周。

圖2 實(shí)驗(yàn)場(chǎng)景示意

初始化IMU參數(shù)后，手持標(biāo)簽沿著上述路徑移動(dòng)，并將無(wú)KF組合定位算法、MCKF組合定位算法與I-MCKF組合定位算法的輸出進(jìn)行對(duì)比，結(jié)果如圖3、圖4以及表1所示。

圖3 三種組合定位算法定位結(jié)果

圖4 三種定位算法誤差對(duì)比結(jié)果

實(shí)驗(yàn)結(jié)果表明: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組合定位算法進(jìn)一步降低了62.9 %。

表1 三種定位方法在室內(nèi)環(huán)境下的誤差

5 結(jié) 論

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