騰紅磊，王躍鋼，楊 波，單 斌，張復(fù)建

(1.火箭軍工程大學(xué) 三系，陜西 西安 710025；2.火箭軍士官學(xué)院，山東 青州 262500)

0 引言

近年來(lái)，非線性卡爾曼濾波由于其優(yōu)點(diǎn)而得到國(guó)內(nèi)外學(xué)者的深入研究。其中，擴(kuò)展卡爾曼濾波器(EKF)及其一些改進(jìn)算法得到較廣泛的應(yīng)用，但需計(jì)算雅克比矩陣，且一階線性化近似精度低[1]。為了克服EKF的理論局限性，Julier等提出無(wú)跡卡爾曼濾波(UKF)算法[2]，避免雅克比矩陣的復(fù)雜計(jì)算，但應(yīng)對(duì)高維數(shù)系統(tǒng)存在發(fā)散可能。Arasaratnam等[3]使用三階球面-相徑容積法則代替Unscented變換近似計(jì)算策略，提出容積卡爾曼濾波(CKF)，并被應(yīng)用于導(dǎo)航定位、目標(biāo)跟蹤和動(dòng)態(tài)定位等領(lǐng)域[4-5]。上述方法是基于狀態(tài)先驗(yàn)分布選擇確定數(shù)目的采樣點(diǎn)，并對(duì)非線性函數(shù)直接傳遞后的采樣點(diǎn)加權(quán)處理，以近似獲得狀態(tài)后驗(yàn)概率密度分布，數(shù)學(xué)意義上統(tǒng)一歸為確定采樣型濾波器。

非線性卡爾曼濾波在應(yīng)用于目標(biāo)跟蹤時(shí)性能依賴于噪聲統(tǒng)計(jì)特性和系統(tǒng)模型是否準(zhǔn)確，否則引起系統(tǒng)估計(jì)誤差持續(xù)增大，甚至濾波發(fā)散。針對(duì)先驗(yàn)統(tǒng)計(jì)特性未知和估計(jì)誤差較大約束條件下的EKF算法，通過(guò)選擇合適的噪聲協(xié)方差矩陣能提高濾波數(shù)值穩(wěn)定性和收斂速率[6]。因此，學(xué)者提出當(dāng)模型的非線性和噪聲先驗(yàn)統(tǒng)計(jì)特性滿足一定條件時(shí)UKF估計(jì)誤差有界收斂，并指出調(diào)整噪聲協(xié)方差矩陣能確保濾波器收斂[7]。孫妍等將STF算法引入CKF算法框架，提出容積卡爾曼的自適應(yīng)改進(jìn)算法(ACKF)，仿真結(jié)果證實(shí)了可行性[8]。雖然CKF及其相關(guān)改進(jìn)算法能有效提高濾波精度和數(shù)值穩(wěn)定性，抑制濾波發(fā)散，但缺少相關(guān)的理論對(duì)CKF的數(shù)值穩(wěn)定性和濾波收斂性進(jìn)行系統(tǒng)的分析[9]。

本文基于已獲得的非線性算法的收斂性結(jié)論，提出一種基于殘差的模糊自適應(yīng)(RTSFA)非線性目標(biāo)跟蹤算法，利用李雅普諾夫第二方法給出算法收斂的充分條件，將T-S模糊邏輯和量測(cè)橢球界限規(guī)則引入非線性跟蹤算法，最后給出RTSFA算法的具體流程。

1 RTSFA目標(biāo)跟蹤算法機(jī)理

假設(shè)非線性離散系統(tǒng)和量測(cè)模型描述如下：

xk+1=f(xk,uk)+ωk

(1)

yk+1=h(xk+1)+νk+1

(2)

式中：xk∈Rn為k時(shí)刻系統(tǒng)的狀態(tài)向量；uk∈Rp為控制向量；yk∈Rm為量測(cè)向量；ωk∈Rn，νk∈Rm分別為零均值且線性無(wú)關(guān)的高斯白噪聲；函數(shù)f(xk)和h(xk)分別為狀態(tài)和量測(cè)的非線性函數(shù)。

首先，定義估計(jì)誤差和一步預(yù)測(cè)誤差為

(3)

(4)

這里基于文獻(xiàn)[10-11]對(duì)誤差表達(dá)式進(jìn)行簡(jiǎn)化：

(5)

因線性化近似時(shí)忽略了高階項(xiàng)，為了得到嚴(yán)格的預(yù)測(cè)誤差等式，這里定義2個(gè)未知對(duì)角矩陣βk=diag(β1,k,…，βn,k)，αk=diag(α1,k,…，αn,k)，使得下式成立：

(6)

(7)

由Sigma點(diǎn)卡爾曼濾波(SPKF)算法[12]可得，系統(tǒng)濾波滿足高斯分布積分：

(8)

(9)

(10)

量測(cè)預(yù)測(cè)及協(xié)方差、互協(xié)方差為

(11)

(12)

(13)

狀態(tài)更新及協(xié)方差、卡爾曼增益為

(14)

(15)

(16)

由式(3)、(4)、(15)易得

(17)

整理式(17)、(7)，代入(6)可得

(18)

因此，可求得預(yù)測(cè)誤差協(xié)方差為

(19)

(20)

(21)

(22)

(23)

(24)

通過(guò)定義未知的對(duì)角矩陣，給出基于SPKF基本框架且含有線性誤差的系統(tǒng)濾波嚴(yán)格形式，下面進(jìn)一步分析RTSFA算法機(jī)理收斂的充分條件。

2 RTSFA目標(biāo)跟蹤算法機(jī)理收斂性分析

對(duì)式(3)的估計(jì)誤差進(jìn)行分析，下面不加證明的給出關(guān)于隨機(jī)過(guò)程有界性的結(jié)論[14]：

引理 1 假設(shè)存在隨機(jī)過(guò)程Vk(ηk)，實(shí)數(shù)vmin，vmax，μ>0，0<λ≤1，使得隨機(jī)過(guò)程ηk的每個(gè)解都滿足下式：

vmin‖ηk‖2≤Vk(ηk)≤vmin‖ηk‖2

(25)

E[Vk(ηk)|ηk]-Vk-1(ηk-1)≤μ-λVk-1(ηk-1)

(26)

則隨機(jī)過(guò)程均方指數(shù)有界，即

(27)

為證明過(guò)程更準(zhǔn)確，有如下假設(shè)：

假設(shè)1 非線性系統(tǒng)(見(jiàn)式(1)、(2))對(duì)于k≥0，存在umin,umax,smin,smax,βmin,βmax,αmin,αmax,φmax≠0使以下不等式成立：

(28)

(29)

證明：定義李雅普諾夫函數(shù)：

(30)

(31)

(32)

(33)

(34)

根據(jù)假設(shè)條件1、2的各界限值可得

(35)

將式(35)代入式(34)，并分別對(duì)不等式兩端求逆：

(36)

(37)

(38)

考慮恒等式tr(UV)=tr(VU)，對(duì)式(38)兩端求跡，存在實(shí)數(shù)μk>0，且在假設(shè)條件下有界。將式(36)代入式(37)可得

(39)

(40)

(41)

聯(lián)合假設(shè)1、2的各界限值可得

(42)

3 RTSFA目標(biāo)跟蹤算法步驟及應(yīng)用

根據(jù)上述的收斂性結(jié)論，結(jié)合式(10)、(12)，Rk和Qk的值成為關(guān)鍵。因此，引入自適應(yīng)協(xié)方差矩陣，構(gòu)造自適應(yīng)Rk和Qk如下[10]：

(43)

(44)

采用單點(diǎn)模糊產(chǎn)生器[15]，確定2個(gè)模糊集：D為減少，I為增加。選擇T-S模型設(shè)計(jì)推理引擎，ρ和σ的變化邏輯關(guān)系通過(guò)Qk和Rk的遞推關(guān)系為

(45)

(46)

2) 根據(jù)式(43)、(44)，本文構(gòu)造的自適應(yīng)狀態(tài)方程噪聲協(xié)方差和量測(cè)方程噪聲協(xié)方差僅與殘差有關(guān)，當(dāng)出現(xiàn)異常量測(cè)更新時(shí)會(huì)產(chǎn)生較大殘差。因此，這里選擇量測(cè)橢球界限規(guī)則確定量測(cè)空間的有效區(qū)域，用于判定異常量測(cè)野值，描述如下：

(47)

式中γG為界限閾值，可從χ2狀態(tài)分布中得到[16-17]。

RTSFA的非線性目標(biāo)跟蹤算法總結(jié)如下：

a.初始化后驗(yàn)概率密度函數(shù)

(48)

b.時(shí)間更新

4) 根據(jù)式(47)進(jìn)行判定，不滿足則按下式進(jìn)行更新：

(49)

若滿足則進(jìn)行下一步。

6) 重復(fù)執(zhí)行算法步驟2)～5)。

4 仿真實(shí)驗(yàn)

4.1 實(shí)例1：濾波初值信息不明純方位跟蹤

二維笛卡爾坐標(biāo)系下，假設(shè)雷達(dá)測(cè)站位于坐標(biāo)原點(diǎn)，目標(biāo)在空間內(nèi)沿方向角30°做近似勻加速直線飛行，目標(biāo)運(yùn)動(dòng)模型的狀態(tài)方程描述為

xk+1=Fkxk+ωk

(50)

(51)

(52)

目標(biāo)運(yùn)動(dòng)的量測(cè)方程為

(53)

假設(shè)目標(biāo)的理論初始位置為[200 0]，初始速度和加速度分別為10 m/s、2 m/s2。為了驗(yàn)證初始狀態(tài)偏差較大時(shí)算法的性能，設(shè)置濾波器的初始狀態(tài)x0=[10 30 10 50 10 10]T，初始協(xié)方差矩陣P0=10-2I6，δ和ζ分別取10-6和10-4，仿真數(shù)據(jù)產(chǎn)生步數(shù)N=100。

首先，分析不同調(diào)節(jié)因子的影響，以x方向加速度信息為對(duì)象，取調(diào)節(jié)因子ρ和σ為常量102、104和模糊值，并與CKF進(jìn)行對(duì)比，結(jié)果如圖2所示。在初始狀態(tài)存在較大偏差時(shí)，CKF失去跟蹤能力，雖然能夠基本收斂但濾波精度下降且收斂較慢。104RAKF(RAKF為殘差自適應(yīng)卡爾曼濾波)和RTSFA較CKF明顯具有較快的收斂速度，并減少由初始狀態(tài)不準(zhǔn)確引起的估計(jì)誤差。盡管兩者能克服濾波發(fā)散，但隨著估計(jì)誤差的減小，RTSFA的模糊自適應(yīng)調(diào)節(jié)因子相比于常量調(diào)節(jié)因子具有更高的濾波精度。102RAKF較104RAKF(調(diào)節(jié)因子為102和104常值的RAKF)具有更高的濾波精度，但其選擇較小的調(diào)節(jié)因子，其收斂速度不理想。

圖1 初始狀態(tài)偏差較大時(shí)x方向加速度估計(jì)曲線

其次，為了更好的對(duì)比算法性能，使用加速度信息定義的均方誤差均值：

(54)

表1 幾種算法下x方向加速度均方誤差均值

當(dāng)系統(tǒng)初值不明甚至存在較大偏差時(shí)，RTSFA的算法跟蹤性能優(yōu)于其他確定采樣卡爾曼濾波器，尤其是與常量調(diào)節(jié)因子的卡爾曼濾波相比具有更快的收斂速度，并能在估計(jì)誤差減小時(shí)確保估計(jì)值更接近真實(shí)值。

4.2 實(shí)例2：量測(cè)噪聲時(shí)變純方位目標(biāo)跟蹤

基于實(shí)例1的仿真環(huán)境，目標(biāo)在空間內(nèi)以一定角速度做機(jī)動(dòng)轉(zhuǎn)彎，運(yùn)動(dòng)量測(cè)方程和噪聲特性形式仿照實(shí)例1定義，目標(biāo)運(yùn)動(dòng)模型的狀態(tài)方程描述為

xk+1=Fkxk+Γωk

(55)

(56)

(57)

(58)

在量測(cè)噪聲時(shí)變的條件下，圖2～4分別為4種算法跟蹤目標(biāo)的位置、速度和角速度的均方根誤差結(jié)果。在目標(biāo)跟蹤30 s內(nèi)，量測(cè)噪聲與先驗(yàn)量測(cè)噪聲相對(duì)一致，4種濾波估計(jì)誤差相差不大。

圖2 4種算法下位置的均方根誤差

圖3 4種算法下加速度均方根誤差

圖4 4種算法下角速度均方根誤差

在31～90 s和91～150 s時(shí)量測(cè)噪聲發(fā)生變化，先驗(yàn)量測(cè)噪聲協(xié)方差不能對(duì)系統(tǒng)量測(cè)噪聲進(jìn)行準(zhǔn)確數(shù)學(xué)描述，4種算法估計(jì)誤差都開(kāi)始迅速變大。與其他3種算法相比，標(biāo)準(zhǔn)CKF算法不具備調(diào)整量測(cè)噪聲協(xié)方差的能力，估計(jì)誤差隨時(shí)間更新逐步加大，濾波精度下降，收斂較遲緩；102RAKF采用較小調(diào)節(jié)因子，能有效提高濾波精度，避免濾波發(fā)散，但收斂較緩慢；104RAKF收斂快，但收斂后精度較差。表2為4種算法的狀態(tài)估計(jì)均方根誤差均值統(tǒng)計(jì)數(shù)據(jù)。

表2 4種算法下?tīng)顟B(tài)估計(jì)均方誤差均值

RTSFA算法基于量測(cè)殘差和模糊邏輯遞推量測(cè)噪聲協(xié)方差陣，使R更接近真實(shí)值，克服時(shí)變?cè)肼暯y(tǒng)計(jì)特性引起的濾波發(fā)散問(wèn)題。由圖2～4及表2可知，本文算法的濾波估計(jì)精度和收斂速率優(yōu)于其他3種算法，跟蹤數(shù)據(jù)更接近真實(shí)值。

5 結(jié)論

1) 提出一種基于殘差的模糊自適應(yīng)(RTSFA)非線性目標(biāo)跟蹤算法。該算法根據(jù)Sigma點(diǎn)卡爾曼濾波(SPKF)的基本框架和球面徑向規(guī)則推導(dǎo)出線性化誤差約束條件下的近似高斯權(quán)值積分一般形式。同時(shí)，利用李雅普諾夫第二方法證明了RTSFA非線性目標(biāo)跟蹤算法估計(jì)誤差有界收斂的充分條件，并構(gòu)造一種噪聲估計(jì)器在線修正噪聲統(tǒng)計(jì)特性，引入T-S模糊邏輯算法和量測(cè)橢球界限規(guī)則選擇準(zhǔn)確的自適應(yīng)噪聲協(xié)方差調(diào)節(jié)因子，有效抑制傳統(tǒng)非線性跟蹤算法因噪聲統(tǒng)計(jì)不準(zhǔn)引起的濾波發(fā)散，增強(qiáng)非線性濾波器應(yīng)對(duì)目標(biāo)跟蹤的魯棒性。

2) 將本文算法應(yīng)用于信息不明和量測(cè)噪聲時(shí)變純方位目標(biāo)跟蹤中進(jìn)行仿真驗(yàn)證，在目標(biāo)因初值誤差和量測(cè)時(shí)變引起估計(jì)誤差變大時(shí)，與其他確定性采樣卡爾曼濾波器和常量調(diào)節(jié)因子的殘差自適應(yīng)卡爾曼濾波相比，該方法具有更強(qiáng)的魯棒性和更快的收斂速度，表現(xiàn)出更好的跟蹤能力。