劉 戀， 向鳳紅， 毛劍琳

(昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院，云南 昆明 650500)

修正的UKF濾波時(shí)差定位算法*

劉 戀， 向鳳紅， 毛劍琳

(昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院，云南 昆明 650500)

為了降低到達(dá)時(shí)間差(TDOA)測(cè)距在非視距(NLOS)傳播環(huán)境中的誤差，提出了在強(qiáng)跟蹤無(wú)跡卡爾曼濾波(UKF)基礎(chǔ)上改進(jìn)的算法。在狀態(tài)發(fā)生突變時(shí)，給預(yù)測(cè)協(xié)方差矩陣加入次優(yōu)漸消因子；對(duì)NLOS誤差進(jìn)行正負(fù)判斷，利用整體偏移法修改濾波增益，但估計(jì)協(xié)方差矩陣不做改進(jìn)，以免出現(xiàn)不收斂。實(shí)驗(yàn)結(jié)果表明：該算法不僅能有效地抑制突變帶來(lái)的影響，也能高效地消除NLOS誤差，提高了NLOS傳播的到達(dá)時(shí)間差定位精度。

到達(dá)時(shí)間差； 無(wú)線傳感器網(wǎng)絡(luò)； 非視距傳播； 無(wú)跡卡爾曼濾波； 濾波增益

0 引 言

隨著全球不同地區(qū)和用戶應(yīng)用的網(wǎng)絡(luò)不同，尤其是GSM/GPRS/EDGEUMTS、CDMA2000、PHS、WLAN和WiMAX等無(wú)線網(wǎng)絡(luò)技術(shù)互補(bǔ)融合，無(wú)線異構(gòu)網(wǎng)絡(luò)受到廣泛關(guān)注[1]。與在同構(gòu)網(wǎng)絡(luò)中的定位技術(shù)一樣，異構(gòu)無(wú)線傳感器網(wǎng)絡(luò)，很多功能的實(shí)現(xiàn)都依賴網(wǎng)絡(luò)定位技術(shù)的精度。

到達(dá)時(shí)間差(time difference of arrival,TDOA)[2]定位技術(shù)是無(wú)線傳感器網(wǎng)絡(luò)節(jié)點(diǎn)定位的一種重要技術(shù)，它具有精度高、易組網(wǎng)實(shí)現(xiàn)、能對(duì)付寬帶低譜密度信號(hào)等優(yōu)點(diǎn)。TDOA技術(shù)是測(cè)量移動(dòng)臺(tái)(mobile station,MS)到不同基站(base station,BS)的時(shí)間差而進(jìn)行估計(jì)MS的位置。由于無(wú)線電信號(hào)在實(shí)際傳輸過(guò)程中易被障礙物遮擋只能以反射、折射等非視距(non line of sight,NLOS)傳播方式進(jìn)行傳播，因此,MS位置估計(jì)問(wèn)題就轉(zhuǎn)化成高度非線性方程組估計(jì)問(wèn)題。

在引入了NLOS誤差的TDOA定位中，擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)[3,4]被廣泛應(yīng)用，它對(duì)非線性模型進(jìn)行一階Taylor展開(kāi)，缺點(diǎn)是近似線性化在對(duì)狀態(tài)后驗(yàn)分布的估計(jì)中引入很大誤差。所以，學(xué)者們提出了一種確定性樣本點(diǎn)計(jì)算的濾波算法無(wú)跡卡爾曼濾波(unscented Kalman filtering,UKF)[5]，它直接利用真實(shí)的系統(tǒng)模型，其后驗(yàn)值和協(xié)方差可以精確到三階Taylor展開(kāi)，但它的缺點(diǎn)是在模型不準(zhǔn)確或狀態(tài)發(fā)生突變的情況下魯棒性不佳。在文獻(xiàn)[6]中,提出先對(duì)系統(tǒng)狀態(tài)進(jìn)行突變判斷，狀態(tài)發(fā)生突變時(shí)給預(yù)測(cè)協(xié)方差矩陣加入多重次優(yōu)漸消因子，以消除狀態(tài)突變時(shí)的影響,雖然這種方法明顯優(yōu)于標(biāo)準(zhǔn)UKF,但它僅改進(jìn)了異常狀態(tài)的部分誤差，在狀態(tài)正常時(shí)對(duì)引入NLOS誤差卻不能很好地消除。針對(duì)這個(gè)問(wèn)題，本文將基于改進(jìn)的強(qiáng)跟蹤UKF算法引入到TDOA抗NLOS的處理中，并對(duì)此進(jìn)行改進(jìn)，避免原文中的算法對(duì)引入NLOS誤差可能出現(xiàn)的不收斂問(wèn)題，提高了系統(tǒng)的自適應(yīng)能力和魯棒性。仿真結(jié)果驗(yàn)證了所提算法的有效性。

1 時(shí)差定位數(shù)學(xué)模型

假設(shè)在二維空間中有M個(gè)基站，基于時(shí)差定位的空間示意圖如圖1所示。圖中，u=[x,y]T為MS待估計(jì)位置坐標(biāo)；si=[xi,yi]T為第i個(gè)BS的實(shí)際位置坐標(biāo)。MS到BSi之間的距離為

(1)

式中 ‖‖為模。

圖1 時(shí)差定位原理圖

ti為電信號(hào)從MS到BSi的視距(lineofsight,LOS)傳播時(shí)間，tei為從MS到BSi的NLOS超量時(shí)延，它為一個(gè)正值的隨機(jī)變量，各tei之間相互獨(dú)立，當(dāng)電信號(hào)從MS到BSi的是LOS傳播時(shí),tei=0。不失一般性，選取s1作為參考接收站，可以得到MS到BSi的TDOA定位方程為

i=2,…,M

(2)

i=2,…,M

(3)

r=f(u)+η

(4)

式中r=[r21,…,rM1]T；f(u)=[f21(u),…,fM1(u)]T；η=[η21,…,ηM1]T，為方便起見(jiàn)，認(rèn)為此噪聲η服從均值為零、正定協(xié)方差矩陣為Q的高斯分布

Q=E[η-E[η])(η-E[η])T]

(5)

式中 E[]為期望值。根據(jù)式(4)可得似然函數(shù)[6]為

exp{-(1/2)(r-f(u))TQ-1(r-f(u))}

(6)

式中 Q-1為Q的逆矩陣，Q為對(duì)稱正定矩陣，其逆存在。

最大似然估計(jì)是最大化(6)中u的值，因此,可以轉(zhuǎn)化為最小化二次型目標(biāo)函數(shù)，即

J(u)=(r-f(u))TQ-1(r-f(u))

(7)

根據(jù)式(7)可知，求解u必須求J(u)的最小值，但是用解析方法求解高度非線性函數(shù)非常困難[8]。因此，本文采用最優(yōu)估計(jì)求解此非線性方程，確定MS的位置坐標(biāo)值。

2 基于修正UKF的時(shí)差定位算法

2.1 標(biāo)準(zhǔn)UKF算法

由于式(3)為高度非線性函數(shù)，得到的式(7)的幾何結(jié)構(gòu)很不規(guī)則，且存在局部極小點(diǎn)[9]。而濾波主要解決以某種最優(yōu)方式聯(lián)合求解未知方程和測(cè)量方程的問(wèn)題，因此，先將時(shí)差定位問(wèn)題轉(zhuǎn)化成非線性系統(tǒng)方程，建立狀態(tài)方程和測(cè)量方程

(8)

式中sk和zk分別為第k個(gè)時(shí)刻的狀態(tài)向量和測(cè)量向量，F(xiàn)k和Hk分別為狀態(tài)轉(zhuǎn)移矩陣和測(cè)量矩陣,wk和vk分別為第k個(gè)時(shí)刻的過(guò)程噪聲向量和測(cè)量噪聲向量，其統(tǒng)計(jì)特性滿足

(9)

式中Qk和Rk分別為wk和vk的協(xié)方差矩陣，δkj為Kronecker-δ函數(shù)。UKF的算法過(guò)程如下：

1)濾波初值

(10)

2)樣本點(diǎn)計(jì)算

(11)

式中a∈R為調(diào)節(jié)參數(shù)，取值為一個(gè)小的正值。

3)時(shí)間更新

(12)

(13)

(14)

4)量測(cè)更新

(15)

(16)

(17)

(18)

式中 Kk為第k個(gè)時(shí)刻的濾波增益，Rk為vk的協(xié)方差矩陣,ek為zk所對(duì)應(yīng)的新息過(guò)程。

pk=p(s0∶k,z1∶k)

(19)

(20)

(21)

(22)

(23)

式中n=1,2,…,nx，sk[n]為狀態(tài)矢量sk的第n維。

從上述過(guò)程式(12)～式(18)可看出，狀態(tài)的每一次更新都是由前一次狀態(tài)估計(jì)和新的變量測(cè)量值得到，而且Kk是估計(jì)值隨新息過(guò)程值變化程度的標(biāo)志。因此，可通過(guò)改變?cè)鲆鍷k的大小來(lái)控制估計(jì)值。第一，在狀態(tài)突變時(shí)不受上次修正Kk影響的情況下將次優(yōu)漸消因子引入到預(yù)測(cè)協(xié)方差矩陣P～k中。第二，利用整體偏移法[10]直接修正增益。這樣可以實(shí)時(shí)調(diào)整濾波增益Kk，提高系統(tǒng)的魯棒性。

2.2 修正UKF算法

2.2.1 強(qiáng)跟蹤UKF算法

在式(16)中可知濾波器輸出的新息序列為ek，衡量濾波性能優(yōu)良時(shí)它應(yīng)滿足

(24)

此式要求不同時(shí)刻的新息向量保持正交關(guān)系，且具有類似白噪聲的性質(zhì)，表明已將新息序列中的一切有效信息提取出來(lái)。

利用新息序列的統(tǒng)計(jì)信息，采用假設(shè)檢驗(yàn)的方法對(duì)系統(tǒng)異常進(jìn)行檢測(cè)，定義統(tǒng)計(jì)量

(25)

統(tǒng)計(jì)量αk服從自由度為m的χ2分布。判斷狀態(tài)是否突變

(26)

若檢測(cè)到狀態(tài)突變，給預(yù)測(cè)協(xié)方差矩陣P～k引入次優(yōu)漸消因子λk，則

(27)

(28)

令Vj=0，由式(15)與式(28)可得

(29)

令λk=diag(λ1,k,λ2,k,…,λn,k)，對(duì)式(27)與式(29)求跡可得解

(30)

(31)

狀態(tài)突變時(shí)，式(15)改變?nèi)缦?/p>

(32)

2.2.2 修正濾波增益

由第1節(jié)可知，NLOS傳播給TDOA測(cè)量引入的誤差tei1是兩個(gè)隨機(jī)變量之差，其值與MS到BSi的距離有關(guān)。而在式(17)中新息ek代表預(yù)測(cè)值往估計(jì)值校正的方向和大小。因此，可以根據(jù)ek的大小和引入誤差是正還是負(fù)，改變式(17)中Kk大小，使偏離的估計(jì)值拉回到正確的方向。

增益Kk修正情況如下

(33)

式中 “+”代表正偏時(shí)的情況，“-”代表負(fù)偏時(shí)的情況；θ≥1為可調(diào)修正因子，其根據(jù)實(shí)驗(yàn)統(tǒng)計(jì)取值。

式(17)則作出如下修改

(34)

2.2.3 修正UKF算法實(shí)現(xiàn)

1)采用式(10)進(jìn)行初始化；

2)利用式(11)進(jìn)行樣點(diǎn)計(jì)算；

3)采用式(12),式(13)計(jì)算狀態(tài)預(yù)測(cè)；

4)采用式(26)的方法對(duì)狀態(tài)突變做出預(yù)判，若存在突變, 則用式(27)計(jì)算P～k，式(32)代替(15)計(jì)算Kk，否則，采用式(13)計(jì)算P～k；

6)完成剩下的時(shí)間和量測(cè)更新，返回步驟(2)，進(jìn)行下一次濾波解算。

3 仿真實(shí)驗(yàn)與分析

在一片區(qū)域內(nèi)，將4個(gè)傳感器節(jié)點(diǎn)按設(shè)定位置部署。設(shè)主節(jié)點(diǎn)位置s1為(0,0)，3個(gè)輔節(jié)點(diǎn)位置為s2(100,400),s3(800,400),s4(800,0)。輻射源沿著運(yùn)動(dòng)軌跡(見(jiàn)圖2)運(yùn)動(dòng)：依次以1m/s從A,B,C到D。在B點(diǎn)有一次狀態(tài)突變，運(yùn)動(dòng)曲線AB到BC的斜率由4/3轉(zhuǎn)變?yōu)?/4；在C點(diǎn)也發(fā)生了一次狀態(tài)突變，運(yùn)動(dòng)曲線BC到CD的斜率由3/4轉(zhuǎn)變?yōu)?4/3。從A運(yùn)動(dòng)到B，需要100s，共100m；從B運(yùn)動(dòng)到C，需要600s，共600m；從C運(yùn)動(dòng)到D，需要500s，共500m。

圖2 運(yùn)動(dòng)軌跡

仿真時(shí)，每秒測(cè)量一次TDOA值，仿真時(shí)間為1 200s，每次實(shí)驗(yàn)中共有1 200個(gè)TDOA測(cè)量值。NLOS傳播分3種情況：1)主節(jié)點(diǎn)與輻射源是LOS傳播，輔節(jié)點(diǎn)與輻射源是NLOS傳播，仿真時(shí)由經(jīng)驗(yàn)值選取θ=5，E=1μs，TDOA重構(gòu)誤差仿真結(jié)果見(jiàn)圖3；2)主節(jié)點(diǎn)與輻射源是NLOS傳播，輔節(jié)點(diǎn)與輻射源是LOS傳播，仿真時(shí)由經(jīng)驗(yàn)值選取θ=3,E=1μs，TDOA重構(gòu)誤差仿真結(jié)果見(jiàn)圖4；3)輻射與主節(jié)點(diǎn)輔節(jié)點(diǎn)間都是NLOS傳播，仿真時(shí)由經(jīng)驗(yàn)值選取θ=2,E=1μs，TDOA重構(gòu)誤差仿真結(jié)果見(jiàn)圖5。

圖3 NLOS只影響主節(jié)點(diǎn)

圖4 NLOS只影響輔節(jié)點(diǎn)

圖5 主節(jié)點(diǎn)和輔節(jié)點(diǎn)都受NLOS影響的仿真結(jié)果

仿真結(jié)果如圖3、圖4和圖5所示，將標(biāo)準(zhǔn)UKF算法、強(qiáng)跟蹤改進(jìn)UKF算法和本文算法進(jìn)行測(cè)量值重構(gòu)TDOA誤差進(jìn)行仿真。在0s開(kāi)始時(shí)，輻射源從A出發(fā)，由于本文算法中先對(duì)NLOS誤差進(jìn)行判決，用整體偏移法修正了濾波增益，所以,本文算法在開(kāi)始時(shí)誤差就比其他兩者小。在0～100s階段，輻射源從A運(yùn)動(dòng)到B，3種算法的TDOA誤差都是由大到小，這是因?yàn)殡S著迭代次數(shù)逐漸增加誤差減??；在100～200s，輻射源從B點(diǎn)出發(fā)，在B點(diǎn)有一次突變，所以TDOA誤差都有一定的波動(dòng)；在700～810s時(shí)，輻射源從C點(diǎn)出發(fā)，在C點(diǎn)有一次突變，這次突變比B點(diǎn)突變對(duì)系統(tǒng)影響大，這是因?yàn)樵诖它c(diǎn)突變比B點(diǎn)大。在1 000s以后，在C點(diǎn)突變影響結(jié)束后，隨著迭代次數(shù)的增加，TDOA誤差趨于穩(wěn)定。在整個(gè)過(guò)程中，強(qiáng)跟蹤改進(jìn)UKF算法比UKF誤差相對(duì)低，是因?yàn)閺?qiáng)跟蹤UKF算法在面對(duì)突變時(shí)加入了次優(yōu)漸消因子，在以后的迭代也起了相應(yīng)的作用，但在圖4中出現(xiàn)超調(diào)現(xiàn)象，所以，在此基礎(chǔ)上本文算法不僅使此次迭代不受上次修正的預(yù)測(cè)方差矩陣影響，而且修正了濾波增益。因此,在圖3、圖4和圖5中，本文算法比其他2種算法，TDOA誤差明顯小，而且在面對(duì)突變時(shí)，本文算法雖有一點(diǎn)波動(dòng)，但相較于其他2種算法，波動(dòng)較小。

比較仿真結(jié)果圖3、圖4和圖5，會(huì)發(fā)現(xiàn)主節(jié)點(diǎn)和輔節(jié)點(diǎn)只有一個(gè)受NLOS影響比兩者都受影響時(shí)，UKF和強(qiáng)跟蹤改進(jìn)UKF算法的TDOA誤差更大，這是因?yàn)樵趦烧叨际苡绊憰r(shí)，NLOS誤差做差抵消了一部分NLOS影響。本文的算法在這3種情況下，TDOA誤差都比較小，這是因?yàn)樵谟?jì)算過(guò)程中根據(jù)NLOS誤差的正負(fù)對(duì)增益Kk進(jìn)行了修正，有效地減少了NLOS誤差。

根據(jù)式(23)分別對(duì)情況一、二、三進(jìn)行了200次蒙特卡洛實(shí)驗(yàn)，并計(jì)算得到TDOA估計(jì)值的RMSE，其值見(jiàn)表1。

表1 3種情況下TDOA估計(jì)值的RMSE

由表1可看出，本文算法比另2種算法明顯削弱了NLOS和突變對(duì)TDOA定位的影響，顯著提高了定位精度。

4 結(jié)束語(yǔ)

本文提出了在強(qiáng)跟蹤UKF的基礎(chǔ)上減小NLOS對(duì)系統(tǒng)的影響的算法。利用新息序列判斷狀態(tài)是否突變，若突變則將次優(yōu)漸消因子引入預(yù)測(cè)協(xié)方差矩陣中，使?fàn)顟B(tài)突變對(duì)系統(tǒng)的誤差減小；判斷出NLOS誤差正負(fù)，對(duì)TDOA重構(gòu)，進(jìn)行濾波增益的修正。仿真結(jié)果表明：經(jīng)過(guò)改進(jìn)后的算法，在狀態(tài)突變時(shí)，不僅沒(méi)有增加計(jì)算的復(fù)雜度，也大大減小了NLOS對(duì)定位的影響，提高了定位精度，并且改善了系統(tǒng)不收斂的現(xiàn)象。另外，如何將此方法運(yùn)用到不同模型中，也是將繼續(xù)研究的內(nèi)容。

[1] 杜曉玉,孫力娟,郭 劍,等.異構(gòu)無(wú)線傳感器網(wǎng)絡(luò)覆蓋優(yōu)化算法[J].電子與信息學(xué)報(bào),2014(3):696-702.

[2] 高洪元,于雪梅,趙忠凱.基于文化魚(yú)群算法的到達(dá)時(shí)間差定位技術(shù)[J].計(jì)算機(jī)工程,2011,37(14):137-139.

[3] 羅 磊,田增山,陳俊亞.EKF定位跟蹤算法研究[J].重慶郵電大學(xué)學(xué)報(bào):自然科學(xué)版,2009,21(1):50-52.

[4] 李仕云，鄧洪權(quán).基于EKF的雙站測(cè)向/時(shí)差聯(lián)合定位算法[J].無(wú)線通信技術(shù)，2015，41(1)：38-40.

[5] 姚依翔,謝俊元.基于UKF區(qū)域交叉定位的WSNs Sink節(jié)點(diǎn)動(dòng)態(tài)跟蹤算法[J].傳感器與微系統(tǒng),2015,34(4):123-126.

[6] 胡高歌,劉逸涵,高社生,等.改進(jìn)的強(qiáng)跟蹤UKF算法及其在INS/GPS組合導(dǎo)航中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào),2014,22(5):634-639.

[7] Torrieri D J. Statistical theory of passive location systems[J].IEEE Transactions on Aerospace and Electronic Systems,1984,20(2):183-198.

[8] 朱國(guó)輝,馮大政,李 進(jìn),等.一種利用修正牛頓迭代的時(shí)差定位算法[J].西安電子科技大學(xué)學(xué)報(bào)：自然科學(xué)版,2014,41(5):36-41.

[9] Dogancay K,Sakhtsari A H.Target tracking by time of difference of arrival using recursive smoothing[J].Signal Processing,2005,85(4):667-679.

[10] 李 靜,劉 琚.用卡爾曼濾波器消除TOA中NLOS誤差的三種方法[J].通信學(xué)報(bào),2005,26(1):130-135.

[11] 薛 麗,高社生,胡高歌.自適應(yīng)Sage-Husa粒子濾波及其在組合導(dǎo)航中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào),2013,21(1):84-88.

Modified UKF filtering time difference localization algorithm*

LIU Lian, XIANG Feng-hong, MAO Jian-lin

(Faculty of Information Engineering and Automation，Kunming University of Science and Technology，Kunming 650000，China)

In order to reduce time difference of arrival(TDOA)ranging error in non line of sight(NLOS) environments,two improvements are introduced to enhance unscented Kalman filtering(UKF).When the system states change, suboptimal fading factors are added into the prediction covariance to eliminate the influence of mutational states.Secondly,judging type of NLOS error and using the total-deflection method can modify filtering gain,but estimation covariance matrix is not modified to avoid non-convergence.Experimental result indicates that the proposed method can not only restrain the influence of mutation,but also eliminate NLOS error highly effectively and improve positioning precision greatly.

time difference of arrival(TDOA); wireless sensor networks(WSNs); non line of sight(NLOS)propagation; unscented Kalman filtering(UKF); filtering gain

10.13873/J.1000—9787(2017)04—0138—05

2016—03—25

國(guó)家自然科學(xué)基金資助項(xiàng)目(61163051,F020809)

TN 923

A

1000—9787(2017)04—0138—05

劉 戀(1991-)，女，碩士研究生，主要研究方向?yàn)槲锫?lián)網(wǎng)、無(wú)線傳感器網(wǎng)絡(luò)定位。