基于UWB的消防員室內(nèi)協(xié)同定位算法

楊 剛，朱士玲，李 強，趙克松，趙 杰，郭 建

西安郵電大學(xué) 通信與信息工程學(xué)院，西安710121

建筑物內(nèi)火災(zāi)事故頻發(fā)，常需消防員深入火場內(nèi)部展開滅火救援，如果負責(zé)協(xié)調(diào)調(diào)度任務(wù)的消防指揮人員能夠獲取消防員的精確位置，那么在消防員自身遭遇險情時，便可通過對講設(shè)備通知鄰近消防員施以救助，并為其合理規(guī)劃逃生路線，降低消防員的傷亡率?；诖?，如何實現(xiàn)對消防員的高精度定位是本文的研究重點。

雖然GPS等全球衛(wèi)星導(dǎo)航系統(tǒng)發(fā)展成熟，但由于衛(wèi)星信號難以穿透建筑物，導(dǎo)致室內(nèi)定位實現(xiàn)困難。超寬帶（ultra-wideband，UWB）作為室內(nèi)定位領(lǐng)域的新興技術(shù)，與藍牙、WiFi、RFID、Zigbee 等技術(shù)相比，具有穿透能力強、測距精度高、抗多徑能力強等優(yōu)點[1-3]，成為實現(xiàn)消防員室內(nèi)精確定位的關(guān)鍵技術(shù)。文獻[4]基于IR-UWB搭建無線傳感器網(wǎng)絡(luò)，利用標簽和固定錨點之間的多個測距信息，采用三邊定位算法獲取消防員所在位置。文獻[5]提出將小功率UWB 基站模塊安裝于智能應(yīng)急標志燈具中，利用TDOA技術(shù)實現(xiàn)對攜帶定位標簽的消防員實時定位。文獻[6]提出一種人體運動輔助的UWB室內(nèi)定位方法，當人體轉(zhuǎn)動時根據(jù)UWB信號的TOA和RSS 測量值，判斷被困人員所在方位，有效引導(dǎo)救援人員進行施救。以上方法雖然實現(xiàn)簡單，但因未考慮UWB 信號穿墻等因素引起的非視距（non-line-of-sight，NLOS）誤差，導(dǎo)致定位精度過低。為此部分學(xué)者提出了多技術(shù)融合的定位方法。文獻[7]將UWB 與藍牙相結(jié)合建立目標函數(shù)及其幾何約束條件，并利用粒子群優(yōu)化算法估計目標位置。文獻[8]提出將UWB 與慣性測量單元相融合，利用最大似然估計法消除NLOS 影響，并通過擴展卡爾曼濾波信息融合策略推算目標軌跡。文獻[9]提出將UWB 與WiFi、慣性測量單元相結(jié)合，并通過粒子濾波進行數(shù)據(jù)融合，估算移動目標位置。以上方法雖然在一定程度上提升了定位精度，但融合的定位技術(shù)越多，成本越高，計算越復(fù)雜。為了避免上述問題，減少對定位基礎(chǔ)設(shè)施的依賴，將定位群體之間的內(nèi)在聯(lián)系考慮在內(nèi)，在室外環(huán)境下，文獻[10]提出了一種動態(tài)協(xié)同定位算法，解決了復(fù)雜環(huán)境下可見衛(wèi)星不足導(dǎo)致的動態(tài)用戶無法定位問題。文獻[11]提出了一種利用距離信息進行協(xié)同車輛位置估計的方法，提高了車輛的GPS定位精度。在室內(nèi)環(huán)境下，文獻[12]提出了一種基于UWB測距的協(xié)同定位方法，通過最大限度地利用待定位節(jié)點之間的視距（line-of-sight，LOS）鏈路來提高NLOS 環(huán)境中的定位精度。然而，LOS 鏈路可能被錯誤地識別為NLOS 鏈路，反之亦然，這會嚴重影響定位性能?；诖耍墨I[13]提出了一種基于半定規(guī)劃的協(xié)作定位算法，緩解了NLOS誤差的影響，但在強NLOS環(huán)境下，定位性能的改善并不明顯。文獻[14]提出了一種分布式協(xié)同定位方法，為在無特征環(huán)境中定位一組機器人提供了解決方案，但每次定位至少需要一個機器人作為臨時地標保持靜止。文獻[15]提出了一種基于擴展卡爾曼濾波的移動群體定位算法，利用移動群體之間的距離信息提升定位性能。雖然這種方法可以相對提高定位精度，但在NLOS 環(huán)境下得到的測量值不準確時，目標的估計位置往往偏離實際位置。

本文結(jié)合UWB技術(shù)的優(yōu)良特性和協(xié)同技術(shù)在群體定位中的性能增強特性，提出一種基于UWB 的消防員室內(nèi)協(xié)同定位算法，在待定位消防員群體之間建立通信連接，以降低定位對UWB 基站分布密度高或者通信半徑長的要求，最大程度地使用整個協(xié)同網(wǎng)絡(luò)的資源，從而獲取更多的定位參考信息，對深入火場內(nèi)部的消防員因障礙物遮擋導(dǎo)致的定位困難或定位不準問題予以解決。首先，通過線性擬合方式對UWB 測量距離進行預(yù)處理，以減小距離中的標準偏差。其次，設(shè)計了一種基于偏移擴展卡爾曼濾波的協(xié)同定位算法，該算法將待定位目標看作一個整體，每個目標都可作為彼此的移動參考節(jié)點輔助定位，以增加定位參考信息，提高定位穩(wěn)定性。此外，通過判定距離測量殘差的正負，對卡爾曼增益做出調(diào)整，如果為正，則下調(diào)卡爾曼增益以削弱狀態(tài)預(yù)測值往正向修正的程度，如果為負，則上調(diào)卡爾曼增益以削弱狀態(tài)預(yù)測值往負向修正的程度，將偏離的位置估計值拉回到正確方向，緩解NLOS 誤差，實現(xiàn)目標的初步定位。最后，設(shè)計了一種基于閾值篩選的均值濾波算法，如果定位目標在某一時刻的位移超出設(shè)定閾值，則將該時刻的定位坐標舍棄，將濾波窗口內(nèi)其他定位坐標的均值作為該時刻的濾波輸出值，以消除定位坐標跳變問題。

1 UWB測距及協(xié)同定位原理

1.1 UWB測距原理

通過UWB模塊獲得可靠的測距信息是保證消防員室內(nèi)精確定位的前提。圖1 為三消息雙邊雙向測距（double-sided two-way ranging，DS-TWR）法的測距原理圖，其中RMARKER 是發(fā)送/接收時間戳，用于標記UWB 模塊每一次發(fā)送/接收數(shù)據(jù)的時刻。模塊A 作為測距的發(fā)起者在Ta1時刻首次啟動消息發(fā)送幀，發(fā)送一個Poll消息，之后模塊B在Tb1時刻接收到該消息，并經(jīng)過Treply1后在Tb2發(fā)送一個Response 消息，以響應(yīng)該消息發(fā)送幀，模塊A 在Ta2時刻接收到該響應(yīng)消息，并經(jīng)過Treply2后在Ta3發(fā)送一個Final消息，請求結(jié)束本次測距，模塊B在Tb3時刻接收到消息后，整個測距過程結(jié)束。最終得到4個時間差數(shù)據(jù)Tround1、Treply1、Tround2、Treply2。

圖1 三消息雙邊雙向測距Fig.1 DS-TWR with 3 messages

從而可計算出UWB信號的飛行時間[16]：

由此可得到模塊A 和模塊B 之間的距離dAB=c×Tfab，其中c為光速。

在實際環(huán)境中，由于UWB脈沖信號是以光速傳播，顯然信號在傳播過程中會受到其他因素影響，導(dǎo)致測距結(jié)果不理想，其中NLOS誤差是造成定位精度下降的主要原因之一。在k時刻，兩個UWB 模塊之間的測量距離d(k)可表示為：

其中，r為真實距離值；e為標準測量誤差；εNLOS為非視距誤差，一般認為它是一個正的隨機變量，在不同的信道環(huán)境下有可能服從高斯、指數(shù)、均勻或δ分布。

為了減小標準測量誤差e，可對UWB 測距數(shù)據(jù)進行一次線性擬合，并利用擬合優(yōu)度判定系數(shù)R2來判斷擬合優(yōu)劣[17]：

其中，y為真實測量距離；Y為擬合距離；R2的取值范圍為（0，1），R2越小，擬合得越差，R2越大，擬合得越好。

當UWB 模塊之間有障礙物遮擋時，由于NLOS 誤差εNLOS的存在，使得測量距離變大，如果不進行消除，會使定位精度明顯下降。文中將通過第2 章提出的定位算法間接消除非視距誤差，提高定位精度。

1.2 協(xié)同定位原理

復(fù)雜的火場環(huán)境通常會存在定位盲區(qū)，僅依靠數(shù)量有限的基站進行定位既不能保證定位準確性，又不能保證定位覆蓋率。與傳統(tǒng)定位方式相比，協(xié)同定位充分考慮到消防員團體作戰(zhàn)的特性，在定位過程中，基站充當固定參考節(jié)點，標簽充當彼此的移動參考節(jié)點，使用標簽到基站、標簽到標簽的距離信息來估計消防員的位置。

圖2 是以三基站為例的定位方式示意圖。圖2（a）是傳統(tǒng)定位方式，圖2（b）是協(xié)同定位方式，圖中A1、A2、A3 代表定位基站，虛線圓代表基站的通信范圍，T1、T2、T3 代表待定位標簽。圖2（a）中待定位標簽T1在基站A1、A2、A3的通信范圍內(nèi)，根據(jù)傳統(tǒng)的三邊定位法，實現(xiàn)定位的前提是要知道標簽和至少3個已知基站間的距離信息，故T1的位置能唯一確定。而T2和T3分別在兩個基站的通信范圍內(nèi)，即只知道標簽和兩個基站的距離信息，不滿足傳統(tǒng)三邊定位方法的條件，因此T2、T3不能保證被定位。與圖2（a）不同的是，圖2（b）的T1、T2、T3 能夠相互通信測距，一旦T1 的位置確定后，便可作為移動參考節(jié)點輔助T2、T3定位。

圖2 傳統(tǒng)定位方式與協(xié)同定位方式示意圖Fig.2 Schematic diagram of traditional positioning mode and collaborative positioning mode

2 基于偏移擴展卡爾曼濾波和改進均值濾波的協(xié)同定位算法

下面將通過基于偏移擴展卡爾曼濾波和改進均值濾波的協(xié)同定位算法對消防員進行定位，并對定位過程中出現(xiàn)的NLOS 誤差及定位坐標跳變問題予以緩解。首先建立適合多目標協(xié)同場景的狀態(tài)模型和量測模型；然后根據(jù)此模型推導(dǎo)出可緩解NLOS 誤差的偏移擴展卡爾曼濾波算法；最后通過分析目標運動的特征，推導(dǎo)出基于閾值篩選的均值濾波算法，改善定位坐標跳變問題。

2.1 系統(tǒng)狀態(tài)模型

給定一個由M個UWB 基站和N個UWB 標簽組成的二維協(xié)同定位網(wǎng)絡(luò)，其中基站是位置已知的固定錨節(jié)點，其坐標向量記為xa∈R2,a=1,2,…,M，UWB 標簽是待定位的移動目標節(jié)點，其坐標向量記為xi∈R2,i=1,2,…,N。假設(shè)消防員將標簽攜帶在頭盔上，并保持勻速行走，則第i個移動標簽在k時刻的狀態(tài)向量可表示為：

假設(shè)標簽i在運動過程中受到的隨機擾動為ui(k)=[uxi(k),uyi(k)]T,且，可以將系統(tǒng)表示為[18]：

其中，pxi(k)、pyi(k)、vxi(k)、vyi(k)分別代表標簽i在k時刻的橫坐標、縱坐標、x軸方向的速度分量、y軸方向的速度分量；ΔT為采樣時間間隔。根據(jù)式（5），可將系統(tǒng)的狀態(tài)方程表示為：

式中，φ為狀態(tài)轉(zhuǎn)移矩陣，τ為噪聲驅(qū)動矩陣，u的協(xié)方差為q，定義如下：

其中，和的取值與環(huán)境有關(guān)，標簽i的過程噪聲協(xié)方差為Qi=τiqiτiT。

基于以上對單個標簽的分析，N個相互協(xié)同的移動標簽在時刻k的狀態(tài)向量可以表示為：

系統(tǒng)狀態(tài)方程可表示為：

其中，系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣Φ可表示為：

系統(tǒng)過程噪聲W的協(xié)方差矩陣Q可表示為：

式（12）、（13）中的0 表示4階零矩陣。

2.2 系統(tǒng)量測模型

協(xié)同定位所需的距離測量值來自于標簽i和基站a之間的測量距離ria，以及標簽i和標簽j之間的測量距離dij，文中取距離的平方作為觀測量，則在k時刻，M個UWB 基站和N個UWB 標簽之間可形成維數(shù)為的測量向量Z(k)：

式（14）可通過下式求得：

其中，‖ ? ‖表示歐氏距離，xa=(lxa,lya)，xi=(pxi,pyi)，xj=(pxj,pyj) 分別表示基站a(a=1,2,…,M)、標簽i,j(i,j=1,2,…,N且i≠j) 在直角坐標系下的真實坐標。假定via、vij分別表示標簽i和基站a、標簽j之間的距離測量噪聲，且，δR的取值由UWB 模塊的測量精度決定，則系統(tǒng)的量測方程可以表示為：

其中，V～N(0,R)，R為測量噪聲協(xié)方差矩陣：

通過泰勒級數(shù)展開可將非線性量測方程（17）轉(zhuǎn)化為線性量測方程，即：

2.3 偏移擴展卡爾曼濾波算法

擴展卡爾曼濾波算法的基本思想就在于利用2.1和2.2 節(jié)構(gòu)造的狀態(tài)模型和量測模型，通過預(yù)測和更新兩個階段把定位目標的狀態(tài)值與當前時刻的測量值和狀態(tài)預(yù)測值聯(lián)系起來，從而估計出目標的位置，具體步驟如下：

在預(yù)測階段，根據(jù)移動目標群組k-1 時刻的狀態(tài)和誤差協(xié)方差預(yù)測其在k時刻的狀態(tài)和誤差協(xié)方差：

然后根據(jù)式（20）得到的預(yù)測狀態(tài)計算出系統(tǒng)在k時刻的觀測矩陣和測量殘差：

再由式（21）、（22）計算出k時刻的卡爾曼增益：

在更新階段，由式（20）～（24）可計算出目標在k時刻的狀態(tài)估計和誤差協(xié)方差矩陣：

擴展卡爾曼濾波算法對測量值的準確度要求較高，在視距環(huán)境下還能表現(xiàn)出優(yōu)良的跟蹤性能[19]，一旦UWB信號受到障礙物遮擋，UWB 模塊間的距離測量值就會產(chǎn)生較大的正向偏移，此時將距離值直接代入該算法必然會降低定位精度。為此文中將常規(guī)擴展卡爾曼濾波算法改進為偏移擴展卡爾曼濾波算法，通過調(diào)整卡爾曼增益把偏離的狀態(tài)估計值拉回到正確的方向上[20]。具體做法是在式（24）和（25）之間加入如下公式：

ξ的推導(dǎo)過程如下：

在式（23）中，測量殘差向量中的元素可表示為yres(k)=[yres1(k),yres2(k),…,yresi(k),…,yresn(k)]T,i=1,2,…,n,n為總的測量值個數(shù)，則：

其中0<α<1，β≥1。

在式（28）中，先對元素yresi進行符號判斷，如果yresi>0，代表狀態(tài)估計值會大于狀態(tài)預(yù)測值，可通過系數(shù)α上調(diào)卡爾曼增益以削弱預(yù)測值往正向修正的程度，如果yresi<0，代表狀態(tài)估計值會小于狀態(tài)預(yù)測值，可通過系數(shù)β下調(diào)卡爾曼增益以削弱預(yù)測值往負向修正的程度；然后將得到的n個增益系數(shù)構(gòu)造成式（29）所示的系數(shù)矩陣ξ；再通過式（27）用新的卡爾曼增益代替原來的卡爾曼增益，把估計值往正確的方向修正，從而達到緩解非視距誤差的目的。另外，式（28）中α和β的取值與實驗環(huán)境密切相關(guān)，且關(guān)系到定位算法的定位性能。

2.4 改進均值濾波算法

均值濾波算法是一種依據(jù)圖像局部統(tǒng)計信息對圖像進行濾波的方法，可有效消除圖像中的高斯椒鹽混合噪聲[21]?？紤]到火場環(huán)境中測量距離受到隨機因素影響產(chǎn)生的隨機誤差與圖像的噪聲特點有相似之處，故提出用均值濾波算法對基于偏移擴展卡爾曼濾波的協(xié)同定位算法得到的定位結(jié)果進行二次優(yōu)化，以達到進一步緩解定位坐標跳變問題，使定位結(jié)果更加平滑，更加逼近真實值。

經(jīng)典均值濾波的基本原理為：

其中，x(k)為濾波中心點，k=1,2,…,T;(k)為經(jīng)過均值濾波后的結(jié)果；L=2l+1 為均值濾波窗口的大小。經(jīng)典均值濾波本質(zhì)上是在濾波窗口范圍內(nèi)，將隨機噪聲產(chǎn)生的誤差由濾波窗口內(nèi)的各個點均分承擔，使數(shù)據(jù)變得相對平滑，這對抑制誤差具有一定效果，但是并不能真正消除誤差。

考慮到正常情況下人員的行走狀態(tài)，移動目標節(jié)點i從k-1 時刻到k時刻的位移應(yīng)相對穩(wěn)定，即：

其中，xi(k-1)、xi(k)為目標節(jié)點在k時刻、k-1 時刻的定位坐標；Uth為位移閾值上限。

如果出現(xiàn)位移較大的情況，很有可能該時刻的定位結(jié)果受到了隨機噪聲的影響。為此，本文提出基于閾值篩選的改進均值濾波算法，以提高移動目標節(jié)點的定位精度。具體做法是當k-1時刻到k時刻的位移滿足式（31）時，對基于偏移擴展卡爾曼濾波的協(xié)同定位算法得出的定位坐標直接進行均值濾波處理；當不滿足式（31）時，將k時刻的定位坐標舍棄，將窗口內(nèi)其他定位坐標的均值作為k時刻目標節(jié)點i的濾波輸出值。改進算法公式表示如下：

2.5 算法流程

基于偏移擴展卡爾曼濾波和改進均值濾波的協(xié)同定位算法通過不斷的循環(huán)迭代，實時估計移動目標節(jié)點位置，具體算法分為兩部分：

第一部分是根據(jù)偏移擴展卡爾曼濾波算法初步估計移動目標的位置坐標。由2.3 節(jié)式（20）、（21）首先預(yù)測出目標節(jié)點在k時刻的狀態(tài)和誤差協(xié)方差；然后根據(jù)系統(tǒng)觀測矩陣計算出卡爾曼增益，并通過系數(shù)矩陣調(diào)整卡爾曼增益；在更新階段便可根據(jù)當前的狀態(tài)預(yù)測值、卡爾曼增益、測量殘差估計出目標在k時刻的最佳狀態(tài)和誤差協(xié)方差矩陣，并為下一次狀態(tài)預(yù)測和更新做準備。

第二部分是二次優(yōu)化移動目標的位置坐標。在初步估計出一個時刻的移動目標狀態(tài)后，累計一個濾波窗口長度，當累計窗口長度L未達到預(yù)設(shè)要求時，返回第一部分進行下一次狀態(tài)估計，直至達到要求，就可以進入基于閾值篩選的改進均值濾波算法環(huán)節(jié)。假如濾波窗口長度達到要求的時刻是k，那么需要判斷移動目標節(jié)點從k′-1 到k′時刻(k′=k-(L-1)/2)的位移是否在預(yù)設(shè)的閾值范圍內(nèi)。然后根據(jù)2.4節(jié)式（32）進行相應(yīng)的濾波處理，處理完成后，將濾波窗口中數(shù)據(jù)隊列的隊首去掉。此時窗口長度已不滿足要求，如果定位時間未到，那么再次通過窗口長度判斷，返回第一部分估計出最新時刻的移動目標節(jié)點位置，將其加入到隊尾，周而復(fù)始，達到滑動濾波的效果。詳細算法流程如圖3所示。

圖3 定位算法流程圖Fig.3 Flow chart of positioning algorithm

3 實驗及分析

3.1 仿真實驗及結(jié)果分析

3.1.1 仿真實驗配置

圖4為仿真實驗的定位場景示意圖，定位區(qū)域是一個20 m×26 m 的長方形區(qū)域，4 個UWB 基站A0～A3 分別位于長方形區(qū)域的4個角，其平面坐標分別是(-1,-1)、(19,-1)、(19,25)、(-1,25)。UWB 基站和標簽的通信半徑為35 m，5個標簽T0～T4分別按照預(yù)先設(shè)計好的L形、三角形、矩形、蝶形、圓弧與直線相混合的路線從坐標原點（0，0）開始移動。其中標簽T0 模擬消防員在行走過程中突然靜止不動，其他標簽?zāi)M消防員正常行走。假設(shè)在該場景下，所有節(jié)點都是連通的，那么基站到標簽、標簽到標簽之間總共有30條通信路徑，且基站2和基站3處于非視距狀態(tài)，所有和這兩個基站連通的路徑都屬于非視距路徑，總共有10條。為了簡便起見，仿真實驗中使用的UWB測距值是在真實距離值上添加噪聲得到的。其中視距路徑下的測距值是在真實距離值上添加了均值為0，方差為0.4的高斯白噪聲，非視距路徑下的測距值在真實距離值上除添加了高斯白噪聲之外，還添加了0.5～1.5 m 的隨機噪聲，仿真采樣時間間隔ΔT為1 s，總的采樣點數(shù)為120個，5個標簽的初始狀態(tài)都設(shè)為[0,0,0,0]T，即，初始誤差協(xié)方差矩陣P(0)=10-6I20×20,過程噪聲協(xié)方差矩陣Q中的Q1=，測量噪聲協(xié)方差矩陣R=0.2I30×30,I為單位矩陣，增益系數(shù)α=0.2，β=1，滑動均值濾波窗口長度L設(shè)為3。

圖4 定位場景示意圖Fig.4 Schematic diagram of positioning scene

3.1.2 仿真結(jié)果及分析

圖5展示的是5個定位標簽T0～T4在4種定位算法下解算出的行走軌跡，紅線代表真實軌跡，綠線代表不同算法解算出的行走軌跡。其中TRI、EKF分別是基于傳統(tǒng)固定基站的三邊定位算法、擴展卡爾曼濾波算法，CO-EKF 是基于擴展卡爾曼濾波的協(xié)同定位算法，COBEKF-IMF 是本文提出的基于偏移擴展卡爾曼濾波和改進均值濾波的協(xié)同定位算法。

圖5 各標簽的定位軌跡Fig.5 Positioning trajectory of each tag

通過對比發(fā)現(xiàn)，TRI算法解算出的定位坐標波動明顯，與真實軌跡偏差較大，說明該算法的抗噪聲能力較差；EKF 算法在解算當前時刻的位置坐標時，利用了前一時刻的定位信息和當前時刻的測量信息、預(yù)測信息，使得前后時刻的定位聯(lián)系更加緊密，減少了定位突變點的產(chǎn)生，但與真實軌跡仍有差距；CO-EKF 算法在EKF的基礎(chǔ)上，將待定位標簽T0～T4 看作一個整體，充分利用了UWB 基站和標簽之間、標簽和標簽之間的測距信息，最大程度地使用了整個協(xié)同網(wǎng)絡(luò)的定位資源，使得定位參考信息增多，定位結(jié)果得到進一步改善，但從圖中可以看出仍未達到理想水平；本文提出的CO-BEKFIMF 算法在CO-EKF 算法的基礎(chǔ)上考慮了測量距離中非視距誤差的特性，根據(jù)測量信息和預(yù)測信息的大小，通過系數(shù)矩陣調(diào)整卡爾曼增益，有效抑制了非視距誤差對定位結(jié)果的影響，實現(xiàn)了初步定位，為了使定位軌跡更加平滑，又通過基于閾值篩選的改進均值濾波算法，丟棄異常偏差值，對定位坐標做了二次優(yōu)化，從圖中可以看出，所提算法解算出的定位軌跡最貼近真實軌跡。

圖6給出了不同算法下各標簽的均方根定位誤差，CO-BEKF-IMF算法與TRI、EKF、CO-EKF算法相比，分別降低了47.4%～57.7%、38.6%～48.4%、27.6%～38.9%，證實了所提算法在多節(jié)點協(xié)同、偏移擴展卡爾曼濾波和改進均值濾波的相互作用下，可有效抑制噪聲對定位結(jié)果帶來的不良影響，定位精度得到了提升。

圖6 標簽與基站正常通信下各算法的均方根定位誤差Fig.6 Root mean square positioning error of each algorithm under normal communication between tags and base stations

圖7是各算法下5個定位標簽的平均定位誤差隨非視距誤差增大的變化情況。從圖中可以看出，非視距誤差越大，4種算法的平均定位誤差也越來越大，這是由各節(jié)點間的距離信息準確度不斷下降造成的，但相比較而言，CO-BEKF-IMF 表現(xiàn)出了更好的抗非視距誤差性能。在0～1.0 m的非視距誤差下，與TRI、EKF、CO-EKF相比，CO-BEKF-IMF 算法的平均定位誤差分別降低了55.1%、41.5%、31.7%；在0.5～1.5 m 的非視距誤差下，分別降低了54.2%、44.7%、36%；在1.0～2.0 m 的非視距誤差下，分別降低了51.8%、44.2%、36.2%；在1.5～2.5 m 的非視距誤差下，分別降低了50.3%、43.4%、36.4%；在2.0～3.0 m 的非視距誤差下，分別降低了47.9%、42.9%、34.5%。

圖7 不同非視距誤差下的平均定位誤差Fig.7 Average positioning error under different NLOS error

圖8 是各算法的平均定位誤差隨標簽數(shù)量增加的變化情況。通過對比發(fā)現(xiàn)，TRI和EKF算法的平均定位誤差較大，但波動較小，原因是這兩種算法只利用了基站到標簽之間的測距信息，標簽數(shù)量的增加對定位結(jié)果基本沒有產(chǎn)生影響。CO-EKF和CO-BEKF-IMF算法隨著標簽數(shù)量的增加，平均定位誤差逐漸減小，且COBEKF-IMF算法的定位誤差更小。從圖中還可以看出，當標簽數(shù)量增加到10 個時，所提算法的平均誤差基本趨于穩(wěn)定，說明參與協(xié)同定位的標簽增加到一定數(shù)量時，由于可參考定位信息足以獲得最佳定位性能，再增加標簽數(shù)量基本不會影響定位結(jié)果。

圖8 不同標簽數(shù)量下的平均定位誤差Fig.8 Average positioning error under different number of positioning tags

為了更進一步說明所提算法的抗非視距性能，圖9展示了各種算法的平均定位誤差隨非視距路徑增加的變化情況。從圖中可以看出，隨著非視距路徑的增加，TRI、EKF算法的平均定位誤差呈不斷上升趨勢，但因為兩種算法不涉及標簽和標簽通信測距，所以在非視距路徑數(shù)達到20 條后，再增加標簽到標簽之間的非視距路徑對它們沒有影響；CO-EKF算法雖與前兩種算法相比定位誤差有所下降，但并未很好地抑制非視距誤差；CO-BEKF-IMF算法的平均定位誤差與其他3種算法相比大幅下降，有效抑制了非視距誤差，定位穩(wěn)定性更好。

圖9 不同非視距路徑下的平均定位誤差Fig.9 Average positioning error under different NLOS paths

以上從多個角度證明了如果T0～T4在4個UWB基站的通信范圍內(nèi)，通過CO-BEKF-IMF算法，可使T0～T4的定位精度得到有效提高。但在實際環(huán)境中，定位標簽不一定在所有基站的通信范圍內(nèi)，假如T3 超出了A1、A2 的通信范圍，在基站A0、A3 的左側(cè)從(-16,-1) 到(-16,14)做往返直線運動；T4 超出了A2、A3 的通信范圍，在基站A0、A1的下側(cè)從(-1,-10)到(8,-10)做往返直線運動，此時就需要通過兩階段CO-BEKF-IMF 算法才能解算出T3、T4的位置。首先根據(jù)T0、T1、T2到4個固定基站的UWB 測距值以及它們相互之間的距離信息，利用CO-BEKF-IMF 算法解算出T0、T1、T2 的精確位置；然后選擇能和T3、T4 可靠通信的T0 作為虛擬信標，再根據(jù)T3到T0、固定基站A0、A3的距離，T4到T0、固定基站A0、A1的距離，以及T3、T4之間的距離，再次利用CO-BEKF-IMF算法便可解算出T3、T4的位置。

圖10是幾種算法的均方根定位誤差對比圖。由于標簽T3、T4分別只能和兩個固定基站通信測距，傳統(tǒng)的基于固定基站的TRI算法已無法適用；EKF算法雖能解算出T3、T4 的坐標，但是產(chǎn)生的定位誤差較大；采用CO-EKF算法雖然可以緩解這一問題，但由于已定位的T0存在的定位誤差較大，在升級為虛擬信標協(xié)助T3、T4定位時，會將誤差代入到定位算法中，導(dǎo)致解算出的坐標誤差也較大；兩階段CO-BEKF-IMF 算法在第一階段已將T0 的誤差降低到較小的程度，因此在第二階段由T0協(xié)助定位T3、T4時產(chǎn)生的誤差也較小，與EKF、CO-EKF算法相比，分別降低了39.1%～44.6%、32.4%～33.8%。

圖10 標簽與基站非正常通信下各算法的均方根定位誤差Fig.10 Root mean square positioning error of each algorithm under abnormal communication between tags and base stations

圖11 對比了T3、T4 在帶有和不帶誤差的虛擬信標T0 協(xié)助下，采用CO-BEKF-IMF 算法解算出的行走軌跡，以說明虛擬信標的定位精度對被定位目標的影響程度。圖中T0、T1、T2的行走軌跡與前文保持一致。因為有誤差的T0坐標是通過CO-BEKF-IMF算法得到的，所以與無誤差的T0 相比，解算出的T3、T4 的行走軌跡略差一些。

圖11 虛擬信標有無誤差下的定位軌跡Fig.11 Positioning trajectory with or without error of virtual beacon

圖12展示了T3、T4在各采樣點處坐標估計值與真實值的絕對偏差。從整體來看，有誤差的T0 要比無誤差的T0 協(xié)助定位時產(chǎn)生的定位誤差大，但并不是隨著定位時間的增加而逐漸增大的，這說明虛擬信標的加入并未導(dǎo)致誤差累計。

圖12 絕對定位誤差Fig.12 Absolute positioning error

表1 給出了虛擬信標有無誤差兩種情況下所提算法的均方根定位誤差變化情況。與無誤差的虛擬信標相比，有誤差信標協(xié)助定位T3、T4時產(chǎn)生的定位誤差分別增長了9.40%、3.03%，表明虛擬信標的位置越精確，協(xié)助定位其他目標時產(chǎn)生的定位誤差越小。文中所提兩階段CO-BEKF-IMF算法已在第一階段改善了虛擬信標的定位精度，使其在第二階段參與運算時，不會代入較大的誤差。與文中其他算法相比，所提算法不僅提高了目標的定位精度，而且擴大了定位覆蓋范圍，同時也提高了協(xié)同網(wǎng)絡(luò)資源的利用率。

表1 虛擬信標對定位精度的影響分析Table 1 Influence of virtual beacon on positioning accuracy

3.2 實測實驗及結(jié)果分析

3.2.1 實驗設(shè)備和環(huán)境配置

為了驗證所提方法在實際環(huán)境中的可行性和有效性，使用研創(chuàng)物聯(lián)網(wǎng)科技有限公司開發(fā)的UWB Mini4splus 模塊進行數(shù)據(jù)采集，其中的UWB 收發(fā)模組DWM1000是基于Decawave公司DW1000芯片設(shè)計的，主控芯片采用STM32F103T8U6單片機。通過keil軟件的二次開發(fā)，實現(xiàn)了基站與標簽、標簽與標簽之間的通信測距功能，且基站A0 作為匯聚節(jié)點通過串口與計算機相連，負責(zé)將所有測試數(shù)據(jù)匯總到一起。在實驗中，UWB模塊工作電壓為5 V，數(shù)據(jù)采樣頻率設(shè)置為50 Hz，波特率設(shè)置為921 600 bit/s。實驗場地為一間9 m×7 m×4 m的倉庫，倉庫內(nèi)部布局如圖13所示。

圖13 實驗環(huán)境布局Fig.13 Layout of experimental environment

實驗分為兩組：第一組實驗中4 個UWB 基站A0～A3通過支架放置在室內(nèi)4個角落，其平面坐標分別是（0，0）、（0，8）、（6，8）、（6，0）。4 名測試人員將4 個UWB 標簽T0～T3 分別佩戴在頭上，根據(jù)提前規(guī)劃好的矩形路線分別以矩形的4個角為起始點，按逆時針方向同時行走，其中矩形大小為7.0 m×4.5 m。第二組實驗中將第一組實驗的UWB基站A0、A3平移1 m放置在室外，此時A0 的坐標調(diào)整為(0,-1)，A3 的坐標調(diào)整為(6,-1)，其他條件保持不變。從圖13 中可以看出，測試人員在行走過程中，UWB 信號會受人體、柱子、倉儲物品、墻壁、鐵門等的影響。第一組實驗屬于弱非視距環(huán)境，第二組實驗屬于強非視距環(huán)境。為了便于數(shù)據(jù)分析，在Matlab 軟件上根據(jù)匯總的測試數(shù)據(jù)，使用不同的定位算法解算出測試人員的行走軌跡，通過對比分析驗證所提算法的定位性能。實驗的參數(shù)配置與3.1節(jié)仿真實驗一致。

3.2.2 UWB測距值校準

由于UWB模塊自身的精度有限以及環(huán)境因素的影響，導(dǎo)致測量數(shù)據(jù)與真實數(shù)據(jù)存在偏差，為了準確解算出測試人員的位置坐標，在使用定位算法前需對測量值進行校準，以消除UWB模塊的標準測量偏差。

在室內(nèi)無障礙物遮擋的視距環(huán)境下，將UWB 標簽T0與計算機相連固定在一個位置，UWB標簽T1相對于T0 沿直線每0.5 m 移動一次，移動至10 m，總共20 個測距點，每個測距點采樣50 個距離數(shù)據(jù)，用3σ準則剔除粗大數(shù)據(jù)后，取剩余數(shù)據(jù)的平均值作為該點的測量距離，與真實距離進行線性擬合，擬合函數(shù)為：

其中，x是測量距離，y是擬合距離。由式（3）可得出擬合優(yōu)度的判定系數(shù)R2=0.999 7。由于本實驗中所有的UWB 模塊具有相同的結(jié)構(gòu)，故測量距離都可以根據(jù)上述擬合函數(shù)進行校準。圖14 為UWB 標準偏差擬合曲線。從圖中可以看出，測量距離與真實距離之間達到了較為理想的擬合效果。

圖14 UWB標準偏差擬合曲線Fig.14 UWB standard deviation fitting curve

3.2.3 實測結(jié)果及分析

實驗1UWB基站A0～A3在室內(nèi)

因為4名測試人員都是沿著矩形路線行走，且解算出的行走軌跡基本相同，所以只給出圖15 所示的標簽T1的定位軌跡。圖中紅色方塊代表UWB基站，紅線代表真實軌跡，綠線代表不同算法解算出的行走軌跡。測試人員在行走過程中，因UWB信號受到柱子、倉儲物品和人體的阻擋，使得UWB 模塊的發(fā)射和接收信號時間延長，導(dǎo)致模塊間的測量距離加大。與仿真實驗結(jié)果相一致，直接使用傳統(tǒng)的TRI算法解算出的行走軌跡明顯偏離了真實軌跡，定位效果較差；傳統(tǒng)的EKF算法雖減少了定位突變點的產(chǎn)生，但仍然與真實軌跡存在差距；CO-EKF算法使定位結(jié)果得到進一步改善，但仍未達到理想水平；CO-BEKF-IMF 算法解算出的行走軌跡與其他3種算法相比，在軌跡的平滑度以及與真實軌跡的貼合度上達到了最佳水平。

圖15 T1的定位軌跡（實驗1）Fig.15 Positioning trajectory of T1（Experiment 1）

圖16 為定位標簽T0～T3 在4 種定位算法下的均方根定位誤差，CO-BEKF-IMF 算法與TRI、EKF、CO-EKF算法相比，分別降低了22.0%～49.0%、18.4%～20.2%、

圖16 弱非視距環(huán)境下各算法的均方根定位誤差Fig.16 Root mean square positioning error of each algorithm in weak NLOS environment

圖18 為定位標簽T0～T3 在4 種定位算法下的均方根定位誤差，CO-BEKF-IMF 算法與TRI、EKF、CO-EKF算法相比，分別降低了34.0%～48.8%、31.2%～42.3%、19.6%～31.5%，其定位精度在0.28～0.34 m 之間，優(yōu)于其他3種算法，且與實驗1的弱非視距環(huán)境相比，在強非視距環(huán)境下，所提算法表現(xiàn)出了更加顯著的定位性能。

圖18 強非視距環(huán)境下各算法的均方根定位誤差Fig.18 Root mean square positioning error of each algorithm in strong NLOS environment

4 結(jié)束語

為了準確獲取消防員在復(fù)雜火災(zāi)環(huán)境中的位置，提出了一種基于UWB 的消防員室內(nèi)協(xié)同定位算法，較傳統(tǒng)定位方法的改進之處為每個消防員攜帶的定位標簽可以作為其他標簽的移動參考節(jié)點，并且它們自身的定位信息以及相互之間的距離信息都可以用來輔助定12.4%～16.5%，其定位精度在0.17～0.26 m 之間，在弱非視距環(huán)境下能達到比其他3種算法更優(yōu)的定位效果。

實驗2UWB基站A0、A3在室外，A1、A2在室內(nèi)

圖17 展示了在強非視距環(huán)境下由4 種定位算法解算出的標簽T1的定位軌跡。此環(huán)境下，UWB信號不僅受到柱子、倉儲物品和人體的遮擋，還受到了墻壁和鐵門的遮擋。與實驗1的分析類似，TRI、EKF、CO-EKF算法解算出的定位軌跡與真實軌跡相比逐漸改善，但由障礙物遮擋而導(dǎo)致的軌跡偏移問題沒有得到有效解決。而由本文提出的CO-BEKF-IMF 算法改善了這一問題，解算出的定位軌跡與真實軌跡更加貼近。位。通過對超寬帶測量距離進行線性擬合，減小了距離中存在的標準偏差；通過偏移擴展卡爾曼濾波算法，緩解了障礙物遮擋引起的NLOS誤差，實現(xiàn)了目標的初步定位；通過改進的均值濾波算法，進一步優(yōu)化了定位坐標，使定位軌跡更加平滑，更接近真實軌跡。仿真和實測實驗一致驗證了所提算法的良好定位性能。在弱NLOS 環(huán)境下，所提算法的定位精度可達0.17～0.26 m，在強NLOS 環(huán)境下，定位精度可達0.28～0.34 m，明顯優(yōu)于其他定位算法。本文提出的算法為消防員的高精度定位提供了一種有效的解決方案。在未來的研究中，將考慮使用無人機搭載UWB 基站，使其懸停至建筑物四周，以減少基站的部署時間，適應(yīng)真實的火災(zāi)環(huán)境。

圖17 T1的定位軌跡（實驗2）Fig.17 Positioning trajectory of T1（Experiment 2）