GNSS/INS 組合導航中自適應抗差KF 算法研究*

吳 昕 ，劉 超

（安徽理工大學空間信息與測繪工程學院，安徽 淮南 232001）

0 引言

全球導航衛星系統（Global Navigation Satellite System, GNSS）能夠提供全天候的導航、定位以及授時服務，且定位誤差不隨時間累積[1]。但在復雜城市環境下，由于高大密集建筑物、茂密樹木和高架橋等的遮擋，衛星信號頻繁受到干擾，此時的觀測值會受到影響，用戶的定位精度也會受到影響[2]。慣性導航系統（Inertial Navigation System, INS）是一種完全自主的且被廣泛使用的導航定位技術，但隨時間增長其誤差會快速積累，導致精度快速下降，因此無法滿足長時間、高精度的導航定位需求[3-4]。因此，將GNSS和INS 結合起來可較好地滿足用戶的需求[5]。近年來，將兩者融合以彌補單一系統的技術缺陷成為研究熱點之一[6-7]。

在組合導航領域最常用的一種最優數據融合方法便是卡爾曼濾波（Kalman Filter, KF），其已經被廣泛應用于數據處理領域。而標準卡爾曼濾波的狀態參數想要得到最優估計，就需要準確獲取狀態模型參數、狀態噪聲協方差矩陣以及觀測噪聲的統計特性等信息[8]。然而，在實際應用中，狀態模型參數及噪聲統計特性是難以準確確定的，在很多情況下以上兩類參數的獲取都會存在一些誤差，致使卡爾曼濾波的精度降低，嚴重時還可能會引起濾波發散，進而不能正確地反映載體運動狀態[9-10]。

眾多學者針對隨機系統的系統噪聲協方差矩陣Q和測量噪聲協方差矩陣R進行在線估計，或者采用自適應抗差卡爾曼濾波（Adaptive and Robust Kalman Filter, ARKF）對其進行參數優化[11-12]。目前常用的構建自適應因子統計量的方法有狀態不符值、速度不符值、方差分量比和量測新息。前三個構建方法要求在該時刻有多余觀測信息，即該時刻的觀測數據個數要多于待估狀態參數的個數，否則在求解狀態最優估計值時，使用最小二乘原理的方法會出現秩虧現象，所以這兩個構建方法不適用于GNSS/INS 松組合[13]。在提出的改進的卡爾曼濾波算法中，使用量測新息構建馬氏距離作為檢驗統計量，以判定觀測數據是否存在異常，最后基于馬氏距離構造調節因子，來調節觀測噪聲協方差矩陣和新息協方差矩陣，進而調整卡爾曼濾波的增益，以此減小動力學模型擾動和觀測異常對濾波結果的影響。

1 GNSS/INS組合導航卡爾曼濾波模型

1.1 卡爾曼濾波模型

卡爾曼濾波是組合導航系統中大多數狀態估計常用的方法，其采用遞推的方法進行狀態參數的預計算，使用前一時刻的狀態估計值和狀態估計均方誤差矩陣來估計狀態參數，再結合當前時刻的觀測值，估計當前時刻的狀態估計值和狀態估計均方誤差矩陣，按照“預測—修正”的形式進行遞推，如此便能夠實時估計系統中的狀態參數。

卡爾曼濾波隨機系統的動力學模型和觀測模型為：

式中，Xk和zk分別是k時刻的狀態向量和量測向量，Φk,k-1和Hk分別稱為狀態一步轉移矩陣和量測矩陣，ωk-1和vk是互不相關的動力學模型誤差向量和量測噪聲向量，且兩者都是零均值的高斯白噪聲向量序列。

經過推導可以得到更為一般的狀態一步預測和狀態一步預測均方誤差矩陣，即卡爾曼濾波的時間更新：

根據遞推最小二乘估計方法，可以得到當前時刻的狀態估計及其均方誤差矩陣：

式（3）稱為卡爾曼濾波的量測更新，式中第一個等式的Kk是在狀態方差最小的約束準則下得到的，稱為卡爾曼濾波的增益矩陣，從式（3）的第二個等式可以看出其能夠調節狀態預測值與觀測值之間的權重。式（3）的第三個等式稱為協方差量測更新的Joseph 穩定形式，這種形式可以提高系統解算的穩定性和魯棒性。

1.2 自適應抗差卡爾曼濾波模型

在實際GNSS/INS 組合導航系統中，理論濾波模型與實際情況存在一定的偏差，隨機動態系統的結構參數和噪聲統計特性參數難以準確估計，導致先驗協方差矩陣即狀態一步預測均方誤差矩陣的計算存在偏差，嚴重時甚至會導致標準卡爾曼濾波發散。在濾波過程中，濾波器發散時，誤差協方差矩陣的理論計算值與實際估計相差較大，實際使用時一般采用量測新息構造的協方差矩陣，以此比較其理論計算值與實際估計。

量測新息的定義從式（3）中可以得出：

此時基于量測新息zk,k-1的協方差矩陣的理論計算值和實際估值分別為：

實際計算時，式中第二個等式中的m一般取1，此時=。由此可以構造如下基于量測新息的統計量：

其中，“tr”表示矩陣的跡。構造兩段函數模型自適應因子如下：

其中，c為調整系數，可取1～1.5，最優值是1。

可以看出，自適應因子α會隨著量測新息的增大而減小，但并不會減小為0。

當隨機系統的動力學模型存在偏差且觀測模型存在異常時，使用楊元喜在貝葉斯估計的基礎上推導的M-LS 抗差濾波算法[13]。即對觀測值序列進行M 估計、對狀態參數進行LS（Least Square）估計。由此可得M-LS 估計的抗差濾波解的等價遞推形式解為：

式中，ˉKk稱為等價增益矩陣，可表示為：

量測噪聲向量vk的協方差矩陣ˉRk的逆矩陣即為等價權矩陣ˉPk，因此在卡爾曼濾波中應用等價權抗差估計方法就可以得到參數的抗差解。而三段降權因子中IGGⅢ法能夠充分考慮到觀測數據的實際情況，因此使用IGGⅢ等價權函數構造等價權因子βk如下：

其中，k0和k1是閾值，k0的取值范圍是2.5～3.5，k1的取值范圍是3.5～4.5，表示第i個標準化殘差，且zk,k-1(i)是量測新息的第i分量和其標準差矩陣對角線的第i個分量。

2 改進的自適應抗差卡爾曼濾波模型

式中，n表示統計量的自由度，α是顯著性水平，表示顯著性水平為α和自由度為n時的χ2臨界值。觀測信息故障檢測的判別準則為：當(n)時，觀測信息無異常；當γk＞時，觀測信息異常。若觀測值中存在粗差，則需要調節觀測噪聲協方差矩陣Rk。此時可以引入調節因子λk，可得調節后的觀測噪聲協方差矩陣ˉRk：

而此時的卡方檢驗統計量為：

將調節后的觀測噪聲協方差矩陣ˉRk代入寫成函數形式后可得：

上式是關于λ的非線性方程，使用牛頓法進行迭代求解可得：

迭代i次后可以得到λ的解為：

基于上述理論，可以再引入一步預測觀測協方差矩陣調節因子τk調節新息協方差矩陣Pzk,k-1，則調整后的新息協方差矩陣即出現觀測粗差時的實際新息協方差矩陣可表示為：

其中，τk的表達式可以根據γk與(n)的關系解算得到，如下所示：

在下節實驗中會使用自由度n為6 和顯著性水平α為0.005 的卡方分布即(6)的臨界值作為判定標準。最后得到卡爾曼濾波的增益為：

綜上所述，在構建兩個調節因子后，可以有效抑制觀測值異常帶來的影響。

3 算例分析

使用車載移動測量系統采集數據分析所提算法性能，車輛上裝備GNSS 接收機和慣性測量單元IMU（Inertial Measurement Unit），可獲取動態載體（實驗車輛）在城市環境中的GNSS 數據和IMU 數據。實驗中使用GNSS偽距單點定位數據計算得到的數據與IMU慣性導航數據經機械編排后得到的數據以松組合模式進行GNSS/INS組合，GNSS采樣頻率為5 Hz，IMU采樣頻率為200 Hz，組合采樣頻率為1 Hz，將IE（Inertial Explorer）軟件差分后處理解算結果作為參考值。實驗設置三種方案，分別是：1）GNSS/INS組合標準卡爾曼濾波（KF）；2）GNSS/INS組合自適應抗差卡爾曼濾波（ARKF）；3）GNSS/INS組合改進自適應抗差卡爾曼濾波（IARKF）。三種方案對應的卡爾曼濾波算法解算得到的結果分別與IE解算的參考值對比，得到的位置誤差分別如圖1、圖2、圖3 所示，三種方案所得誤差的均方根如表1所示。

表1 三種方案均方根比較

圖1 標準卡爾曼濾波（KF）位置誤差

圖2 自適應抗差卡爾曼濾波（ARKF）位置誤差

圖3 改進自適應抗差卡爾曼濾波（IARKF）位置誤差

對比圖1、圖2、圖3可以看出，相比于方案2、3的ARKF 算法和IARKF 算法，方案1 的KF 算法在東方向、北方向、天方向三個方向的位置誤差更大，說明方案2 和方案3 的算法可以有效抑制隨機動態系統的不確定性對濾波結果的影響；從圖2中可以看出方案2算法的定位結果仍然存在個別位置誤差較大的點，而從圖3 中可以看出方案3 算法將方案2 中誤差較大的點平滑掉了，由此可見，方案3 算法能夠對存在的觀測值異常進行較為準確的判斷調節，可以得到更準確的濾波結果，因此方案3算法要優于方案2算法。

對比表1 中三種方案誤差的均方根，可以更直觀地比較三種濾波算法的性能。當存在噪聲干擾時，與方案1 相比，方案2 解算的位置誤差均方根分別減小了0.340 3 m、0.159 8 m、0.134 2 m，方案3 解算的位置誤差均方根分別減小了0.490 8 m、0.499 9 m、0.139 0 m。 結合上述對圖1、圖2、圖3 的分析可知，在對GNSS/INS 組合導航數據進行抗粗差處理后，ARKF 和IARKF 解算得到的位置精度均高于 KF，其中IARKF 略有優勢，在數據處理過程中通過假設檢驗對粗差進行判斷，在模型異常時應用改進的自適應因子調節位置向量，有效避免了粗差和觀測異常對系統帶來的影響。

4 結語

針對城市環境中動力學模型異常擾動和GNSS 觀測值存在粗差等因素影響，導致GNSS/INS 組合的數據濾波解存在誤差，在給出的改進自適應抗差卡爾曼濾波算法中，使用量測新息構建馬氏距離作為檢驗統計量，與卡方分布的臨界值比較，以此判定觀測數據是否存在異常。通過解算實際跑車數據對算法進行分析，結果表明改進的算法能夠控制模型誤差和測量異常值對導航結果的影響，提高GNSS/INS 組合導航性能。