易清明 陸景龍 石 敏

暨南大學信息科學與技術(shù)學院, 廣州 510632

單一的導航系統(tǒng)有著本身無法克服的缺點，近年來，融合多個導航系統(tǒng)的組合導航系統(tǒng)越來越受到關(guān)注，成為研究的熱點領(lǐng)域[1-3]。全球定位系統(tǒng)(GPS)可以實時地提供全天候、高精度的位置和速度信息，但是在森林、多山地區(qū)、隧道和城市峽谷等場景則無法進行定位導航[2-4]；慣性導航系統(tǒng)(INS)具有自主導航的特點，但是誤差會隨著時間累積，無法保證導航系統(tǒng)的準確性[1,4-5]。GPS觀測值可以抑制INS系統(tǒng)的漂移，而INS系統(tǒng)則對GPS導航結(jié)果進行了平滑并補償信號中斷，GPS/INS組合導航系統(tǒng)結(jié)合2種技術(shù)優(yōu)勢，提供連續(xù)、高帶寬和高精度完整的導航信息[4-6]。卡爾曼濾波器在GPS/INS組合導航系統(tǒng)中得到廣泛應用，在系統(tǒng)模型確定的情況下，利用貝葉斯估計理論以及迭代理論可以得到最小均方誤差(MMSE)意義下的最優(yōu)濾波效果。然而，有時無法獲取精確的實際模型或受到多種因素的干擾導致觀測噪聲不確定，這將影響參數(shù)估計的精度和可靠性，甚至導致濾波結(jié)果發(fā)散[1,3,6]。解決系統(tǒng)模型不確定、噪聲干擾等問題的一個有效途徑是采用自適應濾波技術(shù)[1,7-10]。

針對以上問題，文獻[8]提出了基于貝葉斯方差估計的自適應算法,文獻[10-14]提出自適應漸消濾波算法,文獻[15]提出抗粗差自適應濾波。文獻[5，16-20]提出基于信息估計的自適應算法，但是當誤差矩陣以及測量矩陣均未知時，導致結(jié)果不確定。對此，文獻[1,7]提出多模型自適應算法，文獻[13-14]基于遺傳學自適應算法,可以進一步解決以上問題，但是計算量巨大，建立模型過程復雜。

漸消卡爾曼濾波器通過漸消因子減少過去噪聲與當前噪聲的相關(guān)性，對不確定的模型也能進行很好的濾波[6-9]。在噪聲干擾少、匹配性好的情況下，漸消因子的“漸消”速度慢，否則,“漸消”速度加快[1,4,20]。自適應卡爾曼濾波器通過自適應因子調(diào)整增益[5,10-14]。為了減少外界噪聲以及模型不確定性，需要同時調(diào)整漸消因子和自適應因子。由于是單一因子只能進行整體調(diào)整,無法精確調(diào)整各個通道，使調(diào)整結(jié)果不精確[1-5,16-18]。

本論文提出一種2步自適應濾波算法：1)構(gòu)造多重漸消因子減少對過去信息的記憶長度，同時保證誤差協(xié)方差的對稱性；2)利用多重自適應因子精確調(diào)整通道增益，降低噪聲干擾，克服系統(tǒng)模型不確定的影響。多重自適應因子和多重漸消因子可以限制濾波器的記憶長度，增加當前觀測數(shù)據(jù)權(quán)重，對各個通道進行精確調(diào)整，其中關(guān)鍵問題是多重因子的定義。

1 卡爾曼濾波器系統(tǒng)模型

卡爾曼濾波是導航系統(tǒng)中大多數(shù)狀態(tài)估計算法的基礎(chǔ)?？柭鼮V波器之所以能夠獲得最小均方誤差(MMSE)意義下的最優(yōu)濾波效果，關(guān)鍵是對實際系統(tǒng)建立合理、準確的模型。

卡爾曼濾波器在得到有效信息后，利用系統(tǒng)參數(shù)的確定特性和統(tǒng)計特性等先驗知識及觀測結(jié)果來獲得最優(yōu)估計。在提供初始值的基礎(chǔ)上，用先驗值和最新觀測數(shù)據(jù)中得到的新值的加權(quán)平均更新狀態(tài)估計[6]。首先，介紹系統(tǒng)狀態(tài)方程和觀測方程。

系統(tǒng)狀態(tài)方程表示為[7]：

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

(1)

觀測方程表示為:

Zk=HkXk+vk

(2)

式中，Xk∈Rn是被估計狀態(tài)向量；Φk,k-1∈Rn×n是k-1到k時刻的一步狀態(tài)轉(zhuǎn)移矩陣；Zk∈Rm是觀測向量；Hk∈Rm×n是觀測矩陣；wk和vk分別是系統(tǒng)噪聲和觀測噪聲，均為零均值白噪聲，其協(xié)方差分別是Qk和Rk；其中要求Xk，wk和vk三者互不相關(guān)。

接下來，介紹標準卡爾曼濾波器遞推方程[6]，式(3)～(7)不僅描述了卡爾曼濾波遞推公式，同時也描述濾波器的工作流程。

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

(3)

(4)

(5)

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

(6)

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

(7)

式中，Pk是Xk的協(xié)方差矩陣，在GPS/INS組合系統(tǒng)中則為誤差協(xié)方差矩陣，對角線元素為各個狀態(tài)的方差，非對角元素表示不同狀態(tài)估計誤差之間的相關(guān)性；I是單位矩陣；Kk是增益矩陣，它的大小體現(xiàn)了實際的觀測數(shù)據(jù)在估計中的作用。當已知初值X0和P0，再利用k時刻的觀測值Zk進行更新，就可以得到k時刻的系統(tǒng)狀態(tài)向量Xk[5-8]。

2 兩種單因子自適應濾波器

自適應卡爾曼濾波器跟蹤參數(shù)變化而進行相應的動態(tài)調(diào)整，被視為解決模型不確定的一種主要途徑。這里介紹2種主要的自適應卡爾曼濾波器。

2.1 漸消卡爾曼濾波

漸消卡爾曼濾波器與標準卡爾曼濾波器的不同在于引入漸消因子λk，增加新數(shù)據(jù)在迭代中的作用，減少舊數(shù)據(jù)的權(quán)重。其誤差協(xié)方差變成如下形式[5-9]：

(8)

基于殘差定義一步最佳漸消因子λk如下[8-10]：

γk=Zk-HkXk,k-1

(9)

(10)

(11)

(12)

漸消因子λk的作用是減少觀測值對遞推過程中系統(tǒng)建模不確定性和噪聲特性未知對濾波的影響。當殘差γk誤差增大時，引起殘差序列自協(xié)方差增大，漸消因子λk相應增大，抑制噪聲影響，以抑制濾波發(fā)散[7-10]。

2.2 自適應增益濾波器

在傳統(tǒng)卡爾曼濾波中存在2點不足：1)缺少冗余觀測信息時，導致卡爾曼濾波的穩(wěn)定性和效率下降，誤差增大；2)觀測衛(wèi)星數(shù)少于4顆時，無法用GPS系統(tǒng)糾正INS的精度。針對這2個問題，引入自適應因子。

引入自適應因子αk(0<αk≤1)后的增益方程，變成如下形式：

(13)

自適應因子αk有2種確定方法：基于預測狀態(tài)和基于殘差?；跉埐畹姆椒ǜm合在GPS/INS松耦合系統(tǒng)中使用，因此，文中采用基于殘差的方法確定自適應因子αk：

(14)

(15)

(16)

3 兩步自適應濾波算法

通過上節(jié)分析可知，單一漸消因子和自適應因子可以降低噪聲的干擾，但是只能進行整體調(diào)整，不能精確濾波[15-17]，這里提出一種改進的自適應濾波算法，即兩步自適應濾波算法。在GPS/INS組合系統(tǒng)中，狀態(tài)變量Xk是一組多維向量矩陣，含有多個狀態(tài)變量，當存在外界噪聲干擾時，需要一個因子矩陣進行逐個精確調(diào)整，即λk和αk都應該是一個因子矩陣，但按上述方法，λk和αk實際上是一個數(shù)值，只對整個協(xié)方差矩陣Pk和增益矩陣Kk進行整體調(diào)整，并沒有對各個通道進行調(diào)整，導致調(diào)整效果不佳。為此，論文引入多重因子，它可以精確調(diào)整各個通道，而不是平均調(diào)整。其中關(guān)鍵是多重漸消因子和自適應因子的定義。

兩步自適應濾波算法的基本思想是:第1步利用多重漸消因子抑制噪聲，減少記憶長度，減少噪聲相關(guān)性，減少模型偏差；第2步利用多重增益因子調(diào)整卡爾曼增益矩陣，優(yōu)化濾波結(jié)果。

3.1 多重漸消因子定義

把狀態(tài)預測協(xié)方差矩陣Pk,k-1寫成如下形式：

(17)

把式(17)帶入式(15)中，得到：

(18)

以Λ為優(yōu)化變量，采用Frobenius范數(shù)的平方表示代價函數(shù)，如下：

(19)

(20)

其中，

(21)

式(21)對Λk進行微分，考慮到β為反對稱矩陣，有：

(22)

求取代價函數(shù)J(Λ)的最小值，則有：

(23)

由于多重漸消因子Λk為對角矩陣，聯(lián)合式(21)和式(23)，得到：

(24)

由于僅對誤差協(xié)方差矩陣的對角元素進行加權(quán)，其仍然滿足MMSE準則，因此仍然有MMSE條件下的最優(yōu)解。不論是單一漸消因子還是多重漸消因子都是依據(jù)殘差定義[16-17]，單一因子僅能對整體進行平均調(diào)整，而多重自適應因子可以精確調(diào)整各個通道的權(quán)重，可以獲得精確的濾波效果。

3.2 多重自適應因子定義

引入多重自適應因子后，式(13)變?yōu)槿缦滦问剑?/p>

(25)

在k時刻的多重自適應因子αk為對角矩陣，表示為：

(26)

推廣式(14)～(16)的自適應因子的定義，可以得到多重自適應因子的定義：

(27)

(28)

diag(·)是對角矩陣，對于矩陣而言，diag(·)即把矩陣主對角元素構(gòu)成對角矩陣；di為Dk對角矩陣上的第i個元素。

基于殘差定義的多重自適應因子可以直接對增益矩陣進行多通道、精確調(diào)整，降低噪聲的干擾，增加真實觀測對濾波器的調(diào)整作用，避免使用多個平行卡爾曼濾波器。

依據(jù)殘差進行定義的多重自適應因子，可以滿足調(diào)整各通道增益的需求。當存在粗差時，第1步多重漸消因子可以減少粗差干擾，降低觀測異常的影響，修正模型；第2步多重自適應因子調(diào)整卡爾曼濾波增益，降低噪聲影響，濾波效果得到提高[14-15]。

4 實驗及分析

為了驗證算法，GPS/INS組合導航系統(tǒng)采用松耦合方式。松耦合方式采用速度、位置的組合形式，以GPS的位置和速度作為參考值，以INS和GPS系統(tǒng)的位置和速度差值作為觀測值，由于結(jié)構(gòu)簡單，工程易于實現(xiàn)，因此獲得廣泛使用。

4.1 實驗環(huán)境介紹

組合導航系統(tǒng)安裝在一臺測試車輛上,如圖1所示，圖1中的圖a為搭載組合系統(tǒng)車輛的示意圖，b為組合系統(tǒng)平臺，固定在車上的實驗臺上，c為GPS接收機，它的天線安裝在車頂部(保證能搜到4顆星)，d表示INS固定在實驗臺底部，與車輛相對靜止，Y軸指向車前進方向，Z軸正方向指向天向，根據(jù)右手法則確定Y軸，如圖1(a)所示。

圖2 部分車輛測試路線

測試車輛在廣州天河區(qū)進行道路測試，以北緯22.7°，東經(jīng)113.6°為起點，行駛1h，回到起點，部分測試路線如圖2所示。由于數(shù)據(jù)量巨大，為了減少數(shù)據(jù)量，提高處理速度，截取60s的一段數(shù)據(jù)，包括直線和90°轉(zhuǎn)彎。獲取的數(shù)據(jù)在MATLAB 12.0環(huán)境下進行仿真驗證。平臺硬件采用加拿大NovAtel公司基于OEM6的LCI系列產(chǎn)品，它采用SPAN技術(shù)，可以獲得連續(xù)的速度、位置、姿態(tài)信息，滿足高精度、高穩(wěn)定性的需求，表1給出INS的主要技術(shù)參數(shù)。

表1 INS的主要技術(shù)參數(shù)

4.2 實驗結(jié)果及分析

這里給出標準卡爾曼濾波器與3種自適應卡爾曼濾波器的對比：漸消卡爾曼濾波器，常規(guī)自適應卡爾曼濾波器以及改進后的自適應卡爾曼濾波器。

值得注意的是，論文中采用松耦合的組合導航直接獲取的觀測數(shù)據(jù)為位置和速度，濾波效果對位置、速度反映靈敏，在姿態(tài)上則不明顯。由于論文篇幅受限，這里以北、東及地速度誤差結(jié)果對比進行說明。

圖3～5分別為4種算法在北、東、地方向上的速度誤差對比。實驗模擬路線既有直線路段，也有90°轉(zhuǎn)彎，覆蓋實際路況。在開始的2s內(nèi)，由于濾波器還無法進行跟蹤，因此出現(xiàn)從負值大幅度跳動到正值的情況，之后則正常跟蹤。

從圖3～4的仿真結(jié)果可以看出，原始卡爾曼濾波出現(xiàn)1個較大上下波動，1個較低的波谷以及2個較高的波峰，漸消卡爾曼濾波器和自適應濾波器相對原始卡爾曼濾波的波動小，在車輛速度變化時，波動較小，在波峰波谷部分有明顯改進，說明這2種算法可以較好地抑制噪聲干擾。改進的自適應算法的跟蹤性最好，誤差波動更小，尤其在圖4的東向速度分量中可以看出。改進的自適應算法分2步進行濾波，并且以多重因子代替單因子，可以較好地抑制噪聲，實現(xiàn)動態(tài)增益，濾波效果得到較好的改進，速度誤差波動較小，跟蹤性能較強，收斂趨勢更加迅速，穩(wěn)定性得到提高。圖4是在地向速度誤差對比，3種方法都對原始濾波有優(yōu)化，但是由于沒有高度計輔助觀測，原始數(shù)據(jù)誤差較大，優(yōu)化效果差別不是很明顯。當路線長度增加，相應數(shù)據(jù)增加或存在粗差時，多重因子可以控制各個通道進行精確濾波，提高濾波器的穩(wěn)定性和可靠性。

圖3 北向誤差對比

圖4 東向誤差對比

圖5 地向誤差對比

5 結(jié)論

單因子的漸消卡爾曼濾波器和自適應卡爾曼濾波器可以部分解決噪聲模型抖動和偏差問題，但只能進行整體、平均調(diào)整，不能精確調(diào)整各個通道。本論文提出的2步自適應卡爾曼濾波算法,構(gòu)造基于殘差協(xié)方差估計的多重漸消因子和自適應因子，精確調(diào)整誤差協(xié)方差矩陣和增益矩陣的各個通道，克服動態(tài)環(huán)境下跟蹤性差的局限性。實驗和仿真結(jié)果表明，改進后的自適應卡爾曼濾波算法可以精確調(diào)整各通道，增強系統(tǒng)的定位精度和魯棒性、多變量跟蹤能力，對算法改進以及實際應用有一定指導意義。

參 考 文 獻

[1] Gao Weixi, Miao Lingjuan, Ni Maolin. Multiple Fading Factors Kalman Filter for SINS Static Alignment Application[J]. Chinese Journal of Aeronautics, 2011,4: 476-483.

[2] Mohammad A K J, Mamoun F A H. Enhanced, Delay Dependent, Intellignen Fusion for INS/GPS Navigation Systemt[J]. IEEE Sensors Journal, 2014, 14(5): 1545-1554.

[3] 周衛(wèi)東,喬相偉,吉宇人,等.基于殘差和殘差的自適應UKF算法[J].宇航學報,2010,31(7): 1798-1804.(Zhou Weidong, Qiao Xiangwei , Ji Yuren, et al. An Innovation and Residual-Based Adaptive UKF Algorithm[J]. Journal of Astronautics, 2010,31(7): 1798-1804).

[4] Zhou Yang, Wu Panlong, Li Xingxiu, et al. A New Adaptive Extended Kalman Filter for Cooperative Localization[J]. IEEE Transactions on Aerospace and Electronic Systems, 2017：1-14.

[5] Liu Yahui, Fan Xiaoqian, Lv Chen, Wu Jian, et al. An Innovative Information Fusion Method with Adaptive Kalman Filter for Integrated INS/GPS Navigation of Autonomous Vehicles[J]. Mechanical Systems and Signal Processing,2017,100:605-616.

[6] 楊元喜,任夏,許艷等.自適應抗差濾波理論及應用的主要進展[J].導航定位學報,2013,(1):9-15. (Yang Yuanxi, Ren Xia, Xu Yan, et al. Main Progress of Adaptively Robust Filter with Applications in Navigation[J]. Journal of Navigation and Positioning, 2013, (1): 9-15.)

[7] Gao Bingbing, Gao Shesheng, Zhong Yongmin, et al. Interacting Multiple Model Estimation-based Adaptive Robust Unscented Kalman Filter[J]. International Journal of Control, Automation and Systems, 2017, 15(5):2013-2025.

[8] Wang Shiyuan, Chao Yin, Duan Shukai, et al. A Modified Variational Bayesian Noise Adaptive Kalman Filter[J]. 2017,36(10): 4260-4277.

[9] Chen Jie, Song Jian, Li Liang, et al. UKF-based Adaptive Variable Structure Observer for Vehicle Sideslip with Dynamic Correction[J]. IET Control Theory & Applications, 2016, 10(14): 1641 - 1652.

[10] Roozbeh D, Mohammad S E, Edward R D. Intrinsically Bayesian Robust Kalman Filter: An Innovation Process Approach[J]. IEEE Transactions on Signal Processing, 2017, 65(10): 2531 - 2546.

[11] Bao J L, Gu Y L, Hsu L T, et al. Vehicle Self-localization using 3D Building Map and Stereo Camera[J]. Intelligent Vehicles Symposium (IV), 2016, 927 - 932.

[12] Kaiser S. A., Christianson A. J., Narayanan R. M.. Global Positioning System Processing Methods for GPS Passive Coherent Location [J]. IET Radar, Sonar & Navigation 2017, 11(9): 1406 - 1416.

[13] 譚興龍,王堅,趙長勝.神經(jīng)網(wǎng)絡(luò)輔助的GPS/INS組合導航自適應UKF算法[J].測繪學報, 2015,44(4):384-392.(Tan Xinglong, Wang Jian, Zhao Changsheng. Neural Network Aided Adaptive UKF Algorithm for GPS/INS Integration Navigation[J]. Acta Geodaetica et Cartographica Sinica, 2015,44(4):384-392.)

[14] Miao Zhiyong, Shi Hongyang, Zhang Yi, et al.Neural Network-aided Variational Bayesian Adaptive Cubature Kalman Filtering for Nonlinear State Estimation[J].

[15] Huang Fuyi, Zhang Jiashu, Zhang Sheng. Maximum Versoria Criterion-Based Robust Adaptive Filtering Algorithm[J]. IEEE Transactions on Circuits and Systems II: Express Briefs, 2017, 64(10): 1252 - 1256.

[16] 馬龍,李曉明.基于GPS/INS組合導航的改進自適應漸消卡爾曼濾波算法[J]. 科學技術(shù)與工程, 2013, 13(33):9973-9977. (Ma Long, Li Xiaoming. Improved Algorithm of Adaptive Fading Kalman Based on GPS/INS Integrated Navigation[J].Science Technology and Engineering, 2013,13(33): 9973-9977.)

[17] 薛海建,郭曉松,周召發(fā).基于自適應多重漸消因子卡爾曼濾波的SINS初始對準方法[J].系統(tǒng)工程與電子技術(shù), 2017,(3):620-626. ( Xue Haijian, Guo Xiaosong, Zhou Zhaofa. SINS Initial Alignment Method Based on Adaptive Multiple Fading Factors Kalman Filter[J]. Systems Engineering and Electronics Technology, 2017, 39(3):620-626.)

[18] Deng Fang, Chen Jie, Chen Chen. Adaptive Unscented Kalman Filter for Parameter and State Estimation of Nonlinear High-speed Objects[J]. Journal of Systems Engineering and Electronics,2013, 24(4): 655-665.

[19] 胡高歌,高社生,趙巖.一種新的自適應UKF算法及其在組合導航中的應用[J].中國慣性技術(shù)學報,2014,22(3):357-362. (Hu Gaoge, Gao Shesheng, Zhao Yan. Novel Adaptive UKF and Its Application in Integrated Navigation[J]. Journal of Chinese Inertial Technology, 2014,22(3):357-362.)

[20] Yu M. J..INS/GPS Integration System Using Adaptive Filter for Estimating Measurement Noise Variance[J]. IEEE Transactions on Aerospace and Electronic Systems, 2012,48(2):1786-1792.