基于UWB的加權(quán)自適應(yīng)卡爾曼濾波室內(nèi)定位算法*

郝占軍， 安 瑩， 陳紅紅， 黨小超， 張金龍

(1.西北師范大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院，甘肅 蘭州 730070； 2.甘肅省物聯(lián)網(wǎng)工程研究中心，甘肅 蘭州 730070)

0 引 言

傳感網(wǎng)、物聯(lián)網(wǎng)發(fā)展至今[1]，位置服務(wù)已然成為學(xué)術(shù)界所關(guān)注的焦點(diǎn)之一。相比于室外定位，室內(nèi)環(huán)境中的多徑效應(yīng)與周邊其他干擾因素使得室內(nèi)精確定位面臨更多的難題。國(guó)內(nèi)外研究人員在室內(nèi)定位中引入了一系列新興的室內(nèi)定位技術(shù)[2]：射頻識(shí)別、WiFi[3]、超寬帶(ultra-wide band，UWB)、藍(lán)牙[4]、ZigBee[5]等。

UWB逐漸成為室內(nèi)和結(jié)構(gòu)化環(huán)境中高精度定位和跟蹤的首選技術(shù)。文獻(xiàn)[6]提出了一種基于UWB的貝葉斯濾波室內(nèi)定位系統(tǒng)，采用線性回歸模型校準(zhǔn)距離測(cè)量誤差，利用固定UWB基站與移動(dòng)UWB基站的空間相關(guān)性來提高定位精度。文獻(xiàn)[7]在實(shí)際煤礦環(huán)境下，結(jié)合慣性測(cè)量單元和UWB技術(shù)，通過卡爾曼濾波器融合后獲得了較高的室內(nèi)定位精度。由于傳統(tǒng)的到達(dá)時(shí)間差(time difference of arrival，TDOA)定位技術(shù)通常需要精確的時(shí)間同步，部署難度較大。文獻(xiàn)[8]通過引入LoRa技術(shù)來提高錨節(jié)點(diǎn)布局的靈活性，解決了TDOA錨節(jié)點(diǎn)之間時(shí)間同步的問題?？柭鼮V波(Kalman filtering，KF)能夠較好地適應(yīng)于動(dòng)態(tài)跟蹤定位[9]。文獻(xiàn)[10]運(yùn)用EKF對(duì)UWB室內(nèi)定位算法進(jìn)行了改進(jìn)，使復(fù)雜環(huán)境對(duì)測(cè)距的影響得到了有效地降低。

傳統(tǒng)的KF在復(fù)雜室內(nèi)環(huán)境下的誤差抑制效果較差，所以本文采用誤差補(bǔ)償來減小測(cè)距過程中環(huán)境、行人等引入的噪聲，并將KF中的過程噪聲協(xié)方差和測(cè)量噪聲協(xié)方差遞歸更新，利用遞推估計(jì)進(jìn)行加權(quán)自適應(yīng)卡爾曼濾波(WKF)，實(shí)現(xiàn)了動(dòng)態(tài)的跟蹤定位。

1 WKF-TDOA算法設(shè)計(jì)

在UWB定位系統(tǒng)中，基站節(jié)點(diǎn)數(shù)目與定位精度成正相關(guān)。然而，出于對(duì)硬件成本等因素的考慮，基站的數(shù)量必須是有限的。本文采用四基站一標(biāo)簽的定位系統(tǒng)，針對(duì)KF定位中過程噪聲協(xié)方差和測(cè)量噪聲協(xié)方差的不確定性，提出了WKF-TDOA算法，算法流程如圖1所示。

圖1 WKF-TDOA算法流程

1.1 TDOA的獲取

Step1 目標(biāo)節(jié)點(diǎn)發(fā)送通知的時(shí)刻分別為R={R1，R2，R3，R4}，目標(biāo)節(jié)點(diǎn)到各個(gè)基站的發(fā)送時(shí)延為τ={τ1,τ2,τ3,τ4}，而各個(gè)基站到目標(biāo)節(jié)點(diǎn)的反饋時(shí)延為σ={σ1,σ2,σ3,σ4}，推出各個(gè)基站接收到信號(hào)的時(shí)刻T={T1,T2,T3,T4}

Ti=Ri-τi-σi,i∈{1,2,3,4}

(1)

Step2T0為待測(cè)標(biāo)簽同時(shí)向UWB基站發(fā)送信號(hào)的時(shí)刻，TOA可由各基站收到信號(hào)的時(shí)刻計(jì)算得出

ti=Ti-T0,i∈{1,2,3,4}

(2)

Step3 根據(jù)不同基站的TOA計(jì)算兩個(gè)基站間的TDOA

Tij=ti-tj,i∈{1,2,3,4},j∈{1,2,3,4}

(3)

1.2 誤差補(bǔ)償

將小網(wǎng)格上的4個(gè)頂點(diǎn)作為參考點(diǎn)，用UWB節(jié)點(diǎn)測(cè)量4個(gè)參考點(diǎn)的位置坐標(biāo)，在經(jīng)過無線時(shí)鐘同步后，記為Dw(Xi,k,Yi,k)，參考點(diǎn)的精確位置記為Dc，可計(jì)算出矢量修正值Dx

Dx=Dc-Dw

(4)

則4個(gè)參考節(jié)點(diǎn)的矢量修正值為Fi,i∈{1,2,3,4}，最后WKF-TDOA通過UWB節(jié)點(diǎn)測(cè)量目標(biāo)節(jié)點(diǎn)的位置D，并使用修正值F對(duì)D進(jìn)行修正，得到修正后的位置坐標(biāo)DA

(5)

1.3 WKF

WKF-TDOA運(yùn)用卡爾曼濾波器對(duì)過程和測(cè)量噪聲協(xié)方差的不確定性具有自適應(yīng)性這一屬性，通過遞歸更新噪聲協(xié)方差動(dòng)態(tài)調(diào)整過程噪聲協(xié)方差Qi,k和測(cè)量噪聲協(xié)方差Ri,k的值。

系統(tǒng)的狀態(tài)和測(cè)量方程為

xk=Fxk-1+wk-1

(6)

zk=Hxk+vk

(7)

式中xk和xk-1為時(shí)間為k和k-1時(shí)的系統(tǒng)狀態(tài)，F(xiàn)和H為狀態(tài)轉(zhuǎn)移方程，zk為時(shí)間為k時(shí)的測(cè)量值，wk-1為過程噪聲，vk為測(cè)量噪聲，則其協(xié)方差分別為Qk和Rk。

濾波增益矩陣為

(8)

狀態(tài)估計(jì)

(9)

預(yù)測(cè)的狀態(tài)誤差協(xié)方差矩陣為

(10)

其中,Q為過程協(xié)方差，假設(shè)Q中存在一個(gè)不確定的值稱為ΔQ，則式(10)可寫為

(11)

(12)

狀態(tài)估計(jì)的測(cè)量值記為NQ，平均值和樣本協(xié)方差分別為

(13)

(14)

結(jié)合式(11)和式(14)，通過數(shù)學(xué)計(jì)算可得

(15)

因?yàn)樯鲜鲇绊懸蜃用看味紩?huì)更新，故其上一時(shí)刻的協(xié)方差與目前位置的協(xié)方差就會(huì)具有一定的相關(guān)性，為獲得其中的關(guān)聯(lián)性，第一組的樣本協(xié)方差可近似表示為

(16)

則式(15)可擴(kuò)展為

(17)

其中

(18)

(19)

(20)

則

(21)

使用相同的近似值進(jìn)行計(jì)算，結(jié)合式(8),式(9)得

(22)

在經(jīng)過誤差補(bǔ)償與WKF之后，可以進(jìn)一步提高定位精度，解算出最佳的標(biāo)簽位置。

2 實(shí)驗(yàn)測(cè)試與分析

2.1 實(shí)驗(yàn)環(huán)境

采用商用的I—UWB LPS定位系統(tǒng)，輸出頻率50 Hz，最大定位距離可達(dá)35 m，標(biāo)簽采用手持移動(dòng)的方式，實(shí)驗(yàn)環(huán)境大小設(shè)置為3 m×3 m的室內(nèi)空間，采用4個(gè)基站1個(gè)標(biāo)簽的定位模式，將4個(gè)基站以主從式基站組網(wǎng)，0#基站為主基站，其余3個(gè)基站為副基站，設(shè)備布設(shè)高度為1 m。實(shí)驗(yàn)分別設(shè)置在實(shí)驗(yàn)室、會(huì)議室及空曠的大廳分別進(jìn)行測(cè)試，實(shí)驗(yàn)環(huán)境場(chǎng)景如圖2所示。

圖2 實(shí)驗(yàn)環(huán)境場(chǎng)景

2.2 不同狀態(tài)比較

2.2.1 靜態(tài)下的目標(biāo)檢測(cè)

在實(shí)驗(yàn)室、會(huì)議室及空曠大廳環(huán)境下，劃分3 m×3 m的室內(nèi)空曠區(qū)域，分別選取測(cè)試點(diǎn)C1、C2、C3進(jìn)行測(cè)試，準(zhǔn)確坐標(biāo)為(0,2,1),收集測(cè)試點(diǎn)的位置信息，經(jīng)誤差補(bǔ)償后各點(diǎn)在X，Y方向分量的偏差如圖3～圖5所示。橫軸為采樣個(gè)數(shù)，采樣時(shí)間為10 s；縱軸為誤差，單位為m。

圖3 C1點(diǎn)在X,Y軸上的偏差

圖4 C2點(diǎn)在X,Y軸上的偏差

圖5 C3點(diǎn)在X,Y軸上的偏差

由靜態(tài)目標(biāo)實(shí)驗(yàn)可得，經(jīng)誤差補(bǔ)償后三個(gè)點(diǎn)的靜態(tài)測(cè)試結(jié)果在定位精度方面顯著上升。對(duì)其最大誤差(MPE)及均方根誤差(RMSE)進(jìn)行統(tǒng)計(jì)，見表1。

表1 均方根誤差及最大誤差 m

從表1可以看出，在經(jīng)過誤差補(bǔ)償后，三種實(shí)驗(yàn)場(chǎng)景的均方根誤差都有較為明顯地減小，綜合上述測(cè)試結(jié)果，可以看出，相較于前兩個(gè)實(shí)驗(yàn)場(chǎng)景的定位結(jié)果，空曠大廳中存在的影響因素少，且在測(cè)試環(huán)境中無其他干擾信號(hào)的物體存在，定位精度顯著上升，其測(cè)量曲線更平穩(wěn)，與真實(shí)點(diǎn)位置信息相比存在差異較小，三個(gè)場(chǎng)景下的定位精度在經(jīng)誤差補(bǔ)償后都有明顯的提高，靜態(tài)最大定位誤差在0.3 m以內(nèi)。WKF-TDOA能夠較好地應(yīng)用于經(jīng)典的室內(nèi)場(chǎng)景中，為不同室內(nèi)環(huán)境提供更加精確便捷的靜態(tài)位置服務(wù)。

2.2.2 動(dòng)態(tài)下的目標(biāo)檢測(cè)

實(shí)驗(yàn)人員手持標(biāo)簽0分別在3個(gè)實(shí)驗(yàn)場(chǎng)景中行走，設(shè)置行走路徑為矩形，同樣選取3 m×3 m的室內(nèi)空間，考慮到室內(nèi)環(huán)境的不確定性較大，為保證測(cè)量結(jié)果的可靠性，攜帶標(biāo)簽的受試者均采取勻速運(yùn)動(dòng)。

圖6(a)～(c)分別為在大廳、實(shí)驗(yàn)室、會(huì)議室環(huán)境下動(dòng)態(tài)濾波之后的結(jié)果，可以看出,在動(dòng)態(tài)多干擾因素的環(huán)境下，定位精度急劇下降，偏離真實(shí)軌跡，在卡爾曼濾波后，定位結(jié)果有所提升，但在誤差波動(dòng)較大的位置依然與真實(shí)軌跡有較大差別，經(jīng)WKF-TDOA后，通過時(shí)域下動(dòng)態(tài)調(diào)整權(quán)值，定位結(jié)果明顯比傳統(tǒng)KF后的結(jié)果更接近于預(yù)設(shè)軌跡，定位系統(tǒng)的魯棒性得到了有效提升。

圖6 空曠大廳運(yùn)動(dòng)軌跡

綜上所述，在室內(nèi)環(huán)境下，由于環(huán)境的多樣性與環(huán)境內(nèi)部的多徑效應(yīng)，單一的KF算法已經(jīng)無法滿足室內(nèi)人員的精確位置服務(wù)，WKF-TDOA通過在時(shí)域內(nèi)對(duì)測(cè)量路徑進(jìn)行WKF，使得濾波后的軌跡有效收斂于真實(shí)軌跡。實(shí)驗(yàn)證明，WKF-TDOA在復(fù)雜環(huán)境下對(duì)于人員的動(dòng)態(tài)軌跡追蹤有良好的應(yīng)用，能為人員提供準(zhǔn)確、有效的位置信息。

2.3 性能分析

2.3.1 不同基站數(shù)量比較

在實(shí)驗(yàn)室、會(huì)議室和空曠大廳三種室內(nèi)環(huán)境下分別設(shè)置不同數(shù)量的基站進(jìn)行單目標(biāo)定位實(shí)驗(yàn)。不同基站數(shù)量對(duì)比下的定位結(jié)果如圖7所示。

圖7 不同基站數(shù)量對(duì)比

圖7顯示了同一個(gè)環(huán)境下不同基站數(shù)量對(duì)定位精度的影響，增大標(biāo)簽與基站的距離對(duì)三種不同環(huán)境下的定位精度都存在不同程度的影響，由圖可知,在相對(duì)復(fù)雜的室內(nèi)環(huán)境中，定位精度是隨著基站數(shù)量而線性增長(zhǎng)的，出于對(duì)實(shí)際應(yīng)用環(huán)境考慮，在規(guī)模較小的室內(nèi)復(fù)雜環(huán)境下部署四個(gè)基站足以提供穩(wěn)定且準(zhǔn)確的定位精度。

2.3.2 不同算法比較

對(duì)比方法有Taylor級(jí)數(shù)[11]、EKF以及最小二乘法[12]，采用RMSE、MPE作為評(píng)價(jià)指標(biāo)。不同算法對(duì)比結(jié)果如圖8所示。

圖8 不同算法對(duì)比

圖8可以看出，隨著距離的增加，不同方法定位誤差都有一定程度的上升，但EKF和WKF-TDOA定位誤差相對(duì)較小，原因是兩種算法不是單純的定位算法，而是作為跟蹤濾波算法，因此,在目標(biāo)節(jié)點(diǎn)動(dòng)態(tài)的情況下，EKF與WKF-TDOA的性能遠(yuǎn)優(yōu)于Taylor級(jí)數(shù)法和最小二乘法。隨著距離持續(xù)增加，經(jīng)WKF-TDOA方法的識(shí)別誤差更小，定位精度更高。

3 結(jié)束語

本文提出一種基于UWB的復(fù)雜室內(nèi)定位算法—WKF-TDOA，綜合靜態(tài)與動(dòng)態(tài)兩種標(biāo)簽移動(dòng)狀態(tài)，使用誤差補(bǔ)償對(duì)采集的UWB數(shù)據(jù)進(jìn)行修正。動(dòng)態(tài)的情況下，本文將噪聲協(xié)方差進(jìn)行遞歸更新，動(dòng)態(tài)調(diào)整權(quán)值以增強(qiáng)濾波器的穩(wěn)定性，改進(jìn)了傳統(tǒng)的KF方法，進(jìn)一步提高了定位精度。實(shí)驗(yàn)在多個(gè)真實(shí)的室內(nèi)環(huán)境中進(jìn)行，結(jié)果表明：WKF-TDOA在復(fù)雜室內(nèi)環(huán)境下定位精度有很大提升，在靜態(tài)與動(dòng)態(tài)的環(huán)境中定位精度較高，具有較強(qiáng)的普適性，可在多種室內(nèi)場(chǎng)景下進(jìn)行使用。在后續(xù)工作中，將研究在室內(nèi)復(fù)雜環(huán)境下多人精確定位的問題，進(jìn)一步提高算法的普適性與魯棒性。