一種基于模糊推理的導(dǎo)航自適應(yīng)濾波算法

戴 卿，葉城睿，馬云峰,2

(1.重慶水利電力職業(yè)技術(shù)學(xué)院 建筑工程系，重慶 402160；2.重慶交通大學(xué) 土木工程學(xué)院，重慶 400074)

0 引言

GNSS/SINS組合導(dǎo)航系統(tǒng)因其良好的互補(bǔ)性、定位精度和可靠性，近幾年來在軍事與民用領(lǐng)域得到了廣泛的應(yīng)用[1]。拓展卡爾曼濾波(Extended Kalman Filter，EKF)是狀態(tài)最優(yōu)估計(jì)的有效算法，當(dāng)GNSS/SINS導(dǎo)航系統(tǒng)的數(shù)學(xué)模型準(zhǔn)確，且忽略計(jì)算誤差時(shí)，解算結(jié)果最優(yōu)，否則會(huì)降低精度，甚至引起發(fā)散問題[2]。由于動(dòng)態(tài)載體在觀測中會(huì)出現(xiàn)多徑現(xiàn)象，導(dǎo)致GNSS量測噪聲統(tǒng)計(jì)模型的不準(zhǔn)確和時(shí)變性，尤其在長時(shí)間持續(xù)機(jī)動(dòng)狀態(tài)工作中，這種影響更不容小覷。有學(xué)者進(jìn)行了大量反復(fù)的試驗(yàn)研究，但發(fā)現(xiàn)建立在實(shí)際工作時(shí)的精確量測噪聲模型仍很困難[3]。

為解決此類問題，有效的數(shù)據(jù)融合算法成為眾多學(xué)者的研究熱點(diǎn)。由此產(chǎn)生了一系列在濾波計(jì)算中通過量測新息在線修正噪聲隨機(jī)模型的自適應(yīng)濾波技術(shù)，有衰減記憶因子算法，平方根濾波、Husa-sage濾波等，在提高濾波精度、改善系統(tǒng)魯棒性等方面取得了一定成效[3-4]。 H∞濾波不限制系統(tǒng)先驗(yàn)信息，但是計(jì)算負(fù)擔(dān)較大。基于神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)的自適應(yīng)濾波算法，精度較高，但計(jì)算量較大，應(yīng)用對象受限[5]。近年來一系列將EKF與模糊推理系統(tǒng) (Fuzzy Inference System，F(xiàn)IS) 相結(jié)合的智能算法被眾多學(xué)者提出，如相關(guān)因子法、協(xié)方差法、衰減因子法和指數(shù)加權(quán)法等，并廣泛應(yīng)用于計(jì)算機(jī)、自動(dòng)化、電子技術(shù)等領(lǐng)域，但在導(dǎo)航解算上的研究則較為鮮見[6]。

本文受上述研究啟發(fā)，提出一種基于模糊理論的自適應(yīng)擴(kuò)展卡爾曼濾波融合算法方案，通過計(jì)算量測新息所得實(shí)際方差與理論方差之比，并利用模糊推理系統(tǒng)對GNSS量測噪聲統(tǒng)計(jì)模型實(shí)時(shí)在線估計(jì)和修正，有效解決了由于模型不準(zhǔn)確導(dǎo)致的濾波發(fā)散現(xiàn)象，適應(yīng)于量測噪聲先驗(yàn)特性未知的時(shí)變系統(tǒng)。通過試驗(yàn)測試表明，相比于傳統(tǒng)算法，新算法在時(shí)變量測噪聲情況下，精度明顯提高，計(jì)算負(fù)擔(dān)小，能有效克服載體機(jī)動(dòng)狀態(tài)下的估計(jì)問題，改善濾波估計(jì)效果的穩(wěn)定性和抗差性，易于工程實(shí)現(xiàn)。

1 常規(guī)組合導(dǎo)航EKF算法問題描述

對SINS/GNSS組合導(dǎo)航系統(tǒng)建模，得離散時(shí)間系統(tǒng)

Xk=Φk,k-1Xk-1+Wk
Zk=HkXk+Vk

(1)

式中，Xk為包含姿態(tài)、速度、位置、陀螺零偏和加速度偏置的15維狀態(tài)量，Zk為姿態(tài)、速度和位置的9維組合觀測量。Φk,k-1和Hk為狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣，Wk和Vk為過程噪聲和量測噪聲，具體設(shè)置見文獻(xiàn)[7]。

這里通常假設(shè)過程噪聲和量測噪聲為零均值的高斯白噪聲，其協(xié)方差陣為Q和R。然而，通過對組合導(dǎo)航系統(tǒng)的大量反復(fù)試驗(yàn)，發(fā)現(xiàn)量測噪聲先驗(yàn)特性隨環(huán)境變化，統(tǒng)計(jì)特征是未知的。當(dāng)量測噪聲模型不準(zhǔn)確時(shí)，會(huì)使EKF濾波性能下降，嚴(yán)重時(shí)引起發(fā)散。因此,為解決這個(gè)問題，需設(shè)計(jì)自適應(yīng)濾波技術(shù)。

2 基于模糊理論的自適應(yīng)EKF算法

2.1 自適應(yīng)EKF算法設(shè)計(jì)

依據(jù)文獻(xiàn)[8-10]的研究思路，設(shè)計(jì)自適應(yīng)EKF算法過程如下

Xk|k-1=Φk,k-1Xk-1

(2)

(3)

(4)

Xk=Xk|k-1+Kk(Zk-HkXk|k-1)

(5)

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

(6)

(7)

計(jì)算殘差方差理論值

(8)

計(jì)算殘差實(shí)測方差

(9)

rk=Zk-HkXk|k-1

(10)

式中，實(shí)測方差是對M個(gè)殘差向量方差的開窗均值，窗口大小由經(jīng)驗(yàn)獲得。則比值μk為

(11)

2.2 模糊規(guī)則設(shè)計(jì)

其采用的隸屬度函數(shù)如圖1和圖2所示。

圖1 FIS輸入μk的隸屬度函數(shù)

圖2 FIS輸出的隸屬度函數(shù)

2.3 模糊度自適應(yīng)EKF算法

綜上所述，本文設(shè)計(jì)的模糊自適應(yīng)EKF算法流程如圖3所示。首先，由式8～式11計(jì)算殘差方差的實(shí)際值和理論值的跡的比值；其次，對比值進(jìn)行模糊化處理，得到模糊子集上對應(yīng)的隸屬度函數(shù)；最后，當(dāng)量測噪聲不平穩(wěn)時(shí)，調(diào)整量測噪聲協(xié)方差矩陣使其逐漸逼近于真實(shí)噪聲水平。

圖3 基于模糊自適應(yīng)的EKF算法流程

3 實(shí)驗(yàn)結(jié)果與分析

3.1 仿真測試

方案1：拓展卡爾曼濾波

方案2：基于模糊自適應(yīng)的拓展卡爾曼濾波

圖4 北向定位誤差曲線

圖5 東向定位誤差曲線

算法北(m)東(m)天(m)總耗時(shí)(s)耗時(shí)/歷元(s)方案17.8636.5386.4586.650.011方案24.6424.3475.3288.520.014

由圖4和圖5可見，初始階段因隨機(jī)模型較為準(zhǔn)確，因此兩種濾波計(jì)算方案估計(jì)結(jié)果較為一致。在歷元[200,800]期間，由于量測噪聲發(fā)生兩次突變，初始噪聲協(xié)方差不能準(zhǔn)確描述GNSS噪聲數(shù)學(xué)統(tǒng)計(jì)特征，導(dǎo)致方案1的估計(jì)誤差明顯增大，而方案2是基于模糊邏輯的自適應(yīng)濾波算法，可以在線調(diào)整噪聲協(xié)方差，保證模型的準(zhǔn)確性，在偏差較大處，有明顯的糾偏作用，使得濾波結(jié)果仍能保持較高的精度，估計(jì)偏差在5 m以內(nèi)，滿足大多普通用戶的需求。

為進(jìn)一步證明算法的有效性，對歷元[200,800]期間的實(shí)驗(yàn)情景進(jìn)行了50次蒙特卡洛仿真實(shí)驗(yàn)，結(jié)果如表1所示。可見，方案2定位效果明顯優(yōu)于方案1，而耗時(shí)卻沒有明顯增加。說明本文通過模糊理論改進(jìn)的EKF算法能提高系統(tǒng)的抗差性，使得解算結(jié)果更為穩(wěn)定，適合工程應(yīng)用。

3.2 跑車試驗(yàn)

為進(jìn)一步驗(yàn)證本文所提算法的有效性，進(jìn)行了跑車實(shí)驗(yàn)。其中慣性裝置采用MEMS IMU420C，GNSS接收機(jī)采用OEM-G2板卡，進(jìn)行約10 min的數(shù)據(jù)測試，沿途經(jīng)過樹林、人工湖等可產(chǎn)生明顯的多徑效應(yīng)，采用節(jié)3.1中的兩種不同方案進(jìn)行處理，解算結(jié)果如圖6和圖7所示。

圖6 北向定位誤差曲線

圖7 東向定位誤差曲線

從跑車試驗(yàn)結(jié)果來看，總體上新算法明顯優(yōu)于傳統(tǒng)算法。圖6中傳統(tǒng)算法在北向位置誤差優(yōu)于10 m，最大誤差為16.13 m。經(jīng)過基于模糊邏輯改進(jìn)的自適應(yīng)拓展卡爾曼濾波處理后，北向誤差優(yōu)于5 m，最大誤差為8.53 m。說明在采取基于模糊推理的自適應(yīng)EKF方法之后，有效減輕了GNSS測量噪聲統(tǒng)計(jì)模型不確定對濾波精度的影響，總體精度有進(jìn)一步提升。將該算法用于時(shí)變性大，量測干擾不可知的導(dǎo)航中，可以發(fā)揮較好的導(dǎo)航定位性能。

4 結(jié)論

為改善GNSS/SINS組合導(dǎo)航系統(tǒng)在穩(wěn)態(tài)突變或多路徑影響嚴(yán)重的觀測環(huán)境中作業(yè)時(shí)，量測噪聲隨機(jī)模型不確定導(dǎo)致的濾波性能下降問題，本文研究了一種基于模糊推理系統(tǒng)的自適應(yīng)EKF數(shù)據(jù)融合算法。首先，借鑒模糊推理系統(tǒng)進(jìn)行在線估計(jì)；然后，通過系統(tǒng)殘差變化自適應(yīng)調(diào)整噪聲模型，完成時(shí)間噪聲協(xié)方差陣的實(shí)時(shí)估計(jì)與修正。該算法運(yùn)算簡單，確保了導(dǎo)航計(jì)算的實(shí)時(shí)性，且估計(jì)誤差收斂效果良好。最后，通過實(shí)驗(yàn)表明：將此算法用于GNSS/SINS組合導(dǎo)航數(shù)據(jù)融合中，在時(shí)變GNSS噪聲情況下，對系統(tǒng)模型的依賴小，能較好的跟蹤量測噪聲方差，有效提高導(dǎo)航計(jì)算的精度和可靠性，對工程實(shí)踐具有一定參考意義。

[1] 陳建峰.面向高動(dòng)態(tài)環(huán)境的SINS/GPS高精度組合測量中的關(guān)鍵問題研究[D].南京:東南大學(xué),2015:57-81.

[2] FARIZ O,FU L,LIN Y,et al.A new SINS/GPS sensor fusion scheme for UAV localization problem using nonlinear SVSF with covariance derivation and an adaptive boundary layer[J].Chinese Journal of Aeronautics,2016,29(2):424-440.

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

[4] 趙欣,王仕成,閆循良,等. 基于遺傳模糊控制的智能自適應(yīng)濾波算法[J].北京工業(yè)大學(xué)學(xué)報(bào),2012,38(12):1893-1900.

[5] LI X,ZHANG W G.Multi-sensor integrated navigation for intelligent vehicles based on H∞ filter[J].Journal of System Simulation,2007,19(18):4168-4177.

[6] 高運(yùn)廣,王仕成,劉志國,等.一種基于遺傳模糊推理的自適應(yīng)容錯(cuò)濾波算法[J].中國慣性技術(shù)學(xué)報(bào),2012,20(3):315-319.

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

[8] 曹夢龍,李振. 濾波增益模糊自適應(yīng)調(diào)節(jié)導(dǎo)航算法[J].青島科技大學(xué)學(xué)報(bào)(自然科學(xué)版),2017,38(5):101-106.

[9] 呂新知.FAST-SR-UKF算法及其在組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].計(jì)算機(jī)工程與應(yīng)用,2015,51(6):254-259.

[10] 萬振塬,楊功流,涂勇強(qiáng).一種新型模糊自適應(yīng)Kalman濾波器在組合導(dǎo)航中的應(yīng)用[J].導(dǎo)航與控制,2017,16(2):81-86.

[11] 楊春,郭健,張磊,等.采用卡方檢驗(yàn)的模糊自適應(yīng)無跡卡爾曼濾波組合導(dǎo)航算法[J].控制與決策,2018,33(1):81-87.