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

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

0 引言

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

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

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

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

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

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

(1)

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

(2)

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

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

(3)

(4)

這里基于文獻[10-11]對誤差表達式進行簡化：

(5)

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

(6)

(7)

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

(8)

(9)

(10)

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

(11)

(12)

(13)

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

(14)

(15)

(16)

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

(17)

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

(18)

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

(19)

(20)

(21)

(22)

(23)

(24)

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

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

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

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

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

(25)

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

(26)

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

(27)

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

假設(shè)1 非線性系統(tǒng)(見式(1)、(2))對于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)，并分別對不等式兩端求逆：

(36)

(37)

(38)

考慮恒等式tr(UV)=tr(VU)，對式(38)兩端求跡，存在實數(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)

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

(45)

(46)

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

(47)

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

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

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

(48)

b.時間更新

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

(49)

若滿足則進行下一步。

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

4 仿真實驗

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

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

xk+1=Fkxk+ωk

(50)

(51)

(52)

目標(biāo)運動的量測方程為

(53)

假設(shè)目標(biāo)的理論初始位置為[200 0]，初始速度和加速度分別為10 m/s、2 m/s2。為了驗證初始狀態(tài)偏差較大時算法的性能，設(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方向加速度信息為對象，取調(diào)節(jié)因子ρ和σ為常量102、104和模糊值，并與CKF進行對比，結(jié)果如圖2所示。在初始狀態(tài)存在較大偏差時，CKF失去跟蹤能力，雖然能夠基本收斂但濾波精度下降且收斂較慢。104RAKF(RAKF為殘差自適應(yīng)卡爾曼濾波)和RTSFA較CKF明顯具有較快的收斂速度，并減少由初始狀態(tài)不準(zhǔn)確引起的估計誤差。盡管兩者能克服濾波發(fā)散，但隨著估計誤差的減小，RTSFA的模糊自適應(yīng)調(diào)節(jié)因子相比于常量調(diào)節(jié)因子具有更高的濾波精度。102RAKF較104RAKF(調(diào)節(jié)因子為102和104常值的RAKF)具有更高的濾波精度，但其選擇較小的調(diào)節(jié)因子，其收斂速度不理想。

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

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

(54)

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

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

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

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

xk+1=Fkxk+Γωk

(55)

(56)

(57)

(58)

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

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

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

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

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

表2 4種算法下狀態(tài)估計均方誤差均值

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

5 結(jié)論

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

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