基于UWB的消防員室內協同定位算法

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

西安郵電大學 通信與信息工程學院，西安710121

建筑物內火災事故頻發，常需消防員深入火場內部展開滅火救援，如果負責協調調度任務的消防指揮人員能夠獲取消防員的精確位置，那么在消防員自身遭遇險情時，便可通過對講設備通知鄰近消防員施以救助，并為其合理規劃逃生路線，降低消防員的傷亡率。基于此，如何實現對消防員的高精度定位是本文的研究重點。

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

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

1 UWB測距及協同定位原理

1.1 UWB測距原理

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

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

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

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

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

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

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

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

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

1.2 協同定位原理

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

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

圖2 傳統定位方式與協同定位方式示意圖Fig.2 Schematic diagram of traditional positioning mode and collaborative positioning mode

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

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

2.1 系統狀態模型

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

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

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

式中，φ為狀態轉移矩陣，τ為噪聲驅動矩陣，u的協方差為q，定義如下：

其中，和的取值與環境有關，標簽i的過程噪聲協方差為Qi=τiqiτiT。

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

系統狀態方程可表示為：

其中，系統狀態轉移矩陣Φ可表示為：

系統過程噪聲W的協方差矩陣Q可表示為：

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

2.2 系統量測模型

協同定位所需的距離測量值來自于標簽i和基站a之間的測量距離ria，以及標簽i和標簽j之間的測量距離dij，文中取距離的平方作為觀測量，則在k時刻，M個UWB 基站和N個UWB 標簽之間可形成維數為的測量向量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 模塊的測量精度決定，則系統的量測方程可以表示為：

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

通過泰勒級數展開可將非線性量測方程（17）轉化為線性量測方程，即：

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

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

在預測階段，根據移動目標群組k-1 時刻的狀態和誤差協方差預測其在k時刻的狀態和誤差協方差：

然后根據式（20）得到的預測狀態計算出系統在k時刻的觀測矩陣和測量殘差：

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

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

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

ξ的推導過程如下：

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

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

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

2.4 改進均值濾波算法

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

經典均值濾波的基本原理為：

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

考慮到正常情況下人員的行走狀態，移動目標節點i從k-1 時刻到k時刻的位移應相對穩定，即：

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

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

2.5 算法流程

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

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

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

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

3 實驗及分析

3.1 仿真實驗及結果分析

3.1.1 仿真實驗配置

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

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

3.1.2 仿真結果及分析

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

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

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

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

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

圖7是各算法下5個定位標簽的平均定位誤差隨非視距誤差增大的變化情況。從圖中可以看出，非視距誤差越大，4種算法的平均定位誤差也越來越大，這是由各節點間的距離信息準確度不斷下降造成的，但相比較而言，CO-BEKF-IMF 表現出了更好的抗非視距誤差性能。在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 是各算法的平均定位誤差隨標簽數量增加的變化情況。通過對比發現，TRI和EKF算法的平均定位誤差較大，但波動較小，原因是這兩種算法只利用了基站到標簽之間的測距信息，標簽數量的增加對定位結果基本沒有產生影響。CO-EKF和CO-BEKF-IMF算法隨著標簽數量的增加，平均定位誤差逐漸減小，且COBEKF-IMF算法的定位誤差更小。從圖中還可以看出，當標簽數量增加到10 個時，所提算法的平均誤差基本趨于穩定，說明參與協同定位的標簽增加到一定數量時，由于可參考定位信息足以獲得最佳定位性能，再增加標簽數量基本不會影響定位結果。

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

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

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

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

圖10是幾種算法的均方根定位誤差對比圖。由于標簽T3、T4分別只能和兩個固定基站通信測距，傳統的基于固定基站的TRI算法已無法適用；EKF算法雖能解算出T3、T4 的坐標，但是產生的定位誤差較大；采用CO-EKF算法雖然可以緩解這一問題，但由于已定位的T0存在的定位誤差較大，在升級為虛擬信標協助T3、T4定位時，會將誤差代入到定位算法中，導致解算出的坐標誤差也較大；兩階段CO-BEKF-IMF 算法在第一階段已將T0 的誤差降低到較小的程度，因此在第二階段由T0協助定位T3、T4時產生的誤差也較小，與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 協助下，采用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 協助定位時產生的定位誤差大，但并不是隨著定位時間的增加而逐漸增大的，這說明虛擬信標的加入并未導致誤差累計。

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

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

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

3.2 實測實驗及結果分析

3.2.1 實驗設備和環境配置

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

圖13 實驗環境布局Fig.13 Layout of experimental environment

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

3.2.2 UWB測距值校準

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

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

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

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

3.2.3 實測結果及分析

實驗1UWB基站A0～A3在室內

因為4名測試人員都是沿著矩形路線行走，且解算出的行走軌跡基本相同，所以只給出圖15 所示的標簽T1的定位軌跡。圖中紅色方塊代表UWB基站，紅線代表真實軌跡，綠線代表不同算法解算出的行走軌跡。測試人員在行走過程中，因UWB信號受到柱子、倉儲物品和人體的阻擋，使得UWB 模塊的發射和接收信號時間延長，導致模塊間的測量距離加大。與仿真實驗結果相一致，直接使用傳統的TRI算法解算出的行走軌跡明顯偏離了真實軌跡，定位效果較差；傳統的EKF算法雖減少了定位突變點的產生，但仍然與真實軌跡存在差距；CO-EKF算法使定位結果得到進一步改善，但仍未達到理想水平；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 弱非視距環境下各算法的均方根定位誤差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 之間，優于其他3種算法，且與實驗1的弱非視距環境相比，在強非視距環境下，所提算法表現出了更加顯著的定位性能。

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

4 結束語

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

實驗2UWB基站A0、A3在室外，A1、A2在室內

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

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