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

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

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

0 引言

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

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

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

1 三維機動模型

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

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

其中w為旋轉(zhuǎn)速率，并定義為

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

對應的離散形式為

其中，T為采樣周期。

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

2 濾波算法

2.1 IMM 算法

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

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

2.2 角速度估計算法

角速度估計是實現(xiàn)AIMM算法的基礎(chǔ)，通過構(gòu)造角速度估計的狀態(tài)方程和偽測量方程，綜合考慮角速度的估計值和估計方差，利用卡爾曼濾波得到角速度的估計值。狀態(tài)方程和偽測量方程為

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

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

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

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

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

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

2.3 IMM算法和角速度估計相結(jié)合

IMM算法和角速度估計融合過程的具體結(jié)構(gòu)如圖1所示。

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

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

3 算法實現(xiàn)與仿真分析

假設飛行曲線如圖2所示，目標從A點(30 km，30 km，30 km)出發(fā)，0～50 s做勻速直線運動;50～100 s在B點做順時針協(xié)同轉(zhuǎn)彎，初始加速度為(40，－80，20)(單位為m/s2)，此時目標機動為9.3522g，且加速度向量與速度向量垂直，滿足在同一個平面內(nèi)做圓周運動時速度和加速度垂直的條件。之后100～200 s目標繼續(xù)做勻速直線運動;200～250 s繞C點作逆時針運動，此時的加速度初始向量為(－50，50，－62)(單位為m/s2)，目標機動為9.59622g，且轉(zhuǎn)動不滿足在同一個平面內(nèi)做圓周運動的條件;之后250～300 s目標繼續(xù)做勻速直線運動直到終點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可以看出，通過角速度估計以后，相比偽角速度觀測值，角速度估計值曲線更加平滑穩(wěn)定，更加接近于真實值曲線。由圖5可以看出，無論有沒有經(jīng)過角速度估計，npe的值基本都在0.6～0.8之間，因此濾波算法大大減小了觀測誤差。

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

4 結(jié)論

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

［1］ 孫福明.機動目標跟蹤狀態(tài)估計與數(shù)據(jù)關(guān)聯(lián)技術(shù)的研究［D］.合肥:中國科學技術(shù)大學，2007.

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

［3］ 周宏仁，敬忠良，王培德.機動目標跟蹤［M］.北京:國防工業(yè)出版社，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］ 何衍.機動目標跟蹤與傳感器網(wǎng)絡自組織［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.