胡輝,顏瑜軍,歐敏輝,彭雄明

(1.華東交通大學(xué) 信息工程學(xué)院,江西 南昌 330013；2.武漢夢芯科技有限公司,湖北 武漢 430073)

?

基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤卡爾曼濾波在GPS定位中的應(yīng)用

胡輝1,顏瑜軍1,歐敏輝1,彭雄明2

(1.華東交通大學(xué) 信息工程學(xué)院,江西 南昌 330013；2.武漢夢芯科技有限公司,湖北 武漢 430073)

當(dāng)載體速度頻繁發(fā)生突變時,針對卡爾曼濾波在GPS定位解算中對系統(tǒng)模型依賴性強(qiáng)、魯棒性差的問題,提出了一種基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤卡爾曼濾波算法。該算法主要利用模糊邏輯自適應(yīng)控制器監(jiān)測系統(tǒng)輸出的殘差均值,并根據(jù)模糊規(guī)則動態(tài)調(diào)整弱化因子,從而對強(qiáng)跟蹤濾波器中次優(yōu)漸消因子進(jìn)行自適應(yīng)調(diào)整,來實現(xiàn)抑制載體突變影響。仿真結(jié)果表明:該算法在載體突變的跟蹤精度高于擴(kuò)展卡爾曼濾波和強(qiáng)跟蹤卡爾曼濾,其穩(wěn)定性也有所提高。

GPS;模糊邏輯自適應(yīng)控制器;強(qiáng)跟蹤濾波器;卡爾曼濾波器

0 引 言

標(biāo)準(zhǔn)的卡爾曼濾波器(KF) 是在已知系統(tǒng)模型和量測模型以及系統(tǒng)噪聲和量測噪聲的統(tǒng)計特性下才會獲得理想的濾波效果。但是,當(dāng)系統(tǒng)模型參數(shù)與實際過程參數(shù)存在較大差異時,KF的估計精度會大大下降,甚至發(fā)散,且KF在系統(tǒng)達(dá)到平穩(wěn)狀態(tài)時,將喪失對突變狀態(tài)的跟蹤能力,擴(kuò)展卡爾曼濾波器(EKF) 也存在類似的缺點(diǎn)[1]。為此,文獻(xiàn)[2]～[5]的研究者對EKF提出了引入漸消因子進(jìn)行自適應(yīng)處理的方法,采用漸消因子來抑制濾波器的記憶長度,以便充分利用現(xiàn)時的觀測數(shù)據(jù),減小陳舊量測值的影響,這類處理方法處理的基準(zhǔn)是認(rèn)為系統(tǒng)狀態(tài)協(xié)方差過小,通過在系統(tǒng)狀態(tài)協(xié)方差上乘以漸消因子來實現(xiàn)穩(wěn)定跟蹤,但當(dāng)系統(tǒng)模型存在較大誤差或狀態(tài)突變較大的情況下,其漸消因子調(diào)節(jié)的效果難以區(qū)分且漸消因子的計算復(fù)雜,因此如何選取合適的漸消因子是該方法面臨的一個難題。而文獻(xiàn)[6]～[9]的研究者在引入漸消因子方法的基礎(chǔ)上提出了基于模糊控制的自適應(yīng)濾波方法,利用模糊邏輯自適應(yīng)控制器(FLAC)來在線調(diào)整卡爾曼濾波器,對漸消因子進(jìn)行自適應(yīng)模糊調(diào)節(jié),從而防止濾波器發(fā)散,目前這類算法主要在組合導(dǎo)航系統(tǒng)中數(shù)據(jù)融合比較成功。本文針對卡爾曼濾波在GPS定位解算中對系統(tǒng)模型依賴性強(qiáng)、魯棒性差的問題,提出了一種基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤卡爾曼濾波(FASTKF) 算法,該算法采用一種FLAC來動態(tài)調(diào)整該弱化因子,從而對次優(yōu)漸消因子進(jìn)行“在線”自適應(yīng)調(diào)整,進(jìn)一步提高濾波器跟蹤精度,仿真結(jié)果表明，F(xiàn)ASTKF具有應(yīng)對突變狀態(tài)的強(qiáng)跟蹤能力,且估計精度高等特點(diǎn)。

1 GPS定位解算模型

將EKF應(yīng)用于GPS定位解算中,首要是建立狀態(tài)模型和觀測模型。定義Xk為系統(tǒng)狀態(tài)向量,Zk為系統(tǒng)觀測向量。

(1)

Zk=[ρi,k,di,k],

(2)

1.1 狀態(tài)方程建立

在位置速度(PV)模型中,狀態(tài)量包括三個正交方向上的位置和速度、鐘差、鐘漂,其中每個方向上的位置和速度所含噪聲是相互獨(dú)立的,相關(guān)系數(shù)為零。系統(tǒng)的離散狀態(tài)轉(zhuǎn)移方程:

Xk=ΦXk-1+Wk-1，

(3)

式中：Φ為離散時間域的狀態(tài)轉(zhuǎn)移系數(shù)矩陣；Wk-1為系統(tǒng)噪聲，由連續(xù)時間域F(t)推導(dǎo)獲得[10]:

(4)

(5)

式中:

1.2 觀測方程建立

系統(tǒng)的觀測方程,偽矩和系統(tǒng)狀態(tài)參數(shù)的關(guān)系為

ρi,k=|rr,k-ri,k|+cδtk+εi,k

=[(xr,k-xi,k)2+(yr,k-yi,k)2+

(zr,k-zi,k)2]1/2+cδtk+εik,

(6)

式中:ρi,k為第i顆衛(wèi)星k時刻的偽矩；rr,k為接收機(jī)k時刻的位置；ri,k為第i顆衛(wèi)星k時刻衛(wèi)星的位置；εi,k為第i顆衛(wèi)星k時刻總的測量誤差,εi,k用白噪聲進(jìn)行建模。其偽矩方程中有四個未知的變量,接收機(jī)的位置(xr,k,yr,k,zr,k)以及接收機(jī)時鐘偏差δtk； 其k時刻第i顆衛(wèi)星的位置為(xi,k,yi,k,zi,k),其由導(dǎo)航電文提取后求得。

多普勒頻率測量值的計算:

di,k= [(xr,k-xi,k)(vx,k-vxi,k)+(yr,k-

yi,k)(vy,k-vyi,k)+(zr,k-zi,k)

(vz,k-vzi,k)]/[(xr,k-xi,k)2+

(7)

式中:vx,k,vy,k,vz,k分別為接收機(jī)在X、Y、Z方向的速度；vxi,k,vyi,k,vzi,k分別為第i顆衛(wèi)星X、Y、Z方向的速度。

對觀測方程式(6)和式(7),可以寫成矩陣的形式

Zk=HkXk+Vk,

(8)

式中:Xk為k時刻點(diǎn)的狀態(tài)向量；Vk為觀測噪聲矩陣；Zk為k時刻的觀測值；Hk為觀測方程矩陣。式(3)中狀態(tài)噪聲Wk和式(8)中量測噪聲Vk的期望和協(xié)方差矩陣滿足:

式中:E為數(shù)學(xué)期望符號；Qk為n×n維的半正定矩陣；Rk為m×m維正定對稱陣。

式(8)中觀測方程矩陣Hk滿足:

(9)

式中:

hxi=(xi,k-xr,k)/r；hyi=(yi,k-yr,k)/r；

hzi=(zi,k-zr,k)/r;

r= [(xr,k-xi,k)2+(yr,k-yi,k)2+(zr,k-

zi,k)2]1/2.

2 強(qiáng)跟蹤卡爾曼濾波器

上述已建立GPS定位系統(tǒng)的狀態(tài)方程和觀測方程。現(xiàn)將建立的模型應(yīng)用于強(qiáng)跟蹤卡爾曼濾波器(STKF),STKF與標(biāo)準(zhǔn)的KF相比具有以下特性[11]:① 較強(qiáng)的關(guān)于模型參數(shù)失配的魯棒性； ② 較低的關(guān)于噪聲及初值統(tǒng)計特性的敏感性； ③極強(qiáng)的關(guān)于突變狀態(tài)的跟蹤能力,并在濾波器達(dá)到穩(wěn)態(tài)時保持這種能力； ④適中的計算復(fù)雜性。

一類STKF應(yīng)具有如下一般結(jié)構(gòu):

(10)

(11)

(12)

Pk|k-1=λi,kΦPk-1ΦT+Qk-1,

(13)

Pk=(I-KkHk)Pk|k-1,

(14)

(15)

式中:Zk為濾波器的量測值；γk為殘差序列；λk=diag[λ1,k,λ2,k,λ3,k…λ8,k]為漸消因子矩陣,分別對應(yīng) 8個狀態(tài)變量的漸消因子,考慮到每個狀態(tài)變量的突變情況不同,當(dāng)由系統(tǒng)的先驗知識可大致確定λ1,k∶λ2,k∶λ3,k∶…∶λ8,k的初值為α=α1:α2:α3:…:α8,其中αi≥1 (i=1,2,…,8),迭代過程中, 令λi,k=αick, 其中ck為待定因子。則得到λi,k的一步近似算法:

(16)

ck=tr(Nk) r(αMk),

(17)

(18)

(19)

(20)

其中,β≥1為弱化因子,0<ρ≤1為遺忘因子。從上式可以看出漸消因子的物理意義,狀態(tài)突變時,估計誤差γk的增大引起誤差方差陣Vk增大,λi,k漸消因子相應(yīng)增大,濾波器的跟蹤能力增強(qiáng),使濾波器動態(tài)性能得到改善。

3 模糊自適應(yīng)強(qiáng)跟蹤卡爾曼濾波器

由上述分析可以看出, 現(xiàn)有強(qiáng)跟蹤濾波器選取的都是固定弱化因子, 該固定弱化因子需依靠經(jīng)驗或計算機(jī)仿真來選擇, 無法建立精確的數(shù)學(xué)模型。針對此問題,本文通過模糊邏輯自適應(yīng)控制器與強(qiáng)跟蹤卡爾曼濾波器的相結(jié)合, 提出了模糊自適應(yīng)強(qiáng)跟蹤卡爾曼濾波算法,該框圖如圖1所示。

由圖1可以看出,FASTKF中的狀態(tài)估計算法與STKF相同,其不同之處是通過FLAC實現(xiàn)了弱化因子β及次優(yōu)漸消因子λi,k的“在線”自適應(yīng)調(diào)整。FLAC的輸入為系統(tǒng)的統(tǒng)計特征量,對特征量的隸屬函數(shù)進(jìn)行模糊化,然后根據(jù)模糊推理規(guī)則, 實現(xiàn)對β的自適應(yīng)調(diào)節(jié), 從而提高了濾波器估計精度。FLAC具體設(shè)計如下：

為監(jiān)測數(shù)據(jù)的突變情況,引入殘差的均值統(tǒng)計特征量,殘差的表達(dá)式參照式(16),γk=[υ1υ2υ3…υm]T，m為可見衛(wèi)星的顆數(shù),則殘差的均值滿足以下式子:

(21)

根據(jù)先驗知識庫得到的3條模糊推理規(guī)則,其模糊規(guī)則描述為

①IFζiszero,Thenβ=100ζ+36.

②IFζissmall,Thenβ=491.

③IFζislarge,

Thenβ=45.7 ζ2-513.9 ζ+1893.7.

FLAC輸出β反模糊化方法采用重心法, 計算公式為

(22)

式中:μ為計算出的精確值；μ(ui)為隸屬函數(shù);ui為模糊集合的元素。

4 實驗結(jié)果與分析

為了驗證載體頻繁突變場景下算法的估計性能,本文基于VC++6. 0 軟件平臺上, 通過GPS軟件模擬器輸出的數(shù)字中頻信號作為信號源,設(shè)置的載體運(yùn)動狀態(tài)如圖4所示,仿真時長為105s,在0～30s載體靜止?fàn)顟B(tài),30～50s載體合速度突變幅值較小,50～85s載體合速度突變幅值較大,85～105s載體勻速狀態(tài),分別用FASTKF、STKF與EKF三種方法進(jìn)行仿真實驗。FASTKF設(shè)置載體的初始位置、速度、鐘差、鐘漂為X0=[-2583646.831,4475006.977,3729132.222,…,0,0,0,0,0],P0=diag[106,106,106,106,106,106,106,106]觀測量的偽矩、多普勒速度協(xié)方差分別Rρ=0.1、Rd=0.04,采樣時間間隔T=0.1 s,鐘差和鐘漂的噪聲功率譜密度取值分別為σb=0.4×10-18和σd=1.58×10-18,位置和速度的噪聲功率譜密度分別為σp=10-6和σv=10-4, 初始漸消因子λi,0取值α1:α2:α3:…:α8=1:1:1:…:1,遺忘因子ρ=0.95,EKF和STKF設(shè)置的參數(shù)和FASTKF相同,其中STKF固定弱化因子β=491。

仿真結(jié)果如圖5所示,圖5(a)、圖5(b)和圖5(c)分別對應(yīng)EKF、STKF 與FASTKF方法在X、Y、Z方向上的定位結(jié)果誤差,在 0～13.5 s時間段內(nèi),接收機(jī)進(jìn)行捕獲跟蹤階段,因此無定位結(jié)果。從圖中可以看出,在13.5～30 s載體靜止?fàn)顟B(tài),三種方法的效果近似相同,在85～105 s載體勻速狀態(tài),STKF 與FASTKF的收斂速度強(qiáng)于EKF。同時,可以看出在30～85 s時間段載體速度突變的情況下,STKF 與FASTKF應(yīng)對載體突變的跟蹤能力和收斂速度明顯強(qiáng)于EKF,而在30～50 s中載體合速度突變幅值較小時刻,STKF 與FASTKF的濾波效果近似一樣,但在50～85 s中載體合速度突變幅值較大時刻,FASTKF的跟蹤性能和收斂速度強(qiáng)于STKF。同時從表1中可以看出,FASTKF在X方向的均方差為1.011 1,比STEKF的1.557 6和EKF的6.310 6,分別提高了84.0%和35.1%；Y方向分別提高了88.0%和44.7%；Z方向分別提高了87.9%和41.8%,總體的估計性能FASTKF明顯好于STKF和EKF.

表1 EKF、STKF 與FASTKF均方差比較

5 結(jié)束語

在GPS定位解算中,提出了一種模糊自適應(yīng)強(qiáng)跟蹤卡爾曼濾波方法應(yīng)用于載體狀態(tài)突變,該方法將STKF中弱化因子選取與模糊自適應(yīng)控制器相結(jié)合起來,實現(xiàn)對STKF中次優(yōu)漸消因子進(jìn)行自適應(yīng)調(diào)整,進(jìn)而實現(xiàn)載體突變的抑制。根據(jù)仿真實驗結(jié)果表明:在沒有突變的情況下,FASTKF跟蹤效果和STKF、EKF一致； 在載體突變大時,FASTKF比STKF、EKF具有更強(qiáng)的跟蹤能力,其穩(wěn)定性和估計精度都有所提高。因此本文提出的FASTKF針對數(shù)據(jù)出現(xiàn)突變的情況,能為衛(wèi)星導(dǎo)航定位中實時快速穩(wěn)定收斂提供一定參考。

[1] 周東華,葉銀忠. 現(xiàn)代故障診斷與容錯控制[M]. 北京:清華大學(xué)出版社,2000.

[2] 夏啟軍,孫優(yōu)賢,周春暉.漸消卡爾曼濾波器的最佳自適應(yīng)算法及其應(yīng)用[J].自動化學(xué)報,1990,16(3):210-216.

[3] 徐定杰,賀瑞,沈鋒,等.基于新息協(xié)方差的自適應(yīng)漸消卡爾曼濾波器[J].系統(tǒng)工程與電子術(shù),2011,33(12):2696-2699.

[4] 楊柳慶,肖前貴,牛妍,等.基于漸消卡爾曼濾波器的定位系統(tǒng)設(shè)計[J].南京航空航天大學(xué)學(xué)報,2012,44(1):134-138.

[5] GENG Y, WANG J. Adaptive estimation of multiple fading factors in Kalman filter for navigation applications[J]. GPS Solutions, 2008, 12(4): 273-279.

[6] SASIADEK J Z, HARTANA P. Sensor data fusion using Kalman filter[C]//Proceedings of the Third International Conference onInformation Fusion. 2000: 19-25.

[7] 馬忠孝,劉宗玉,陳明. 基于模糊邏輯的自適應(yīng)卡爾曼濾波在GPS/INS組合導(dǎo)航中的應(yīng)用[J].信息與控制,2006,35(4):457-461.

[8] 張濤,徐曉蘇.基于模糊自適應(yīng)的組合導(dǎo)航系統(tǒng)信息融合算法[J].中國慣性技術(shù)學(xué)報,2007,15(2):197-201.

[9] 肖志濤,趙培培,李士心.基于INS/GPS組合導(dǎo)航的自適應(yīng)模糊卡爾曼濾波[J].中國慣性技術(shù)學(xué)報,2010,2(2):195-198.

[10] 魯郁.GPS全球定位接收機(jī)——原理與軟件實現(xiàn)[M].北京:電子工業(yè)出版社,2009.

[11] 周東華,席裕庚,張忠俊.一種帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器[J].自動化學(xué)報,1991,17(6): 689-695.

Application of Adaptive Strong Tracking Kalman Filter Based on Fuzzy Logic to GPS Positioning

HU Hui1,YAN Yujun1,OU Minhui1,PENG Xiongming2

(1.SchoolofInformationEngineering,EastChinaJiaotongUniversity,Nanchang330013,China; 2.WuhanMengxinTechnologyLimitedCompany,Wuhan430073,China)

When the velocity of carrier frequently was mutated, Kalman filter have strong dependence of system model and poor robustness in GPS positioning computation. A new improved strong tracking Kalman filter based on fuzzy logic adaptive controller is presented. By monitoring the residual mean, the fuzzy logic adaptive controller of this method dynamically adjusts the softening factor according to fuzzy rules. For this reason, this method modifies the fading adaptively and achieve the inhibition of carrier mutation. The results of simulation indicate that this new approach has a better accuracy than the extended Kalman filter and strong tracking Kalman filter in the field of maneuvering target tracking in carrier mutation, its stability are also improved.

GPS； fuzzy logic adaptive controller； strong tracking filter； Kalman filter

10.13442/j.gnss.1008-9268.2017.01.002

2016-10-10

江西省自然科學(xué)基金(批準(zhǔn)號:20142BAB207001)； 江西省教育廳科學(xué)技術(shù)研究項目(編號:GJJ14369)

P228.4

A

1008-9268(2017)01-0006-06

胡輝 (1970-),男,江西南昌人,博士,教授,主要研究方向為衛(wèi)星導(dǎo)航定位,并行算法與并行處理,機(jī)器視覺。

顏瑜軍 (1990-),男,湖南衡陽人,碩士,主要研究方向為衛(wèi)星導(dǎo)航定位。

歐敏輝 (1990-),男,湖南株洲人,碩士,主要研究方向為衛(wèi)星導(dǎo)航定位。

彭雄明 (1989-),男,湖北孝感人,碩士,主要研究方向為衛(wèi)星導(dǎo)航定位。

聯(lián)系人： 顏瑜軍 E-mail: gnss523@163.com