楊 剛，朱士玲，李 強(qiáng)，趙克松

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

0 概述

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

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

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

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

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

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

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

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

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

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

1.1 INS 模塊

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

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

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

1.2 UWB 模塊

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

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

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

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

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

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

本文所提的松耦合數(shù)據(jù)融合方式可以利用任意數(shù)量的UWB 測(cè)量距離值，以增加定位系統(tǒng)的健壯性。最終的定位結(jié)果皆為融合定位所得，根據(jù)測(cè)量噪聲殘差矩陣自適應(yīng)定位環(huán)境的變化，避免了定位模式的硬切換，增加了系統(tǒng)的實(shí)用性?；诘目柭鼮V波器使整個(gè)系統(tǒng)具備較高的實(shí)時(shí)性。

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

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

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

2.1 慣性導(dǎo)航的EKF

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

狀態(tài)變量為：

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

觀測(cè)量為：

通過(guò)EKF 的預(yù)測(cè)過(guò)程和測(cè)量過(guò)程可以得到消防員的初始狀態(tài)估計(jì)：

k時(shí)刻消防員在ENU 坐標(biāo)系中的初始位置估計(jì)坐標(biāo)為

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

2.2 NLOS 檢測(cè)算法

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

圖5 NLOS 檢測(cè)算法模型Fig.5 NLOS detection algorithm model

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

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

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

其中：閾值Tthreshold是由UWB 設(shè)備的測(cè)量精度和慣性導(dǎo)航的精度決定的。

對(duì)包含NLOS 誤差的UWB 距離測(cè)量值求取殘差：

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

不包含NLOS 誤差的UWB 距離測(cè)量值殘差為1，用所有殘差值構(gòu)成一個(gè)殘差矩陣：

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

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

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

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

第2 級(jí)EKF 的觀測(cè)量為：

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

觀測(cè)噪聲矩陣為：

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

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

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

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

由此可以得到UWB 和INS 融合定位的位置坐標(biāo)估計(jì)值為

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

3 實(shí)驗(yàn)結(jié)果與分析

3.1 MATLAB 仿真驗(yàn)證與分析

3.1.1 仿真方案

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

在仿真實(shí)驗(yàn)中，步長(zhǎng)、航向角、UWB 的距離測(cè)量值被加上了高斯白噪聲代表測(cè)量誤差。為了模擬UWB 標(biāo)簽和基站在通信的過(guò)程中被介質(zhì)遮擋而產(chǎn)生的NLOS 誤差，隨機(jī)選取一定數(shù)量的UWB 距離測(cè)量值為其加入正向偏差。文獻(xiàn)［24］描述了UWB 信號(hào)被人、大理石柱遮擋時(shí)NLOS 誤差為0.50～3.45 m之間。本文在2 組實(shí)驗(yàn)中分別模擬了NLOS 誤差為1.2 m 和0.6 m 左右的情況。仿真實(shí)驗(yàn)中共設(shè)有6 個(gè)UWB 基站，在ENU 坐標(biāo)系中的坐標(biāo)分別為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）。模擬的行走軌跡是一個(gè)正方形，4 個(gè)頂點(diǎn)坐標(biāo)分別為（3 m，3 m），（3 m，15 m），（15 m，15 m），（15 m，3 m）。

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

仿真實(shí)驗(yàn)分為以下2 種：

1）仿真實(shí)驗(yàn)1

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

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

圖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 為兩種模擬情況下各算法的均方根定位誤差，通過(guò)對(duì)比可知，處于NLOS 情況下的基站越多，定位誤差越大。從定位方法上分析，基于UWB的EKF 算法的均方根定位誤差高于融合UWB/INS的其他3 種定位算法，這說(shuō)明融合定位要優(yōu)于單一定位方式。通過(guò)比較3 種融合算法的均方根定位誤差可以看出，有NLOS 檢測(cè)的三角形算法和雙級(jí)EKF 算法要明顯優(yōu)于無(wú)NLOS 檢測(cè)的雙級(jí)EKF算法。

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

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

2）仿真實(shí)驗(yàn)2

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

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

圖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 所示，各個(gè)算法的均方根定位誤差也驗(yàn)證了上述結(jié)論，三角形原理檢測(cè)算法的均方根定位誤差甚至大于未包含NLOS 檢測(cè)的一般性融合算法。

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

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

3.2 實(shí)測(cè)驗(yàn)證與分析

3.2.1 實(shí)驗(yàn)環(huán)境布置和設(shè)備

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

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

圖13 融合定位實(shí)驗(yàn)設(shè)備Fig.13 Fusion positioning experimental equipment

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

3.2.2 結(jié)果分析

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

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

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

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

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

4 結(jié)束語(yǔ)

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