宋 闖，張 航，郝明瑞

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

0 引言

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

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

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

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

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

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

(1)

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

xk+1=Φxk+1+Γwk

(2)

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

(3)

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

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

(4)

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

2 CI算法和FCI算法

2.1 CI算法[5-6,16]

可以由一致性估計(jì)的定義得到

(5)

(6)

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

(7)

(8)

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

2.2 FCI算法

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

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

(9)

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

當(dāng)N=2時(shí)，非負(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的條件時(shí)，上面的方程有唯一解

(13)

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

對(duì)于N≥2,有

(14)

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

(15)

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

3 CDKF-FCI算法

CDKF利用一組確定的采樣點(diǎn)來(lái)捕捉系統(tǒng)的充分統(tǒng)計(jì)量，從而可以估計(jì)狀態(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是中心差分步長(zhǎng)。

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

1)初始化

(18)

2)計(jì)算時(shí)間更新Sigma點(diǎn)

(19)

3)時(shí)間更新

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

(20)

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

(21)

(22)

4)計(jì)算量測(cè)更新的Sigma點(diǎn)

(23)

5)量測(cè)更新

Sigma點(diǎn)隨量測(cè)方程的傳播為

(24)

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

(25)

(26)

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

(27)

濾波增益為

(28)

量測(cè)殘差為

(29)

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

(30)

(31)

式中

(32)

(33)

(34)

(35)

6)融合

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

(36)

(37)

式中

7)反饋

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

(38)

(39)

4 仿真分析

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

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

RMSEp(k)=

(40)

仿真中分別采用基于CI的CDKF-CI算法和本文提出的基于FCI的CDKF-FCI算法，進(jìn)行100次蒙特卡羅仿真，結(jié)果如圖1和圖2所示。由圖中可以看出，本文提出的CDKF-FCI算法較好地估計(jì)了系統(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)中的多個(gè)傳感器對(duì)目標(biāo)的跟蹤定位，該算法與傳統(tǒng)算法相比具有以下幾點(diǎn)優(yōu)勢(shì)：

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

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