基于CDKF的快速協(xié)方差交叉融合跟蹤算法研究

宋 闖，張 航，郝明瑞

(復(fù)雜系統(tǒng)控制與智能協(xié)同技術(shù)重點實驗室，北京 100074)

0 引言

隨著科技的不斷進(jìn)步和武器裝備的快速發(fā)展，戰(zhàn)場環(huán)境日益復(fù)雜，武器裝備面臨嚴(yán)峻挑戰(zhàn)，必須提高其抗干擾性能和精確制導(dǎo)能力。而傳統(tǒng)單一模式制導(dǎo)的方式已很難滿足上述需求，因此，必須要研究基于多傳感器信息融合的制導(dǎo)技術(shù)，才能更好地發(fā)揮導(dǎo)彈武器的作戰(zhàn)效能。制導(dǎo)信息需要通過對目標(biāo)進(jìn)行跟蹤估計得到。

跟蹤估計方法主要有擴展卡爾曼濾波(Extended Kalman Filter，EKF)和Sigma點卡爾曼濾波(Sigma-Point Kalman Filter，SPKF)。EKF是最早提出的非線性濾波算法，并得到了廣泛的應(yīng)用。但該算法有明顯的缺點：需要得到系統(tǒng)的解析形式來計算雅克比矩陣，以及在非線性較強的情況下估計精度下降明顯。SPKF算法利用加權(quán)統(tǒng)計線性回歸技術(shù)，通過Sigma點的選取和變換來近似狀態(tài)和均值的非線性變換，對高斯分布而言，均值和方差的近似可以達(dá)到3階(泰勒展開)精度[1]。根據(jù)采樣點選取策略的不同，SPKF算法包括無跡卡爾曼濾波(Unscented Kalman Filter，UKF)和中心差分卡爾曼濾波(Central Difference Kalman Filter，CDKF)算法[2]。

對于分布式融合系統(tǒng)，冗余信息的多重利用往往會降低系統(tǒng)的融合性能，傳統(tǒng)的貝葉斯估計方法很難處理這一問題。針對這種情況，J.K.Uhlmann等[3]提出了一種可應(yīng)用于任意復(fù)雜分布式系統(tǒng)的協(xié)方差交叉(Covariance Intersection，CI)融合算法，當(dāng)局部狀態(tài)估計滿足一致性估計的條件時，算法所得融合估計也能保證估計的一致性。CI算法需要計算非線性評價函數(shù)的極值，這是一個很大的缺陷。因此，文獻(xiàn)[4]提出了一種非迭代的快速協(xié)方差交叉(Fast Covariance Intersection，F(xiàn)CI)算法。

本文針對多傳感器分布式融合跟蹤問題，提出了一種基于CDKF和FCI的融合跟蹤算法。局部傳感器采用CDKF處理非線性變換問題，避免了求解復(fù)雜的雅克比矩陣。與鄰近的其他傳感器采用FCI算法對獲得的信息進(jìn)行融合處理并輸出融合結(jié)果相比，保證了融合算法的實時性。最后將得到的融合估計反饋給局部傳感器。該算法充分利用了CDKF和FCI的優(yōu)點，仿真結(jié)果表明了該算法的有效性。

1 系統(tǒng)模型的建立

在多傳感器目標(biāo)跟蹤融合過程中，每個傳感器一般的系統(tǒng)方程為

(1)

式中，xk為nx維的系統(tǒng)狀態(tài)向量，zk為第s個傳感器的ny維觀測向量。w為系統(tǒng)噪聲，協(xié)方差矩陣為Q。vs為第s個傳感器的觀測噪聲，協(xié)方差矩陣為Rs。假定w和vs都是互不相關(guān)的高斯白噪聲，且都是加性噪聲。針對目標(biāo)跟蹤問題，需要采用特定的系統(tǒng)模型，具體如下所述。

xk+1=Φxk+1+Γwk

(2)

式中，wk是均值為0的白噪聲序列，Φ和Γ為轉(zhuǎn)移矩陣。假設(shè)目標(biāo)做近似勻速直線運動，則有

(3)

其中，T為采樣時間，I2為2×2的單位陣。

假設(shè)第s個傳感器對目標(biāo)的觀測為距離和方位角zs=[rs,θs]T，則有觀測方程為

(4)

式中，Δxs=xt-xs，Δys=yt-ys，(xs,ys)為傳感器位置坐標(biāo)。

2 CI算法和FCI算法

2.1 CI算法[5-6,16]

可以由一致性估計的定義得到

(5)

(6)

對于2個估計量的情況，CI算法可以總結(jié)如下

(7)

(8)

參數(shù)ω決定了估計的最優(yōu)性，該參數(shù)可以根據(jù)Pcc某一范數(shù)最小的準(zhǔn)則，通過最優(yōu)化方法搜索得到。

2.2 FCI算法

相對于標(biāo)準(zhǔn)卡爾曼濾波算法而言，CI算法的特點是需要計算非線性評價函數(shù)的極值[9-15]。因此，需要發(fā)展一個非迭代的FCI算法。

在最小均方誤差意義下的優(yōu)化，如下所示。

(9)

需要迭代計算價值函數(shù)的最小值以求得相應(yīng)的權(quán)重系數(shù)ωn。為了減小計算負(fù)擔(dān)，采用下面一種次優(yōu)的非迭代算法。

當(dāng)N=2時，非負(fù)權(quán)重系數(shù)滿足下面的線性約束。

ω1+ω2=1

(10)

tr(P1)ω1-tr(P2)ω2=0

(11)

上面兩式聯(lián)立，可產(chǎn)生下面的線性系統(tǒng)

(12)

當(dāng)滿足tr(P1)+tr(P2)>0的條件時，上面的方程有唯一解

(13)

注意到tr(Pn)≥0保證了ωn≥0，進(jìn)而滿足0≤ωn≤1。

對于N≥2,有

(14)

ω1+ω2+…+ωN=1 0≤wn≤1

(15)

這里εn=tr(Pn)，對于任意tr(Pn)>0，權(quán)重系數(shù)ωn是唯一且非負(fù)的。

3 CDKF-FCI算法

CDKF利用一組確定的采樣點來捕捉系統(tǒng)的充分統(tǒng)計量，從而可以估計狀態(tài)變量經(jīng)過任意非線性變換后的均值和方差。CDKF采用Sterling差值公式，利用中心差分代替泰勒展開中的前兩階導(dǎo)數(shù)

f′(x)=[f(x+h)-f(x-h)]/2h

(16)

f″(x)=[f(x+h)+f(x-h)-2f(x)]/h2

(17)

式中，h≥1是中心差分步長。

CDKF-FCI算法采用中心差分來近似非線性變換，同時利用FCI算法對其他傳感器得到的信息進(jìn)行融合處理，并將結(jié)果反饋給局部傳感器。對于第s個局部傳感器，算法流程具體如下：

1)初始化

(18)

2)計算時間更新Sigma點

(19)

3)時間更新

Sigma點隨狀態(tài)方程的傳播為

(20)

狀態(tài)的預(yù)測值和相應(yīng)的誤差方差陣為

(21)

(22)

4)計算量測更新的Sigma點

(23)

5)量測更新

Sigma點隨量測方程的傳播為

(24)

量測的預(yù)測值和相應(yīng)的誤差方差陣為

(25)

(26)

狀態(tài)和量測的互協(xié)方差矩陣為

(27)

濾波增益為

(28)

量測殘差為

(29)

后驗狀態(tài)和相應(yīng)的誤差方差陣為

(30)

(31)

式中

(32)

(33)

(34)

(35)

6)融合

得到局部傳感器狀態(tài)和誤差方差陣后，計算融合后的狀態(tài)和相應(yīng)的誤差方差陣為

(36)

(37)

式中

7)反饋

將局部傳感器的狀態(tài)和誤差方差陣用融合后的結(jié)果替換為

(38)

(39)

4 仿真分析

為了驗證算法的有效性，采用如下場景：目標(biāo)初始位置為(1000,1000)m，以速度(100,10)m/s做勻速直線運動。共有2個傳感器對目標(biāo)進(jìn)行觀測，位置分別為(1900,3200)m和(3575,3425)m。狀態(tài)方程和觀測方程分別由式(2)和式(3)給出。傳感器為主動傳感器，可以觀測角度和距離，其測距誤差標(biāo)準(zhǔn)差為10m，測角誤差標(biāo)準(zhǔn)差為0.2°，采樣時間為1s。仿真時間50s。

為了進(jìn)行性能比較，使用位置和速度的均方根誤差(Root Mean Squared Error，RMSE)作為性能指標(biāo)。k時刻的位置RMSE定義為

RMSEp(k)=

(40)

仿真中分別采用基于CI的CDKF-CI算法和本文提出的基于FCI的CDKF-FCI算法，進(jìn)行100次蒙特卡羅仿真，結(jié)果如圖1和圖2所示。由圖中可以看出，本文提出的CDKF-FCI算法較好地估計了系統(tǒng)的位置和速度，跟蹤精度相比CDKF-CI算法也有較大的提高。結(jié)果表明所提出的方法具有較高的融合跟蹤定位精度。

圖1 位置的均方根誤差Fig.1 RMS error of position

圖2 速度的均方根誤差Fig.2 RMS error of velocity

5 結(jié)論

本文提出了一種基于CDKF和FCI的融合跟蹤算法，用于協(xié)同制導(dǎo)中的多個傳感器對目標(biāo)的跟蹤定位，該算法與傳統(tǒng)算法相比具有以下幾點優(yōu)勢：

1)CDKF-FCI融合跟蹤算法不需要計算復(fù)雜的雅克比矩陣，相比CI算法無需迭代優(yōu)化過程，簡化了融合系統(tǒng)的設(shè)計；

2)仿真結(jié)果有效地證明了CDKF-FCI融合跟蹤算法具有較高的估計精度。