張曉春，馬新斐，劉志宏，杜國宏，楊 耀

(1.成都信息工程大學(xué)電子工程學(xué)院，四川 成都610225;2.重慶光電技術(shù)研究所，重慶400060)

0 引言

運(yùn)動(dòng)目標(biāo)跟蹤是模式識(shí)別、圖像處理、計(jì)算機(jī)視覺等領(lǐng)域的重要課題目標(biāo)跟蹤算法是紅外圖像末制導(dǎo)的核心算法，核心算法精確與否直接關(guān)系到導(dǎo)彈的命中精度，在軍事及民用領(lǐng)域都有很重要的研究價(jià)值和實(shí)用意義。目前主要包括粒子濾波、MeanShiift模型、CamShift模型、魔板匹配跟蹤、感知哈希算法、MAD和ABS等，跟蹤一方面從像素值之間關(guān)系出發(fā)進(jìn)行處理，另一方面可通過顏色相關(guān)圖統(tǒng)計(jì)進(jìn)行實(shí)現(xiàn)。在非線性處理算法中，擴(kuò)展卡爾曼濾波(EKF)將非線性系統(tǒng)線性化是通過截?cái)嗵├占?jí)數(shù)展開式實(shí)現(xiàn)的，因此對于非線性程度較高的系統(tǒng)，EKF的濾波精度受到限制。無跡卡爾曼濾波(UKF)利用UT變換后的采樣點(diǎn)的方式近似非線性函數(shù)的概率分布，對非線性程度較高的系統(tǒng)也有很強(qiáng)的適應(yīng)性，但是隨著系統(tǒng)狀態(tài)向量維數(shù)的增加，特別是當(dāng)維數(shù)時(shí)，系統(tǒng)狀態(tài)協(xié)方差的半正定性很難保證，因此會(huì)UKF對系統(tǒng)狀態(tài)的估計(jì)精度造成不良影響。針對高維情況下非線性濾波估計(jì)，容積卡爾曼濾波(cubature kalman filter，CKF)應(yīng)運(yùn)而生，CKF 是 Arasaratnam 等[1]于2009年提出的一種新型非線性濾波算法，算法使用基于球面－徑向積分準(zhǔn)則的數(shù)值積分方法計(jì)算非線性系統(tǒng)非線性變換的隨機(jī)變量后的均值和協(xié)方差，結(jié)構(gòu)簡單，實(shí)現(xiàn)方便，且濾波精度高。

對于非線性濾波算法而言，其濾波精度都是建立在模型準(zhǔn)確的基礎(chǔ)之上，包括EKF、UKF以及CKF，當(dāng)模型不準(zhǔn)確時(shí)，則濾波算法對該系統(tǒng)的非線性估計(jì)精度下降，甚至導(dǎo)致濾波發(fā)散。但在實(shí)際應(yīng)用當(dāng)中，非線性系統(tǒng)復(fù)雜程度不同，所建立的模型往往不能準(zhǔn)確描述系統(tǒng)狀態(tài)。為解決在系統(tǒng)模型不準(zhǔn)確時(shí)常規(guī)濾波算法估計(jì)精度不高的問題，專家學(xué)者提出很多自適應(yīng)濾波算法以及魯棒濾波算法，取得不錯(cuò)的濾波效果[2－3]，其中強(qiáng)跟蹤濾波算法(strong tracking filter，STF)對狀態(tài)突變有較強(qiáng)自適應(yīng)跟蹤能力，在各領(lǐng)域也得到廣泛應(yīng)用[4]。強(qiáng)跟蹤算法最初是在線性系統(tǒng)中提出的，周東華等[5－6]將該算法推廣到EKF中，王小旭等[7]將其推廣到UKF及CDKF當(dāng)中，都取得較好的效果。

目前強(qiáng)跟蹤擴(kuò)展卡爾曼濾波器能夠適用在非線性系統(tǒng)，但是EKF的算法框架致使該算法在強(qiáng)非線性系統(tǒng)中濾波精度不高，甚至引起發(fā)散，且非線性系統(tǒng)需要計(jì)算其雅克比矩陣，實(shí)現(xiàn)復(fù)雜。CKF算法采用球面-徑向容積準(zhǔn)則，具有泰勒級(jí)數(shù)展開的三階精度，而且強(qiáng)跟蹤算法能夠克服CKF對于狀態(tài)突變自適應(yīng)跟蹤能力差的問題，因此，建立基于CKF的強(qiáng)跟蹤濾波算法具有重要的現(xiàn)實(shí)意義。

提出一種改進(jìn)的強(qiáng)跟蹤算法，針對強(qiáng)機(jī)動(dòng)目標(biāo)的狀態(tài)估計(jì)問題，在分析擴(kuò)展卡爾曼濾波(EKF)以及無跡卡爾曼濾波(UKF)對非線性、高維系統(tǒng)狀態(tài)估計(jì)性能不足的前提下，使用容積卡爾曼濾波(cubature kalman filter，CKF)解決非線性、高維系統(tǒng)對高機(jī)動(dòng)目標(biāo)的狀態(tài)估計(jì)問題。

1 改進(jìn)的強(qiáng)跟蹤算法

強(qiáng)跟蹤濾波對狀態(tài)估計(jì)過于依賴量測值，雖然理論上可以得到最優(yōu)估計(jì)，但由于殘差協(xié)方差矩陣對真實(shí)的殘差方差近似不夠準(zhǔn)確，致使對判定濾波異常的閾值設(shè)置較小，漸消因子的產(chǎn)生出現(xiàn)異常，使得濾波增益達(dá)不到最優(yōu)，且其估計(jì)值也不夠平滑。

文獻(xiàn)[8]將判定濾波異常的閾值調(diào)整至 βC0，k+1，當(dāng)β=3時(shí)，若不考慮V0，k的影響，如式(1)所示

即判定異常的概率為1.5%，式中:C0，k+1為殘差方差V0，k+1的理論值，即式(2)所示

若考慮V0，k，由于V0，k＜C0，k+1，假定二者相等，式(2)變?yōu)?/p>

由式(3)可知，將閾值提高到3C0，k+1時(shí)，濾波器在正常濾波的情況對濾波發(fā)散進(jìn)行誤判的概率為1.5%～2.6%，大大降低了產(chǎn)生漸消因子的概率，使強(qiáng)跟蹤的估計(jì)精度進(jìn)一步提高。

因此，提高閾值后，殘差方差為

通過式(4)推導(dǎo)可得

由式(3)～(5)可知，改進(jìn)的強(qiáng)跟蹤算法(ISTF)的漸消因子取法如式(6)～(9)所示

其中，弱化因子與量測向量的維數(shù)n的關(guān)系表示如式(10)所示

由式(10)可知，弱化因子β取值越小，強(qiáng)跟蹤算法的魯棒性越強(qiáng)，同時(shí)誤判概率增大，估計(jì)平滑度降低;取值越大，則對濾波發(fā)散的誤判概率越小，同時(shí)系統(tǒng)狀態(tài)估計(jì)的魯棒性減弱，甚至使該算法退化為一般濾波器。

2 改進(jìn)的強(qiáng)跟蹤C(jī)KF算法

在計(jì)算強(qiáng)跟蹤濾波算法漸消因子時(shí)，需要計(jì)算系統(tǒng)方程和量測方程的雅可比矩陣，但是CKF算法中，采用的是近似非線性函數(shù)的高斯權(quán)值積分，在濾波方程中不存在Fk和Hk+1，而且對于某些不可微的非線性系統(tǒng)，其雅克比矩陣也難以求取，因此需要加以改進(jìn)，建立強(qiáng)跟蹤C(jī)KF算法。

對于系統(tǒng)狀態(tài)及量測的一步預(yù)測值和，其求解如式(11)和(12)所示

式中:

為求取漸消因子，使C0，k+1與經(jīng)Pk+1|k求得的量測方差矩陣相等，即得式(16)為

當(dāng)量測函數(shù)h(xk+1)為非線性時(shí)，經(jīng)其傳遞后的為關(guān)于λ 的非線性函數(shù)，因此漸消因子不易直k+1接求得，因此，現(xiàn)將近似為C0，k+1的線性函數(shù)，可得如下形式

根據(jù)式(18)則有

令式(19)中，V0，k+1=βC0，k+1，即可得

則可得到如下公式

根據(jù)式(21)～(24)，可得強(qiáng)跟蹤C(jī)KF算法的濾波流程為:

首先，求取系統(tǒng)狀態(tài)向量相應(yīng)的容積點(diǎn)，并計(jì)算其經(jīng)f(Xk+1)傳播后的容積點(diǎn)，進(jìn)而計(jì)算狀態(tài)預(yù)測估計(jì)以及預(yù)測誤差協(xié)方差，隨后求解量測向量相應(yīng)的容積點(diǎn)，并計(jì)算其經(jīng)h(Xk+1)傳播后的容積點(diǎn)，進(jìn)而計(jì)算量測預(yù)測估計(jì)、新息協(xié)方差以及交叉協(xié)方差。

其次，由式(19)～(24)計(jì)算出強(qiáng)跟蹤漸消因子λk+1，結(jié)合式(15)求出加入漸消因子后的預(yù)測誤差協(xié)方差Pk+1|k，如式(25)所示

再次，由加入漸消因子后的預(yù)測誤差協(xié)方差Pk+1|k，求解量測向量相應(yīng)的容積點(diǎn)，并計(jì)算其經(jīng)h(Xk+1)傳播后的容積點(diǎn)，進(jìn)而計(jì)算量測預(yù)測估計(jì)、新息協(xié)方差Pzz，k+1|k以及交叉協(xié)方差Pxz，k+1|k。

最后，計(jì)算系統(tǒng)濾波增益Wk、狀態(tài)更新方程以及協(xié)方差更新方程，求出狀態(tài)估計(jì)以及Pk+1。

3 結(jié)果與分析

通過對坦克車輛的階躍機(jī)動(dòng)和圓機(jī)動(dòng)進(jìn)行仿真驗(yàn)證，并與CKF和UKF算法進(jìn)行比較，驗(yàn)證機(jī)動(dòng)目標(biāo)跟蹤算法的有效性。首先設(shè)定直角坐標(biāo)系二維平面，X軸和Y軸上的量測噪聲均是均值為0、標(biāo)準(zhǔn)偏差為1 m的高斯序列，掃描周期為0.25 s，機(jī)動(dòng)頻率為0.05。

統(tǒng)計(jì)模型算法中，最大機(jī)動(dòng)加速度設(shè)為amax=5 m/s2，仿真次數(shù)為100次。目標(biāo)運(yùn)動(dòng)軌跡設(shè)定為:目標(biāo)初始在X軸做勻速運(yùn)動(dòng)，初速8 m/s，隨后以加速度2 m/s2減速為0;在第40～52個(gè)掃描區(qū)間，以加速度4 m/s2沿Y軸先加速至12 m/s再勻減速為零;第64～72個(gè)掃描區(qū)間沿X軸先以加速度4 m/s2加速至8 m/s，再以加速度2 m/s2加速至12m/s，隨后保持勻速運(yùn)動(dòng);第100～138個(gè)掃描區(qū)間，以向心加速度4 m/s2做180°勻速圓機(jī)動(dòng);第138～162個(gè)掃描區(qū)間，沿X軸保持勻速運(yùn)動(dòng);第162～170個(gè)掃描區(qū)間，在X軸上保持勻速，Y軸上以加速度4 m/s2加速至8 m/s;第170～178個(gè)掃描區(qū)間，在X軸上以加速度4 m/s2加速至20 m/s，Y軸上以加速度4 m/s2減速至0;隨后沿X軸以速度20 m/s保持勻速運(yùn)動(dòng)。其運(yùn)動(dòng)軌跡如圖1所示。

圖1 目標(biāo)運(yùn)動(dòng)軌跡圖

為克服初始狀態(tài)對濾波精度的影響，統(tǒng)一假定濾波初始狀態(tài)為

這里，系統(tǒng)噪聲wk、過程噪聲vk符合高斯分布，且wk～N(0，Qk)，vk～N(0，Rk)，初始狀態(tài)估計(jì)取為0，并且初始協(xié)方差矩陣取為P0|0=diag{10，20，0.04，10，2，0.04}。

為公平比較各非線性濾波算法，進(jìn)行100次獨(dú)立仿真取其平均。將文中改進(jìn)的強(qiáng)跟蹤容積卡爾曼濾波算法(ISTCKF)與CKF、UKF進(jìn)行比較。圖2表示3種濾波算法的跟蹤軌跡對比。

圖2 3種濾波算法的跟蹤軌跡對比

由圖2可見，提出的ICKF跟蹤比CKF和UKF更穩(wěn)定，跟蹤誤差更小，在目標(biāo)非機(jī)動(dòng)和弱機(jī)動(dòng)時(shí)，ICKF保持了較好的跟蹤性能，與CKF相當(dāng)，優(yōu)于UKF，尤其是在狀態(tài)突變的時(shí)刻，ICKF跟蹤穩(wěn)定，收斂速度更快。同時(shí)CKF跟蹤性能略優(yōu)于UKF。

為定量比較各非線性濾波算法性能，定義均方根誤差(RMSE)為

式(26)中:的濾波估計(jì)值。

表1列出了各濾波算法x，y，和軸的狀態(tài)估計(jì)RMSE均值。

表1 狀態(tài)估計(jì)均方誤差

通過表1可知，文中的ICKF算法在各坐標(biāo)方向的濾波誤差明顯小于其他非線性濾波器，尤其表現(xiàn)在目標(biāo)狀態(tài)突變處。CKF的均方誤差性能要好于UKF。同時(shí)，同時(shí)也發(fā)現(xiàn)ICKF對運(yùn)動(dòng)目標(biāo)，尤其是階躍機(jī)動(dòng)目標(biāo)位置和速度具有最強(qiáng)的跟蹤能力，能自適應(yīng)地跟蹤狀態(tài)較大變化，CKF的跟蹤能力與UKF相差不大，CKF略優(yōu)于UKF。

4 結(jié)束語

針對強(qiáng)機(jī)動(dòng)目標(biāo)的狀態(tài)估計(jì)問題，在分析擴(kuò)展卡爾曼濾波(EKF)以及無跡卡爾曼濾波(UKF)對非線性、高維系統(tǒng)狀態(tài)估計(jì)性能不足的前提下，使用容積卡爾曼濾波(Cubature Kalman Filter，CKF)解決非線性、高維系統(tǒng)對高機(jī)動(dòng)目標(biāo)的狀態(tài)估計(jì)問題。針對CKF對于狀態(tài)突變自適應(yīng)跟蹤能力差的問題，采用改進(jìn)的強(qiáng)跟蹤算法與CKF結(jié)合形成強(qiáng)跟蹤C(jī)KF算法，仿真結(jié)果表明，該算法能很好地適應(yīng)目標(biāo)機(jī)動(dòng)的變化，尤其對機(jī)動(dòng)狀態(tài)突變有良好的跟蹤性能，具有一定的實(shí)際應(yīng)用價(jià)值。

[1] Arasaratnam I，Haykin S.Cubature Kalman Filters[J].IEEE Trans.on Automatic Control，2009，54(6):1254－1269.

[2] 楊廷梧，黨懷義，蘇明.基于信息偏差的觀測奇異值自適應(yīng)濾波算法[J].測控技術(shù)，2010，29(10):16－19.

[3] 徐進(jìn)寶，封錫盛，任申真.濾波在純方位跟蹤中的應(yīng)用[J].計(jì)算機(jī)仿真，2012，29(7):388－392.

[4] Liu Z B，Shi Z Y，Xu W L.Multi－ component Information－equalized Extended Strong Tracking Filter for Global Localization:A Scheme Robust to Kidnapping and Symmetrical Environments[J].Robotics and Autonmous Systems，2010，58(5):465－487.

[5] 周東華，席裕庚，張鐘俊.非線性系統(tǒng)帶次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波[J].控制與決策，1990，23(5):1－6.

[6] 周東華，席裕庚，張鐘俊.一種帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波[J].自動(dòng)化學(xué)報(bào)，1991，17(6):689－695.

[7] 王小旭，趙琳，夏全喜，等.基于Unscented變換的強(qiáng)跟蹤濾波器[J].控制與決策，2010，25(7):1063－1068.

[8] 葛磊.容積卡爾曼濾波算法研究及其在導(dǎo)航中的應(yīng)用[D].哈爾濱:哈爾濱工程大學(xué)博士學(xué)位論文，2013.