UWB輔助INS的室內組合定位方法

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

(1.遼寧工程技術大學 測繪與地理科學學院，遼寧 阜新 123000；2.武漢大學 衛星導航定位技術研究中心，湖北 武漢 430079)

0 引言

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

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

1 UWB RTT測距誤差分析

UWB測距方法通常包括：基于角度到達(Angle of Arrive，AOA)、基于接收信號強度(Received Signal Strength，RSS)、基于到達時間(Time of Arrive，TOA)、基于到達時間差(Time Difference of Arrive，TDOA)和基于RTT等5種測距方法。上述UWB測距方法中，基于TOA、基于TDOA和基于RTT方法均利用了UWB信號極高時間分辨率的特點，在測距定位過程中均能有效發揮UWB信號的特點與優勢。相對于TOA和TDOA方法，RTT方法不需要保持UWB基準站之間、UWB基準站與流動站之間的時間同步，因此本文采用RTT測距方法進行UWB測距。

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

圖1 UWB RTT測距中的時間不同步Fig.1 Time out of sync in UWB RTT Ranging

UWB測距中的時間不同步將限制UWB定位系統只能進行載體低速狀態下的定位，極大地限制了UWB定位的應用范圍，比如當UWB流動站與1個基準站之間的時間延遲為0.02 s，在載體運動速度為2 m/s的情況下將產生4 cm的定位誤差，在4個基準站的情況下將造成16 cm的定位誤差。因此，UWB定位的精度不僅會受到NLOS測距誤差和測距異常值的影響，還會受到時間不同步的影響。

2 UWB輔助INS組合定位模型

2.1 INS誤差方程

由文獻[14]可得經緯高形式的INS誤差方程為：

(1)

且有：

(2)

經緯高形式的INS誤差方程以弧度為單位，在中小型區域內，緯度和經度誤差較小，也存在與高度誤差數值差距太大而產生舍入誤差。UWB/INS組合場景多為室內環境，因此本文推導了ENU形式的位置誤差方程。對慣性導航系統的經度、緯度和高度微分方程式變形可得：

(3)

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

(4)

且有：

(5)

同時，對緯度和高度進行替換，可得ENU形式的INS姿態、速度誤差方程：

(6)

且有：

(7)

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

(8)

2.2 狀態模型構建

(9)

式中，F(t)為系統狀態轉移矩陣；G(t)為系統的噪聲映射矩陣；W(t)為陀螺儀和加速度計的隨機噪聲，假設其為零均值的高斯白噪聲，其協方差矩陣為Qk，結合式(8)，有：

(10)

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

2.3 量測模型構建

在UWB/INS組合中，利用INS解算的位置反算UWB流動站天線與UWB基準站天線之間的距離為dINS，利用UWB的RTT方法量測的UWB流動站天線與UWB基準站天線之間的距離為dUWB，假設UWB流動站天線與UWB基準站天線之間的真實距離為dtrue，則：

(11)

(12)

(13)

(14)

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

(15)

UWB量測的RTT測距信息可以表示為：

dUWB=dtrue+εd，UWB。

(16)

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

Zk=HkXk+Vk，

(17)

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

(18)

2.4 組合濾波方法

由于標準卡爾曼濾波只能處理離散方程，因此需要將線性連續形式的狀態方程進行離散化處理，對于離散化后的線性系統，卡爾曼濾波的基本流程為[13-16]：

① 計算預測狀態向量及其協方差矩陣：

(19)

② 計算增益矩陣，計算新的狀態向量估值及其協方差矩陣：

(20)

由于UWB/INS組合系統中UWB的RTT測距信息會受到NLOS測距誤差和測距異常值的影響，因此UWB輔助INS系統采用自適應抗差方法提高組合系統的精度和穩定性，其關鍵步驟為：

(21)

(22)

通過上述過程可以完成UWB輔助INS系統中卡爾曼濾波的自適應濾波和抗差估計處理，從而提高組合系統的精度、穩定性和可靠性。

3 實驗與分析

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

圖2 UWB/INS組合實驗中采用的硬件設備Fig.2 The hardware used in the UWB and INS combined experiment

UWB與INS組合實驗中，將UWB模塊固定組合導航產品上方，利用螺絲將組合導航產品固定在實驗鋁板上。實驗開始前測量UWB天線中心與INS儀器中心的桿臂，UWB時間采用電腦時間，利用組合導航產品對實驗筆記本電腦進行授時，保持UWB系統和INS系統的時間同步。

為了驗證分析文中UWB輔助INS的組合定位方法，在實驗過程中分別采用以下5種方法進行定位解算：

方法1：標準卡爾曼濾波的UWB定位(常規UWB)；

方法2：自適應抗差卡爾曼濾波的UWB定位(改進UWB)；

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

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

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

實驗中僅有3個UWB基準站，且實驗區域很平緩，故利用全站儀觀測UWB流動站天線的高度，將其作為高度約束加入到上述5種方法中，將高度誤差大小設為5 cm。

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

圖3 室內實驗中的UWB測距信息Fig.3 UWB ranging information in the indoor experiment

圖4 室內實驗中陀螺數據Fig.4 Gyro data in the indoor experiment

圖5 室內實驗中加速度計數據Fig.5 Accelerometer data in the indoor experiment

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

圖6 室內UWB解算的平面軌跡Fig.6 Calculated plane trajectory used by UWB

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

圖8 室內實驗中5種方法在東方向的位置誤差Fig.8 East position error of 5 methods in the indoor experiment

圖9 室內實驗中5種方法在北方向的位置誤差Fig.9 North position error of 5 methods in the indoor experiment

表1 室內實驗中5種方法的誤差統計信息Tab.1 Error statistics of 5 methods in the indoor experiment

通過5種方法實驗結果和誤差統計可以得到：

① 在實驗過程中，每次靜止時由于人的遮擋較為頻繁，UWB的原始測距中頻繁的短時間的NLOS測距誤差較多。此時常規UWB定位方法的結果波動明顯，穩定性很差；而自適應抗差卡爾曼濾波方法能利用提前設定的閾值判斷NLOS測距誤差，并降低含有NLOS測距誤差項的測距信息的權重，結合自適應估計方法降低系統模型的誤差，能大幅度提升定位的精度和穩定性。

② 實驗中UWB/INS松組合的定位精度和自適應抗差卡爾曼濾波的UWB方法精度水平相當，且在單獨UWB的基礎上略微提高；由于UWB/INS緊組合和UWB輔助INS定位方法中加入了抗差估計方法，對原始測距中的NLOS測距誤差和測距異常值具有較好的抗干擾能力，能提升系統的定位精度，其平面誤差達到了0.05 m；UWB輔助INS定位方法避免了UWB同一組測距中的時間不同步，能在UWB/INS緊組合的基礎上進一步提高系統的精度和穩定性，其平面位置精度優于0.05 m。

③ 需要說明的是，由于室內定位中很難精確地測定UWB流動站天線坐標，且實驗過程中受到了人為的頻繁干擾，實驗環境的不同也可能會對實驗結果產生影響，因此室內定位實驗中不同方法的誤差精度僅代表各方法的大體精度水平。

4 結束語

在室內定位技術中，UWB與INS組合定位可實現優勢互補，是當前研究熱點。目前已有的組合方法通常認為同一組UWB測距是時間同步的，但經過分析，UWB RTT測距方法存在同一組測距時間不同步現象，針對該問題本文設計了UWB輔助INS數學模型，利用單個UWB RTT測距值和INS系統進行組合，以此消除UWB測距的時間不同步誤差影響。室內定位實驗表明，UWB/INS組合系統比單獨UWB系統有更高的精度和更強的穩定性，UWB/INS緊組合方法能有效減小UWB定位過程中產生的誤差，UWB輔助INS定位方法在UWB/INS緊組合的基礎上能有效避免UWB同一組測距中的時間不同步誤差，進一步提高了室內定位的精度、穩定性和可靠性。