隋 心，高 嵩，黃俊堯，劉 韜，徐愛功

(1.遼寧工程技術(shù)大學(xué) 測(cè)繪與地理科學(xué)學(xué)院，遼寧 阜新 123000；2.武漢大學(xué) 衛(wèi)星導(dǎo)航定位技術(shù)研究中心，湖北 武漢 430079)

0 引言

隨著人工智能、智慧城市和無(wú)線網(wǎng)絡(luò)的不斷發(fā)展，無(wú)線定位技術(shù)越來(lái)越受到人們的關(guān)注。以全球衛(wèi)星導(dǎo)航系統(tǒng)和組合導(dǎo)航系統(tǒng)為代表的室外定位技術(shù)相對(duì)趨于成熟[1-2]。在室內(nèi)定位技術(shù)中，UWB定位技術(shù)具有測(cè)距定位精度更高、穩(wěn)定性更強(qiáng)的定位能力，在視距(Line of Sight，LOS)環(huán)境下能達(dá)到厘米級(jí)甚至更高級(jí)別的測(cè)距定位精度[3]。UWB系統(tǒng)雖能提供高精度的定位結(jié)果，但其定位結(jié)果受NLOS誤差影響明顯。雖然采用NLOS鑒別與抑制方法可在一定程度上削弱該項(xiàng)誤差影響，但是當(dāng)UWB受到長(zhǎng)時(shí)間的穩(wěn)態(tài)NLOS誤差影響時(shí)，目前沒(méi)有有效的方法較好地解決這一問(wèn)題。此外，UWB定位系統(tǒng)無(wú)法提供載體的姿態(tài)角信息，其數(shù)據(jù)更新速率較慢，難以滿足實(shí)時(shí)快速定位需求[4-5]。INS系統(tǒng)短期定位精度高，不受外界干擾，自主性強(qiáng)，然而其導(dǎo)航定位誤差隨時(shí)間迅速累積[6-8]。UWB與INS組合能充分利用2個(gè)子系統(tǒng)的優(yōu)點(diǎn)，一方面利用INS的短期高精度特性消除UWB中NLOS測(cè)距誤差的影響；另一方面利用UWB系統(tǒng)修正INS的誤差發(fā)散，從而得到穩(wěn)定性更高、可靠性更強(qiáng)的定位結(jié)果，還能提供載體的姿態(tài)角信息，UWB/INS高頻率的定位結(jié)果也能滿足實(shí)時(shí)定位的需求[9-11]。

UWB測(cè)距方法有多種，由于基于往返時(shí)間(Round Trip Time，RTT)測(cè)距在測(cè)距精度和基站布設(shè)效率方面更具優(yōu)勢(shì)，因此被廣泛關(guān)注，但該測(cè)距方法存在測(cè)距時(shí)間不同步問(wèn)題[12-13]。在UWB/INS組合中，為了避免UWB測(cè)距的時(shí)間不同步誤差影響，本文設(shè)計(jì)了UWB輔助INS的室內(nèi)組合定位方法。該方法利用單個(gè)UWB RTT測(cè)距值和INS系統(tǒng)進(jìn)行組合，采用一維的UWB/INS緊組合模型對(duì)組合系統(tǒng)觀測(cè)數(shù)據(jù)進(jìn)行處理，以此消除UWB測(cè)距的時(shí)間不同步誤差影響。

1 UWB RTT測(cè)距誤差分析

UWB測(cè)距方法通常包括：基于角度到達(dá)(Angle of Arrive，AOA)、基于接收信號(hào)強(qiáng)度(Received Signal Strength，RSS)、基于到達(dá)時(shí)間(Time of Arrive，TOA)、基于到達(dá)時(shí)間差(Time Difference of Arrive，TDOA)和基于RTT等5種測(cè)距方法。上述UWB測(cè)距方法中，基于TOA、基于TDOA和基于RTT方法均利用了UWB信號(hào)極高時(shí)間分辨率的特點(diǎn)，在測(cè)距定位過(guò)程中均能有效發(fā)揮UWB信號(hào)的特點(diǎn)與優(yōu)勢(shì)。相對(duì)于TOA和TDOA方法，RTT方法不需要保持UWB基準(zhǔn)站之間、UWB基準(zhǔn)站與流動(dòng)站之間的時(shí)間同步，因此本文采用RTT測(cè)距方法進(jìn)行UWB測(cè)距。

在采用RTT測(cè)距進(jìn)行定位過(guò)程中，UWB流動(dòng)站依次向各UWB基準(zhǔn)站發(fā)送脈沖信號(hào)，UWB基準(zhǔn)站識(shí)別接收到脈沖信號(hào)后再產(chǎn)生響應(yīng)脈沖信號(hào)并將其發(fā)送出去，UWB流動(dòng)站識(shí)別接收到響應(yīng)脈沖信號(hào)后完成1次測(cè)距。UWB流動(dòng)站重復(fù)上述過(guò)程直到與所有的基準(zhǔn)站之間都完成測(cè)距。由于UWB流動(dòng)站和基準(zhǔn)站之間測(cè)距存在時(shí)間延遲(包括信號(hào)響應(yīng)延遲、硬件延遲、信號(hào)傳輸時(shí)間和NLOS傳播時(shí)間延遲)，每一次產(chǎn)生的時(shí)間延遲將累加到下一次測(cè)距過(guò)程中，導(dǎo)致UWB流動(dòng)站的同一組測(cè)距信息中的時(shí)間不同步，UWB測(cè)距中的時(shí)間不同步情況如圖1所示。

圖1 UWB RTT測(cè)距中的時(shí)間不同步Fig.1 Time out of sync in UWB RTT Ranging

UWB測(cè)距中的時(shí)間不同步將限制UWB定位系統(tǒng)只能進(jìn)行載體低速狀態(tài)下的定位，極大地限制了UWB定位的應(yīng)用范圍，比如當(dāng)UWB流動(dòng)站與1個(gè)基準(zhǔn)站之間的時(shí)間延遲為0.02 s，在載體運(yùn)動(dòng)速度為2 m/s的情況下將產(chǎn)生4 cm的定位誤差，在4個(gè)基準(zhǔn)站的情況下將造成16 cm的定位誤差。因此，UWB定位的精度不僅會(huì)受到NLOS測(cè)距誤差和測(cè)距異常值的影響，還會(huì)受到時(shí)間不同步的影響。

2 UWB輔助INS組合定位模型

2.1 INS誤差方程

由文獻(xiàn)[14]可得經(jīng)緯高形式的INS誤差方程為：

(1)

且有：

(2)

經(jīng)緯高形式的INS誤差方程以弧度為單位，在中小型區(qū)域內(nèi)，緯度和經(jīng)度誤差較小，也存在與高度誤差數(shù)值差距太大而產(chǎn)生舍入誤差。UWB/INS組合場(chǎng)景多為室內(nèi)環(huán)境，因此本文推導(dǎo)了ENU形式的位置誤差方程。對(duì)慣性導(dǎo)航系統(tǒng)的經(jīng)度、緯度和高度微分方程式變形可得：

(3)

對(duì)式(3)右邊等式兩邊同時(shí)求導(dǎo)數(shù)，整理成矩陣形式，即可得到ENU形式的INS位置誤差方程：

(4)

且有：

(5)

同時(shí)，對(duì)緯度和高度進(jìn)行替換，可得ENU形式的INS姿態(tài)、速度誤差方程：

(6)

且有：

(7)

整理可得ENU形式的INS誤差方程為：

(8)

2.2 狀態(tài)模型構(gòu)建

(9)

式中，F(xiàn)(t)為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；G(t)為系統(tǒng)的噪聲映射矩陣；W(t)為陀螺儀和加速度計(jì)的隨機(jī)噪聲，假設(shè)其為零均值的高斯白噪聲，其協(xié)方差矩陣為Qk，結(jié)合式(8)，有：

(10)

式中，wg,wa分別為陀螺儀和加速度計(jì)隨機(jī)噪聲。

2.3 量測(cè)模型構(gòu)建

在UWB/INS組合中，利用INS解算的位置反算UWB流動(dòng)站天線與UWB基準(zhǔn)站天線之間的距離為dINS，利用UWB的RTT方法量測(cè)的UWB流動(dòng)站天線與UWB基準(zhǔn)站天線之間的距離為dUWB，假設(shè)UWB流動(dòng)站天線與UWB基準(zhǔn)站天線之間的真實(shí)距離為dtrue，則：

(11)

(12)

(13)

(14)

從而得到式(13)的近似表達(dá)式為：

(15)

UWB量測(cè)的RTT測(cè)距信息可以表示為：

dUWB=dtrue+εd，UWB。

(16)

為了避免UWB測(cè)距的時(shí)間不同步誤差影響，本文設(shè)計(jì)了UWB輔助INS的室內(nèi)組合定位方法，即利用單個(gè)UWB RTT測(cè)距值和INS系統(tǒng)進(jìn)行組合，通過(guò)式(15)和式(16)，可得UWB輔助INS的量測(cè)方程為：

Zk=HkXk+Vk，

(17)

式中，Hk為量測(cè)矩陣；Vk為量測(cè)噪聲向量，假設(shè)為零均值的高斯白噪聲，其協(xié)方差矩陣為Rk，且有：

(18)

2.4 組合濾波方法

由于標(biāo)準(zhǔn)卡爾曼濾波只能處理離散方程，因此需要將線性連續(xù)形式的狀態(tài)方程進(jìn)行離散化處理，對(duì)于離散化后的線性系統(tǒng)，卡爾曼濾波的基本流程為[13-16]：

① 計(jì)算預(yù)測(cè)狀態(tài)向量及其協(xié)方差矩陣：

(19)

② 計(jì)算增益矩陣，計(jì)算新的狀態(tài)向量估值及其協(xié)方差矩陣：

(20)

由于UWB/INS組合系統(tǒng)中UWB的RTT測(cè)距信息會(huì)受到NLOS測(cè)距誤差和測(cè)距異常值的影響，因此UWB輔助INS系統(tǒng)采用自適應(yīng)抗差方法提高組合系統(tǒng)的精度和穩(wěn)定性，其關(guān)鍵步驟為：

(21)

(22)

通過(guò)上述過(guò)程可以完成UWB輔助INS系統(tǒng)中卡爾曼濾波的自適應(yīng)濾波和抗差估計(jì)處理，從而提高組合系統(tǒng)的精度、穩(wěn)定性和可靠性。

3 實(shí)驗(yàn)與分析

利用一組室內(nèi)實(shí)驗(yàn)對(duì)本文所述方法進(jìn)行分析驗(yàn)證。實(shí)驗(yàn)中以Time Domain的PulsON400通信與測(cè)距模塊作為UWB數(shù)據(jù)采集模塊，其帶寬為3.1～5.3 GHz，數(shù)據(jù)采集頻率為2 Hz；利用國(guó)產(chǎn)某組合導(dǎo)航產(chǎn)品進(jìn)行陀螺儀和加速度計(jì)的數(shù)據(jù)采集，該組合導(dǎo)航產(chǎn)品內(nèi)置NovAtel的OEM617D專業(yè)型GNSS板卡、GNSS與INS的時(shí)間同步系統(tǒng)和慣性測(cè)量單元，其陀螺儀的零偏穩(wěn)定性每小時(shí)不超過(guò)0.1°，零偏重復(fù)性每小時(shí)不超過(guò)0.1°，其加速度計(jì)的零偏穩(wěn)定性不超過(guò)60 ug，零偏重復(fù)性不超過(guò)60 ug；利用Leica TS09建立UWB定位的坐標(biāo)系統(tǒng)，其精度為1.5 mm+2 ppm，利用其測(cè)量UWB基準(zhǔn)站天線的三維坐標(biāo)并假定沒(méi)有誤差。實(shí)驗(yàn)中采用硬件設(shè)備如圖2所示。

圖2 UWB/INS組合實(shí)驗(yàn)中采用的硬件設(shè)備Fig.2 The hardware used in the UWB and INS combined experiment

UWB與INS組合實(shí)驗(yàn)中，將UWB模塊固定組合導(dǎo)航產(chǎn)品上方，利用螺絲將組合導(dǎo)航產(chǎn)品固定在實(shí)驗(yàn)鋁板上。實(shí)驗(yàn)開始前測(cè)量UWB天線中心與INS儀器中心的桿臂，UWB時(shí)間采用電腦時(shí)間，利用組合導(dǎo)航產(chǎn)品對(duì)實(shí)驗(yàn)筆記本電腦進(jìn)行授時(shí)，保持UWB系統(tǒng)和INS系統(tǒng)的時(shí)間同步。

為了驗(yàn)證分析文中UWB輔助INS的組合定位方法，在實(shí)驗(yàn)過(guò)程中分別采用以下5種方法進(jìn)行定位解算：

方法1：標(biāo)準(zhǔn)卡爾曼濾波的UWB定位(常規(guī)UWB)；

方法2：自適應(yīng)抗差卡爾曼濾波的UWB定位(改進(jìn)UWB)；

方法3：UWB/INS松組合導(dǎo)航定位(松組合)；

方法4：自適應(yīng)抗差卡爾曼濾波的UWB/INS緊組合定位(緊組合)；

方法5：自適應(yīng)抗差卡爾曼濾波的UWB輔助INS定位(輔助)。

實(shí)驗(yàn)中僅有3個(gè)UWB基準(zhǔn)站，且實(shí)驗(yàn)區(qū)域很平緩，故利用全站儀觀測(cè)UWB流動(dòng)站天線的高度，將其作為高度約束加入到上述5種方法中，將高度誤差大小設(shè)為5 cm。

實(shí)驗(yàn)環(huán)境為遼寧工程技術(shù)大學(xué)惠和樓207會(huì)議室，利用三腳架安裝3個(gè)UWB基準(zhǔn)站，在通電后利用全站儀確定其三維坐標(biāo)并保持不動(dòng)，實(shí)驗(yàn)平臺(tái)上去掉GNSS天線和連接線。實(shí)驗(yàn)過(guò)程中墻柱和實(shí)驗(yàn)人員對(duì)UWB信號(hào)造成遮擋，由人推著小車沿固定軌跡移動(dòng)，每隔一段時(shí)間利用全站儀測(cè)量UWB流動(dòng)站的天線中心坐標(biāo)，并將其作為參考真值，整個(gè)實(shí)驗(yàn)時(shí)長(zhǎng)750 s，前300 s為靜止?fàn)顟B(tài)，后450 s為運(yùn)動(dòng)狀態(tài)。實(shí)驗(yàn)中運(yùn)動(dòng)部分的UWB原始測(cè)距信息如圖3所示，可以看出UWB原始測(cè)距中存在部分的NLOS測(cè)距誤差和測(cè)距異常值。陀螺和加速度計(jì)輸出信號(hào)分別如圖4和圖5所示。

圖3 室內(nèi)實(shí)驗(yàn)中的UWB測(cè)距信息Fig.3 UWB ranging information in the indoor experiment

圖4 室內(nèi)實(shí)驗(yàn)中陀螺數(shù)據(jù)Fig.4 Gyro data in the indoor experiment

圖5 室內(nèi)實(shí)驗(yàn)中加速度計(jì)數(shù)據(jù)Fig.5 Accelerometer data in the indoor experiment

實(shí)驗(yàn)中UWB解算的平面軌跡和UWB/INS組合解算的平面軌跡如圖6和圖7所示。圖中紅色五角星為全站儀測(cè)量的UWB流動(dòng)站天線的坐標(biāo)。實(shí)驗(yàn)小車共停10次，每次利用全站儀確定UWB流動(dòng)站天線坐標(biāo)并將其作為參考真值，在全站儀觀測(cè)期間利用5種方法解算的位置平均值與全站儀觀測(cè)的位置進(jìn)行比較。5種方法的東向誤差和北向誤差如圖8和圖9所示。5種方法在東方向和北方向的誤差統(tǒng)計(jì)信息如表1所示。

圖6 室內(nèi)UWB解算的平面軌跡Fig.6 Calculated plane trajectory used by UWB

圖7 室內(nèi)UWB/INS組合解算的平面軌跡Fig.7 Calculated plane trajectory used by indoor UWB and INS integrated method

圖8 室內(nèi)實(shí)驗(yàn)中5種方法在東方向的位置誤差Fig.8 East position error of 5 methods in the indoor experiment

圖9 室內(nèi)實(shí)驗(yàn)中5種方法在北方向的位置誤差Fig.9 North position error of 5 methods in the indoor experiment

表1 室內(nèi)實(shí)驗(yàn)中5種方法的誤差統(tǒng)計(jì)信息Tab.1 Error statistics of 5 methods in the indoor experiment

通過(guò)5種方法實(shí)驗(yàn)結(jié)果和誤差統(tǒng)計(jì)可以得到：

① 在實(shí)驗(yàn)過(guò)程中，每次靜止時(shí)由于人的遮擋較為頻繁，UWB的原始測(cè)距中頻繁的短時(shí)間的NLOS測(cè)距誤差較多。此時(shí)常規(guī)UWB定位方法的結(jié)果波動(dòng)明顯，穩(wěn)定性很差；而自適應(yīng)抗差卡爾曼濾波方法能利用提前設(shè)定的閾值判斷NLOS測(cè)距誤差，并降低含有NLOS測(cè)距誤差項(xiàng)的測(cè)距信息的權(quán)重，結(jié)合自適應(yīng)估計(jì)方法降低系統(tǒng)模型的誤差，能大幅度提升定位的精度和穩(wěn)定性。

② 實(shí)驗(yàn)中UWB/INS松組合的定位精度和自適應(yīng)抗差卡爾曼濾波的UWB方法精度水平相當(dāng)，且在單獨(dú)UWB的基礎(chǔ)上略微提高；由于UWB/INS緊組合和UWB輔助INS定位方法中加入了抗差估計(jì)方法，對(duì)原始測(cè)距中的NLOS測(cè)距誤差和測(cè)距異常值具有較好的抗干擾能力，能提升系統(tǒng)的定位精度，其平面誤差達(dá)到了0.05 m；UWB輔助INS定位方法避免了UWB同一組測(cè)距中的時(shí)間不同步，能在UWB/INS緊組合的基礎(chǔ)上進(jìn)一步提高系統(tǒng)的精度和穩(wěn)定性，其平面位置精度優(yōu)于0.05 m。

③ 需要說(shuō)明的是，由于室內(nèi)定位中很難精確地測(cè)定UWB流動(dòng)站天線坐標(biāo)，且實(shí)驗(yàn)過(guò)程中受到了人為的頻繁干擾，實(shí)驗(yàn)環(huán)境的不同也可能會(huì)對(duì)實(shí)驗(yàn)結(jié)果產(chǎn)生影響，因此室內(nèi)定位實(shí)驗(yàn)中不同方法的誤差精度僅代表各方法的大體精度水平。

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

在室內(nèi)定位技術(shù)中，UWB與INS組合定位可實(shí)現(xiàn)優(yōu)勢(shì)互補(bǔ)，是當(dāng)前研究熱點(diǎn)。目前已有的組合方法通常認(rèn)為同一組UWB測(cè)距是時(shí)間同步的，但經(jīng)過(guò)分析，UWB RTT測(cè)距方法存在同一組測(cè)距時(shí)間不同步現(xiàn)象，針對(duì)該問(wèn)題本文設(shè)計(jì)了UWB輔助INS數(shù)學(xué)模型，利用單個(gè)UWB RTT測(cè)距值和INS系統(tǒng)進(jìn)行組合，以此消除UWB測(cè)距的時(shí)間不同步誤差影響。室內(nèi)定位實(shí)驗(yàn)表明，UWB/INS組合系統(tǒng)比單獨(dú)UWB系統(tǒng)有更高的精度和更強(qiáng)的穩(wěn)定性，UWB/INS緊組合方法能有效減小UWB定位過(guò)程中產(chǎn)生的誤差，UWB輔助INS定位方法在UWB/INS緊組合的基礎(chǔ)上能有效避免UWB同一組測(cè)距中的時(shí)間不同步誤差，進(jìn)一步提高了室內(nèi)定位的精度、穩(wěn)定性和可靠性。