GPS/INS松耦合組合導(dǎo)航的自適應(yīng)卡爾曼濾波算法研究

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

GPS/INS松耦合組合導(dǎo)航的自適應(yīng)卡爾曼濾波算法研究

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

2. 中國(guó)科學(xué)院 國(guó)家授時(shí)中心，西安 710600；中國(guó)科學(xué)院 精密導(dǎo)航定位與定時(shí)技術(shù)重點(diǎn)實(shí)驗(yàn)室，西安 710600；3. 中國(guó)科學(xué)院大學(xué)，北京 100049；4.中國(guó)船舶集團(tuán)有限公司 昆明船舶設(shè)備研究試驗(yàn)中心，昆明 650051）

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

組合導(dǎo)航；自適應(yīng)卡爾曼濾波；新息；噪聲方差

0 引言

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

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

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

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

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

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

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

2.1 利用檢驗(yàn)檢測(cè)量測(cè)數(shù)據(jù)異常值

設(shè)置適當(dāng)?shù)姆治粩?shù)，檢驗(yàn)拒絕域?yàn)?/p>

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

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

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

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

3 基于新息的自適應(yīng)卡爾曼濾波算法在GPS/INS松耦合組合導(dǎo)航中的應(yīng)用

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

4 車載路測(cè)實(shí)驗(yàn)及結(jié)果分析

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

4.1 實(shí)驗(yàn)安排

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

表1 試驗(yàn)設(shè)備參數(shù)設(shè)置

實(shí)驗(yàn)將第三方SBG Ekinox 2的組合導(dǎo)航輸出結(jié)果（理論定位精度為0.02 m）作為參考值，實(shí)驗(yàn)分析的各項(xiàng)定位測(cè)速誤差都是以該參考值為基準(zhǔn)得出。將該參考值輸入到谷歌地圖中，得到跑車真實(shí)運(yùn)動(dòng)軌跡如圖2白色粗線所示。

圖2 跑車試驗(yàn)軌跡

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

圖3 GPS定位結(jié)果

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

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

圖4 KF、AKF位置誤差對(duì)比

圖5 KF、AKF速度誤差對(duì)比

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

表2 算法性能比較

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

5 結(jié)論

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

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

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

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

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

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

[6] 胡鋒, 孫國(guó)基. Kalman濾波的抗野值修正[J]. 自動(dòng)化學(xué)報(bào), 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濾波技術(shù)在目標(biāo)跟蹤中的應(yīng)用研究[D]. 長(zhǎng)沙: 國(guó)防科學(xué)技術(shù)大學(xué), 2006.

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

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

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

[15] 蔡佳, 黃長(zhǎng)強(qiáng), 井會(huì)鎖, 等. 基于指數(shù)加權(quán)的改進(jìn)衰減記憶自適應(yīng)濾波算法[J]. 探測(cè)與控制學(xué)報(bào), 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ā)散的組合導(dǎo)航算法研究[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] 魯平, 趙龍, 陳哲. 改進(jìn)的Sage-Husa自適應(yīng)濾波及其應(yīng)用[J]. 系統(tǒng)仿真學(xué)報(bào), 2007(15): 3503-3505.

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

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

[22] 袁美桂, 嚴(yán)玉國(guó), 龐春雷,等. 改進(jìn)的自適應(yīng)Kalman濾波在GPS/SINS中的應(yīng)用[J]. 空軍工程大學(xué)學(xué)報(bào)(自然科學(xué)版), 2015, 16(5): 65-69.

[23] 沈凱, 管雪元, 李文勝. 擴(kuò)展卡爾曼濾波在組合導(dǎo)航中的應(yīng)用[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

基礎(chǔ)研究重大項(xiàng)目前期研究專項(xiàng)資助（11703030）

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