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

(1.陸軍工程大學(xué)，江蘇 南京 210007；2.解放軍78111部隊(duì)，四川 成都 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)星信號(hào)的定位系統(tǒng)精度嚴(yán)重惡化，甚至無(wú)法定位，嚴(yán)重阻礙了位置服務(wù)、行人導(dǎo)航的發(fā)展[1]。因此，人們對(duì)室內(nèi)定位技術(shù)的需求日益增加。近年來(lái)，各種室內(nèi)定位方法層出不窮，但是精度普遍不高，用戶(hù)體驗(yàn)不佳，普適的高精度的室內(nèi)定位依然是目前研究的熱點(diǎn)。

目前，常見(jiàn)的室內(nèi)定位技術(shù)主要有：基于射頻信號(hào)的定位技術(shù)，包括紅外線、超聲波、藍(lán)牙、超寬帶、WiFi等[2-6]；基于慣性傳感器的定位技術(shù)，包括行人航跡推算、地磁匹配等?；谏漕l信號(hào)的定位技術(shù)具有使用方便、定位精度高等優(yōu)勢(shì)，但存在非視距、多路徑效應(yīng)的情況[7]。基于行人航跡推算的定位技術(shù)具有自主性和連續(xù)性的優(yōu)勢(shì)，但隨著時(shí)間的推移存在累積誤差缺陷[8]。因此，聯(lián)合上述兩種定位方法，發(fā)揮各自的技術(shù)優(yōu)勢(shì)，是實(shí)現(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)域，已有大量的算法研究，而針對(duì)室內(nèi)組合定位系統(tǒng)融合算法的相關(guān)研究較少，主要的方法是卡爾曼濾波[9]。但是在室內(nèi)組合定位系統(tǒng)中，其狀態(tài)方程和觀測(cè)方程都為非線性方程，傳統(tǒng)的卡爾曼濾波并不適用。擴(kuò)展卡爾曼濾波可以將上述方程線性化，但是其近似誤差降低了模型的準(zhǔn)確性，隨著時(shí)間的累計(jì)，其估計(jì)精度難以保證[10]。無(wú)跡卡爾曼濾波采用無(wú)損變換，無(wú)需對(duì)非線性狀態(tài)函數(shù)和觀測(cè)函數(shù)求導(dǎo)，收斂速度快，估計(jì)精度高，但是由于計(jì)算誤差和噪聲引起的誤差協(xié)方差矩陣為負(fù)定會(huì)導(dǎo)致濾波發(fā)散[11]。同時(shí)，傳統(tǒng)的組合定位方法并未考慮復(fù)雜室內(nèi)環(huán)境中存在非視距的特殊情況，其在實(shí)際應(yīng)用中可能出現(xiàn)定位誤差的劇烈抖動(dòng)和濾波發(fā)散的情況。本文采用誤差狀態(tài)量卡爾曼濾波，將每個(gè)系統(tǒng)的真實(shí)值對(duì)消，其殘差的狀態(tài)和觀測(cè)方程均為線性方程，因此沒(méi)有近似誤差，提高了融合后的定位精度，同時(shí)加入了非視距檢測(cè)模塊，增加了系統(tǒng)的魯棒性，提高了定位精度。

1 UWB定位

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

(1)

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

2 PDR定位

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

(2)

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

3 UWB/PDR融合定位算法

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

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

3.1 非視距檢測(cè)

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

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

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

(3)

假定系統(tǒng)應(yīng)用的實(shí)際環(huán)境中，非視距僅僅是臨時(shí)由于墻、柱子、管道等的遮擋造成的，如地下停車(chē)場(chǎng)、工廠房等環(huán)境，而不考慮長(zhǎng)期的非視距情況，在該情況下，可以多布設(shè)UWB節(jié)點(diǎn)滿(mǎn)足需求。因此，在非視距區(qū)域，行人移動(dòng)的距離并不大?；谏鲜黾俣ǎ绻麧M(mǎn)足如下條件，則認(rèn)為離開(kāi)非視距區(qū)域：

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

(4)

3.2 卡爾曼濾波

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

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

(5)

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

xk=Axk-1+Fwk-1

(6)

(7)

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

zk=Hxk+vk

(8)

4 仿真結(jié)果分析

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

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

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

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

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

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

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

5 實(shí)驗(yàn)結(jié)果及分析

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

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

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

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

圖6 NLOS檢測(cè)Kalman和傳統(tǒng)Kalman的實(shí)測(cè)累計(jì)誤差分布對(duì)比圖

6 結(jié)論

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