周恭謙 楊露菁 劉 忠

（海軍工程大學(xué)電子工程學(xué)院 武漢 430033）

1 引言

UWB無線定位技術(shù)是目前定位領(lǐng)域研究的熱點之一，無論是在軍事領(lǐng)域還是商業(yè)領(lǐng)域都有廣泛的應(yīng)用價值。高精度、低功耗的特點使UWB無線定位技術(shù)成為未來定位技術(shù)的首選。UWB脈沖由于具有極高的帶寬，持續(xù)時間短至ns級，因而具有很強的時間分辨能力［1］。為了充分利用UWB時間分辨能力強這個特點，使用基于信號到達時間估計（TOA）的測距技術(shù)是最適合于以UWB無線定位的。UWB脈沖的TOA估計算法近幾年得到了較為充分的研究，包括采用高采樣速率、高精度的匹配濾波技術(shù)（Match Filter，MF）來實現(xiàn)的基于相干檢測的TOA 估計算法［2～4］，以及采用較低采樣率、降低了復(fù)雜度的基于非相干能量檢測的TOA估計算法［5～8］。在國外，UWB 定位上的研究主要有：Win和Scholtz提出多徑UWB環(huán)境下的最大似然（ML）檢測方法［9］；Cramer等基于CLEAN算法提出了一種包 含 TOA、AOA 參 數(shù) 的 UWB 信 道 模 型［10］；Joon-Yong Lee等提出了在UWB無線鏈路上基于TOA的范圍測量方法［2］。而TOA方法是利用信號的傳播時延進行測距，由于UWB信號具有良好的時間分辨率，信道環(huán)境的變化對信號的傳播時間也沒有影響，因此基于時間的定位具有相對較高的精度。但目標節(jié)點和基站之間時鐘的嚴格同步存在困難，如果不完全同步將造成很大的定位誤差。TDOA在實際中實現(xiàn)該技術(shù)的可能性與TOA相比要高的多，這是因為它不要求基站與待測目標之間時鐘同步，只需要每個基站之間時鐘同步就能得到TDOA值，并且在對到達時間做差的過程中會減小部分誤差，定位精度能相對提高。而針對TDOA動態(tài)目標跟蹤定位問題張麗［11］提出了一種基于TDOA定位的擴展卡爾曼濾波算法（EKF）并將其應(yīng)用到UWB系統(tǒng)中，通過仿真結(jié)果和Taylor算法，Chan式算法對比表明該算法能夠降低定位誤差，實現(xiàn)良好的跟蹤定位，但該算法當TDOA測量誤差較大時定位精度降低而且容易發(fā)散，針對這種情況，本文提出了一種自適應(yīng)迭代擴展卡爾曼濾波算法（AIEKF），并通過仿真對比證明了該算法在測距誤差增大時，仍能保持良好的定位精度和收斂速度。

2 基于UWB的TDOA跟蹤算法研究

在無線定位中，一旦取得TDOA測量值，就可以得到待測目標到兩個定位基站之間的距離差，多個TDOA測量值就可以構(gòu)成一組關(guān)于待測目標位置的雙曲線方程組，求解該雙曲線方程組就可得到待測目標的估計位置。設(shè)（x，y）為待測目標的待估計位置，(xi，yi)為第i個定位基站的已知位置，則待測目標和第i個定位基站之間的距離ri可以表示為

對式（1）求解即可得到待測節(jié)點坐標（x，y）。

2.1 基于TDOA定位的擴展卡爾曼濾波算法

卡爾曼濾波算法適用于解決噪聲為高斯噪聲的線性問題，但由于實際中經(jīng)常會碰到非線性問題，因此研究者對卡爾曼濾波算法進行改進，將卡爾曼濾波方法推廣到非線性問題，提出了處理非線性問題的擴展卡爾曼濾波（EKF）算法。非線性系統(tǒng)的狀態(tài)方程和觀測方程分別為［12］

對于TDOA二維定位，設(shè)參與定位的基站數(shù)為M（M ≥3），可以得到M-1個TDOA數(shù)據(jù)，系統(tǒng)的狀態(tài)方程是線性的，觀測方程是非線性的，系統(tǒng)的觀測方程如式（4）所示。

ri為待測目標和第i個定位基站之間的距離其表達式可見式（1），vi1表示觀測噪聲。

對應(yīng)觀測方程Y=h(X)+V的矩陣形式的各個參數(shù)可表示為

移動待測目標的狀態(tài)方程如式（5）所示

其中Sk=[xk， yk， vxk， vyk]T是tk時刻的狀態(tài)向量，（xk，yk）是目標節(jié)點在tk時刻的二維坐標信息，vxk，vyk分別代表目標節(jié)點在x方向和y方向的分速度。

噪聲向量Wˉk-1的協(xié)方差矩陣為Qk-1，狀態(tài)轉(zhuǎn)移矩陣為φ'，它們的定義如下：

其對應(yīng)的觀測過程如式（7）所示

其中Zk是測量數(shù)據(jù)向量，h(Sk)為非線性變換，Vˉk是測量噪聲，則協(xié)方差矩陣R'可表示為

其中

由于h(Sk)是非線性變換，對h(Sk)利用高等數(shù)學(xué)中的Taylor級數(shù)展開，為k時刻 h(Sk)在處的Jacoby矩陣。

擴展卡爾曼濾波的運算與卡爾曼濾波方法相似，同樣由兩個遞歸過程完成。

不同的是，在擴展卡爾曼濾波過程中函數(shù)h(Sk)需要進行線性近似，通過非線性函數(shù)的局部線性化特性來近似描述其非線性。擴展卡爾曼的算法過程如下：

2.2 自適應(yīng)迭代擴展卡爾曼濾波

EKF系統(tǒng)中，測量誤差增大時Vk=h(Sk)-Zk會增大，而增益矩陣因離線計算的原因，未能根據(jù)實時的狀態(tài)在線調(diào)整增益矩陣，仍將趨于極小值，不會隨測量誤差Vk的增大而變大，導(dǎo)致EKF濾波精度下降，甚至發(fā)散。傳統(tǒng)傳統(tǒng)FEKF［13］在式（21）中引入 λk

欲使狀態(tài)估計值接近于真實值此時滿足［14］：

其中AK表示誤差方差矩陣，此時

欲使式（14）右邊滿足等式則有

將式（10）代入式（14）化簡可得：

對式（15）求解 λk可得

當 λk＜1時，系統(tǒng)噪聲不進行迭代，而當 λk≥1時，系統(tǒng)噪聲進行迭代，此時隨著λk增大系統(tǒng)噪聲增大，能夠?qū)崟r調(diào)整增益矩陣，從而提高測量精度。

2.3 仿真實驗

假設(shè)目標作勻速直線運動，初始位置坐標為(1000m，1000m)，航向角 KW=π/4，速度 vx=10m/s，vy=10m/s。采用3個基站，其坐標依次為（0，0），（2400，0），（0，2200），采樣時間為0.1s，信道采用IEEE802.15.4a標準中的室內(nèi)居住環(huán)境CM1（LOS）信道。

當測距誤差服從均值為0，均方差為1m的高斯白噪聲，其跟蹤曲線和速度估計如圖1、2。當測距誤差服從均值為0，均方差為100m的高斯白噪聲，其跟蹤曲線和速度估計如圖3、4所示。其算法性能比較如圖8所示，其中均方根誤差RMSE（Root-Mean-Square error）可由式（17）計算：

從圖1和圖3對比可知當測距誤差改變時自適應(yīng)迭代卡爾曼濾波定位精度明顯上升，這是因為當測量誤差增大時，漸消因子也隨之改變從而系統(tǒng)噪聲也隨之變化，最后導(dǎo)致增益矩陣的改變，定位精度隨之提高，圖2和圖4對比顯示出在對目標跟蹤定位上自適應(yīng)迭代卡爾曼濾波具有更高的定位精度，這也是和漸消因子的引入相關(guān)的。從圖5對比兩種算法的RMSE可以看出，自適應(yīng)迭代濾波在保持高定位精度的同時能夠更快速的收斂，從而驗證了其比擴展卡爾曼濾波具有更高的定位精度和更快的收斂性。

圖1 測距誤差較小時跟蹤曲線

圖2 測距誤差較小時目標速度估計

圖3 測距誤差較大時跟蹤曲線

圖5 測距誤差較大時EKF與AIEKF算法性能比較

圖4 測距誤差較大時目標速度估計

3 結(jié)語

本文對UWB中基于卡爾曼濾波的TDOA跟蹤定位算法研究。針對EKF算法會隨著測量誤差增大定位精度降低這一缺點，提出了一種自適應(yīng)迭代擴展卡爾曼濾波算法（AIEKF），通過仿真對比表明該算法在測距誤差增大時，該算法能保持良好的定位精度和收斂速度。