基于IMM－SRCDKF的高速機動目標(biāo)運動狀態(tài)估計算法研究

付 磊，張 龍，張 丁

（上海機電工程研究所，上海 201109）

0 引 言

隨著各國對臨近空間高速飛行器研究的不斷深入，該類飛行器具備長時間大范圍的機動能力，能夠打破現(xiàn)有的戰(zhàn)略平衡，已經(jīng)成為未來戰(zhàn)爭中的重要武器之一。各國飛行試驗的成功也標(biāo)志著高速飛行器技術(shù)正在逐步成熟，其突出的戰(zhàn)略意義對臨近空間高速武器的防御技術(shù)提出了更高的要求［1－3］。因此，在發(fā)展臨近空間高速飛行器技術(shù)的同時，也應(yīng)該加強其防御策略的研究。

在對臨近空間高速飛行器的攔截制導(dǎo)過程中，必然需要利用外部信息支援預(yù)警系統(tǒng)對機動彈道、運動信息等進行估計，并且需要在攔截器中末制導(dǎo)階段為攔截器提供與目標(biāo)間相對運動的精確信息［4］。針對高速機動目標(biāo)運動狀態(tài)估計問題，主要從構(gòu)造目標(biāo)模型、機動檢測等不同角度來提高估計精度。交互式多模型方法（interaction multiple model，IMM）是目前工程中廣泛應(yīng)用的方法之一，它主要通過設(shè)計多種模型來代表系統(tǒng)所有可能的行為方式，并對各模型的估計結(jié)果進行融合，從而綜合不同模型的優(yōu)點。目前，IMM方法在應(yīng)用中的實現(xiàn)主要是基于擴展卡爾曼濾波（Extended Kalman filter，EKF）算法。隨著 EKF算法研究和應(yīng)用的深入，其缺點也慢慢顯現(xiàn)。如有時需要計算復(fù)雜的Jacobi矩陣，EKF算法精度不高，甚至發(fā)散。

針對這些不足，有研究人員提出了粒子濾波（particle filter，PF）等算法，但此類方法計算量過大，很難在工程中應(yīng)用。而近些年出現(xiàn)的一類非線性濾波方 法，如 Sigma－Point卡 爾 曼 濾 波 （Sigma－Point Kalman filter，SPKF），以其運算量小、計算穩(wěn)定、精度高和不需要計算Jacobi矩陣等優(yōu)點，漸漸被工程技術(shù)人員所關(guān)注，正成為替代EKF的較為理想的算法。SPKF方法利用加權(quán)統(tǒng)計線性化回歸技術(shù)，通過一組確定性采樣點（Sigma點）來捕獲系統(tǒng)的相關(guān)統(tǒng)計參量，從而避免了EKF方法中的Jacobi矩陣計算，也比PF方法節(jié)約了大量運算過程。根據(jù)Sigma點選取的不同，SPKF方法主要分為無跡卡爾曼濾波（unscented Kalman filter，UKF）和中心差 分卡爾曼濾 波（central difference Kalman filter，CDKF）。與UKF相比，CDKF具有理論精度稍高和更易實現(xiàn)等特點。為克服舍入誤差可能引起的濾波發(fā)散，有學(xué)者進一步提出了平方根（square root，SR）形式的CDKF方法，保證了協(xié)方差矩陣在傳播過程中的半正定性，從而具有良好的數(shù)值穩(wěn)定性。

本文針對臨近空間高速目標(biāo)的運動狀態(tài)估計問題，結(jié)合交互多模型算法提出了IMM－SRCDKF目標(biāo)信息估計算法，通過對比仿真驗證了該算法對臨近空間高速機動目標(biāo)具有更好適應(yīng)性的特點。

1 目標(biāo)運動模型

對于臨近空間高速目標(biāo)，目標(biāo)運動模型的準(zhǔn)確性是影響目標(biāo)跟蹤性能的關(guān)鍵因素，建立符合實際目標(biāo)飛行特性的目標(biāo)運動模型以及便于數(shù)學(xué)計算的模型是實現(xiàn)目標(biāo)信息提取的首要條件。由于目標(biāo)機動狀態(tài)具有較多的外界因素以及人為控制的影響，無法事先確切知道目標(biāo)機動情況，因此建立和選取合適的目標(biāo)運動模型是實現(xiàn)目標(biāo)運動狀態(tài)估計的難點之一。

“當(dāng)前”統(tǒng)計模型的主要思想是當(dāng)目標(biāo)以某一加速度做機動時，在下一時刻其機動加速度只可能在該機動加速度的鄰域范圍內(nèi)改變。因此，對于不同的目標(biāo)機動情況，只需考慮目標(biāo)在當(dāng)前狀態(tài)的機動加速度的概率密度，并不需要考慮目標(biāo)機動加速度變化的所有可能性。相關(guān)研究和應(yīng)用都表明“當(dāng)前”統(tǒng)計模型能更準(zhǔn)確地描述目標(biāo)機動模型［5］。

以臨近空間高速飛行器為例，“當(dāng)前”統(tǒng)計模型假設(shè)飛行器機動加速度服從修正的瑞利分布，且均值為當(dāng)前時刻的機動加速度，并利用機動加速度的均值與方差之間的關(guān)系來建立機動加速度的均值和方差自適應(yīng)算法。

目標(biāo)運動模型離散狀態(tài)方程方程如式（1）所示。

式中：X（k｜k）為k時刻目標(biāo)狀態(tài)矩陣；

X（k＋1｜k）為k＋1時刻狀態(tài)預(yù)測矩陣；

W（k）為系統(tǒng)噪聲矩陣，其協(xié)方差矩陣為

2 觀測模型

根據(jù)雷達的工作原理，雷達觀測模型建立在雷達球坐標(biāo)系，以距離R、方位角q1、高低角q2描述目標(biāo)位置。該坐標(biāo)系與雷達直角坐標(biāo)系Orxryrzr重合，是一種常見的非慣性坐標(biāo)。雷達直角坐標(biāo)系和飛行器的地理坐標(biāo)系相似，坐標(biāo)原點Or為雷達站中心，坐標(biāo)軸Orxr和Oryr是地球參考球體的切線，分別指向地理東向和北向，而Orzr軸垂直于水平面向上，因此該坐標(biāo)系也被稱為東北天地理坐標(biāo)系。地心坐標(biāo)系與雷達直角坐標(biāo)系之間的關(guān)系如圖1所示，雷達直角坐標(biāo)系與雷達球坐標(biāo)系的關(guān)系如圖2所示。

圖1 地心坐標(biāo)系與雷達直角坐標(biāo)系Fig．1 Geocentric coordinate system and Radar rectangular coordinate system

圖2 雷達直角坐標(biāo)系與雷達球坐標(biāo)系Fig．2 Radar rectangular coordinate system and radar spherical coordinate system

對于圖2雷達球坐標(biāo)系，雷達測量值Z＝［r，q1，q2］T，由幾何關(guān)系可得

3 平方根中心差分卡爾曼濾波器

相比于EKF算法，平方根中心差分卡爾曼濾波（square root central difference Kalman filter，SRCDKF）算法與UKF算法都是基于Sigma點的卡爾曼濾波算法。SRCDKF借助Sterling插值獲得后驗均值和協(xié)方差，不需要對系統(tǒng)的狀態(tài)方程以及觀測方程進行線性化處理，避免了擴展卡爾曼濾波算法中狀態(tài)轉(zhuǎn)移矩陣的雅克比矩陣求解過程，從而避免了由線性化造成的精度損失。同時，工程應(yīng)用中數(shù)值計算會引入舍入誤差，可能導(dǎo)致UKF算法與EKF算法在濾波過程中出現(xiàn)濾波發(fā)散或狀態(tài)協(xié)方差矩陣非對稱或負(fù)定情況，從而使濾波器失效。SRCDKF算法直接利用協(xié)方差的平方根矩陣進行迭代計算，提高了實際應(yīng)用中的數(shù)值穩(wěn)定性［6－7］。

SRCDKF的計算流程如下：

1）計算所用的初值

式中：x0／0為系統(tǒng)狀態(tài)初值；為系統(tǒng)狀態(tài)協(xié)方差平方根矩陣初值。

2）計算用作時間更新的σ點

狀態(tài)變量預(yù)測值的均值和均方誤差分別為

3）時間更新

擴張狀態(tài)變量xaw的維數(shù)；對于高斯分布，半步長h的最優(yōu)值為；qr（·）表示對矩陣進行QR分解運算。4）計算觀察點更新σ點

5）測量更新

式中：yk＋1／k為k＋1時刻觀測更新值；Yi，k＋1／k為k＋1時刻觀測采樣點i的更新值；為k＋1時刻觀測協(xié)方差矩陣；Pxk＋1，yk＋1為狀態(tài)、觀測量互協(xié)方差矩陣；cholupdate表示一階Cholesky分解。

職業(yè)高中是培養(yǎng)社會所需人才的基地，所以教學(xué)方式一定要滿足時代發(fā)展的最新特征，特別是會計電算化和數(shù)字化。一來是因為現(xiàn)代先進的教學(xué)手段，可以有效吸引學(xué)生更專注的課堂注意力，提高職業(yè)高中會計教學(xué)的效率和質(zhì)量；二來讓學(xué)生在學(xué)校就掌握現(xiàn)代會計的操作流程，更有利于學(xué)生盡早適應(yīng)社會，為學(xué)生日后的就業(yè)前景奠定夯實的基礎(chǔ)。

4 交互多模型SRCDKF算法（IMM－SRCDKF）

交互多模型算法是由BLOM和BAR－SHALOM在20世紀(jì)80年代基于廣義貝葉斯算法提出的一種具有馬爾可夫轉(zhuǎn)移概率的結(jié)構(gòu)自適應(yīng)算法。交互多模型算法作為一種軟切換算法，能夠?qū)Σ煌瑸V波器的估計值進行加權(quán)融合［8］，有效避免了單模型不能準(zhǔn)確描述不同類型機動的問題，并且不需要對目標(biāo)的實時機動參數(shù)進行測量，在工程上具有較強的實用性。本文將SRCDKF濾波器與IMM算法相結(jié)合，得到了一種適用于臨近空間高速目標(biāo)跟蹤濾波的多模型目標(biāo)運動信息估計算法。

交互模型算法的融合過程如圖3所示。針對目標(biāo)跟蹤濾波問題，根據(jù)目標(biāo)的機動能力以及特性，選擇多個符合目標(biāo)不同狀態(tài)的機動模型來建立一個有限數(shù)量的模型集。交互多模型算法利用上一時刻不同模型濾波器的估計殘差計算得到當(dāng)前時刻不同模型的匹配概率值；對該匹配概率值歸一化后，再對當(dāng)前時刻不同模型濾波器的濾波估計進行混合，從而得到當(dāng)前時刻狀態(tài)估計；同時，利用該值計算獲得下一時刻的不同模型濾波器的濾波初值。

圖3 交互多模型算法流程圖Fig．3 Flow chart of IMM algorithm

各模型之間的相互轉(zhuǎn)換概率可以用狀態(tài)轉(zhuǎn)移概率矩陣表示為

設(shè)vj（k）為濾波器j所對應(yīng)的濾波器的估計殘差，表示濾波估計所對應(yīng)的觀測值與目標(biāo)實際觀測值的差值，相應(yīng)的協(xié)方差矩陣為Sj（k），假設(shè)其服從高斯分布，則濾波器j的當(dāng)前狀態(tài)的概率為

式中：vj（k）＝Z（k）－H（k）（k｜k－1），Sj（k）＝H（k）Pj（k｜k－1）H（k）T＋R（k）。為了更方便地利用模型概率，需要對Λj（k）進行歸一化處理，即uj（k）＝

最后，根據(jù)上述計算流程得到的不同濾波器的概率、狀態(tài)估計值以及協(xié)方差矩陣，對不同模型濾波器的輸出值進行加權(quán)融合，即可得到混合狀態(tài)濾波值（k｜k）和混合狀態(tài)方差P（k｜k）為

5 仿真對比

為驗證所提出的臨近空間高速飛行器運動狀態(tài)估計方法的有效性，本文以Lockheed Martin公司的通用高超聲速飛行器CAV－H的公開氣動模型為研究對象，基于高斯偽譜法計算得到高速目標(biāo)最優(yōu)再入?yún)⒖架壽E［9］，采用蒙特卡洛仿真進行檢驗。

對比仿真采用不同機動模型的EKF濾波算法、SRCDKF濾波算法以及多模型IMM－SRCDKF濾波算法的濾波結(jié)果，以目標(biāo)的狀態(tài)估計值和目標(biāo)運動的實際值的均方誤差（root mean square error，RMSE）數(shù)據(jù)作為評價指標(biāo)分析估計結(jié)果。

仿真條件：目標(biāo)初始位置為（－25，－1 500，80）km，以5 000m／s水平初始速度飛行，落點位置約束為（0，0，0）km，俯仰角約束為 －5°；假設(shè)雷達站位于地理坐標(biāo)系原點，地面站雷達數(shù)據(jù)采樣率為10 Hz，其測距精度為10 m（1σ），俯仰角及方位角的測角精度均為0．1°（1σ）。

5．1 不同濾波方法濾波效果對比

使用基于統(tǒng)計模型的EKF濾波算法和SRCDKF濾波算法對相同的目標(biāo)軌跡、速度、加速度信息進行濾波估計，設(shè)模型機動頻率參數(shù)為0．05，仿真結(jié)果如圖4～12所示。

圖4 觀測目標(biāo)軌跡Fig．4 The observed target trajectories

圖5 濾波軌跡對比Fig．5 Contrast of filtered trajectories

圖6 X軸向速度估計曲線Fig．6 Velocity estimate in Xdirection

圖7 Y軸向速度估計曲線Fig．7 Velocity estimate in Ydirection

圖8 Z軸方向速度估計曲線Fig．8 Velocity estimate in Zdirection

圖9 X軸方向加速度估計曲線Fig．9 Acceleration estimate in Xdirection

圖10 Y軸方向加速度估計曲線Fig．10 Acceleration estimate in Ydirection

圖11 Z軸方向加速度估計曲線Fig．11 Acceleration estimate in Zdirection

圖12 位置估計均方誤差曲線Fig．12 Root－mean－square errors of position estimate

由仿真結(jié)果可以得出，目標(biāo)在整個飛行過程中具有較大的機動特征不確定性，EKF和SRCDKF濾波算法均能對目標(biāo)信息有效地做出估計。在100 s到200 s之間，就目標(biāo)機動產(chǎn)生的估計誤差而言，SRCDKF算法的估計誤差比EKF算法的估計誤差更小，且SRCDKF收斂速度更快。由圖13～14可知，在600 s后，隨著飛行機動特性減弱，飛行速度和加速度降低，SRCDKF算法的速度、加速度均方根誤差降低，精度提高。

圖13 速度估計均方誤差曲線Fig．13 Root－mean－square errors of velocity estimate

5．2 多模型與單模型濾波效果對比

設(shè)定IMM－SRCDKF濾波算法分別采用3個不同機動頻率的“當(dāng)前”統(tǒng)計機動模型組合，其機動頻率分別為0．01、0．07、0．2，單模型SRCDKF算法機動頻率為0．07。仿真結(jié)果如圖15～18所示。

圖14 加速度均方誤差曲線Fig．14 Root－mean－square errors of acceleration estimate

圖15 位置估計均方根誤差曲線Fig．15 Root－mean－square errors of position estimate

圖16 速度估計均方根誤差曲線Fig．16 Root－mean－square errors of velocity estimate

圖17 加速度估計均方根誤差曲線Fig．17 Root－mean－square errors of acceleration estimate

圖18 IMM－SRCDKF模型概率分布Fig．18 Probability distributions of IMM－SRCDKF model

由圖18可知，IMM－SRCDKF算法能夠自動選擇與當(dāng)前實際機動狀態(tài)相符的機動模型，與單模型SRCDKF濾波算法相比，在目標(biāo)進行大機動時其精度更高。在0～400 s內(nèi)，其位置、速度、加速度均方根誤差明顯較低，位置均方根誤差峰值由單模型的2 889 m下降到2 415 m，誤差均值由452 m 下降到385 m。在200 s、400s等幾個機動結(jié)束時刻，IMM－SRCDKF算法估計得到的速度和加速度均方根誤差均能以更快的速度收斂到零值附近，對不同大小的機動具有更強的魯棒性。

5 結(jié)束語

本文針對臨近空間高速目標(biāo)給出了目標(biāo)機動模型，并建立了雷達對目標(biāo)飛行器的跟蹤和觀測模型，提出了基于SRCDKF方法的目標(biāo)信息提取算法。通過仿真，驗證了本文提出的IMM－SRCDKF目標(biāo)信息估計算法的估計精度。對比EKF算法以及單模型SRCDKF算法，本文提出的算法具有收斂速度快、精度高特點。同時，基于SRCDKF的目標(biāo)信息提取算法能夠有效避免EKF算法所需的Jacobi矩陣計算，同時也避免了由數(shù)值計算舍入誤差導(dǎo)致的狀態(tài)協(xié)方差矩陣負(fù)定產(chǎn)生的濾波失效問題。