李銳君，胡代弟

（鄭州西亞斯學(xué)院，河南鄭州，451150）

0 引言

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

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

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

1 UWB 與慣性導(dǎo)航

■1.1 UWB 測(cè)距原理

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

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

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

■1.2 慣性導(dǎo)航的運(yùn)動(dòng)軌跡

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

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

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

經(jīng)過以上步驟即可對(duì)指北方位慣性導(dǎo)航運(yùn)動(dòng)軌跡進(jìn)行解算。

2 UWB 與慣性導(dǎo)航融合定位算法

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

圖1 互補(bǔ)濾波器

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

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

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

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

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

3 測(cè)試驗(yàn)證

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

圖2 測(cè)試平臺(tái)示意圖

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

圖3 固定基站

圖4 待測(cè)節(jié)點(diǎn)

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

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

圖6 UWB 與慣導(dǎo)模塊結(jié)合后的定位效果

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

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