蔡 佳，黃長(zhǎng)強(qiáng)，李美亞，齊曉林

(1.空軍工程大學(xué), 陜西 西安 710038；2.西安機(jī)電信息研究所, 陜西 西安 710056)

目標(biāo)跟蹤是為了維持對(duì)目標(biāo)當(dāng)前狀態(tài)的估計(jì)，同時(shí)也是對(duì)傳感器接收到的量測(cè)信息進(jìn)行處理的過(guò)程[1]。但是，傳感器的測(cè)量方程很多都是非線性的，需要采用非線性的估計(jì)方法。擴(kuò)展卡爾曼濾波(Extended Kalman Filtering，EKF)算法是一種常用的方法[2-4]，但它有兩點(diǎn)缺陷：必須滿足小擾動(dòng)假設(shè)，只適合弱非線性系統(tǒng)，對(duì)于強(qiáng)非線性系統(tǒng)，濾波性能極不穩(wěn)定，甚至發(fā)散；需要計(jì)算Jacobian矩陣，計(jì)算復(fù)雜，容易出錯(cuò)。

Julier和Wan等人提出了基于U變換的無(wú)跡卡爾曼濾波算法[5-7]。UKF不是像EKF那樣去近似非線性模型，而是對(duì)后驗(yàn)概率密度函數(shù)進(jìn)行近似以得到次優(yōu)的濾波算法，具有不需求導(dǎo)，估計(jì)精度高等優(yōu)點(diǎn)。然而，UKF算法需要知道系統(tǒng)模型參數(shù)和噪聲統(tǒng)計(jì)特性等先驗(yàn)知識(shí)。如果先驗(yàn)知識(shí)未知或不準(zhǔn)確，濾波器將會(huì)產(chǎn)生較大的估計(jì)誤差，甚至造成濾波發(fā)散。傳統(tǒng)做法是將未知的噪聲統(tǒng)計(jì)特性假設(shè)為1組均值為0、協(xié)方差已知的高斯白噪聲序列，但是零均值假設(shè)并不適于處理目標(biāo)持續(xù)機(jī)動(dòng)的情形，最終造成濾波發(fā)散。

為此，本文設(shè)計(jì)了一種非零均值的時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)器，引入含有自適應(yīng)衰減因子的加權(quán)系數(shù)，將時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)與傳統(tǒng)UKF算法相結(jié)合形成一種自適應(yīng)濾波(Adaptive UKF，AUKF)算法。

1 非零均值UKF算法

UKF規(guī)定一組確定性的σ點(diǎn)，當(dāng)狀態(tài)向量的概率密度函數(shù)是高斯型時(shí)，利用這組σ點(diǎn)獲取高斯密度函數(shù)的真正均值和協(xié)方差，當(dāng)高斯型狀態(tài)向量的概率密度函數(shù)經(jīng)由非線性系統(tǒng)進(jìn)行傳遞時(shí)，對(duì)任何一種非線性系統(tǒng)，利用這組σ點(diǎn)能獲取精確到二階的后驗(yàn)均值和協(xié)方差。

假設(shè)非線性離散系統(tǒng)的狀態(tài)方程和測(cè)量方程為：

Xk=f(Xk-1,k-1)+wk-1

(1)

Zk=h(Xk,k)+vk

(2)

式中，過(guò)程噪聲wk與測(cè)量噪聲vk均為零均值高斯白噪聲序列，分別具有協(xié)方差陣：cov(wk,wl)=Qkδkl,cov(vk,vl)=Rkδkl。初始狀態(tài)X0的統(tǒng)計(jì)特性為EX0=x0，varX0=P0，且X0、wk和vk互不相關(guān)。

傳統(tǒng)UKF算法[8-10]通常假定過(guò)程噪聲和測(cè)量噪聲均值為零，然而在實(shí)際情況中，噪聲零均值的條件并不能夠滿足，因此首先要對(duì)非線性系統(tǒng)進(jìn)行修正：

Xk=f(Xk-1,k-1)+wk-1+q

(3)

Zk=h(Xk,k)+vk+r

(4)

式中，q和r分別為狀態(tài)方程和測(cè)量方程的附加噪聲均值，E(wk-1+q)=q，E(vk+r)=r。

當(dāng)噪聲均值非零時(shí)，同樣需要對(duì)傳統(tǒng)UKF算法進(jìn)行修正，具體步驟如下：

(5)

(χk/k-1)i=f((χk-1/k-1)i)+q,i=0,1,2,…,2L

(6)

(7)

(8)

(Zk/k-1)i=h((χk/k-1)i)+r,i=0,1,…,2L

(9)

(10)

(11)

(12)

式中，Rk為測(cè)量噪聲協(xié)方差矩陣。

(13)

(14)

(15)

2 時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)的AUKF算法

2.1 噪聲統(tǒng)計(jì)估計(jì)器

基于極大后驗(yàn)估計(jì)原理[11]和測(cè)量值Zk，根據(jù)參考文獻(xiàn)[12-13]，得到如下次優(yōu)MAP噪聲統(tǒng)計(jì)估計(jì)器：

(16)

(17)

(18)

(19)

上述MAP估計(jì)器是針對(duì)時(shí)變的噪聲統(tǒng)計(jì)特性，定常時(shí)的噪聲統(tǒng)計(jì)估計(jì)比較簡(jiǎn)單，且估計(jì)值已被證明為無(wú)偏的[14]。本文考慮時(shí)變?cè)肼暯y(tǒng)計(jì)特性。

(20)

式中，b為遺忘因子，0

2.2 自適應(yīng)UKF算法

將上述時(shí)變?cè)肼暯y(tǒng)計(jì)的估計(jì)值代入傳統(tǒng)UKF，即可得到改進(jìn)的自適應(yīng)UKF算法，計(jì)算流程如下：

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

由于過(guò)程噪聲可以經(jīng)過(guò)建模進(jìn)行補(bǔ)償，對(duì)模型變化的影響不是很大；而測(cè)量噪聲主要是由工作環(huán)境引起的，其不確定性很大，不能進(jìn)行建模補(bǔ)償。因此，實(shí)際工作中系統(tǒng)模型的變化主要是由測(cè)量噪聲引起的。本文僅對(duì)測(cè)量噪聲的均值r和方差陣R進(jìn)行估計(jì)，暫不考慮過(guò)程噪聲的影響。

3 仿真分析

為了驗(yàn)證本文設(shè)計(jì)的自適應(yīng)UKF算法性能，首先做出以下假設(shè)：

1)狀態(tài)方程線性，測(cè)量方程非線性。

2)過(guò)程噪聲統(tǒng)計(jì)特性：均值q=0，方差為Q；測(cè)量噪聲統(tǒng)計(jì)特性：均值r≠0，方差為R。

根據(jù)以上假設(shè)條件，則系統(tǒng)模型為：

Xk=Φk,k-1Xk-1+wk-1

(29)

Zk=h(Xk,k)+vk+r

(30)

3.1 參數(shù)設(shè)置

初始狀態(tài)設(shè)為：x0=[1 000 3 000 20 50 3 -4]T，P0=diag[100 100 1 1 0.1 0.1]。

時(shí)變?cè)肼暤某跏冀y(tǒng)計(jì)特性為：

Q0=diag[1 1 0.120.120.0120.012]，

采樣時(shí)間0.1 s，仿真時(shí)間100 s，對(duì)本文提出的AUKF算法進(jìn)行100次Monte Carlo仿真實(shí)驗(yàn)。

3.2 結(jié)果分析

仿真結(jié)果的評(píng)價(jià)指標(biāo)選為狀態(tài)向量的均方根誤差(RMSE)，定義為：

(31)

AUKF算法的跟蹤效果如圖1所示，可見(jiàn)估計(jì)值與目標(biāo)的真實(shí)軌跡基本重合，這說(shuō)明采用本文的自適應(yīng)濾波算法可以實(shí)現(xiàn)對(duì)目標(biāo)的有效跟蹤。

圖2～4所示為AUKF算法的濾波結(jié)果及與EKF算法和傳統(tǒng)UKF算法的誤差比較。

圖2至圖4所示為笛卡爾坐標(biāo)系下，目標(biāo)在x和y方向的位置、速度和加速度均方根誤差比較。從仿真結(jié)果圖對(duì)比看出，EKF算法和傳統(tǒng)UKF 算法的狀態(tài)估計(jì)結(jié)果均不及AUKF，尤其是EKF算法的均方根誤差更大，這是因?yàn)镋KF本身忽略了高階截?cái)嗾`差，對(duì)噪聲統(tǒng)計(jì)特性估計(jì)不準(zhǔn)確造成的。本文提出的AUKF 算法可以不斷地實(shí)時(shí)估計(jì)時(shí)變?cè)肼暤慕y(tǒng)計(jì)特性，根據(jù)量測(cè)信息對(duì)濾波發(fā)散趨勢(shì)進(jìn)行判斷，并引入指數(shù)加權(quán)的衰減因子抑制濾波發(fā)散，因此能夠有效地解決由于噪聲統(tǒng)計(jì)特性未知或不準(zhǔn)確而引起的濾波精度不高的問(wèn)題，大大提高了收斂速度和估計(jì)精度。

表1 結(jié)合本文選取的評(píng)價(jià)指標(biāo)-狀態(tài)向量各個(gè)元素的均方根誤差，比較了3種算法的濾波性能。仿真結(jié)果表明，AUKF 算法在濾波精度方面明顯優(yōu)于EKF算法和傳統(tǒng)UKF算法，因此，狀態(tài)估計(jì)也更接近于真實(shí)狀態(tài)，目標(biāo)跟蹤性能更優(yōu)。

表1 算法性能比較(均方根誤差)

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

對(duì)于目標(biāo)跟蹤問(wèn)題，傳統(tǒng)UKF算法在噪聲先驗(yàn)知識(shí)未知時(shí)將其均值假設(shè)為零，這種處理方法容易造成濾波發(fā)散。為此，本文在對(duì)系統(tǒng)模型和傳統(tǒng)濾波算法修正的同時(shí)，設(shè)計(jì)了一種時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)器，并將其引入到傳統(tǒng)UKF算法當(dāng)中，提出了一種自適應(yīng)UKF算法，該算法可以在濾波計(jì)算的同時(shí)，對(duì)噪聲的未知先驗(yàn)信息進(jìn)行實(shí)時(shí)統(tǒng)計(jì)估計(jì)和修正。仿真結(jié)果表明，該算法在保證濾波收斂的同時(shí)提高了跟蹤精度。

[1] 韓崇昭, 朱洪艷, 段戰(zhàn)勝. 多源信息融合[M]. 北京: 清華大學(xué)出版社, 2006: 124-188.

HAN Chong-zhao, ZHU Hong-yan, DUAN Zhan-sheng. Multi-source information fusion[M]. Beijing: Tsinghua University Press, 2006: 124-188. (in Chinese)

[2] EINICKE G A, WHITE L B. Robust extended Kalman filtering[J]. IEEE Transactions on Signal Processing, 1999, 47(9): 2596-2599.

[3] 秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 1998: 182-188.

QIN Yong-yuan, ZHANG Hong-yue, WANG Shu-hua. Kalman filtering and integrated navigation theory[M]. Xi’an: Northwestern Polytechnical University Press, 1998: 182-188. (in Chinese)

[4] JOSEPH J, LAVIOLA Jr. A comparison of unscented and extended Kalman filtering for estimating quaternion motion[C]//Proceedings of the American Control Conference. Denver:[s.n.],2003: 2435-2440.

[5] JULIER S J. The scaled unscented transformation[C]//Proceedings of the American Control Conference.Anchorage:[s.n.],2002: 4555-4559.

[6] JULIER S J, UHLMANN J K. Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE, 2004, 92(3): 401-422.

[7] WAN E A, VAN DER MERVE R. The unscented Kalman filter for nonlinear estimation[C]//Proceedings of the IEEE Adaptive Systems for Signal Processing, Communication and Control Symposium.Lake Louise:[s.n.],2000:153-158.

[8] 楊凱, 倪龍強(qiáng), 張麗華, 等. 基于IMM-UKF的非線性機(jī)動(dòng)目標(biāo)跟蹤仿真研究[J]. 火炮發(fā)射與控制學(xué)報(bào), 2010(3): 12-16.

YAN Kai, NI Long-qiang, ZHANG Li-hua, et al. Study on tracking simulation of nonlinear maneuvering target based on IMM-UKF[J]. Journal of Gun Launch & Control, 2010(3): 12-16. (in Chinese)

[9] 江寶安, 萬(wàn)群. 基于UKF-IMM的雙紅外機(jī)動(dòng)目標(biāo)跟蹤算法[J]. 系統(tǒng)工程與電子技術(shù), 2008,30(8):1454-1459.

JIANG Bao-an, WAN Qun. Maneuvering target passive tracking with dual infrared observers using IMM algorithm based on UKF[J]. Systems Engineering and Electronics, 2008, 30(8): 1454-1459. (in Chinese)

[10] 張文玲, 朱明清, 陳宗海. 基于強(qiáng)跟蹤UKF的自適應(yīng)SLAM算法[J]. 機(jī)器人, 2010, 32(2): 190-195.

ZHANG Wen-ling, ZHU Ming-qing, CHEN Zong-hai. An adaptive SLAM algorithm based on strong tracking UKF[J]. Robot, 2010, 32(2): 190-195. (in Chinese)

[11] SAGE A P, HUSA G W. Adaptive filtering with unknown prior statistics[C].Tokyo: Proceedings of the Joint Automatic Control Conference, 1969, 760-769.

[12] 石勇, 韓崇昭. 自適應(yīng)UKF算法在目標(biāo)跟蹤中的應(yīng)用[J]. 自動(dòng)化學(xué)報(bào), 2011, 37(6): 755-759.

SHI Yong, HAN Chong-zhao. Adaptive UKF method with applications to target tracking[J]. Acta Automatica Sinica, 2011, 37(6): 755-759. (in Chinese)

[13] 孫堯, 張強(qiáng), 萬(wàn)磊. 基于自適應(yīng)UKF算法的小型水下機(jī)器人導(dǎo)航系統(tǒng)[J]. 自動(dòng)化學(xué)報(bào), 2011,37(3):342-353.

SUN Yao, ZHANG Qiang, WAN Lei. Small autonomous underwater vehicle navigation system based on adaptive UKF algorithm[J]. Acta Automatica Sinica, 2011, 37(3): 342-353. (in Chinese)

[14] 趙琳, 王小旭, 薛紅香, 等. 帶噪聲統(tǒng)計(jì)估計(jì)器的Unscented卡爾曼濾波器設(shè)計(jì)[J]. 控制與決策, 2009, 24(10): 1483-1488.

ZHAO Lin, WANG Xiao-xu, XUE Hong-xiang, et al. Design of unscented Kalman filter with noise statistic estimator[J]. Control and Decision, 2009, 24(10): 1483-1488. (in Chinese)