基于UWB/PDR的組合室內(nèi)定位算法*

陳隆亮，楊 昳，袁 恩，趙躍新，張 超

(1.陸軍工程大學(xué)，江蘇 南京 210007；2.解放軍78111部隊，四川 成都 610014；3.成都工業(yè)職業(yè)技術(shù)學(xué)院，四川 成都 610218)

0 引言

全球衛(wèi)星導(dǎo)航系統(tǒng)(北斗、GPS、GLONASS等)的定位精度不斷提高，豐富了室外基于位置信息的服務(wù)，方便了人們的生活。相比室外環(huán)境，室內(nèi)細(xì)粒度的環(huán)境為多樣化的位置信息服務(wù)提供了更多可能。但是在室內(nèi)環(huán)境中，由于遮擋，基于衛(wèi)星信號的定位系統(tǒng)精度嚴(yán)重惡化，甚至無法定位，嚴(yán)重阻礙了位置服務(wù)、行人導(dǎo)航的發(fā)展[1]。因此，人們對室內(nèi)定位技術(shù)的需求日益增加。近年來，各種室內(nèi)定位方法層出不窮，但是精度普遍不高，用戶體驗不佳，普適的高精度的室內(nèi)定位依然是目前研究的熱點。

目前，常見的室內(nèi)定位技術(shù)主要有：基于射頻信號的定位技術(shù)，包括紅外線、超聲波、藍(lán)牙、超寬帶、WiFi等[2-6]；基于慣性傳感器的定位技術(shù)，包括行人航跡推算、地磁匹配等。基于射頻信號的定位技術(shù)具有使用方便、定位精度高等優(yōu)勢，但存在非視距、多路徑效應(yīng)的情況[7]。基于行人航跡推算的定位技術(shù)具有自主性和連續(xù)性的優(yōu)勢，但隨著時間的推移存在累積誤差缺陷[8]。因此，聯(lián)合上述兩種定位方法，發(fā)揮各自的技術(shù)優(yōu)勢，是實現(xiàn)高精度、高可靠的室內(nèi)定位的可行途徑之一。

本文提出基于UWB/PDR的信息融合算法，將UWB定位系統(tǒng)和PDR定位系統(tǒng)組合，互相補(bǔ)充，構(gòu)成一種穩(wěn)定、精度更高的組合定位系統(tǒng)。目前，在衛(wèi)星導(dǎo)航和慣性導(dǎo)航融合領(lǐng)域，已有大量的算法研究，而針對室內(nèi)組合定位系統(tǒng)融合算法的相關(guān)研究較少，主要的方法是卡爾曼濾波[9]。但是在室內(nèi)組合定位系統(tǒng)中，其狀態(tài)方程和觀測方程都為非線性方程，傳統(tǒng)的卡爾曼濾波并不適用。擴(kuò)展卡爾曼濾波可以將上述方程線性化，但是其近似誤差降低了模型的準(zhǔn)確性，隨著時間的累計，其估計精度難以保證[10]。無跡卡爾曼濾波采用無損變換，無需對非線性狀態(tài)函數(shù)和觀測函數(shù)求導(dǎo)，收斂速度快，估計精度高，但是由于計算誤差和噪聲引起的誤差協(xié)方差矩陣為負(fù)定會導(dǎo)致濾波發(fā)散[11]。同時，傳統(tǒng)的組合定位方法并未考慮復(fù)雜室內(nèi)環(huán)境中存在非視距的特殊情況，其在實際應(yīng)用中可能出現(xiàn)定位誤差的劇烈抖動和濾波發(fā)散的情況。本文采用誤差狀態(tài)量卡爾曼濾波，將每個系統(tǒng)的真實值對消，其殘差的狀態(tài)和觀測方程均為線性方程，因此沒有近似誤差，提高了融合后的定位精度，同時加入了非視距檢測模塊，增加了系統(tǒng)的魯棒性，提高了定位精度。

1 UWB定位

因為信號的時域分辨率和信號的帶寬具有倒數(shù)關(guān)系，所以UWB系統(tǒng)的時域分辨率能夠達(dá)到納秒級別，因此，其能夠在時域上分辨通過室內(nèi)多條路徑在不同時間到達(dá)天線的信號[7]。在錨節(jié)點和待定位節(jié)點時間同步的情況下，UWB系統(tǒng)便能準(zhǔn)確測量信號的傳播時間，如果信號傳播存在直達(dá)路徑，那么采用基于到達(dá)時間的定位算法[12]就可以計算出待定位節(jié)點的位置。假設(shè)二維平面中，UWB錨節(jié)點個數(shù)為N，待定位節(jié)點發(fā)送信號的時刻為T，第n個錨節(jié)點接收到信號的時刻為Tn，那么第n個錨節(jié)點的空間位置和其接收信號的時刻存在如下關(guān)系：

(1)

其中，c為光速，(xn,yn)為錨節(jié)點的二維坐標(biāo)。聯(lián)合N個錨節(jié)點的方程，便可以求解出待定位節(jié)點的坐標(biāo)(x,y)。從式(1)可以看出，待定位節(jié)點和錨節(jié)點之間的時間同步直接影響了定位精度，實際中，基于到達(dá)時間的UWB定位系統(tǒng)由于時間同步存在誤差，定位精度下降較大。

2 PDR定位

在室內(nèi)環(huán)境中，大多數(shù)人員攜帶的待定位節(jié)點都是小型設(shè)備，這些設(shè)備上的慣性器件由于成本、功耗、體積等因素，性能較差，遠(yuǎn)遠(yuǎn)不能滿足基于積分方法的慣性導(dǎo)航[13]。但是，由于人員走動的特殊性也帶來了新的航跡推算方法：由于人的行走具有周期性，因此可以利用加速度計的周期起伏判斷人的行走步數(shù)，通過地磁或者電子羅盤判斷人的行走方向，通過步長模型(如步頻-步長模型)判斷人的行走步長。聯(lián)合步數(shù)、方向、步長，便可以實現(xiàn)行人的航跡推算。如果已知出發(fā)點的位置為(x0,y0)，用戶移動方向與地理正北方向夾角為θ，那么用戶到達(dá)的位置(xn,yn)可以由下式計算：

(2)

其中，n為步數(shù)，θi、li分別為第i步的方向和步長。

3 UWB/PDR融合定位算法

本文提出的UWB/PDR融合定位算法的總原理框圖如圖1所示。

圖1 UWB/PDR融合定位原理框圖

3.1 非視距檢測

當(dāng)存在非視距時，由于折射、反射的影響，UWB測量得到的信號到達(dá)時間大于信號直線傳播情況下的時間，在這種情況下，UWB系統(tǒng)的定位誤差將增大。當(dāng)進(jìn)行卡爾曼濾波時，由于方程的誤差不是隨機(jī)噪聲造成的，濾波可能出現(xiàn)發(fā)散現(xiàn)象。因此本文提出了非視距檢測，在濾波前過濾掉被非視距污染的UWB定位數(shù)據(jù)，增加系統(tǒng)的穩(wěn)定性，提高融合精度。

由于UWB定位系統(tǒng)可以以較高的位置更新率進(jìn)行工作，同時行人的步行速度有限，在UWB兩次測量的間隔時間(50 ms)內(nèi)，移動的距離很小，因此UWB的測量值不會存在突變現(xiàn)象。基于上述假定，如果滿足如下條件，本文則認(rèn)為存在非視距情況：

min(αn,αn+1,αn+2)>φ

(3)

假定系統(tǒng)應(yīng)用的實際環(huán)境中，非視距僅僅是臨時由于墻、柱子、管道等的遮擋造成的，如地下停車場、工廠房等環(huán)境，而不考慮長期的非視距情況，在該情況下，可以多布設(shè)UWB節(jié)點滿足需求。因此，在非視距區(qū)域，行人移動的距離并不大。基于上述假定，如果滿足如下條件，則認(rèn)為離開非視距區(qū)域：

max(βn,βn+1,βn+2)<φ

(4)

3.2 卡爾曼濾波

在排除了非視距引起的誤差后，假設(shè)其余誤差都由白噪聲引起。本文采用殘差狀態(tài)量作為UWB/PDR濾波的輸入，其優(yōu)點是系統(tǒng)的狀態(tài)方程和觀測方程都為線性方程，因此可以采用線性卡爾曼濾波將UWB和PDR輸出的信息進(jìn)行融合，避免了非線性帶來的復(fù)雜性。考慮二維平面情況，定義k時刻的殘差狀態(tài)變量為4維向量：

xk=[δNk,δEk,ξLk,ξdk]T

(5)

式中，δNk、δEk、ξLk、ξdk分別為k時刻北向、東向的位置誤差，步長檢測器和陀螺儀的一階馬爾科夫過程漂移。那么根據(jù)PDR原理，該系統(tǒng)的狀態(tài)方程為：

xk=Axk-1+Fwk-1

(6)

(7)

那么觀測向量為zk=[zn,ze]T。觀測方程為：

zk=Hxk+vk

(8)

4 仿真結(jié)果分析

在本文的仿真中，假設(shè)行人以每秒2步的步頻、0.7 m的步長勻速行走，UWB定位系統(tǒng)每秒輸出20次定位結(jié)果，以UWB時間為準(zhǔn)將PDR數(shù)據(jù)對準(zhǔn)。利用卡爾曼濾波進(jìn)行數(shù)據(jù)融合。PDR系統(tǒng)行走方向噪聲方差為(0.7°/步)2，步長噪聲方差為(0.07 m/步)2，UWB系統(tǒng)定位誤差由方差為(1 m)2的白噪聲引起。PDR系統(tǒng)、UWB系統(tǒng)及組合后的系統(tǒng)在東向和北向上的定位誤差仿真曲線如圖2、圖3所示。

圖中虛線為UWB系統(tǒng)定位誤差，點線為PDR系統(tǒng)定位誤差，粗實線為組合系統(tǒng)的定位誤差，從圖中可以看出，PDR系統(tǒng)隨著步數(shù)的增加，其累計誤差效應(yīng)明顯，UWB系統(tǒng)誤差存在較大的抖動。UWB/PDR組合室內(nèi)定位系統(tǒng)既可有效抑制PDR系統(tǒng)的誤差隨時間發(fā)散的問題，同時也明顯減小了UWB定位誤差的抖動。

圖4展示了不同方法的累計誤差分布，從圖中可以看到，總體上，組合后的系統(tǒng)在一定程度上提高了定位精度，其定位誤差90%在1 m以內(nèi)，為行

圖2 UWB、PDR和Kalman組合系統(tǒng)的東向誤差曲線圖

圖3 UWB、PDR和Kalman組合系統(tǒng)的北向誤差曲線圖

圖4 UWB、PDR和Kalman組合系統(tǒng)的仿真累計誤差分布圖

人室內(nèi)導(dǎo)航提供了可靠基礎(chǔ)定位數(shù)據(jù)。

5 實驗結(jié)果及分析

為了驗證本文提出的UWB/PDR組合室內(nèi)定位方法在實際中存在非視距情況下的有效性和魯棒性，選取DW1000型號的UWB節(jié)點和三星Galaxy S9型號的移動智能終端作為硬件平臺進(jìn)行算法實際測試。UWB定位系統(tǒng)定位更新頻率為20 Hz，基于安卓系統(tǒng)開發(fā)的PDR系統(tǒng)中，慣性傳感器數(shù)據(jù)采樣頻率為50 Hz，以UWB信號采集時間戳為基準(zhǔn)，對齊實時步態(tài)檢測的定位位置。實驗場地如圖5所示，中間為封閉天井的環(huán)形走廊，大小為11.18 m×11.70 m。其中，天線標(biāo)志為UWB節(jié)點位置。以UWB初始定位位置為PDR系統(tǒng)初始位置，實驗者手持設(shè)備沿預(yù)定軌跡勻速行走一圈。實驗結(jié)果如圖5所示。黑色實線為真實軌跡，虛線和點線分別為UWB定位和PDR系統(tǒng)定位軌跡，帶五角星的軌跡為卡爾曼濾波融合軌跡。

圖5 UWB、PDR和Kalman組合系統(tǒng)的定位結(jié)果

對比真實的行走軌跡、PDR定位軌跡、UWB定位軌跡和UWB/PDR組合定位軌跡可以發(fā)現(xiàn)，UWB/ PDR組合定位算法能有效修正單獨(dú)采用PDR系統(tǒng)定位產(chǎn)生的積累誤差，并減輕了UWB系統(tǒng)定位抖動的缺點，更接近真實行走路徑。

同時，在測試中，當(dāng)實驗者行走至圖中電梯口處，由于UWB信號被承重梁遮擋，出現(xiàn)了非視距現(xiàn)象，已經(jīng)無法接收到3個以上的UWB信號進(jìn)行定位，單純的UWB定位系統(tǒng)出現(xiàn)了軌跡丟失問題，而在該區(qū)域，PDR系統(tǒng)則表現(xiàn)良好。圖6對比了傳統(tǒng)卡爾曼濾波和本文提出的帶非視距檢測的卡爾曼濾波的誤差分布。可以看到，由于非視距的影響，傳統(tǒng)的卡爾曼濾波出現(xiàn)了很大的誤差，而本文的算法由于檢測到了非視距的存在，消除了非視距的影響，大大降低了整個定位系統(tǒng)的誤差，提升了系統(tǒng)的定位精度。

圖6 NLOS檢測Kalman和傳統(tǒng)Kalman的實測累計誤差分布對比圖

6 結(jié)論

為了提高在復(fù)雜室內(nèi)環(huán)境中的定位效果，本文提出了一種帶非視距檢測的UWB/PDR組合室內(nèi)定位算法，組合定位算法既克服了超寬帶系統(tǒng)在非視距情況下的定位精度抖動問題，又避免了行人航跡推算的累計誤差。均勻步長行走的實際測試表明，組合算法提升了定位的精度和穩(wěn)定性，在復(fù)雜的非視距環(huán)境中能夠穩(wěn)定工作，具有很強(qiáng)的實用性。下一步工作將研究如何將定位結(jié)果反饋回UWB系統(tǒng)，用以對每一步步長進(jìn)行精確校正，解決在非均勻步長行走時帶來的步長誤差問題，實現(xiàn)更加魯棒的定位系統(tǒng)。