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

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

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

1 UWB定位原理

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

1.1 TW-TOF

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

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

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

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

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

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

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

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

1.2 三邊定位法

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

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

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

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

2 卡爾曼濾波

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

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

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

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

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

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

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

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

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

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

2.2.1 計(jì)算模型混合概率

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

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

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

2.2.2 輸入交互

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

2.2.3 各模型卡爾曼濾波

這一部分，對不同的運(yùn)動(dòng)模型選擇合適的卡爾曼濾波器進(jìn)行濾波。

2.2.4 模型概率更新

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

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

模型j的概率為

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

2.2.5 輸出交互

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

3 試驗(yàn)分析

3.1 試驗(yàn)設(shè)計(jì)

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

圖4 基站布置以及標(biāo)簽移動(dòng)路徑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 試驗(yàn)結(jié)果分析

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

4 結(jié)語

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

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

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

[3]邵磊.基于超寬頻距離傳感器的室內(nèi)定位系統(tǒng)的研究與實(shí)現(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.