肖力銘，屈濟(jì)坤，齊海生，岳振軍

(中國(guó)人民解放軍陸軍工程大學(xué)，江蘇 南京 210007)

為了獲得精確的機(jī)動(dòng)目標(biāo)航跡估計(jì)，需要通過(guò)卡爾曼濾波算法去除航跡數(shù)據(jù)中的誤差。當(dāng)機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)過(guò)程和噪聲都確定，且系統(tǒng)噪聲和量測(cè)噪聲都是零均值白噪聲時(shí)，經(jīng)典的卡爾曼濾波方法能夠得到較好的結(jié)果[1]。實(shí)際場(chǎng)景中，尤其在惡劣條件下，可能會(huì)存在較大的模型誤差，噪聲也不會(huì)保持確定的統(tǒng)計(jì)特性，導(dǎo)致卡爾曼濾波器發(fā)散。文獻(xiàn)[2]和文獻(xiàn)[3]提出通過(guò)引入模糊推理系統(tǒng)動(dòng)態(tài)調(diào)整濾波器模型中的誤差力度，實(shí)現(xiàn)對(duì)濾波器發(fā)散的控制。但這些方法假設(shè)噪聲協(xié)方差矩陣之間存在簡(jiǎn)單的線性關(guān)系，即假設(shè)當(dāng)前的噪聲協(xié)方差矩陣是前一時(shí)刻噪聲協(xié)方差矩陣的某一常數(shù)倍，通過(guò)模糊推理來(lái)調(diào)整倍數(shù)值，這一做法默認(rèn)假設(shè)引起誤差的各個(gè)因素是均勻的，對(duì)測(cè)量誤差來(lái)說(shuō)這一假設(shè)具有一定的合理性，但對(duì)于狀態(tài)誤差來(lái)說(shuō)，顯然就不符合事實(shí)了。

基于此，本文進(jìn)一步改進(jìn)了基于模糊推理的卡爾曼濾波算法，將當(dāng)前狀態(tài)噪聲協(xié)方差矩陣看成是前一時(shí)刻噪聲協(xié)方差矩陣的隨機(jī)擾動(dòng)，通過(guò)引入模糊控制器，依據(jù)不同的誤差來(lái)源采用不同推理規(guī)則和協(xié)方差矩陣調(diào)整策略，在線調(diào)整系統(tǒng)噪聲協(xié)方差值的擾動(dòng)程度，使之更好地貼合實(shí)際情況，從而使復(fù)雜條件下的數(shù)據(jù)融合效果得到較大程度的改善，仿真實(shí)驗(yàn)驗(yàn)證了算法的有效性。

1 卡爾曼濾波算法

20世紀(jì)60年代，卡爾曼(R．E．Kalman)發(fā)表了一篇重要的論文《A New Approach to Linear Filtering and Prediction Problems》，提出了一種新的線性濾波和預(yù)測(cè)理論。這種方法的實(shí)用價(jià)值在1963年美國(guó)“阿波羅計(jì)劃”中得到證實(shí)，現(xiàn)在廣泛應(yīng)用于通信、導(dǎo)航、制導(dǎo)與控制、動(dòng)態(tài)系統(tǒng)辨識(shí)、電力系統(tǒng)和經(jīng)濟(jì)等領(lǐng)域[4-8]。

設(shè)系統(tǒng)的態(tài)方程和量測(cè)方程分別為：

X(k)=AX(k-1)+BU(k)+ω(k)

(1)

Z(k)=HX(k)+v(k)

(2)

其中，X(k)、Z(k)、U(k)分別表示k時(shí)刻的系統(tǒng)狀態(tài)、測(cè)量值和系統(tǒng)控制量，A、B和H分別表示系統(tǒng)參數(shù)和測(cè)量系統(tǒng)參數(shù)，ω(k)和v(k)分別是系統(tǒng)的過(guò)程噪聲和測(cè)量噪聲。

卡爾曼濾波算法通過(guò)以下五個(gè)方程描述。

系統(tǒng)狀態(tài)的預(yù)測(cè)方程為

X(k|k-1)=AX(k-1|k-1)+Bu(k)

(3)

該狀態(tài)下的協(xié)方差預(yù)測(cè)方程為

P(k|k-1)=AP(k|k-1)A′+Q

(4)

系統(tǒng)濾波估計(jì)方程：

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))

(5)

卡爾曼濾波增益方程：

Kg(k)=P(k|k-1)H′/HP(k|k-1)H′+R

(6)

濾波協(xié)方差更新方程：

P(k|k)=(I-Kg(k)H)P(k|k-1)

(7)

2 新型模糊卡爾曼濾波算法

2.1 算法描述

在經(jīng)典卡爾曼濾波算法中，ω(k)和v(k)均被看作理想的零均值高斯白噪聲，且其協(xié)方差陣Qk和Rk已知[9-10]。然而，在復(fù)雜環(huán)境中，這一假設(shè)并不能滿(mǎn)足，因而需要實(shí)時(shí)調(diào)整Q，R值，以有效減小誤差。

對(duì)運(yùn)動(dòng)狀態(tài)引起的噪聲協(xié)方差矩陣改變，可假設(shè)k時(shí)刻的系統(tǒng)噪聲協(xié)方差估計(jì)值為：

(8)

另一方面，定義傳感器獲得的偏差值的觀測(cè)噪聲協(xié)方差估計(jì)值為：

(9)

其中，λi(k)為時(shí)變特性引起的量測(cè)噪聲協(xié)方差調(diào)整量。

當(dāng)經(jīng)典卡爾曼濾波算法的條件滿(mǎn)足時(shí)，Pi(k)與Cqi(k)、Si(k)與Cri(k)近似相等，復(fù)雜條件下，其相等性被破壞，即探測(cè)系統(tǒng)中的噪聲和測(cè)量噪聲統(tǒng)計(jì)特性發(fā)生變化，需要調(diào)整Pi(k)和Si(k)的值，使之更好地反應(yīng)實(shí)際情況。

為了刻畫(huà)Cqi(k)和Pi(k)的差別，定義函數(shù)：

其中，F(xiàn)是某一矩陣范數(shù)(本文取為2-范數(shù))，MDQi(k)值與1差別過(guò)大時(shí)，就必須通過(guò)調(diào)整Qi(k)的值改變Pi(k)值的大小，使得MDQi(k)近似等于1。

2.2 協(xié)方差矩陣調(diào)整規(guī)則

1)Q矩陣調(diào)整規(guī)則和算法

Q矩陣反映系統(tǒng)狀態(tài)誤差情況，可以假設(shè)各分量具有均勻性，對(duì)式(8)做如下處理：

如果MDQi(k)接近于1，那么τi(k)=1；

如果MDQi(k)遠(yuǎn)大于1，那么τi(k)>1；

如果MDQi(k)遠(yuǎn)小于1，那么τi(k)<1。

2)R矩陣調(diào)整規(guī)則和算法

R矩陣反映量測(cè)誤差情況，因而對(duì)式(9)采用如下規(guī)則：

如果MDRi(k)接近于1，那么λi(k)=0；

如果MDRi(k)遠(yuǎn)大于1，那么λi(k)>0；

如果MDRi(k)遠(yuǎn)小于1，那么λi(k)<0。

這個(gè)算法的核心思想是，如果理論協(xié)方差矩陣的范數(shù)遠(yuǎn)大于實(shí)際計(jì)算值，則讓理論協(xié)方差矩陣的元素大者變小、小者變大，進(jìn)而減小理論協(xié)方差矩陣的范數(shù)，使之與實(shí)際吻合，反之亦然。

通過(guò)仿真實(shí)驗(yàn)可以得到以下結(jié)論：

當(dāng)MDQi(k)?1時(shí)，1<τi(k)≤20；

當(dāng)MDQi(k)?1時(shí)，0.05≤τi(k)<1；

當(dāng)0<|λi(k)|≤10時(shí)，濾波能夠較好地收斂在目標(biāo)航跡上，否則，濾波會(huì)比較發(fā)散，導(dǎo)致追蹤效果不夠穩(wěn)定。

3 實(shí)驗(yàn)驗(yàn)證

其中，u1(k)和u2(k)是在時(shí)間間隔T內(nèi)，徑向速度和方位角速度的變化情況。由此得到狀態(tài)方程：

X(k+1)=A(k+1,k)X(k)+ω(k)

(10)

記v1(k)和v2(k)為觀測(cè)噪聲，得到測(cè)量方程：

即

Z(k)=CX(k)+v(k)

(11)

狀態(tài)方程激勵(lì)信號(hào)的協(xié)方差陣為

E[ω(k)ωT(j)]=Q(k)δkj

由式(10)可知，ω(k)=[0u1(k) 0u2(k)]T，故

由式(10)，得到狀態(tài)變量X(k)的初始估計(jì)值和真實(shí)值分別為

假設(shè)激勵(lì)信號(hào)u和噪聲源v是獨(dú)立的，則均方誤差矩陣為

分別對(duì)卡爾曼濾波算法和本文方法進(jìn)行實(shí)驗(yàn)仿真，得到結(jié)果如圖1-5所示。

由圖1和圖2可以看出，當(dāng)取樣點(diǎn)達(dá)到一定時(shí)，卡爾曼濾波算法的追蹤效果將出現(xiàn)明顯的偏差，而本文提出的模糊卡爾曼濾波算法則有效地避免了此類(lèi)情況的發(fā)生。由圖3和圖4可以看出，卡爾曼濾波方法在航跡追蹤時(shí)，隨著時(shí)間的推移，積累的絕對(duì)誤差將不斷增加，最終將會(huì)喪失航跡追蹤的功能。而本文提出的模糊卡爾曼濾波算法，隨著噪聲協(xié)方差的模糊調(diào)整量不斷變化，能夠較好地完成航跡的吻合跟蹤，從而達(dá)到預(yù)期的效果。圖5通過(guò)將兩種方法的航跡追蹤效果進(jìn)行對(duì)比印證了本文所述方法的有效性。

4 結(jié)束語(yǔ)

本文通過(guò)對(duì)誤差特性的分析，提出基于不同的誤差來(lái)源采用不同推理規(guī)則和協(xié)方差矩陣調(diào)整策略的模糊卡爾曼濾波算法。實(shí)例表明，在克服運(yùn)動(dòng)目標(biāo)長(zhǎng)時(shí)跟蹤導(dǎo)致的累積誤差和惡劣條件導(dǎo)致的誤差波動(dòng)方面起到了一定的作用，并且算法穩(wěn)定性較好，計(jì)算量也沒(méi)有顯著增加。