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

李銳君，胡代弟

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

0 引言

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

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

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

1 UWB 與慣性導航

■1.1 UWB 測距原理

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

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

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

■1.2 慣性導航的運動軌跡

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

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

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

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

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

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

圖1 互補濾波器

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

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

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

其中l(wèi)oc1為UWB 定位估計位置，loc2為慣性導航估計位置，locend表示最終估計位置。

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

3 測試驗證

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

圖2 測試平臺示意圖

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

圖3 固定基站

圖4 待測節(jié)點

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

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

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

4 結束語

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