角速度估計自適應的IMM三維目標跟蹤算法

田雨芬， 蔣 宏， 丁全心， 梁國威

(1.北京航空航天大學控制一體化技術國家級科技重點實驗室，北京 100191;2.光電控制技術重點實驗室，河南 洛陽 471009)

0 引言

無論在國防領域還是民用領域，目標跟蹤都占據著重要的位置［1］。精確的目標跟蹤是對目標進行成功打擊的前提和基礎。因此，提高機動目標的跟蹤精度是許多科學家、工程師致力研究的課題。但是國內外大量的研究、仿真都是基于二維平面的。而現實中的許多機動往往都是在三維空間內發生的，所以將二維平面的模型和跟蹤算法擴展到三維空間進行研究，具有重要的現實意義［2］。

交互式多模型算法(IMM)是將一組模型交互進行跟蹤的算法。因IMM算法綜合了不同濾波模型對目標狀態進行估計，所以從全局角度講，即便是三維機動目標，其跟蹤性能也是比較好的［3］。但是由于目標機動具有不確定性，為了更好地覆蓋大范圍機動，需要利用參數估計實現模型的自適應調整(AIMM)［4］。本文通過在模型中加入角速度估計，對目標運動角速度進行濾波求精，隨著時間的變化，角速度的估計值和真實值之間的差值越來越小。

本文在濾波時選用CV模型和帶有角速度估計的CSCT模型進行交互。

1 三維機動模型

常速率協同轉彎模型(CSCT模型)是假設目標以一個常速率V(即速度和加速度垂直)在一個平面內做圓周運動，Ω指的是角速度向量。

由于假設的目標軌跡是在同一平面內，則Ω垂直于a和v決定的平面，即Ω⊥v，a·v=0，因此有

其中w為旋轉速率，并定義為

若加速度受到噪聲干擾，其中ω為白噪聲，則有

對應的離散形式為

其中，T為采樣周期。

CSCT模型與參數w的取值有很大關系，而w的值則由式(3)得到，這樣在已知w的情況下，可以保持濾波方程是線性的。但是模型的應用需要滿足條件a·v=0，如果不滿足條件時，濾波誤差會相對大一些。所以解決辦法就是把a·v=0作為運動約束加入濾波迭代過程中［2，5－6］。

2 濾波算法

2.1 IMM 算法

交互多模型算法將目標的機動建模為帶馬爾可夫系數的多個線性模型，利用模型間數據的交互來提高算法的性能。

IMM算法的一個循環包括4步:交互、n個模型濾波、模型概率更新、組合［7］。只要選取了合適的運動模型，通過IMM算法就可以很好地反映目標實際情況，否則交互多模型算法可能還不如單一算法精確;這樣，對目標模型的選取很容易陷入一個誤區，就是盡可能選取大的模型集，使模型集可以囊括目標運動的所有可能情況，但是很明顯這樣會增大算法的計算量，所以迫切需要一種方法，可以通過參數的自適應估計適應目標的不確定機動，即如下所說在濾波算法中加入角速度估計［7］。

2.2 角速度估計算法

角速度估計是實現AIMM算法的基礎，通過構造角速度估計的狀態方程和偽測量方程，綜合考慮角速度的估計值和估計方差，利用卡爾曼濾波得到角速度的估計值。狀態方程和偽測量方程為

式中:zw(k)為角速度觀測值;w(k)為角速度隨機變化量，方差為Qw(k);vw(k)為角速度觀測噪聲，方差為Rw(k);Qw(k)和Rw(k)相互獨立。由于狀態轉移矩陣和觀測矩陣均為1，所以根據類似卡爾曼濾波公式，可得到角速度估計的迭代公式為

其中:w⌒(k)為角速度估計值;Pw(k)為角速度估計方差;Kw(k)為增益。

由于zw(k)的求取公式是強非線性的，所以一般采用Monte Carlo方法求解zw(k)的方差Rw(k)，但計算量太大。本文采用Julier最近提出的一種新的求解方法，隨機變量經非線性變換后，可以不需要計算雅可比矩陣，來估計非線性變換后變量的方差。所得的估計方差的精度可達到方差的四階泰勒展開式以上［8］。方差的近似計算公式為

式中:zi(k)為式(12)中采樣點的非線性變換值;wi為每個采樣點的權值。采樣點按文獻［8］選取，此時三維下選取13個采樣點。由于在三維空間中共有3個相互呈直角關系的平面。所以求解總的zi(k)時分別是各個平面zi(k)的疊加。

求角速度變化方差時，假設ωw=0的概率為P0，時，ωw在±A之間均勻分布，則有

以上便是角速度估計迭代過程［7－9］。

2.3 IMM算法和角速度估計相結合

IMM算法和角速度估計融合過程的具體結構如圖1所示。

圖1 IMM算法與角速度估計相融合Fig.1 Combination of IMM algorithm and angular velocity estimation

將CSCT模型在k－1時刻的角速度信息進行估計，得到角速度估計值，再與CV模型交互就會得到k時刻的各個濾波器的信息。

3 算法實現與仿真分析

假設飛行曲線如圖2所示，目標從A點(30 km，30 km，30 km)出發，0～50 s做勻速直線運動;50～100 s在B點做順時針協同轉彎，初始加速度為(40，－80，20)(單位為m/s2)，此時目標機動為9.3522g，且加速度向量與速度向量垂直，滿足在同一個平面內做圓周運動時速度和加速度垂直的條件。之后100～200 s目標繼續做勻速直線運動;200～250 s繞C點作逆時針運動，此時的加速度初始向量為(－50，50，－62)(單位為m/s2)，目標機動為9.59622g，且轉動不滿足在同一個平面內做圓周運動的條件;之后250～300 s目標繼續做勻速直線運動直到終點D。

圖2 三維機動目標曲線Fig.2 3D maneuvering target curve

圖3 角速度變化曲線Fig.3 Angular velocity curve

圖4 濾波曲線和觀測曲線的均方根誤差對比圖Fig.4 RMSE curve of filter and observations

圖5 npe曲線圖Fig.5 npe curve

由圖3可以看出，通過角速度估計以后，相比偽角速度觀測值，角速度估計值曲線更加平滑穩定，更加接近于真實值曲線。由圖5可以看出，無論有沒有經過角速度估計，npe的值基本都在0.6～0.8之間，因此濾波算法大大減小了觀測誤差。

由圖4和圖5還可以得出，經過角速度估計之后，均方根誤差值和npe值更小，尤其在B彎和C彎處，角速度估計后的曲線具有明顯優勢。但是由于目標在B彎的轉彎角速率是由角速度和速度的估計值計算得到的，滿足CSCT模型同一時刻速度和加速度垂直的使用條件，即a·v=0，而目標在C彎的轉彎角速率則不滿足該條件。所以從圖中都可以看出，B彎時的濾波精度比C彎時的濾波精度要高。

4 結論

本文針對三維空間內的目標機動，提出了將角速度估計運用到三維帶約束常速率轉彎模型中，并且與CV模型進行交互的算法，實現模型的自適應調整。通過對目標角速度進行濾波求精，增加濾波迭代的準確性，隨著濾波時間的疊加，角速度濾波值與真實值之間的誤差越來越小，說明角速度估計算法的實用性。同時本文直接研究的三維目標機動比二維目標機動更具有現實意義。仿真結果表明，即便目標有較大的機動(本文設計的曲線中機動已經達到了9g以上)，濾波精度依然較高，因此本文提出的算法很有效。

［1］ 孫福明.機動目標跟蹤狀態估計與數據關聯技術的研究［D］.合肥:中國科學技術大學，2007.

［2］ 彭冬亮，郭云飛，薛安克.三維高速機動目標跟蹤交互式多模型算法［J］.控制理論與應用，2008，25(5):831－836.

［3］ 周宏仁，敬忠良，王培德.機動目標跟蹤［M］.北京:國防工業出版社，1991.

［4］ EFE M，ATHERTON D P.Interacting model maneuvering target tracking using adaptive turn rate models in the algorithm［C］//Proceedings of the 35th Conference on Decision and Control，1996:3151－3156.

［5］ LI X R，JILKOV V P.A survey of maneuvering target tracking:Dynamic models［C］//Proceedings of SPIE Conference on Signal and Data Processing of Small Targets，2000，4048:212－235.

［6］ MATSUZAKI T，KAMEDA H，TSUJIMICHI S，et al.Maneuvering target tracking using constant velocity and constant angular velocity model［C］//IEEE Transactions on Automatic Control，2000，33(8):3230－3234.

［7］ 何衍.機動目標跟蹤與傳感器網絡自組織［D］.杭州:浙江大學，2001.

［8］ JULIER S，UHIMANN J，DURRANT－WHYTE H F.A new method for the nonlinear transformation of means and covariances in filters and estimators［C］//IEEE Transactions on Automatic Control，2000，45:477－482.

［9］ TIAN Ye，JIANG Hong.Turn rate estimation based adaptive IMM algorithm for maneuvering target tracking［C］//2010 International Conference on Electrical Engineering and Automatic Control(ICEEAC)，2010:444－447.