狀態(tài)自適應(yīng)無(wú)跡卡爾曼濾波算法及其在水下機(jī)動(dòng)目標(biāo)跟蹤中的應(yīng)用

馬艷， 劉小東

(1.西北工業(yè)大學(xué) 航海學(xué)院， 陜西 西安 710072； 2.中國(guó)艦船研究設(shè)計(jì)中心， 湖北 武漢 430061)

0 引言

水下對(duì)抗中，不僅要快速跟蹤機(jī)動(dòng)目標(biāo)，而且要準(zhǔn)確估計(jì)目標(biāo)非機(jī)動(dòng)時(shí)刻的航速與航向。水下主動(dòng)探測(cè)平臺(tái)通常提供的觀測(cè)量有目標(biāo)距離和方位，由于背景噪聲的存在，探測(cè)平臺(tái)估計(jì)的目標(biāo)距離和方位不可避免地與真值之間存在偏差，誤差大小與信噪比呈反比。在目標(biāo)跟蹤中，通常以目標(biāo)的位置矢量和速度矢量作為狀態(tài)向量，觀測(cè)方程中目標(biāo)方位與位置矢量為三角函數(shù)關(guān)系，因此其觀測(cè)方程具有很強(qiáng)的非線性。

對(duì)于非線性目標(biāo)跟蹤，傳統(tǒng)卡爾曼濾波器(KF)將不再適用，常用的非線性目標(biāo)跟蹤算法主要有擴(kuò)展卡爾曼濾波(EKF)[1-3]、無(wú)跡卡爾曼濾波(UKF)和粒子濾波(PF)。其中：EKF是一種線性化處理方法，通過(guò)泰勒展開(kāi)去除高階項(xiàng)影響，對(duì)于強(qiáng)非線性水下的目標(biāo)觀測(cè)方程將會(huì)帶來(lái)較大誤差；PF是基于Monte Carlo方法的重采樣濾波算法[4-5]，無(wú)需考慮系統(tǒng)的概率密度分布，對(duì)非線性具有良好的適應(yīng)性，但存在計(jì)算量大、面臨粒子退化等缺點(diǎn)；UKF是基于無(wú)跡技術(shù)(UT)的KF，該方法首先對(duì)非線性函數(shù)的概率密度分布進(jìn)行近似[6-7]，無(wú)需考慮狀態(tài)方程和觀測(cè)方程的線性化處理，對(duì)于強(qiáng)非線性系統(tǒng)有著良好的濾波效果[8-9]。UKF算法在進(jìn)行目標(biāo)跟蹤時(shí)需要提前建立目標(biāo)的狀態(tài)方程，即固定狀態(tài)噪聲，但是在目標(biāo)機(jī)動(dòng)時(shí)，目標(biāo)運(yùn)動(dòng)狀態(tài)與狀態(tài)方程并不能實(shí)時(shí)完全匹配，從而帶來(lái)較大的狀態(tài)噪聲。若依然采用傳統(tǒng)UKF算法，則可能會(huì)導(dǎo)致跟蹤精度降低甚至發(fā)散等問(wèn)題。

針對(duì)UKF在跟蹤機(jī)動(dòng)目標(biāo)面臨的問(wèn)題，學(xué)者們提出了一些UKF的改進(jìn)算法[10-14]，具有代表性的有Sage-Husa算法及其改進(jìn)算法。Sage-Husa是一種次優(yōu)無(wú)偏極大后驗(yàn)估計(jì)器，在濾波過(guò)程中不斷調(diào)整狀態(tài)噪聲方差陣，但在更新過(guò)程中會(huì)出現(xiàn)負(fù)定的噪聲方差陣，從而導(dǎo)致濾波中斷。為了使UKF適應(yīng)跟蹤過(guò)程噪聲的變化，趙治國(guó)等[10]將改進(jìn)的自適應(yīng)Sage-Husa算法與UKF結(jié)合，在線更新噪聲的統(tǒng)計(jì)特性。石勇等[12]提出一種基于協(xié)方差匹配的方法來(lái)抑制濾波發(fā)散，通過(guò)調(diào)整預(yù)測(cè)協(xié)方差的系數(shù)改變其在濾波過(guò)程中所占的權(quán)值，但該方法易破壞濾波過(guò)程的穩(wěn)定性，目標(biāo)強(qiáng)機(jī)動(dòng)情況下也出現(xiàn)濾波發(fā)散的情況。周衛(wèi)東等[13]提出一種基于新息和殘差的自適應(yīng)UKF算法，但該方法在濾波過(guò)程中對(duì)觀測(cè)方程進(jìn)行泰勒展開(kāi)的線性化處理，帶來(lái)了較大誤差。陳政等[14]提出采用最大期望算法(EM)估計(jì)的過(guò)程噪聲參數(shù)，改善UKF不能適應(yīng)統(tǒng)計(jì)特性未知的非高斯過(guò)程噪聲時(shí)性能，但過(guò)程噪聲參數(shù)估計(jì)復(fù)雜。上述算法充分考慮了濾波的快速跟蹤性，但沒(méi)有考慮跟蹤結(jié)果的平滑性，對(duì)于目標(biāo)非機(jī)動(dòng)情況下速度與航向的預(yù)測(cè)準(zhǔn)確性不高。

本文針對(duì)水下機(jī)動(dòng)目標(biāo)實(shí)時(shí)跟蹤和準(zhǔn)確估計(jì)目標(biāo)航速和航向問(wèn)題，提出一種目標(biāo)狀態(tài)噪聲方差隨殘差概率分布自適應(yīng)調(diào)整的UKF方法，避免了Sage-Husa算法中出現(xiàn)負(fù)定噪聲方差陣的問(wèn)題，同時(shí)避免了線性化帶來(lái)的誤差問(wèn)題。仿真結(jié)果表明，該方法不僅能得出比傳統(tǒng)UKF更加精確的濾波效果，而且能迅速得到平滑的濾波曲線，具有較高的航速和航向估計(jì)精度。

1 狀態(tài)方程以及觀測(cè)方程

圖1 觀測(cè)模型Fig.1 Observation model

在所建立的二維直角坐標(biāo)系中，該跟蹤系統(tǒng)的狀態(tài)方程、觀測(cè)方程和目標(biāo)運(yùn)動(dòng)要素方程分別為

(1)

(2)

(3)

式中：t為采樣間隔；wk為狀態(tài)噪聲，服從N(0,Qk)的高斯分布，Qk為非負(fù)定矩陣；uk為觀測(cè)噪聲，服從N(0,Rk)的高斯分布，Rk為正定矩陣；uk與wk相互獨(dú)立。觀察(2)式可知，跟蹤系統(tǒng)的狀態(tài)方程、觀測(cè)方程和目標(biāo)運(yùn)動(dòng)要素方程是明顯的非線性方程。

2 UKF基本原理

UKF基本原理是以UT變換為基礎(chǔ)，采用KF框架和確定性采樣形式，在減少采樣粒子點(diǎn)數(shù)的同時(shí)保證逼近精度。UKF算法可分為初始化、UT變換、時(shí)間傳播方程和測(cè)量更新方程4個(gè)步驟[6-7]：

1)初始化。

2)UT變換。

3)時(shí)間傳播方程。

4)測(cè)量更新方程。

3 基于殘差概率分布的自適應(yīng)UKF

水下目標(biāo)運(yùn)動(dòng)通常分為勻速直線巡航與機(jī)動(dòng)兩種狀態(tài)。對(duì)于做勻速直線巡航的目標(biāo)而言，對(duì)于(1)式所示的狀態(tài)方程，其所對(duì)應(yīng)目標(biāo)的運(yùn)動(dòng)模型是完全準(zhǔn)確的，在濾波初始值誤差不大情況下，此時(shí)狀態(tài)噪聲方差Q是很小的，若此時(shí)給定Q較大的值，則在步驟2～步驟4迭代過(guò)程中，由于誤差積累，使得濾波協(xié)方差PX增加[14]，即誤差波動(dòng)較大，濾波結(jié)果更依賴于觀測(cè)值，即預(yù)測(cè)值權(quán)重減小。雖然濾波算法可以收斂至真實(shí)值附近，但濾波結(jié)果的起伏較大，曲線不光滑，由(3)式不能準(zhǔn)確估計(jì)目標(biāo)的航速和航向；而在目標(biāo)機(jī)動(dòng)時(shí)，目標(biāo)航速在不斷變化，(1)式狀態(tài)方程將是變化的，此時(shí)若Q取值過(guò)小，將會(huì)使濾波器無(wú)法跟蹤到目標(biāo)的機(jī)動(dòng)，進(jìn)而使濾波結(jié)果發(fā)散。

在實(shí)際目標(biāo)跟蹤過(guò)程中，目標(biāo)運(yùn)動(dòng)狀態(tài)是未知的，為了同時(shí)兼顧跟蹤機(jī)動(dòng)目標(biāo)速度和穩(wěn)定狀態(tài)時(shí)的跟蹤精度，需要對(duì)目標(biāo)狀態(tài)進(jìn)行實(shí)時(shí)監(jiān)測(cè)，即在目標(biāo)出現(xiàn)機(jī)動(dòng)時(shí)取較大的Q值，在目標(biāo)勻速巡航時(shí)取較小的Q值。

3.1 濾波發(fā)散的判決條件

(4)

(5)

(5)式即為判決濾波發(fā)散趨勢(shì)的判決條件。在不滿足該條件時(shí)，判定濾波發(fā)散，即目標(biāo)處于機(jī)動(dòng)狀態(tài)，需要調(diào)整狀態(tài)噪聲方差Q.

3.2 實(shí)際參數(shù)選取

如(5)式所示，判決條件為殘差概率，因此在實(shí)際濾波過(guò)程中，計(jì)算殘差概率需要先做統(tǒng)計(jì)。設(shè)定一個(gè)寬度為N的判定窗口，在濾波初始階段：當(dāng)數(shù)據(jù)長(zhǎng)度小于等于N時(shí)，按照常規(guī)KF來(lái)計(jì)算；當(dāng)數(shù)據(jù)長(zhǎng)度大于N時(shí)，計(jì)算窗口內(nèi)殘差的概率分布：

(6)

由于數(shù)據(jù)長(zhǎng)度N不是無(wú)限長(zhǎng)，即使預(yù)測(cè)值與真值完全相等，也很難得到(5)式結(jié)果，因此再設(shè)定一個(gè)容錯(cuò)門(mén)限η：

(7)

即當(dāng)預(yù)測(cè)值和觀測(cè)值殘差大于0的概率偏離0.5較遠(yuǎn)(>η)時(shí)，判定目標(biāo)正在進(jìn)行機(jī)動(dòng)，否則認(rèn)為目標(biāo)處于勻速巡航狀態(tài)。η取值范圍為(0，1/2)，η值越小，預(yù)測(cè)值和觀測(cè)值微小的差別即可體現(xiàn)，系統(tǒng)對(duì)目標(biāo)機(jī)動(dòng)情況下的跟蹤能力越強(qiáng)，但穩(wěn)態(tài)時(shí)，濾波曲線的平滑程度將降低；η值越大，系統(tǒng)對(duì)目標(biāo)機(jī)動(dòng)情況下的跟蹤能力減弱，但濾波曲線的平滑程度將增加。

當(dāng)(7)式成立時(shí)，表明此時(shí)目標(biāo)在進(jìn)行機(jī)動(dòng)，濾波結(jié)果已發(fā)散，對(duì)狀態(tài)噪聲方差做如下處理：Qk=Qk-1+λ(λ為狀態(tài)變化量)，對(duì)于(1)式所示的狀態(tài)方程，目標(biāo)位置變化過(guò)程始終是正確的，產(chǎn)生誤差的主要原因是速度變化過(guò)程?，F(xiàn)階段能夠做強(qiáng)機(jī)動(dòng)的水下目標(biāo)速度變化率通常不會(huì)很大，在此取λ=diag(0, 0, 0.1, 0.1)，用來(lái)調(diào)整狀態(tài)噪聲方差，使其迅速增加，以減少預(yù)測(cè)值在濾波過(guò)程中的權(quán)重，從而抑制濾波結(jié)果的發(fā)散。

當(dāng)(7)式不成立時(shí)，表明此時(shí)目標(biāo)沒(méi)有進(jìn)行機(jī)動(dòng)，且濾波結(jié)果收斂到真值附近，為了得到更加平滑的濾波結(jié)果，對(duì)狀態(tài)方差做如下處理：Qk=γQk-1,其中γ=diag(1,1,1/(k-1),1/(k-1))為衰減系數(shù)矩陣，用來(lái)使?fàn)顟B(tài)噪聲方差迅速減小，增加預(yù)測(cè)值在濾波過(guò)程中的權(quán)重，使結(jié)果更加平滑。

從調(diào)整的過(guò)程看，雖然Q值在濾波算法發(fā)散時(shí)增加，而在濾波算法收斂時(shí)減小，但是始終是大于0的，因此不會(huì)出現(xiàn)濾波算法中斷的情形，彌補(bǔ)了Sage-Husa算法的不足。

4 數(shù)值仿真

為了驗(yàn)證基于殘差概率分布的自適應(yīng)UKF算法的有效性和性能，進(jìn)行數(shù)值仿真：與傳統(tǒng)UKF算法進(jìn)行對(duì)比，以驗(yàn)證算法的有效性；分析測(cè)量方差、機(jī)動(dòng)性強(qiáng)弱(轉(zhuǎn)彎角度)以及容錯(cuò)門(mén)限對(duì)算法性能的影響。對(duì)每個(gè)仿真條件進(jìn)行100次Monte Carlo實(shí)驗(yàn)，計(jì)算各個(gè)觀測(cè)點(diǎn)濾波結(jié)果的均方根誤差(RMSE)。為了評(píng)價(jià)目標(biāo)勻速運(yùn)動(dòng)時(shí)估計(jì)的航速和航向的穩(wěn)態(tài)值，取目標(biāo)機(jī)動(dòng)前(200～1 000 s)以及機(jī)動(dòng)后(1 200～2 000 s)時(shí)間段內(nèi)目標(biāo)速度與航向的統(tǒng)計(jì)特性，即RMSE均值和誤差標(biāo)準(zhǔn)差的均值，如(8)式和(9)式，RMSE均值用來(lái)驗(yàn)證濾波結(jié)果的準(zhǔn)確性，標(biāo)準(zhǔn)差均值用來(lái)驗(yàn)證濾波曲線的平滑性。

(8)

(9)

通過(guò)收斂時(shí)間評(píng)價(jià)算法對(duì)機(jī)動(dòng)目標(biāo)的跟蹤性能，定義濾波結(jié)果的RMSE大于觀測(cè)結(jié)果的RMSE時(shí)為發(fā)散時(shí)刻點(diǎn)t1，相應(yīng)地，濾波結(jié)果的RMSE小于觀測(cè)結(jié)果的RMSE時(shí)為時(shí)刻點(diǎn)t2，則在目標(biāo)機(jī)動(dòng)時(shí)濾波的收斂時(shí)間為t2-t1.

4.1 仿真實(shí)例

目標(biāo)運(yùn)動(dòng)軌跡和目標(biāo)位置估計(jì)的RMSE分別如圖2和圖3所示，目標(biāo)速度與航向估計(jì)的統(tǒng)計(jì)特性如表1所示。從圖3和表1中可以看出：對(duì)于Qa,即較大的狀態(tài)噪聲方差情況，其位置估計(jì)結(jié)果相對(duì)平穩(wěn)(尤其是1 000 s處)，對(duì)目標(biāo)機(jī)動(dòng)狀態(tài)的適應(yīng)性較強(qiáng)，但其在穩(wěn)態(tài)時(shí)濾波估計(jì)誤差很大，尤其是目標(biāo)航向，這不利于目標(biāo)的前向預(yù)測(cè)；對(duì)于Qb，即較小的狀態(tài)噪聲方差情況，當(dāng)目標(biāo)勻速直線運(yùn)動(dòng)時(shí)(機(jī)動(dòng)前)，其位置估計(jì)結(jié)果誤差較小，但在目標(biāo)做機(jī)動(dòng)后其收斂速度很慢；而狀態(tài)自適應(yīng)UKF算法雖然在目標(biāo)機(jī)動(dòng)時(shí)其位置誤差明顯增加，但在判斷濾波結(jié)果發(fā)散情況下能迅速收斂至真值附近，避免了Qb情況所帶來(lái)的濾波結(jié)果發(fā)散問(wèn)題，同時(shí)在目標(biāo)勻速運(yùn)動(dòng)時(shí)狀態(tài)自適應(yīng)UKF算法濾波結(jié)果誤差最小，濾波曲線相對(duì)平滑，從而可以準(zhǔn)確估計(jì)目標(biāo)的航速與航向。

圖2 目標(biāo)運(yùn)動(dòng)軌跡Fig.2 Target motion trail

參數(shù)誤差類型機(jī)動(dòng)前速度/(m·s-1)機(jī)動(dòng)前航向/(°)機(jī)動(dòng)后速度/(m·s-1)機(jī)動(dòng)后航向/(°)QaMRMSE1.358 713.965 51.296 114.324 2σRMSE1.288 913.921 11.439 814.283 8QbMRMSE0.222 33.411 60.100 71.750 0σRMSE0.167 81.721 60.092 62.689 9自適應(yīng)QMRMSE0.086 11.086 00.098 00.674 4σRMSE0.142 91.636 5 0.074 70.858 6

4.2 算法性能

在4.1節(jié)仿真條件基礎(chǔ)上，分別改變測(cè)量方差(距離和方位)、機(jī)動(dòng)角度(機(jī)動(dòng)性)和容錯(cuò)誤差，每個(gè)條件下進(jìn)行100次Monte Carlo實(shí)驗(yàn)，計(jì)算狀態(tài)自適應(yīng)UKF算法的濾波收斂時(shí)間、機(jī)動(dòng)后的速度，航向的RMSE均值與標(biāo)準(zhǔn)差均值。

4.2.1 測(cè)量方差的影響

假設(shè)探測(cè)系統(tǒng)的距離標(biāo)準(zhǔn)差分別為20 m、50 m和80 m，方位標(biāo)準(zhǔn)差保持不變，為1°；或者方位標(biāo)準(zhǔn)差分別取1°、2°和3°，距離方差保持不變，為50 m. 算法性能如表2所示。從表2中可以看出：不同距離與方位噪聲的增加會(huì)導(dǎo)致對(duì)濾波結(jié)果誤差略微增加，對(duì)濾波性能影響不是很明顯，在不同噪聲條件下，系統(tǒng)均能計(jì)算出較為平穩(wěn)、誤差較小的結(jié)果。

表2 測(cè)量方差的影響

4.2.2 機(jī)動(dòng)性的影響

目標(biāo)初始航向?yàn)?0°，機(jī)動(dòng)后的航向分別取135°、-135°和-90°，其他參數(shù)的選取與4.1節(jié)中相同，目標(biāo)機(jī)動(dòng)后算法的收斂時(shí)間、航速和航向的RMSE和標(biāo)準(zhǔn)差如表3所示。從表3中可以看出，轉(zhuǎn)向角度大小對(duì)性能影響不大，表明該算法可以適應(yīng)不同的機(jī)動(dòng)轉(zhuǎn)向。

4.2.3 容錯(cuò)門(mén)限的影響

從第3節(jié)的容錯(cuò)門(mén)限的定義可以看出：容錯(cuò)門(mén)限的范圍在(0，0.5)范圍內(nèi)，容錯(cuò)門(mén)限越小，算法對(duì)目標(biāo)機(jī)動(dòng)越敏感，算法跟蹤機(jī)動(dòng)目標(biāo)的能力越強(qiáng)；反之，算法跟蹤機(jī)動(dòng)目標(biāo)的能力越弱，但濾波后曲線越平滑(誤差越小)。保持其他參數(shù)不變，容錯(cuò)門(mén)限分別取0.2、0.3、0.4，算法的性能如表4所示。從表4中可以看出，本文算法跟蹤機(jī)動(dòng)目標(biāo)的能力和平滑度與理論結(jié)果基本一致(由于這是統(tǒng)計(jì)結(jié)果，偶爾會(huì)有差別，如航速標(biāo)準(zhǔn)差)。

表3 機(jī)動(dòng)性(轉(zhuǎn)彎角度)的影響

表4 容錯(cuò)門(mén)限的影響

綜合表2～表4可以看出，雖然改變了測(cè)量方差、目標(biāo)機(jī)動(dòng)性和容錯(cuò)門(mén)限，但是算法的收斂時(shí)間均在100 s以內(nèi)，且平穩(wěn)后算法估計(jì)的航速和航向誤差較小，具有較好的適應(yīng)性。

5 聲納信息綜合處理系統(tǒng)

圖4 聲納仿真系統(tǒng)框圖Fig.4 Block diagram of sonar simulation system

為了驗(yàn)證狀態(tài)自適應(yīng)UKF跟蹤算法在實(shí)際系統(tǒng)中的跟蹤性能，將該算法應(yīng)用于聲納信息綜合處理系統(tǒng)算例。該算例是為了檢驗(yàn)多功能標(biāo)準(zhǔn)臺(tái)D系列嵌入式綜合處理機(jī)平臺(tái)能否適應(yīng)聲納信號(hào)處理和綜合分析能力設(shè)計(jì)的。系統(tǒng)由外圍的信號(hào)模擬器、聲納信號(hào)處理模塊、目標(biāo)跟蹤模塊和顯示模塊等組成，如圖4所示，后面3個(gè)模塊將模擬真實(shí)的聲納信息處理過(guò)程。其中聲納信號(hào)處理包含主動(dòng)和被動(dòng)兩種模式，可以完成主動(dòng)探測(cè)中的波束形成、正交解調(diào)、信號(hào)檢測(cè)、目標(biāo)距離和方位估計(jì)，由于該算法只是模擬聲納處理過(guò)程，其目標(biāo)距離和方位估計(jì)方法分別為譜值最大法和三波束定向法，算法簡(jiǎn)單，但誤差較大。

圖5 目標(biāo)運(yùn)動(dòng)軌跡和估計(jì)位置Fig.5 Target motion trail vs. estimated location

圖6為跟蹤的目標(biāo)運(yùn)動(dòng)軌跡和目標(biāo)位置估計(jì)的RMSE，該結(jié)果與第4節(jié)中理論仿真結(jié)果一致。如果狀態(tài)噪聲方差取得較大，則估計(jì)結(jié)果平穩(wěn)，但是穩(wěn)態(tài)誤差較大；如果狀態(tài)噪聲方差取得較小，則UKF算法不能很快地跟蹤目標(biāo)機(jī)動(dòng)，而狀態(tài)自適應(yīng)UKF算法則很好地兼顧了機(jī)動(dòng)時(shí)的快速跟蹤和勻速運(yùn)動(dòng)時(shí)的穩(wěn)態(tài)誤差。

圖6 目標(biāo)運(yùn)動(dòng)軌跡跟蹤的RMSEFig.6 RMSEs of tracking the target motion trail

6 結(jié)論

本文為了能夠及時(shí)、準(zhǔn)確地跟蹤水下機(jī)動(dòng)目標(biāo)，對(duì)UKF算法進(jìn)行了改進(jìn)。以狀態(tài)殘差為標(biāo)準(zhǔn)，自適應(yīng)地調(diào)整狀態(tài)噪聲的方差，使算法更加適應(yīng)目標(biāo)的機(jī)動(dòng)。通過(guò)實(shí)驗(yàn)驗(yàn)證了該算法的有效性和測(cè)量誤差、機(jī)動(dòng)性和容錯(cuò)門(mén)限對(duì)算法性能的影響。結(jié)果表明：相對(duì)于傳統(tǒng)UKF目標(biāo)跟蹤，該算法不僅能夠準(zhǔn)確地對(duì)機(jī)動(dòng)目標(biāo)進(jìn)行跟蹤，而且能夠準(zhǔn)確地估計(jì)出目標(biāo)的航速與航向，進(jìn)而準(zhǔn)確地預(yù)測(cè)出其準(zhǔn)確的前向位置，在水下對(duì)抗中有著重要意義。