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

宋 闖，張 航，郝明瑞

(復雜系統控制與智能協同技術重點實驗室，北京 100074)

0 引言

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

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

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

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

1 系統模型的建立

在多傳感器目標跟蹤融合過程中，每個傳感器一般的系統方程為

(1)

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

xk+1=Φxk+1+Γwk

(2)

式中，wk是均值為0的白噪聲序列，Φ和Γ為轉移矩陣。假設目標做近似勻速直線運動，則有

(3)

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

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

(4)

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

2 CI算法和FCI算法

2.1 CI算法[5-6,16]

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

(5)

(6)

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

(7)

(8)

參數ω決定了估計的最優性，該參數可以根據Pcc某一范數最小的準則，通過最優化方法搜索得到。

2.2 FCI算法

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

在最小均方誤差意義下的優化，如下所示。

(9)

需要迭代計算價值函數的最小值以求得相應的權重系數ωn。為了減小計算負擔，采用下面一種次優的非迭代算法。

當N=2時，非負權重系數滿足下面的線性約束。

ω1+ω2=1

(10)

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

(11)

上面兩式聯立，可產生下面的線性系統

(12)

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

(13)

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

對于N≥2,有

(14)

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

(15)

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

3 CDKF-FCI算法

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

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算法對其他傳感器得到的信息進行融合處理，并將結果反饋給局部傳感器。對于第s個局部傳感器，算法流程具體如下：

1)初始化

(18)

2)計算時間更新Sigma點

(19)

3)時間更新

Sigma點隨狀態方程的傳播為

(20)

狀態的預測值和相應的誤差方差陣為

(21)

(22)

4)計算量測更新的Sigma點

(23)

5)量測更新

Sigma點隨量測方程的傳播為

(24)

量測的預測值和相應的誤差方差陣為

(25)

(26)

狀態和量測的互協方差矩陣為

(27)

濾波增益為

(28)

量測殘差為

(29)

后驗狀態和相應的誤差方差陣為

(30)

(31)

式中

(32)

(33)

(34)

(35)

6)融合

得到局部傳感器狀態和誤差方差陣后，計算融合后的狀態和相應的誤差方差陣為

(36)

(37)

式中

7)反饋

將局部傳感器的狀態和誤差方差陣用融合后的結果替換為

(38)

(39)

4 仿真分析

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

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

RMSEp(k)=

(40)

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

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

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

5 結論

本文提出了一種基于CDKF和FCI的融合跟蹤算法，用于協同制導中的多個傳感器對目標的跟蹤定位，該算法與傳統算法相比具有以下幾點優勢：

1)CDKF-FCI融合跟蹤算法不需要計算復雜的雅克比矩陣，相比CI算法無需迭代優化過程，簡化了融合系統的設計；

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