基于自適應(yīng)無(wú)跡卡爾曼的目標(biāo)跟蹤算法研究

盧穎鵬，陳 曦，杜忠華，侯 杰

（南京理工大學(xué)機(jī)械工程學(xué)院，江蘇南京 210094）

目標(biāo)跟蹤算法是裝甲車(chē)主動(dòng)防護(hù)系統(tǒng)的核心算法之一，目標(biāo)跟蹤精度和收斂速度對(duì)最終的攔截概率影響很大[1-3]。選擇合適的濾波算法可以得到相對(duì)精確的來(lái)襲目標(biāo)運(yùn)動(dòng)信息，進(jìn)而提升目標(biāo)攔截概率。

文獻(xiàn)[4]在引入徑向速度信息的基礎(chǔ)上采用比例對(duì)稱(chēng)采樣策略(Scale symmetry sampling)優(yōu)化UKF(Unscented Kalman Filtering)算法(以下簡(jiǎn)稱(chēng)SUKFR)[4]，得出SUKFR 算法相較于其他算法濾波的收斂速度更快且精度更高，但該算法沒(méi)有考慮異常干擾的影響。文獻(xiàn)[5]提出了一種基于自適應(yīng)無(wú)跡卡爾曼濾波(Adaptive Unscented Kalman Filtering,AUKF)的狀態(tài)估計(jì)算法，優(yōu)化了常規(guī)UKF 算法抗干擾性能差的問(wèn)題[5]，但其濾波收斂速度還可以進(jìn)一步提高。

為增強(qiáng)裝甲車(chē)主動(dòng)防護(hù)系統(tǒng)性能，提高目標(biāo)攔截概率，該文引入噪聲自適應(yīng)系數(shù)對(duì)SUKFR 算法進(jìn)行優(yōu)化(以下簡(jiǎn)稱(chēng)ASUKFR)，通過(guò)Matlab 仿真對(duì)比UKFR、SUKFR和ASUKFR的濾波效果，得出ASUKFR算法濾波效果更好，且在該算法下目標(biāo)攔截概率最優(yōu)。

1 算法描述

常規(guī)UKF 算法利用無(wú)跡（Unscented Transform,UT）變換處理非線(xiàn)性系統(tǒng)均值和協(xié)方差的傳遞問(wèn)題，在保留高階項(xiàng)的基礎(chǔ)上，有效避免了非線(xiàn)性系統(tǒng)的線(xiàn)性化，提高了系統(tǒng)高斯密度的傳遞精度[6]。ASUKF 算法對(duì)常規(guī)UKF 算法容錯(cuò)性能差和采樣非局部性的問(wèn)題進(jìn)行優(yōu)化，具體步驟如下：

步驟1：計(jì)算Sigma 采樣點(diǎn)集，并對(duì)獲得的Sigma點(diǎn)集進(jìn)行一步預(yù)測(cè)：

式（1）中共有2n+1 個(gè)采樣點(diǎn)；X為狀態(tài)變量；n為狀態(tài)的維數(shù)；上標(biāo)i為第i個(gè)采樣點(diǎn)；為狀態(tài)變量均值；P為方差矩陣；λ為縮放比例系數(shù)。

然而，在標(biāo)準(zhǔn)UKF 算法中，當(dāng)維數(shù)n增大時(shí)，Sigma 點(diǎn)與均值點(diǎn)的距離也會(huì)變大，進(jìn)而出現(xiàn)采樣的“非局部效應(yīng)”，且量測(cè)方程函數(shù)非線(xiàn)性強(qiáng)弱會(huì)直接影響算法的整體濾波精度。為解決算法的“非局部效應(yīng)”并且保證協(xié)方差矩陣的半正定性，考慮對(duì)此前得到的Sigma 點(diǎn)集進(jìn)行比例采樣修正[7]，以避免采樣的“非局部效應(yīng)”，其公式如下：

式（2）中，a為比例修正參數(shù)，取值范圍為[0,1]，X(i)(k+1|k)為Sigma 點(diǎn)集的一步預(yù)測(cè)，f為系統(tǒng)狀態(tài)量的非線(xiàn)性變換。

步驟3：對(duì)一步預(yù)測(cè)值使用UT 變換，得到新的Sigma 點(diǎn)集k+1|k)。

步驟4：將步驟3 得到的新Sigma 點(diǎn)集代入觀測(cè)方程，計(jì)算得到新的觀測(cè)量Z(i)(k+1|k)。

步驟5：通過(guò)對(duì)上一步計(jì)算的觀測(cè)預(yù)測(cè)值進(jìn)行加權(quán)，得到系統(tǒng)預(yù)測(cè)均值和協(xié)方差，并引入噪聲自適應(yīng)系數(shù)進(jìn)行優(yōu)化[8]。

步驟1 中引入比例修正系數(shù)，雖然消除了采樣的“非局部效應(yīng)”，但是在實(shí)際應(yīng)用中，UKF 濾波算法對(duì)濾波的初始值比較敏感，這可能導(dǎo)致濾波發(fā)散[9]。因此，該文在比例修正的基礎(chǔ)上引入噪聲自適應(yīng)系數(shù)對(duì)原算法進(jìn)行優(yōu)化，噪聲自適應(yīng)系數(shù)不僅可以估計(jì)和校正不確定系統(tǒng)模型噪聲和噪聲統(tǒng)計(jì)參數(shù)，還可以使用量測(cè)值對(duì)預(yù)測(cè)值進(jìn)行修正[10-11]。所以對(duì)傳統(tǒng)的預(yù)測(cè)協(xié)方差進(jìn)行如下更新：

式（3）中，ω為采樣點(diǎn)權(quán)值，下標(biāo)c表示協(xié)方差；αk為噪聲自適應(yīng)系數(shù)，并且0 ＜αk≤1。

采樣點(diǎn)權(quán)值計(jì)算如下所示：

式（4）中，參數(shù)α決定了采樣點(diǎn)的分布狀態(tài)，取值范圍為[0.000 1,1]；參數(shù)κ為冗余值，一般取0；參數(shù)β為權(quán)系數(shù)；n為狀態(tài)向量的維數(shù)。

選擇適當(dāng)?shù)淖赃m應(yīng)系數(shù)，不僅可以平衡狀態(tài)方程估計(jì)量和觀測(cè)信息的權(quán)重，還可以控制異常干擾對(duì)系統(tǒng)估計(jì)結(jié)果的影響[12]。αk的取值如下：

式（5）中，預(yù)測(cè)殘差mk=Z(i)(k+1|k)-(k+1|k)。

步驟6：計(jì)算Kalman 增益矩陣K(k+1|k)。

由上述算法步驟可知，ASUKF 算法通過(guò)比例修正系數(shù)消除常規(guī)UKF 算法采樣的“非局部效應(yīng)”，再引入噪聲自適應(yīng)系數(shù)來(lái)調(diào)節(jié)k+1|k)在估計(jì)過(guò)程中的權(quán)重，從而提升算法的濾波性能。

2 目標(biāo)跟蹤

2.1 來(lái)襲目標(biāo)假設(shè)

主動(dòng)防護(hù)系統(tǒng)的攔截面是以裝甲車(chē)輛中心為圓心、半徑為8 m 的半球面。由于雷達(dá)探測(cè)距離只有200 m，并且來(lái)襲目標(biāo)飛行速度較快，系統(tǒng)對(duì)雷達(dá)測(cè)量軌跡數(shù)據(jù)的濾波時(shí)間須小于300 ms，因此，雷達(dá)需要保證較快的探測(cè)速率，從而提供較多的來(lái)襲目標(biāo)運(yùn)動(dòng)數(shù)據(jù)，確保目標(biāo)的跟蹤精度，雷達(dá)每1 ms 向系統(tǒng)傳輸一次來(lái)襲目標(biāo)的位置和速度信息。

為簡(jiǎn)化模型，對(duì)主動(dòng)防護(hù)系統(tǒng)目標(biāo)跟蹤作出如下兩點(diǎn)狀態(tài)假設(shè)[13]：

1）由于來(lái)襲目標(biāo)飛行速度遠(yuǎn)大于裝甲車(chē)的行駛速度，所以假設(shè)來(lái)襲目標(biāo)從被探測(cè)到被攔截的過(guò)程中，裝甲車(chē)輛處于近似靜止的狀態(tài)。

2）該系統(tǒng)采用的是超近程主動(dòng)防護(hù)策略，在雷達(dá)探測(cè)到來(lái)襲目標(biāo)時(shí)，其已經(jīng)處于飛行的最后階段，運(yùn)動(dòng)軌跡不會(huì)發(fā)生較大波動(dòng)，因此，可以近似看作勻速直線(xiàn)運(yùn)動(dòng)。

2.2 目標(biāo)運(yùn)動(dòng)方程

該文采用CV（Constant Velocity）模型來(lái)描述來(lái)襲目標(biāo)的三維運(yùn)動(dòng)狀態(tài)。由于雷達(dá)采樣有時(shí)間間隔，為了方便控制系統(tǒng)處理，對(duì)雷達(dá)采樣數(shù)據(jù)進(jìn)行離散化處理，其狀態(tài)方程表達(dá)式如下：

2.3 目標(biāo)量測(cè)方程

雷達(dá)量測(cè)采用球坐標(biāo)系，目標(biāo)量測(cè)數(shù)據(jù)包括徑向距離r、俯仰角α、方位角β和徑向速度vr。該文假設(shè)來(lái)襲目標(biāo)的位置和速度信息噪聲都為獨(dú)立的高斯白噪聲，因此，量測(cè)方程為：

3 仿真試驗(yàn)

3.1 仿真參數(shù)設(shè)置

假設(shè)來(lái)襲目標(biāo)距離裝甲車(chē)200 m，俯仰角為20°，方位角為35°，速度為425 m/s；初始協(xié)方差矩陣P=1 000×eye(6)（eye(6)為6 行6 列的單位矩陣）；過(guò)程噪聲調(diào)節(jié)參數(shù)σ2=1；雷達(dá)采樣周期為1 ms；雷達(dá)采樣總時(shí)間為0.4 s，無(wú)跡變換參數(shù)α=0.01，β=2，κ=0，狀態(tài)向量維數(shù)n=6，比例修正系數(shù)a=0.1。量測(cè)噪聲協(xié)方差矩陣R=

3.2 算法濾波效果對(duì)比

用Matlab 軟件對(duì)ASUKFR、SUKFR 以 及UKFR算法進(jìn)行50 次Monte Carlo 仿真，對(duì)比三種算法的濾波性能。由圖1 可知，ASUKFR 和SUKFR 算法明顯比UKFR 算法濾波效果更好，濾波軌跡更加貼近目標(biāo)的真實(shí)軌跡。

圖1 目標(biāo)濾波軌跡對(duì)比

為了更直觀地對(duì)比三種算法的優(yōu)劣，采用均方根誤差作為性能評(píng)價(jià)準(zhǔn)則對(duì)濾波算法性能進(jìn)行評(píng)價(jià)[14]，其公式如下：

式中，RMSEk為均方根誤差，μ為RMSE 均值，N表示蒙特卡羅仿真次數(shù)，Xi(k)和分別為第i次仿真中時(shí)刻k的目標(biāo)狀態(tài)真實(shí)值和估計(jì)值；L為單次仿真采樣次數(shù)。

由表1可知，ASUKFR和SUKFR的濾波精度都遠(yuǎn)遠(yuǎn)優(yōu)于UKFR，并且ASUKFR 在保留SUKFR速度精度的基礎(chǔ)上，徑向距離濾波誤差降低至原來(lái)的50%左右。圖2 中，在徑向距離濾波均方差方面，ASUKFR算法收斂速度最快，SUKFR 算法次之，UKFR 算法最慢，ASUKFR 算法在150 ms 左右已經(jīng)達(dá)到穩(wěn)態(tài)，而SUKFR算法和UKFR 算法分別在340 ms 和350 ms 附近達(dá)到穩(wěn)態(tài)；而圖3 中，ASUKFR 和SUKFR 算法的速度濾波均方差收斂速度相當(dāng)，明顯優(yōu)于UKFR 算法。因此，ASUKFR 濾波穩(wěn)定性更好且收斂速度更快。

表1 50次蒙特卡羅仿真RMSE均值

圖2 徑向距離濾波均方差

圖3 徑向速度濾波均方差

3.3 攔截概率仿真

以蒙特卡羅方法為理論基礎(chǔ)，采用Matlab 軟件對(duì)系統(tǒng)的攔截概率進(jìn)行仿真。該理論利用不確定變量的隨機(jī)值，通過(guò)大量的仿真實(shí)驗(yàn)得到結(jié)果[15]。

攔截概率計(jì)算過(guò)程：首先，根據(jù)目標(biāo)運(yùn)動(dòng)狀態(tài)方程和雷達(dá)量測(cè)方程生成來(lái)襲目標(biāo)運(yùn)動(dòng)參數(shù)；其次，在生成的標(biāo)準(zhǔn)目標(biāo)運(yùn)動(dòng)參數(shù)的基礎(chǔ)上，疊加經(jīng)過(guò)蒙特卡羅方法處理后的雷達(dá)量測(cè)和目標(biāo)運(yùn)動(dòng)狀態(tài)誤差[16]；然后，對(duì)生成的量測(cè)軌跡進(jìn)行濾波處理和攔截坐標(biāo)點(diǎn)解算；最終，來(lái)襲目標(biāo)攔截成功取決于系統(tǒng)解算的攔截坐標(biāo)點(diǎn)和實(shí)際目標(biāo)攔截點(diǎn)的間距，若間距不大于0.8 m（0.8 m 內(nèi)飛網(wǎng)攔截效果最佳）則判定攔截成功，否則判定攔截失敗。

由圖4 可知，使用UKFR 算法后的系統(tǒng)攔截概率略?xún)?yōu)于直接使用量測(cè)數(shù)據(jù)的攔截概率；使用SUKFR算法后使得系統(tǒng)攔截概率相比UKFR 算法提升了約15%；ASUKFR 算法在SUKFR 算法的基礎(chǔ)上引入自適應(yīng)系數(shù)后，系統(tǒng)攔截概率提升至90%以上，仿真驗(yàn)證了該系統(tǒng)采用ASUKFR 算法能夠更好地減小雷達(dá)量測(cè)噪聲，增加主動(dòng)防護(hù)系統(tǒng)的攔截概率，從而有效地保護(hù)裝甲車(chē)輛。

圖4 不同濾波算法的系統(tǒng)攔截概率

4 結(jié)論

該文對(duì)裝甲車(chē)主動(dòng)防護(hù)目標(biāo)跟蹤技術(shù)進(jìn)行研究，在SUKFR 的基礎(chǔ)上引入噪聲自適應(yīng)系數(shù)來(lái)調(diào)節(jié)異常擾動(dòng)，從而對(duì)系統(tǒng)估計(jì)結(jié)果進(jìn)行修正。通過(guò)蒙特卡羅方法[17-18]驗(yàn)證了噪聲自適應(yīng)系數(shù)的引入能夠有效改善目標(biāo)跟蹤的精度與穩(wěn)定性，提高最終的系統(tǒng)攔截概率，得出ASUKFR 算法作為主動(dòng)防護(hù)系統(tǒng)的目標(biāo)跟蹤算法，可以進(jìn)一步提升裝甲車(chē)戰(zhàn)場(chǎng)作戰(zhàn)和生存能力。