融合UWB與INS的消防員室內(nèi)定位與NLOS檢測算法

楊 剛，朱士玲，李 強，趙克松

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

0 概述

當火災事故發(fā)生時，常需消防員深入火場內(nèi)部進行滅火救援，為了保障消防員的生命安全以及提高救援效率，需要一款精確且穩(wěn)健的定位系統(tǒng)，使消防指揮人員能夠?qū)崟r查看消防員所在位置。基于信號強度的ZigBee、RFID、WIFI 和計算機視覺技術都可用于消防員室內(nèi)定位，但這些技術的定位精度較低，抗環(huán)境干擾能力較差［1］，難以實現(xiàn)對消防員的精確定位。

超寬帶（Ultra-Wideband，UWB）定位技術具有帶寬寬、定位精度高、穿透力強等特點［2］，在火場環(huán)境下對消防員進行定位具有一定的優(yōu)勢［3-4］。UWB精確定位的前提是定位標簽和基站均處于視距（Line-of-Sight，LOS）環(huán)境中，而消防員在實際火場救援過程中需要不斷地移動位置，標簽和基站之間的通信常處于非視距（Non-Line-of-Sight，NLOS）環(huán)境，會導致較大的定位誤差［5］。若在定位過程中能檢測到處于NLOS 環(huán)境中的基站，則計算坐標時對其進行針對性的處理，便可以實現(xiàn)準確定位。常用的NLOS 信號檢測算法有范圍估計法［6］和信道估計法［7］。然而，上述算法的難點在于不易選取適當?shù)腘LOS 誤差模型和恰當?shù)拈撝怠?/p>

慣性導航系統(tǒng)（Inertial Navigation System，INS）無需依靠任何外界設備，即可實現(xiàn)獨立自主定位［8］。行人軌跡推測（Pedestrian Dead Reckoning，PDR）算法依靠慣性測量單元（Inertial Measurement Unit，IMU）計算行人行走過程中的步頻、步長和航向數(shù)據(jù)，根據(jù)初始位置推算出行人的位置坐標。但是基于PDR 算法的慣性定位的誤差隨時間延長而不斷增加，因此無法進行長時間的精確定位［9］。

單一定位方式具有局限性，而多技術組合則可以提高定位的可靠性和精確性。文獻［10］提出一種基于優(yōu)化貝葉斯的WiFi 與藍牙融合定位算法，彌補了單獨定位時信標覆蓋率及定位精度低的缺點。文獻［11］提出WiFi 和PDR 融合定位算法，雖然在一定程度上解決了室內(nèi)WiFi 定位精度低及PDR 定位誤差累積問題，但定位精度仍在米級以上。文獻［12］將UWB 與PDR 相融合推算行人行走軌跡，利用相鄰時刻UWB 的測距差值構造卡爾曼濾波器的測量噪聲矩陣，自適應地改變UWB 與PDR 的定位比重，減小狀態(tài)預測引入的誤差。文獻［13］設計了一種融合INS/UWB 的低成本行人追蹤系統(tǒng)，只依靠1 個UWB 基站參與定位，在跟蹤的初始階段，利用慣導跟蹤算法估計基站位置，將超寬帶距離測量值與慣性導航數(shù)據(jù)融合后再定位。以上2 種方法雖然成本低廉，但定位的可靠性不高。文獻［14］采用UWB/INS 緊組合導航方法，將UWB 基站的位置信息引入到系統(tǒng)狀態(tài)變量中，減少了觀測量對基站信息的依賴，提高了組合導航系統(tǒng)的靈活性和估計精度，但對UWB 的NLOS 誤差緩解效果并不明顯。基于此，文獻［15］構建了一種自適應UWB 測距不確定模型，該模型根據(jù)實時測量質(zhì)量自動調(diào)整測量距離的數(shù)學誤差模型，并將其應用于粒子濾波融合算法中，降低了超寬帶測量誤差，提高了定位精度。但由于使用粒子濾波［16］進行融合定位時可能會發(fā)生軌跡丟失問題，因此文獻［17］提出了粒子重置方法，根據(jù)測量距離來評估行人靠近UWB 基站時粒子集的均方根誤差，對粒子狀態(tài)進行修正。

為了更可靠地評估UWB 測量距離的效果，文獻［18］提出了一種決策算法，該算法根據(jù)信道沖激響應數(shù)據(jù)和隨機森林算法評估UWB 測量距離的可用性，并在視距環(huán)境下使用卡爾曼濾波融合2 個系統(tǒng)的定位信息。為了補償INS 的位置誤差，文獻［19］利用最小二乘支持向量機輔助無偏有限脈沖響應濾波器，以融合UWB 和INS 系統(tǒng)的定位信息，實現(xiàn)對室內(nèi)行人的無縫跟蹤。以上2 項研究雖然都提高了系統(tǒng)的融合精度，但在UWB 距離評估以及數(shù)據(jù)訓練階段花費時間過長，影響定位系統(tǒng)的實時性。為了提高運算效率，文獻［20］提出一種基于移動平均濾波器的IMU/UWB 融合算法，使用UWB 和IMU數(shù)據(jù)生成當前位置的虛擬樣本，并通過平均虛擬樣本值來找到待定位目標的最佳位置估計。文獻［21］對慣性傳感器的隨機誤差進行分析和預濾波，并利用互補濾波器提供導航系統(tǒng)的姿態(tài)信息，在非視距環(huán)境下，通過反磁環(huán)消除UWB 系統(tǒng)的異常值，采用雙態(tài)自適應卡爾曼濾波器提高UWB 和INS 系統(tǒng)的融合定位精度。文獻［22］使用迭代卡爾曼濾波器融合UWB 和IMU 系統(tǒng)，并提出一種三角形不等式定理來檢測NLOS 測量值，若t-1 時刻標簽到基站的距 離、t-1 到t時刻標 簽移動 的距離、t時 刻UWB 測量的標簽到基站的距離滿足三角不等式定理，則判定t時刻UWB 的距離測量值處于LOS 環(huán)境中，否則判定處于NLOS 環(huán)境中。該算法在NLOS 誤差較大時檢測性能較好，但當NLOS 誤差較小時容易出現(xiàn)誤檢和漏檢的現(xiàn)象，導致定位結(jié)果不穩(wěn)定。

本文使用雙級EKF 以松耦合［23］的方式融合UWB 和INS 的定位結(jié)果，并基于INS 提出一種針對UWB 的NLOS 檢測算法，通過動態(tài)調(diào)整融合濾波器的測量誤差矩陣來緩解NLOS 環(huán)境下異常UWB 測量值對定位結(jié)果的影響。

1 消防員室內(nèi)定位系統(tǒng)

消防員室內(nèi)定位系統(tǒng)主要由UWB 定位模塊和INS 定位模塊構成，如圖1 所示。當火災發(fā)生時，使用掛載UWB 基站的無人機懸停在建筑物窗戶外圍，通過特制的定向天線向建筑物內(nèi)部發(fā)射UWB 脈沖。消防員胸前佩戴的定位終端集成了UWB 定位標簽和MPU9250 慣性傳感器，分別用于獲取行走過程中標簽到各個基站的距離和慣性數(shù)據(jù)。通過數(shù)據(jù)融合算法實現(xiàn)消防員的精確定位。

圖1 消防員室內(nèi)定位系統(tǒng)框架Fig.1 System framework of indoor positioning of firefighters

定位系統(tǒng)采用的是東北天（East North Up，ENU）導航坐標系，其中的X、Y、Z軸分別為正東、正北和法線方向。為簡單起見，本文只考慮二維定位。

1.1 INS 模塊

由慣性測量單元集成的INS 模塊包含3 軸加速度計、3 軸陀螺儀和3 軸磁力計。消防員將INS 模塊佩戴在胸前，根據(jù)行走時慣性傳感器輸出的加速度值、角速度值和磁場強度值，使用四元數(shù)求出航向角，根據(jù)加速度幅值的變化趨勢使用峰值檢測法求出步數(shù)，根據(jù)每一步的加速度最大值和最小值使用非線性步長模型求出步長。最后根據(jù)求出的步長、步數(shù)和航向角，使用PDR 算法結(jié)合EKF 求出消防員的初始位置坐標［8］。

PDR 算法是根據(jù)當前時刻的位置坐標(Ek,Nk)，結(jié)合步長Sk和航向角θk推算出下一步行人的位置坐標(Ek+1,Nk+1)，具體過程如圖2 所示。

圖2 PDR 算法模型Fig.2 PDR algorithm model

1.2 UWB 模塊

UWB 定位系統(tǒng)由佩戴在用戶身上的標簽和若干個固定在已知位置上的基站組成，標簽和基站之間使用射頻脈沖進行通信。根據(jù)TDOA 算法，通過標簽到基站之間的信號傳播時間差，便可計算出標簽到各個基站的距離差。

在實際應用中，UWB 設備的測量結(jié)果會受到測量誤差和NLOS 誤差的影響。如果基站和標簽之間處于NLOS 環(huán)境中，則相應的信號傳輸時間會增加，所得到的距離差值存在偏差。如圖3 所示，2 條雙曲線的相交部分不一定是標簽的準確位置，NLOS 誤差越嚴重，最終的定位誤差越大。

圖3 TDOA 算法真實情況下的幾何圖Fig.3 Geometry of TDOA algorithm in real condition

測量誤差來源于硬件系統(tǒng)自身的精度，且較為穩(wěn)定。而NLOS 誤差取決于具體定位環(huán)境的變化，可能產(chǎn)生巨大的波動。因此，本文提出一種雙級EKF 框架實現(xiàn)UWB 和INS 的數(shù)據(jù)融合，在融合的過程中借助INS 的定位結(jié)果對含有NLOS 誤差的UWB距離測量值進行檢測和處理，以提高定位系統(tǒng)的抗NLOS 干擾能力。

融合算法包含2 級EKF：第1 級EKF 用于INS，根據(jù)前一時刻的聯(lián)合位置估計值，使用第1 級EKF計算出當前時刻消防員的初始坐標估計值并轉(zhuǎn)化為到各個基站的距離值，將此距離值和當前時刻UWB的距離測量值相減并取絕對值，然后與預設閾值相比較。若大于閾值，則判定當前時刻的UWB 距離測量值存在異常，極有可能是NLOS 干擾；若小于則判定正常，對正常基站和異常基站的UWB 距離測量值分別計算殘差值并構成殘差矩陣。第2 級EKF 用于數(shù)據(jù)融合，第1 級EKF 所得的殘差矩陣用于調(diào)整第2 級EKF 的測量噪聲矩陣，以此提高定位系統(tǒng)抗NLOS 干擾的能力，使用第1 級EKF 的預測過程并結(jié)合第2 級EKF 的測量過程得到UWB 和INS 的聯(lián)合位置估計值。

2 NLOS 檢測算法與UWB/INS 的數(shù)據(jù)融合

本文所提的松耦合數(shù)據(jù)融合方式可以利用任意數(shù)量的UWB 測量距離值，以增加定位系統(tǒng)的健壯性。最終的定位結(jié)果皆為融合定位所得，根據(jù)測量噪聲殘差矩陣自適應定位環(huán)境的變化，避免了定位模式的硬切換，增加了系統(tǒng)的實用性。基于迭代的卡爾曼濾波器使整個系統(tǒng)具備較高的實時性。

UWB 的NLOS 檢測與UWB/INS 的數(shù)據(jù)融合在雙級EKF 框架下完成。利用INS 短時間內(nèi)可精確定位的優(yōu)勢，將其應用于UWB 的NLOS 檢測中。根據(jù)檢測結(jié)果計算殘差矩陣調(diào)整融合濾波器的測量噪聲矩陣。第1 級EKF 所得的初始估計坐標僅用于NLOS 檢測，并未用于反饋EKF 的預測過程，所以INS 產(chǎn)生的誤差并不會累積。在第2 級EKF 中，INS測量值和較為準確的UWB 測量值都會被輸入到融合濾波器中，INS 的誤差在此過程中得到不斷修正。總體而言，整個雙級EKF 框架在利用INS 定位優(yōu)勢的基礎上很好的抑制了INS 的誤差累積。

雙級EKF框架如圖4所示，第1級EKF用于INS，可得到當前消防員的初始位置估計坐標使用此坐標進行NLOS 檢測，并計算殘差值構成殘差矩陣Rresidue,k。第2 級EKF 用于融合UWB 和IMU的數(shù)據(jù)，將NLOS 檢測過程中獲得的殘差矩陣乘以融合濾波器的測量噪聲矩陣，以此來減小UWB 的NLOS 誤差，第2 級EKF 的定 位結(jié)果作為 整個系統(tǒng)的反饋值，用于下一次定位。

圖4 雙級EKF 融合框架Fig.4 Two-stage EKF fusion framework

2.1 慣性導航的EKF

慣性導航的EKF 即第1 級EKF。由于航向、步長存在一定程度的測量誤差，單獨使用PDR 算法不能獲得準確的INS 位置估計值，因此本文將EKF 融入PDR 算法中，以增加INS 定位結(jié)果的準確性，具體過程如下：

狀態(tài)變量為：

其中：E和N為消防員在ENU 坐標系中的東向和北向坐標；S為步長；θ為航向角（0°≤θ≤360°）。

觀測量為：

通過EKF 的預測過程和測量過程可以得到消防員的初始狀態(tài)估計：

k時刻消防員在ENU 坐標系中的初始位置估計坐標為

雖然通過上述過程，可以得到消防員的初始位置，但是INS 短時間內(nèi)定位較為精確，長時間使用會使定位誤差逐漸增加，因此需要融合UWB 才能實現(xiàn)持續(xù)、穩(wěn)定的精確定位。

2.2 NLOS 檢測算法

為了消除UWB 的NLOS 誤差，借助第1 級EKF所得INS 定位結(jié)果來檢測UWB 的距離測量是否含有NLOS 誤差。NLOS 檢測算法利用INS 系統(tǒng)短期定位精確的特點增加了算法的可靠性；通過分析UWB 的距離測量來檢測NLOS 誤差，算法的復雜度較低，增加了算法的實用性；根據(jù)檢測結(jié)果求取殘差矩陣，方便在數(shù)據(jù)融合中緩解NLOS 誤差。NLOS 檢測算法如圖5 所示。

圖5 NLOS 檢測算法模型Fig.5 NLOS detection algorithm model

U點為k-1 時刻UWB 和INS 的聯(lián)合定位坐標最優(yōu)估計值，根據(jù)U點的坐標使用第2.1 節(jié)的慣性導航算法進行下一步估計，可以得到k時刻的初始位置估計坐標根據(jù)P點的坐標求出P點到各個基站的距離：

其中：(Eum,Num)是第m(m=1,2,…,M)個UWB 基站的位置坐標。

用k時刻定位終端到第m個UWB 基站的距離測量值Dum,k減去Dinsm,k并取絕對值，若此絕對值大于預設的閾值，則判定當前時刻的UWB 距離測量值包含NLOS 誤差；若小于閾值，則判定此刻的UWB 測量值在正常的誤差范圍內(nèi)：

其中：閾值Tthreshold是由UWB 設備的測量精度和慣性導航的精度決定的。

對包含NLOS 誤差的UWB 距離測量值求取殘差：

其中：β為殘差系數(shù)，通常設置為一個較大的值。NLOS 誤差越大，所求取的殘差值越大。

不包含NLOS 誤差的UWB 距離測量值殘差為1，用所有殘差值構成一個殘差矩陣：

在數(shù)據(jù)融合中將使用融合濾波器的測量噪聲矩陣乘以該殘差矩陣來達到緩解NLOS 誤差的目的。

2.3 數(shù)據(jù)融合的EKF

數(shù)據(jù)融合的EKF 即第2 級EKF，以松耦合的方式實現(xiàn)UWB/INS 數(shù)據(jù)融合，可以提高數(shù)據(jù)的利用率。在進行二維定位時，一般需要3 個或3 個以上標簽到基站的距離信息，當少于3 個距離信息時，傳統(tǒng)的3 邊測量算法將導致定位失敗，當大于3 個測距信息時，3 邊測量算法和松散耦合的方式將會浪費多余的UWB 測量數(shù)據(jù)。以松耦合方式實現(xiàn)數(shù)據(jù)融合，可以利用任意數(shù)量的UWB 測量數(shù)據(jù)，增加系統(tǒng)的健壯性。

第2 級EKF 的狀態(tài)變量、狀態(tài)方程、狀態(tài)轉(zhuǎn)移矩陣、預測噪聲矩陣與2.1 節(jié)的第1 級EKF 相同，此處不再贅述。

第2 級EKF 的觀測量為：

其中：Zuwb=[Du1,Du2,…,Dum,…,DuM]；Dum是定位終端到第m(m=1,2,…,M)個UWB 基站的距離測量值，Zins如式（2）所示。

觀測噪聲矩陣為：

其中：Ruwb為UWB 定位模塊的觀測噪聲矩陣；Rins為INS 定位模塊的觀測噪聲矩陣。

將NLOS 誤差檢測所得到的殘差矩陣乘以融合濾波器的觀測矩陣得到調(diào)整后的觀測噪聲矩陣：

殘差矩陣中的元素大小和對應UWB 測量值的NLOS 誤差成正比。將融合濾波器的測量噪聲矩陣乘以殘差矩陣，來調(diào)整對應UWB 測量值的噪聲，從而達到緩解NLOS 誤差的目的。

第2 級EKF 將輸出最終的聯(lián)合位置估計向量：

由此可以得到UWB 和INS 融合定位的位置坐標估計值為

算法的時間復雜度分析：給定初始位置后，根據(jù)圖4 所示的算法流程循環(huán)迭代執(zhí)行本文定位算法。假設定位目標完成1 段測試路線要行走n步，那么定位過程需要進行n次，每執(zhí)行一次定位的時間復雜度為O（1），執(zhí)行完整個定位過程的時間復雜度為O（n），與單一定位算法的運算復雜度在1 個量級上。

3 實驗結(jié)果與分析

3.1 MATLAB 仿真驗證與分析

3.1.1 仿真方案

為了驗證本文NLOS 檢測算法和數(shù)據(jù)融合算法的性能，設計了2 組模擬實驗。在相同的實驗條件下比較基于UWB 的EKF 算法、融合了UWB/INS 的雙級EKF 算法（包括有NLOS 和無NLOS 的檢測算法）的定位精度，通過計算估計坐標和真實坐標的均方根誤差（Root Mean Square Error，RMSE）來評估不同算法的性能。此外，文獻［22］所述的基于三角形原理檢測NLOS 算法同時被比較。

在仿真實驗中，步長、航向角、UWB 的距離測量值被加上了高斯白噪聲代表測量誤差。為了模擬UWB 標簽和基站在通信的過程中被介質(zhì)遮擋而產(chǎn)生的NLOS 誤差，隨機選取一定數(shù)量的UWB 距離測量值為其加入正向偏差。文獻［24］描述了UWB 信號被人、大理石柱遮擋時NLOS 誤差為0.50～3.45 m之間。本文在2 組實驗中分別模擬了NLOS 誤差為1.2 m 和0.6 m 左右的情況。仿真實驗中共設有6 個UWB 基站，在ENU 坐標系中的坐標分別為A1=（1 m，1 m），A2=（1 m，17 m），A3=（9 m，17 m），A4=（17 m，17 m），A5=（17 m，1 m），A6=（9 m，1 m）。模擬的行走軌跡是一個正方形，4 個頂點坐標分別為（3 m，3 m），（3 m，15 m），（15 m，15 m），（15 m，3 m）。

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

仿真實驗分為以下2 種：

1）仿真實驗1

此模擬實驗為了驗證在NLOS 誤差為1.2 m 左右的情況下所提算法的定位性能。實驗中模擬了2 種情況：第1 種情況是基站3、4 處于NLOS 環(huán)境中，第2 種情況是基站1、3、4、6 處于NLOS 環(huán)境中。分別給處于NLOS 環(huán)境中的每個UWB 基站的距離測量隨機加入50 個1.2 m 的正向偏差代表NLOS 誤差。

圖6 和圖7 分別為2 種模擬情況下由不同算法解算出的定位軌跡。從圖中可以看出，基于UWB的EKF 算法解算出的定位軌跡較差。這是由于定位參考信息中僅包括定位標簽到各個基站的距離數(shù)據(jù)，缺少其他輔助參考信息及檢測和緩解NLOS誤差的能力。而融合UWB 和INS 進行定位，加入了慣性測量數(shù)據(jù)，使得定位參考信息增多，有助于更加精確的定位。但在雙級EKF 無NLOS 檢測的情況下，由于UWB 的距離測量值包含NLOS 誤差，UWB 和INS 簡單融合解算出的定位軌跡遠遠偏離真實軌跡；而使用了NLOS 檢測的UWB 和INS 融合算法軌跡較為接近真實軌跡。由此可見NLOS 誤差對定位結(jié)果會產(chǎn)生較大影響。通過對比本文NLOS 檢測算法和三角形原理檢測算法，可以看到兩者所形成的軌跡圖差別不大。這說明在此情況下，2 個算法都可以很好地檢測到包含NLOS 誤差的UWB 距離測量值。

圖6 A3 和A4 處于NLOS 環(huán)境的仿真軌跡1Fig.6 Simulation trajectory 1 when A3 and A4 are in the NLOS environment

圖7 A1、A3、A4 和A6 處于NLOS 環(huán)境的仿真軌跡1Fig.7 Simulation trajectory 1 when A1，A3，A4 and A6 are in the NLOS environment

圖8 為兩種模擬情況下各算法的均方根定位誤差，通過對比可知，處于NLOS 情況下的基站越多，定位誤差越大。從定位方法上分析，基于UWB的EKF 算法的均方根定位誤差高于融合UWB/INS的其他3 種定位算法，這說明融合定位要優(yōu)于單一定位方式。通過比較3 種融合算法的均方根定位誤差可以看出，有NLOS 檢測的三角形算法和雙級EKF 算法要明顯優(yōu)于無NLOS 檢測的雙級EKF算法。

圖8 不同算法的均方根定位誤差1Fig.8 Root mean square positioning error 1 of different algorithms

該模擬實驗驗證了融合定位的優(yōu)勢，說明在NLOS 誤差較大的情況下，使用NLOS 檢測算法可以很好地提高融合定位精度。此外，本文算法和三角形原理檢測算法在1.2 m NLOS 誤差情況下的性能接近，且前者略好于后者。

2）仿真實驗2

此模擬實驗為了驗證在NLOS 誤差為0.6 m 左右的情況下所提算法的定位性能。NLOS 誤差越小，越難檢測，誤檢和漏檢的概率也越大。類似實驗1，本次實驗也模擬了2 種情況，分別給處于NLOS 環(huán)境中的每個UWB 基站的距離測量隨機加入50 個0.6 m 的正向偏差代表NLOS 誤差。

圖9 和圖10 是由各種算法解算出的定位軌跡，由圖可知，基于UWB 的EKF 算法、無NLOS 檢測的UWB 和INS 簡單融合算法所形成的軌跡明顯偏離真實軌跡，和實驗1 結(jié)論相同。和實驗1 不同的是，基于三角形原理的NLOS 檢測算法所形成的軌跡也和真實軌跡有較大出入，大量的誤檢和漏檢導致許多包含NLOS 誤差的UWB 距離測量值被代入坐標運算中，最終導致所得定位軌跡偏離真實軌跡。這說明此算法在NLOS 誤差較小時檢測性能較差。然而，本文所提的NLOS 檢測算法解算出的定位軌跡依然接近真實軌跡，在NLOS 誤差較小時仍舊具備良好的檢測性能。

圖9 A3 和A4 處于NLOS 環(huán)境的仿真軌跡2Fig.9 Simulation trajectory 2 when A3 and A4 are in the NLOS environment

圖10 A1，A3，A4 和A6 處于NLOS 環(huán)境的仿真軌跡2Fig.10 Simulation trajectory 2 when A1，A3，A4 and A6 are in the NLOS environment

如圖11 所示，各個算法的均方根定位誤差也驗證了上述結(jié)論，三角形原理檢測算法的均方根定位誤差甚至大于未包含NLOS 檢測的一般性融合算法。

圖11 不同算法的均方根定位誤差2Fig.11 Root mean square positioning error 2 of different algorithms

該模擬實驗驗證了當NLOS 誤差較小且不易檢測時，本文所提的NLOS 檢測算法依然具備良好的檢測能力和較強的穩(wěn)定性。

3.2 實測驗證與分析

3.2.1 實驗環(huán)境布置和設備

本文選取西安郵電大學3 號實驗樓某實驗室進行實地測試，以驗證所提算法的實際定位性能。該實驗室是一個10 m ×10 m 的方形區(qū)域，如圖12 所示，室內(nèi)擺放有實驗桌、電腦、實驗箱、柜子等物品。圖中共有4 個UWB 基站，其中基站A0 和A3 被放置在室內(nèi)，基站A1 和A2 被放置在室外，中間隔著一堵墻，墻的附近還堆積著一些實驗箱。墻、實驗箱和人體將導致不同程度的NLOS 誤差。經(jīng)過多次測試，這些遮擋物將產(chǎn)生0.5～1 m 的UWB 測距誤差。基站A0 和基站A3 一直處于LOS 環(huán)境中，而A1 和A2 則一直處于NLOS 環(huán)境中。各個UWB 基站的坐標為：A0=（9 m，10.8 m），A1=（9 m，0.75 m），A2=（3 m，0.75 m），A3=（3 m，10.8 m）。測試人員沿著室內(nèi)提前規(guī)劃好的虛線路線行走，其身上攜帶的數(shù)據(jù)采集終端包含2個模塊，1個是UWB標簽，和UWB基站的結(jié)構一樣，另外1 個是集成MPU9250 傳感器的JY901 慣性導航模塊。實驗設備如圖13 所示，其中UWB 基站和標簽搭載Decawave 公司的DWM1000模塊，通信距離可達200 m，在LOS 條件下測距誤差在10 cm 左右。MPU9250 是一款9 軸運動跟蹤傳感器，用來獲取測試人員行走時的慣性數(shù)據(jù)。

圖12 實驗環(huán)境布局Fig.12 Experimental environment layout

圖13 融合定位實驗設備Fig.13 Fusion positioning experimental equipment

實驗中，設定UWB 的數(shù)據(jù)采集頻率是50 Hz，MPU9250 的數(shù)據(jù)采集頻率是100 Hz，波特率是115 200 bit/s。測試人員將數(shù)據(jù)采集終端佩戴在胸前，雙手抱著筆記本電腦。傳感器獲取的數(shù)據(jù)經(jīng)過串口傳輸?shù)絇C 端，再使用MATLAB 軟件進行數(shù)據(jù)處理與算法分析。

3.2.2 結(jié)果分析

圖14 所示為不同算法解算出的行走軌跡。從圖中可以看出，基于UWB 的EKF 算法、無NLOS 檢測的UWB 和INS 簡單融合定位的算法性能較差，所形成的軌跡與真實軌跡偏差較大。由于本次實驗環(huán)境中基站A1 和A2 的NLOS 誤差在0.5～1 m 范圍內(nèi)波動，基于三角形原理的NLOS 檢測算法會產(chǎn)生大量的漏檢，這將對定位產(chǎn)生極大的影響。圖中三角形檢測算法的定位軌跡大幅度偏離真實軌跡，這說明使用三角形算法檢測NLOS 誤差并不穩(wěn)定，和3.1節(jié)的仿真實驗結(jié)論一致。本文所提的雙級EKF 和NLOS 檢測算法具備良好的性能，且定位軌跡相比于其他算法更接近真實軌跡。

圖14 不同算法解算出的行走軌跡Fig.14 Walking trajectory calculated by different algorithms

圖15 所示為各算法在1 個定位周期內(nèi)每解算出1 個位置坐標所耗費的時間，表1 是對檢測實驗中各算法的定位精度和實時性的對比。結(jié)合圖15 和表1可知，與基于UWB 的EKF 算法、融合UWB/INS 的雙級EKF 無NLOS 檢測算法和三角形原理檢測算法相比，融合UWB/INS 的雙級EKF 和NLOS 檢測算法的均方根定位誤差分別降低了85.40 %、83.68 %、87.21 %，驗證了所提算法具有較高的定位精度。計算時間反映了算法的開銷問題，不同計算機可能得到不同的計算時間。4 種算法的運行時間大致如圖15 和表1所示，在1 個迭代周期中，三角形原理檢測算法運行時間最長，其次是雙級EKF 和NLOS 檢測算法，接著是雙級EKF 無NLOS 檢測算法，而基于UWB 的EKF算法的運行時間最短，但總體而言4 種算法的運行時間相差不大。綜上所述，本文所提算法定位精度相對較高，雖然總運行時間與單一定位方式相比雖略有上升，但是基本可以滿足消防員室內(nèi)實時精確定位的需求。

圖15 不同算法實時性比較Fig.15 Comparison of real-time performance of different algorithms

表1 算法的定位精度和實時性分析Table 1 Positioning accuracy and real-time performance analysis of algorithms

4 結(jié)束語

本文以UWB 技術和INS 技術為基礎，針對消防員在火災救援時難以精確定位的問題，設計一種雙級EKF 框架，該框架以松耦合的方式實現(xiàn)UWB 和IMU 的融合定位。在此基礎上，借助INS 提出一種用于UWB 的NLOS 檢測算法。當UWB 基站和標簽之間的通信存在遮擋時，該算法可以很好地檢測出非視距情況，并通過計算殘差矩陣緩解NLOS 誤差所帶來的干擾。實驗結(jié)果表明，本文算法具有良好的定位性能和較好的穩(wěn)定性，為火災救援時精確定位消防員所在位置提供了一種有效的解決方案。由于在大多數(shù)情況下多名消防員會同時進入火場，因此下一步將考慮協(xié)同定位機制，將可以準確定位的消防員作為參考點，定位處于定位盲區(qū)的消防員，以提升定位系統(tǒng)的覆蓋范圍。此外，將定位問題從二維平面擴展到三維空間以直觀地查看消防員所在的空間位置也是下一步的研究重點。