基于TW-TOF的UWB室內(nèi)定位技術(shù)與優(yōu)化算法研究

牛群峰，曹一帆，王 莉，惠延波

（河南工業(yè)大學(xué) 電氣工程學(xué)院，鄭州 450000）

基于UWB的定位技術(shù)有著精度高、功耗低、抗干擾能力強等優(yōu)點，非常適合室內(nèi)定位領(lǐng)域[1]。在導(dǎo)航制導(dǎo)以及目標(biāo)定位跟蹤領(lǐng)域，卡爾曼濾波作為最佳的線性濾波器被廣泛使用[2]。本文使用DWM1000信號收發(fā)模塊，采用基于雙向飛行時間的測距方法并使用三邊定位法構(gòu)建UWB定位系統(tǒng)實現(xiàn)對室內(nèi)移動人員的實時定位。對帶有噪聲干擾的定位數(shù)據(jù)使用IMM卡爾曼濾波對定位數(shù)據(jù)進行處理，并對處理結(jié)果進行分析。

1 UWB定位原理

UWB定位是基于測量距離的定位技術(shù)，其定位方法有TDOA、三邊定位法等，測距的方法也是多種多樣的[3]。本文采用的是基于飛行時間的雙向測距方法TW-TOF測距，然后使用三邊定位法計算坐標(biāo)。

1.1 TW-TOF

基于飛行時間的雙向測距方法TW-TOF是一種不需要通信雙方時間同步的測距方法，避免了為獲得時間同步增添的額外成本。這種測距方法要求基站和標(biāo)簽都具有信號收發(fā)功能，標(biāo)簽周期性的初始化距離測量信息，基站實時接收回復(fù)標(biāo)簽信號，并測量距離。具體步驟如圖1所示。

圖1 基于飛行時間的雙向測距步驟Fig.1 Steps based on TW-TOF

當(dāng)基站收到標(biāo)簽的輪詢信息，基站記錄此時的時間TRP，并發(fā)送回復(fù)信息給標(biāo)簽，并記錄發(fā)送時間TSR。當(dāng)標(biāo)簽收到回復(fù)信息后，記錄接收時間TRR，并在 TSF時將標(biāo)簽記錄的時間（TSP、TRR、TSF）發(fā)送給基站?；精@得標(biāo)簽發(fā)送的最終信息后獲得足夠的數(shù)據(jù)計算距離并進行以下計算：

（1）標(biāo)簽的信號往返延遲時間 TTRT=（TRR-TSP）減去這期間基站的反應(yīng)時間得到 2次標(biāo)簽到基站的單程飛行時間tTA、tAT。

（2）基站的信號往返延遲時間 TART=（TRF-TSR）減去這期間標(biāo)簽的反應(yīng)時間同理得到2次標(biāo)簽到基站的單程飛行時間。

之后，基站將上邊求得的標(biāo)簽到基站的飛行時間求和并取平均值，得到單程飛行時間TOF。這樣

TRSP跟不相同時，采用不對稱雙向測距以獲得TOF計算結(jié)果：

TOF乘以光速（UWB信號傳遞速度）便得到標(biāo)簽到基站的距離。

1.2 三邊定位法

在通過TW-TOF等方法測得至少3個標(biāo)簽到基站的距離，在上位機或是基站使用三邊定位法對標(biāo)簽的位置進行計算便可得到目標(biāo)的位置坐標(biāo)，如圖2所示。

圖2 三邊定位法原理Fig.2 Principle of three sides location method

可以計算出唯一的交點作為標(biāo)簽點的位置。使用式（3）～式（6），根據(jù)測得的標(biāo)簽到基站的距離 di，可得到標(biāo)簽的位置（x，y）。

式（5）和式（6）中 di（i=1，2，3）為標(biāo)簽分別到 3個基站的距離。

2 卡爾曼濾波

通過TW-TOF測距可以算出標(biāo)簽到各個基站的距離，進而得到目標(biāo)的位置。但在實際測量過程中由于非視距，多徑傳播以及人體等造成的隨機干擾，導(dǎo)致觀測到的信息中往往夾雜有隨機噪聲，造成觀測到的定位的結(jié)果存在誤差。卡爾曼濾波是最佳線性濾波器，實現(xiàn)簡單，是純時域的濾波器，被廣泛使用在導(dǎo)航制導(dǎo)、目標(biāo)定位和跟蹤等工程領(lǐng)域[3]，并且其計算復(fù)雜度低，非常適合移動設(shè)備。使用卡爾曼濾波，利用上一時刻位置信息和本時刻的觀測值對本時刻位置進行最優(yōu)估計，被估計量隨時間變化，這是一種動態(tài)估計。在目標(biāo)跟蹤中，不必知道目標(biāo)的運動模型就能夠?qū)崟r地修正目標(biāo)的狀態(tài)參數(shù)如位置、速度等。

2.1 傳統(tǒng)離散卡爾曼濾波器原理

用卡爾曼濾波獲得最優(yōu)估計值分為2個階段。第一階段使用上一時刻的信息對當(dāng)前時刻的位置進行預(yù)測估計；第二個階段，使用當(dāng)前時刻的帶有噪聲的觀測值對估計值進行修正。傳統(tǒng)離散卡爾曼濾波器由以下5個公式組成。

式（7）為狀態(tài)預(yù)測方程，對當(dāng)前狀態(tài)進行預(yù)測。式中矩陣At為狀態(tài)轉(zhuǎn)移矩陣，表示在沒有噪聲和控制量時2個連續(xù)時刻狀態(tài)的關(guān)系；為當(dāng)前狀態(tài)的估計值；ut為t時刻的控制量，如在定位系統(tǒng)中為動力系統(tǒng)的加速度控制信號；矩陣Bt為控制矩陣，描述控制量如何作用于當(dāng)前狀態(tài)。式（8）為預(yù)測估計誤差的協(xié)方差方程。式中Pt

式（9）、式（10）為狀態(tài)更新方程和最優(yōu)增益矩陣方程。式中為最優(yōu)估計值；Kt為最優(yōu)增益；為實際觀測值與預(yù)期觀測值的殘差；H為觀測矩陣。

卡爾曼濾波器最后一個方程為濾波誤差協(xié)方差更新方程：

2.2 交互多模型卡爾曼濾波原理

試驗中發(fā)現(xiàn)，傳統(tǒng)卡爾曼濾波能有效降低做勻速直線運動定位目標(biāo)的噪聲干擾，得到理想的濾波效果。但當(dāng)定位目標(biāo)進行直角轉(zhuǎn)彎或加減速這些室內(nèi)常見動作時，僅采用基本的卡爾曼濾波會出現(xiàn)“滯后”等現(xiàn)象，短時間內(nèi)造成較大的誤差。這時需要自適應(yīng)算法。交互多模型算法IMM使用2個或更多的模型來描述工作過程中可能的狀態(tài)，最后通過有效的加權(quán)融合進行系統(tǒng)狀態(tài)估計，很好地克服了單模型估計誤差較大的問題。

IMM算法結(jié)構(gòu)由5部分組成，如圖3所示，包括計算模型混合概率、輸入交互、對各模型進行卡爾曼濾波、模型概率更新以及輸出交互[4]。

圖3 IMM算法步驟Fig.3 Steps of IMM algorithm

2.2.1 計算模型混合概率

假定目標(biāo)有r種運動狀態(tài)，對應(yīng)有r種狀態(tài)轉(zhuǎn)移方程。各模型之間的轉(zhuǎn)移由馬爾可夫概率轉(zhuǎn)移矩陣確定，其中元素pij表示目標(biāo)由第i個運動模型轉(zhuǎn)移到第j個運動模型的概率，概率轉(zhuǎn)移矩陣如下：

模型i到模型j的混合概率為

式中：μi（k-1）為上一時刻每個濾波器的模型概率；j為模型 j的預(yù)測概率（歸一化常數(shù)），如式（13）所示：

2.2.2 輸入交互

在這一部分，要重新初始化濾波輸入，由上一時刻各模型估計與混合概率得到混合狀態(tài)估計0j（k-1|k-1）和混合協(xié)方差估計 P0j（k-1|k-1），將混合估計作為當(dāng)前循環(huán)的初始。對模型j，混合狀態(tài)估計混合協(xié)方差估計方程可分別由式（14）～式（16）得到。式中：i（k-1|k-1），Pi（k-1|k-1）為上一時刻的狀態(tài)估計與協(xié)方差估計。

2.2.3 各模型卡爾曼濾波

這一部分，對不同的運動模型選擇合適的卡爾曼濾波器進行濾波。

2.2.4 模型概率更新

模型概率計算是假設(shè)檢驗過程，即采用貝葉斯假設(shè)檢驗方法檢驗濾波器組各個濾波器的殘差[5]。采用似然函數(shù)來更新模型概率μj（k），模型j的似然函數(shù)為

式中：vj（k）為濾波殘差，如式（18）所示，Sj（k）可由式（19）得到：

模型j的概率為

式中：c為歸一化常數(shù)，如式（21）所示：

2.2.5 輸出交互

由第四步獲得更新的模型概率后，輸出交互，對每個濾波器的估計結(jié)果加權(quán)合并，得到總的狀態(tài)估計（k|k）和總的協(xié)方差估計模型 P（k|k）。

3 試驗分析

3.1 試驗設(shè)計

在室內(nèi)環(huán)境中，定位目標(biāo)經(jīng)常要進行90°直角拐彎，本文對此要求在室內(nèi)環(huán)境設(shè)計了路徑試驗對比檢驗傳統(tǒng)卡爾曼濾波與IMM濾波效果。首先，使用3基站1標(biāo)簽采集定位數(shù)據(jù)?；景慈切畏植荚冢?，0），（0，3.8），（5.6，2.2）3 個空間固定坐標(biāo)位置。試驗人員按設(shè)定路線從（0，1.4）開始沿x軸正方向運動4.8 m后轉(zhuǎn)90°沿y軸正方向運動2.4 m后再轉(zhuǎn)90°沿x軸負方向運動4.8 m（由于標(biāo)簽由試驗人員手拿采集數(shù)據(jù)，得到的位置與預(yù)定軌道有一定的偏移），如圖4所示。試驗的采樣周期為0.1 s，在基站A0（0，0）處用USB數(shù)據(jù)線連接電腦通過串口調(diào)試助手對標(biāo)簽到3基站的距離數(shù)據(jù)進行采集。使用式（3）～式（6）計算出所有坐標(biāo)，并進行卡爾曼濾波。傳統(tǒng)卡爾曼濾波與IMM卡爾曼濾波效果如圖5所示。并對傳統(tǒng)卡爾曼濾波和IMM卡爾曼濾波做RMS分析，得到其誤差跟蹤圖，如圖6所示。

圖4 基站布置以及標(biāo)簽移動路徑Fig.4 Base station layout and label moving path

圖5 傳統(tǒng)卡爾曼與IMM濾波觀測值軌跡和軌跡Fig.5 Observation trajectory and trajectory of traditional Kalman and IMM filtering

圖6 傳統(tǒng)卡爾曼與IMM濾波誤差跟蹤圖Fig.6 Traditional Kalman and IMM filtering error tracking map

3.2 試驗結(jié)果分析

從圖5可以觀察到，在沿x軸運動初期，卡爾曼濾波對目標(biāo)誤差消除作用是非常明顯的，其曲線相比于觀測值更加平滑。但在x軸向y軸轉(zhuǎn)向和y軸向x軸兩次90°轉(zhuǎn)向之后的過程中，卡爾曼濾波值相對于觀測值都產(chǎn)生了“滯后”的現(xiàn)象，造成短時間內(nèi)誤差值甚至大于觀測值。傳統(tǒng)卡爾曼濾波由于是建立在模型精確和隨機干擾信號統(tǒng)計特性已知的情況下，在卡爾曼濾波整個過程中干擾信號的統(tǒng)計學(xué)特征值保持不變[6]。然而，試驗人員在轉(zhuǎn)彎時突然的加速與減速使得干擾信號統(tǒng)計學(xué)特性發(fā)生了改變[7]。交互多模型采用多個卡爾曼濾波器并行處理，每個濾波器對應(yīng)不同的狀態(tài)空間模型，不同的狀態(tài)空間模型描述不同的目標(biāo)運動狀態(tài)，最后通過有效的加權(quán)融合進行系統(tǒng)估計，很好地克服了傳統(tǒng)卡爾曼濾波單模型估計誤差較大的問題。如圖5所示，在第一次y軸向x軸90°轉(zhuǎn)向之后，傳統(tǒng)卡爾曼濾波會逐漸恢復(fù)其功能，但短時間內(nèi)會造成極大的誤差，如圖上第一次x軸向y軸轉(zhuǎn)向，x軸方向上的誤差最大接近50 cm。IMM濾波雖然在第一次轉(zhuǎn)彎的時候也產(chǎn)生了“滯后”現(xiàn)象，但相比較傳統(tǒng)卡爾曼濾波，其造成的誤差更小，并且收斂的速度明顯快于傳統(tǒng)卡爾曼濾波。并且從圖6傳統(tǒng)卡爾曼濾波與IMM濾波的誤差跟蹤圖中可以明顯看出，IMM的濾波效果要優(yōu)于傳統(tǒng)卡爾曼濾波。

4 結(jié)語

采用UWB技術(shù)的室內(nèi)定位有著精度高、功耗低、傳輸速率快、抗多徑干擾能力強等優(yōu)點，目前基于UWB技術(shù)的定位多采用基于測量距離的定位方法[8]。本文介紹了基于飛行時間的雙向測距方法（TWTOF）的過程。對于室內(nèi)環(huán)境中干擾多以及傳統(tǒng)卡爾曼濾波在目標(biāo)機動時濾波效果不好的特點，本文提出使用交互多模型卡爾曼濾波的方法對定位坐標(biāo)進行最優(yōu)估計以降低誤差，并針對室內(nèi)環(huán)境設(shè)計了試驗檢驗其效果。試驗結(jié)果顯示，交互多模型卡爾曼濾波能有效的減少誤差，在室內(nèi)環(huán)境經(jīng)常遇到的大轉(zhuǎn)角情況下，卡爾曼濾波效果雖然在短時間內(nèi)仍會產(chǎn)生誤差，其濾波值誤差要高于觀測值，但其能更加快速的恢復(fù)濾波效果?？傮w上，交互多模型卡爾曼濾波對觀測值的優(yōu)化效果十分顯著。

[1]楊洲.基于UWB/MEMS的高精度室內(nèi)定位技術(shù)研究[D].徐州：中國礦業(yè)大學(xué)，2015.

[2]張桀，沈重.聯(lián)合TDOA改進算法和卡爾曼濾波的UWB室內(nèi)定位研究[J].現(xiàn)代電子技術(shù)，2016，39（13）：1-5.

[3]邵磊.基于超寬頻距離傳感器的室內(nèi)定位系統(tǒng)的研究與實現(xiàn)[D].北京：北京工業(yè)大學(xué)，2014.

[4]王磊，程向紅.基于期望模式修正的交互多模型組合導(dǎo)航算法[J].光學(xué)精密工程，2014，22（3）：737-744.

[5]Pelka M，Goronzy G，Hellbrück H.Iterative approach for anchor configuration of positioning systems[J].Ict Express，2016，2（1）：1-4.

[6]Zhang R，H?flinger F，Reindl L.TDOA-Based localization using interacting multiple model estimator and ultrasonic transmitter/receiver[J].IEEE Transactions on Instrumentation&Measurement，2013，62（8）：2205-2214.

[7]Hu C，Chen W，Chen Y，et al.Adaptive Kalman filtering for vehicle navigation[J].Journal of Global Positioning Systems，2003，2（1）：42-47.

[8]Abdulrahman A，Abdulmalik A S，Mansour A，et al.Ultra wideband indoor positioning technologies：analysis and recent advances[J].Sensors，2016，16（5）：1-36.