周先林，張慧君，和濤,4，李孝輝

GPS/INS松耦合組合導航的自適應卡爾曼濾波算法研究

周先林1,2,3，張慧君1,2,3，和濤1,2,3,4，李孝輝1,2,3

2. 中國科學院 國家授時中心，西安 710600；中國科學院 精密導航定位與定時技術重點實驗室，西安 710600；3. 中國科學院大學，北京 100049；4.中國船舶集團有限公司 昆明船舶設備研究試驗中心，昆明 650051）

針對常規(guī)卡爾曼濾波應用在GPS/INS組合導航時，由于量測數(shù)據(jù)出現(xiàn)異常值或系統(tǒng)狀態(tài)模型不準確而造成的濾波精度下降問題，提出了一種基于新息的自適應卡爾曼濾波算法（AKF）。該算法首先通過卡方檢驗檢測出量測異常值，在量測異常值處調(diào)整量測噪聲方差陣來抑制濾波發(fā)散；在此基礎上根據(jù)新息協(xié)方差的計算值與新息協(xié)方差的預測值的粗略比率，調(diào)整系統(tǒng)噪聲方差陣，從而提高整體濾波精度。通過跑車試驗，對本文提出的AKF算法進行了驗證。試驗結果表明：本文提出的AKF算法較常規(guī)卡爾曼濾波算法在經(jīng)度、緯度誤差（均方根）上分別降低了67%，34%，在東向速度、北向速度誤差（均方根）上分別降低了47%，38%。從而證明了該算法能有效地抑制由量測異常值導致的狀態(tài)估計誤差，防止濾波發(fā)散，提高濾波穩(wěn)定性。

組合導航；自適應卡爾曼濾波；新息；噪聲方差

0 引言

慣性導航系統(tǒng)（Inertial Navigation System，INS）是一種不依賴于外界信息的自主式導航系統(tǒng)，不僅提供位置和速度信息，還能提供姿態(tài)角信息，具有短時精度高和穩(wěn)定性好的優(yōu)點，但是卻具有誤差隨著時間快速累積增長的缺點[1-2]。全球定位系統(tǒng)（Global Positioning System，GPS）具有全天候、高精度導航定位的優(yōu)點，但是具有信號易受遮擋和干擾的缺點[3-4]。GPS/INS松耦合組合導航既保留了各自解的獨立性，又可以結合二者的優(yōu)點，使組合后的導航精度高于各系統(tǒng)單獨工作的精度[5]。通過卡爾曼濾波在位置域?qū)烧呷诤希商峁┹d體的連續(xù)的位置、速度和姿態(tài)解。同時，濾波器輸出的誤差反饋給慣性測量單元（inertial measurement unit，IMU），可以抑制隨時間增長的慣導誤差。這種組合導航的方式被廣泛應用于國防和軍事中，具有極大的應用價值。

常規(guī)的GPS/INS組合導航卡爾曼濾波（Kalman filter，KF）需要準確的數(shù)學模型以及相對應的系統(tǒng)噪聲和量測噪聲統(tǒng)計特性預先準確已知[6]。然而在實際應用中，量測噪聲和系統(tǒng)噪聲的統(tǒng)計特征值很難準確獲得。例如，系統(tǒng)噪聲方差陣常由陀螺儀和加速度計零偏、速度隨機游走（velocity random walk，VRW）、角度隨機游走（angle random walk，ARW）等一階高斯馬爾可夫過程參數(shù)確定。如果慣導測量單元各項系統(tǒng)噪聲指標在出廠時未經(jīng)過嚴格標定，則無法獲得準確的系統(tǒng)噪聲協(xié)方差陣。另外，隨著應用場景、環(huán)境等的變化會使慣導測量誤差加大；環(huán)境的干擾或遮擋會使GPS信號質(zhì)量迅速下降，導致量測誤差增大。在這些情況下，固定不變的噪聲統(tǒng)計特性會導致狀態(tài)估計誤差加大，濾波結果不準確。不準確的狀態(tài)估計量反饋至慣導測量單元后甚至會引起濾波發(fā)散。

針對上述噪聲統(tǒng)計特性的不準確問題，可采用自適應的卡爾曼濾波算法[7-10]。在利用測量數(shù)據(jù)進行濾波遞推的同時，判斷系統(tǒng)的動態(tài)是否發(fā)生變化。若有變化則把這種變化看作隨機干擾歸到模型和噪聲中去，對模型參數(shù)和噪聲統(tǒng)計特性進行估計和修正使之適應系統(tǒng)動態(tài)的變化[11-12]。自適應卡爾曼濾波的方法有多種，包括基于精度因子（dilution of precision，DOP）自適應濾波[13]，衰減記憶濾波[14-17]，Sage-Husa自適應濾波[18-20]。其中，基于DOP值自適應濾波只能修正由衛(wèi)星幾何布局變化引起的量測噪聲的偏差；衰減記憶濾波在一定程度上能修正模型誤差引起的系統(tǒng)噪聲；Sage-Huse自適應濾波可以修正一定的噪聲統(tǒng)計特性，但當階數(shù)較高時易發(fā)散[21]。本文提出一種基于新息的自適應濾波方法（adaptive Kalman filter，AKF），該濾波方法根據(jù)新息估計量測噪聲和系統(tǒng)噪聲，從而間接調(diào)整卡爾曼濾波增益抑制濾波發(fā)散，提高濾波精度。該方法計算簡單且具有實用性。

1 卡爾曼濾波方法及濾波參數(shù)影響分析

卡爾曼濾波的系統(tǒng)狀態(tài)方程和量測方程如下：

2 基于新息的自適應卡爾曼濾波參數(shù)調(diào)整方法

首先可以利用新息構造卡方檢驗統(tǒng)計量，對量測數(shù)據(jù)進行實時異常值檢測，并通過新息協(xié)方差對進行調(diào)整，在此基礎上，利用新息協(xié)方差構造自適應因子對系統(tǒng)噪聲方差陣進行實時估計和調(diào)整。具體調(diào)整過程如下。

2.1 利用檢驗檢測量測數(shù)據(jù)異常值

設置適當?shù)姆治粩?shù)，檢驗拒絕域為

數(shù)值在檢驗拒絕域內(nèi)說明統(tǒng)計量的實際值與理論值偏離程度過大，的假設不成立，即認為新息的零均值特性喪失，量測量受到異常干擾，出現(xiàn)異常值。

2.2 調(diào)整量測噪聲方差陣R

當通過2.1節(jié)檢測到量測數(shù)據(jù)異常值時，利用新息協(xié)方差進行后驗估計，在原先的值上加入新息協(xié)方差的變化值，對進行上升調(diào)整，減小增益矩陣，降低帶有異常值的量測數(shù)據(jù)對濾波的影響，量測噪聲協(xié)方差調(diào)整公式如下：

2.3 修正系統(tǒng)噪聲方差陣Q

3 基于新息的自適應卡爾曼濾波算法在GPS/INS松耦合組合導航中的應用

圖1 基于新息的GPS/INS松耦合自適應卡爾曼濾波算法流程圖

4 車載路測實驗及結果分析

本文為了驗證基于新息的自適應卡爾曼濾波算法的性能，于2019年7月10日在西安臨潼商業(yè)街道進行了跑車實驗。下面分別給出實驗安排和實驗結果分析。

4.1 實驗安排

本文跑車實驗數(shù)據(jù)采用SBG Ekinox2作為慣性測量單元，使用NovAtel接收機進行GPS（千尋RTK服務）定位，其中，GPS理論定位精度為分米級，其他試驗參數(shù)如表1所示。

表1 試驗設備參數(shù)設置

實驗將第三方SBG Ekinox 2的組合導航輸出結果（理論定位精度為0.02 m）作為參考值，實驗分析的各項定位測速誤差都是以該參考值為基準得出。將該參考值輸入到谷歌地圖中，得到跑車真實運動軌跡如圖2白色粗線所示。

圖2 跑車試驗軌跡

整個車載實驗過程中，部分時段GPS信號受附近高樓和樹木等影響，GPS定位出現(xiàn)異常，圖2中用方框標記圈出的區(qū)域就屬于此種情況，該區(qū)域的GPS定位誤差如圖3所示。由于GPS定位異常會影響組合導航的定位精度，故本文選取方框區(qū)域為研究對象，分別用本文提出的AKF算法和常規(guī)KF算法對該區(qū)域?qū)Ш綌?shù)據(jù)進行處理，通過比較兩種算法的濾波結果來驗證本文算法的有效性。

圖3 GPS定位結果

4.2 實驗結果分析

分別使用AKF算法和KF算法對圖2中方框區(qū)域所對應的測量數(shù)據(jù)進行INS/GPS松耦合解算，將解算結果與參考值進行比較，得到經(jīng)度方向和緯度方向的位置誤差對比如圖4所示，速度誤差對比如圖5所示。其中，虛線表示AKF算法濾波結果，實線表示KF算法濾波結果。

圖4 KF、AKF位置誤差對比

圖5 KF、AKF速度誤差對比

表2列出了KF算法和AKF算法在方框區(qū)域位置誤差和速度誤差的均值（mean）及均方根（root mean square，RMS），可以算出AKF算法在經(jīng)度、緯度、東向速度、北向速度誤差較KF算法均值上分別降低了39%，20%，95%，50%；均方根分別降低了67%，34%，47%，38%。故，本文提出的AKF算法較KF算法具有更好的導航精度和濾波穩(wěn)定性。

表2 算法性能比較

注：mean為均值，RMS為均方根。

5 結論

本文以GPS/INS組合導航應用為背景，針對由于系統(tǒng)噪聲協(xié)方差陣不準確、觀測數(shù)據(jù)突變造成的常規(guī)Kalman濾波精度下降問題，提出一種基于新息的自適應Kalman濾波算法。該算法首先利用卡方檢驗對量測異常值進行識別，再用新息協(xié)方差調(diào)整量測噪聲來抑制異常值；然后，通過自適應因子對系統(tǒng)噪聲進行調(diào)整來改善系統(tǒng)噪聲協(xié)方差陣不準確的問題。并通過跑車試驗進行驗證，實驗結果顯示：AKF算法在經(jīng)度、緯度、東向速度、北向速度誤差較KF算法均值上分別降低了39%，20%，95%，50%；均方根分別降低了67%，34%，47%，38%，有效地提高了整體濾波精度。相對使用KF算法經(jīng)度誤差峰值達到-19 m，使用AKF算法經(jīng)度誤差峰值能降至-5 m，證明AKF算法能有效地抑制異常值，防止濾波發(fā)散。

[1] 王洪先. 陸用慣性導航系統(tǒng)技術發(fā)展綜述[J]. 光學與光電技術, 2019, 17(6): 77-85.

[2] 楊曉明, 王勝利, 王海霞, 等. 基于EKF的GNSS/SINS組合導航系統(tǒng)應用[J]. 山東科技大學學報(自然科學版), 2019, 38(6): 114-122.

[3] 崔杉, 熊力. 全球衛(wèi)星導航系統(tǒng)[J]. 電子制作, 2015(16): 47.

[4] 汪勇, 丁金學. 全球衛(wèi)星導航系統(tǒng)的市場應用前景[J]. 中國航天, 2012(9): 30-36.

[5] 樊宇, 程全. GPS與慣性導航系統(tǒng)的組合應用研究[J]. 制造業(yè)自動化, 2015(3): 74-75.

[6] 胡鋒, 孫國基. Kalman濾波的抗野值修正[J]. 自動化學報, 1999, 25(5): 692-696.

[7] WU F, YANG Y, CUI X. Application of adaptive factor based on partial state discrepancy in tight coupled GPS/INS integration[J]. Geomatics and Information Science of Wuhan University, 2010, 35(2): 156-159.

[8] WU F M, NIE J L, HE Z B. Classified adaptive filtering to GPS/INS integrated navigation based on predicted residuals and selecting weight filtering[J]. Geomatics & Information Science of Wuhan University, 2012, 37(3): 261-264.

[9] YANG Y, HE H, XU G. Adaptively robust filtering for kinematic geodetic positioning[J]. Journal of Geodesy, 2001, 75(2-3): 109-116.

[10] YANG Y, XU T. An adaptive Kalman filter based on sage windowing weights and variance components[J]. Journal of Navigation, 2003, 56(2): 231-240.

[11] 申逸. Kalman濾波技術在目標跟蹤中的應用研究[D]. 長沙: 國防科學技術大學, 2006.

[12] 楊元喜. 動態(tài)Kalman濾波模型誤差的影響[J]. 測繪科學, 2006, 31(1): 17-18.

[13] 姜慶峰, 桑渤, 潘泉有. 基于DOP值的GPS/INS組合導航濾波方法[J]. 海洋測繪, 2016, 36(1): 59-62.

[14] 田寵, 王興亮, 盧艷娥. 一種改進的自適應指數(shù)加權衰減記憶濾波算法[J]. 武漢理工大學學報, 2011, 33(11): 147-152

[15] 蔡佳, 黃長強, 井會鎖, 等. 基于指數(shù)加權的改進衰減記憶自適應濾波算法[J]. 探測與控制學報, 2013, 35(4): 21-26.

[16] ZHAO L, LIU J. An improved adaptive filtering algorithm with applications in integrated navigation[C]//IEEE International Conference on Digital Manufacturing＆ automation, 2012: 182-185.

[17] 譚攀, 伍仲南, 康躍耀. 抑制卡爾曼濾波發(fā)散的組合導航算法研究[J]. 地理空間信息, 2019, 17(9): 109-112.

[18] SAGE A P, HUSSA G W. Adaptive filtering with unknown prior statistics[C]//Proceedings of Joint Automatic Control Conference. Boulder, USA: ASME, 1969: 760-769.

[19] 魯平, 趙龍, 陳哲. 改進的Sage-Husa自適應濾波及其應用[J]. 系統(tǒng)仿真學報, 2007(15): 3503-3505.

[20] 徐恩松, 陸文華, 劉云飛,等. 基于卡爾曼濾波的數(shù)據(jù)融合算法與應用研究[J/OL]. 計算機技術與發(fā)展, 2020(5): 1-7.

[21] 付夢印, 鄧志紅, 閆莉萍. Kalman濾波理論及其在導航系統(tǒng)中的應用[Ｍ]. 北京: 科學出版社, 2010: 111-118．

[22] 袁美桂, 嚴玉國, 龐春雷,等. 改進的自適應Kalman濾波在GPS/SINS中的應用[J]. 空軍工程大學學報(自然科學版), 2015, 16(5): 65-69.

[23] 沈凱, 管雪元, 李文勝. 擴展卡爾曼濾波在組合導航中的應用[J]. 傳感器與微系統(tǒng), 2017, 36(8): 158-160.

Research on adaptive Kalman filter algorithm for GPS/INS loosely coupled integrated navigation

ZHOU Xian-lin1,2,3, ZHANG Hui-jun1,2,3, HE Tao1,2,3,4, LI Xiao-hui1,2,3

(1. National Time Service Center, Chinese Academy of Sciences, Xi’an 710600, China;2. Key Laboratory of Precise Positioning and Timing Technology, Chinese Academy of Sciences, Xi’an 710600, China;3. University of Chinese Academy of Sciences, Beijing 100049, China;4. Kunming Shipborne Equipment Research and Test Center,China State Shipbuilding Corporation Limited, Kunming 650051, China)

Aiming at the reduction of the filtering accuracy when the conventional Kalman filter is used in GPS/INS integrated navigation due to outliers of measurement data or inaccurate system state model, an adaptive Kalman filter algorithm based on innovation is proposed. First, the algorithm detects measured outliers by chi-square test, and adjusts the measurement noise variance matrix at the outliers to suppress filtering divergence. Thereby, the rough ratio between the calculated value and the predicted value of the innovation variance matrix is calculated to adjust the system noise variance matrix and then the overall filtering accuracy will be improved. The AKF algorithm proposed in this paper is verified by a vehicle test. The results show that, the proposed AKF algorithm in this paper reduces the longitude errors and latitude errors (root mean square) with 67% and 34% comparing with the conventional Kalman filter algorithm, and reduces the eastward velocity errors and northward velocity errors (root mean square) with 47% and 38%. It is proved that the algorithm can effectively suppress the state estimation error caused by the measured outliers, and further improve the filtering stability.

integrated navigation; adaptive Kalman filtering; innovation; noise variance

10.13875/j.issn.1674-0637.2020-03-0222-09

2020-01-19；

2020-04-09

基礎研究重大項目前期研究專項資助（11703030）

周先林，女，碩士，主要從事多源輔助的GNSS/INS組合導航性能改善研究。