基于超寬帶與慣性導航融合的定位研究

李銳君，胡代弟

（鄭州西亞斯學院，河南鄭州，451150）

0 引言

移動互聯網的發展，有效的位置服務也越來越重要，地下停車場、隧道等GPS 信號弱的環境中需要代替GPS的精確位置服務，以滿足各種定位需求。UWB (Ultra Wideband Technology)定位系統近年受到了高度重視和深入的研究[1]。基于TOA（Time Of Arrival）和TDOA（Time Difference of Arrival）的常見測距和定位技術，對基于UWB 的定位應用有了很大的改進[2-3]。但是針對移動目標的實時跟蹤，只有當系統誤差為零均值的高斯分布時，才能得到較高精度[4]。

慣性導航的突出優勢在于導航定位的自主性與連續性，在三維空間下有良好的表現[5]。利用搭配磁力計、陀螺儀、加速度計的九軸慣導模塊可精確測得物體的運動信息，通過航跡推算估計出物體運動的方向和距離，但是低成本陀螺儀漂移誤差嚴重，磁力計容易受到各種磁干擾，無法獲取準確的航向值，導致定位精度低，此外，由于航跡推算機制所固有的積分誤差累積效應，也限制了慣導組件長時間的單獨定位工作[6]。

鑒于UWB 帶寬高、能耗低、多徑分辨能力強，以及慣性導航定位的自身優勢，將二者結合用于定位和測距，具有重要的應用價值。本文研究了二者融合算法并搭建平臺驗證了其在定位方面的實用性，并獲取了較高的定位精度。

1 UWB 與慣性導航

■1.1 UWB 測距原理

UWB 定位原理，首先測量基站與目標節點之間的距離，再由算法求解目標節點的位置坐標，本文利用TOA 發進行測距。TOA 定位法假設目標結點發射信號的時刻為t，第i個已知參考基站收到目標節點信號的時刻為ti，i=1～6。已知第i 個參考基站的三維空間坐標為（xi,yi,zi），待測目標節點坐標為（x,y,z），建立方程如式（1），并得出第i 個已知參考基站與目標節點的直線距離。

式中，ζi為第i 個已知參考基站接收到的TOA 值。在理想無誤差情況下，計算所得到的方程組的解只有一個，即有唯一交點，有誤差時即為一個區域。

TOA 算法要求參與定位的所有參考節點都要與目標節點在時間上精確同步，否則定位精度將急速下降，此要求在實際中是很難達到[7]。

■1.2 慣性導航的運動軌跡

初始的四元數值根據初始的姿態角確定，將加速度計的測量信息經過余弦矩陣由地理坐標系轉換到載體坐標系中，根據初始的經緯度、高度和速度值和變換后的比例信息進行指北方位系統的運動解算，求出指令角速度信息、速度信息和位置信息。式（2）為地理坐標系x、y、z 軸方向上的加速度值與其對應的角速度值、速度值的關系。

根據四元數和歐拉角之間的關系求解出新的姿態角。式（3）為姿態角與方向余弦矩陣的關系。

式中θ、γ、φ 分別為載體在三維空間中的俯仰角、橫滾角和航偏角。

經過以上步驟即可對指北方位慣性導航運動軌跡進行解算。

2 UWB 與慣性導航融合定位算法

為解決定位精度偏低的問題，將UWB 和慣性導航的定位數據進行融合。利用九軸慣導模塊獲得待測目標節點的加速度、航偏角、橫滾角等傳感數據，待測節點運動的距離和方向通過互補濾波器進行航跡推算，互補濾波器如圖1 所示。以三維空間室內定位為例，由已知固定基站初始位置對航跡推算系統的起始位置進行初始化，在目標待測節點運動過程中，獲取UWB 與慣性導航的定位估值后，利用信賴度對兩組數據進行加權融合，確定定位結果。

圖1 互補濾波器

當僅用UWB 模塊定位時，實際測得的接收信號強度RSS（Received Signal Strength，RSS）矢量表示為（rss1，…，rssn）。從第i 個定位基站收到的RSS 值rssi(i=1，2，..，n)，與初始化庫中的RSS 進行對比，選取歐式距離最近的3 個RSS：，對應的樣本點位置作加權質心計算定位結果。當已知參考基站與目標節點的直線距離R 與3 個樣本點的最小歐式距離Dis=min(||R-R'||，||R-R''||，||R-R'''||)越小，定位結果可信度越高。當Dis 大于閾值等于μ 時，認為該定位結果無效。UWB 定位數據的可信度采用信賴度因子α ∈[0，1]描述。式（4）為信賴度因子α 與Dis 關系。

慣性導航系統時間越長，其誤差累積越大，當導航時間超過x秒后，定位誤差過大，認為其定位結果不可信。為描述慣導系統定位數據的可信度，定義信賴度因子β ∈[0,1]，該值在x秒內隨時間以1/x 的斜率遞減，β 與時間t 的關系如式（5）。

為有效利用慣性導航信息，當信賴度低于閾值ω 時，重新推算載體位置并更新計時。融合后的定位數據由式（6）計算獲得。

其中loc1為UWB 定位估計位置，loc2為慣性導航估計位置，locend表示最終估計位置。

實測中，設UWB 模塊采集周期為T1，慣性導航模塊采集周期為T2，在一個RSS 采集周期內會有連續多個運動傳感信息，即T1＞T2。在每個T1時刻由式（6）進行定位信息的融合，并將融合的位置作為每輪的初始位置，在T1時間間隔內由慣性導航推算載體的運動位置。此算法由于多種測量誤差的影響會表現出運動軌跡的波動，可通過滑動平均等方法對位置信息進行濾波。

3 測試驗證

為驗證此融合算法，測試平臺如圖2 所示。本系統包括UWB 定位基站、待測目標節點、上位機。本測驗首先需將基站固定安裝，6 個基站放置在一個鐵架的6 個點，彼此間距2 米，并設其中一個基站為三維坐標原點。通過WIFI建立可與上位機通信的網絡。在物體定位跟蹤過程中，目標節點將實時測得的RSS 值和慣性導航信息傳送至上位機，由上位機使用基于信賴度的加權融合定位算法獲得待測目標節點的位置信息。

圖2 測試平臺示意圖

固定基站的硬件由愛爾蘭DecaWave 公司的UWB 集成模塊、意法半導體公司的STM32 芯片與外圍電路組成；待測目標節點由UWB 集成模塊、STM32 芯片及美國PNI公司的九軸慣導模塊、WIFI 模塊與外圍電路組成。固定基站如圖3 所示；待測節點如圖4 所示。

圖3 固定基站

圖4 待測節點

采用MATLAB 進行上位機程序編寫及仿真測試。測試地點為鄭州西亞斯學院一個200m2的空曠房間，待測節點位置每0.5 秒在MATLAB 中記錄1 次。僅用UWB 模塊進行的三維定位效果如圖5 所示，待測節點在室內移動時的定位顯示會有較為明顯的發散現象。UWB與慣導模塊加權融合后的定位效果如圖6 所示，將UWB 與慣導模塊融合后再做定位測試，雖然采集時間更長，但待測節點正常移動時的發散現象明顯下降。定位精度經與實際位置對比，定位誤差都在20cm 以內，定位算法可靠有效。

圖5 僅用UWB 模塊的三維定位效果

圖6 UWB 與慣導模塊結合后的定位效果

4 結束語

本文介紹了基于TOA 測距的UWB 定位算法和慣性導航運動軌跡及姿態角解算步驟，提出了一個基于UWB 與慣性導航相互融合的加權算法，以提高對物體在三維空間長時間定位效果。最后利用硬件平臺做了定位測驗并在上位機進行了實測記錄。實測表明，將UWB 與慣性導航進行加權融合的空間定位方案，有效提高了定位精度，在移動物體三維空間的定位應用中，具有一定的優越性與實用性。