基于TDOA的自適應擴展卡爾曼濾波跟蹤算法研究?

周恭謙 楊露菁 劉 忠

（海軍工程大學電子工程學院 武漢 430033）

1 引言

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

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

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

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

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

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

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

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

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

移動待測目標的狀態方程如式（5）所示

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

噪聲向量Wˉk-1的協方差矩陣為Qk-1，狀態轉移矩陣為φ'，它們的定義如下：

其對應的觀測過程如式（7）所示

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

其中

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

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

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

2.2 自適應迭代擴展卡爾曼濾波

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

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

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

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

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

對式（15）求解 λk可得

當 λk＜1時，系統噪聲不進行迭代，而當 λk≥1時，系統噪聲進行迭代，此時隨著λk增大系統噪聲增大，能夠實時調整增益矩陣，從而提高測量精度。

2.3 仿真實驗

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

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

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

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

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

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

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

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

3 結語

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