基于指數漸消遺忘因子的組合導航自適應濾波算法

曾慶化，趙天鈺，趙 賓,2，劉建業，朱小靈

（1. 南京航空航天大學導航研究中心，南京 210016；2. 南京航空航天大學金城學院，南京 211156）

捷聯慣性導航系統（Strapdown Inertial Navigation System, SINS）有較強的自主性和抗干擾能力，但誤差隨時間累積，易發散[1]；全球衛星導航系統（Global Navigation Satellite System, GNSS）可提供高精度的位置、速度信息，且誤差不隨時間發散，但易受外界環境干擾，定位、測速精度下降[2]。

SINS/GNSS 組合導航系統可以將二者組合在一起，取長補短，從而獲得更高精度的導航參數[3,4]。組合導航系統常采用卡爾曼濾波進行信息融合，但卡爾曼濾波要求精確的數學模型以及準確的先驗噪聲統計特性，在濾波過程中，若噪聲特性出現異常，則濾波精度下降甚至發散[5]。

為了解決這一問題，有學者提出了帶漸消因子的自適應算法，通過以強迫殘差序列正交的方法，確定漸消因子，文獻[6][7]在此基礎上，設計了具有多漸消因子的卡爾曼濾波器，取得了更好的精度和魯棒性，文獻[8]針對非線性以及觀測噪聲異常情況，在容積卡爾曼濾波（Cubature Kalman Filter, CKF）的基礎上，提出了基于假設檢驗的濾波器狀態檢驗算法，在合適的條件下引入多重漸消因子，進一步提升了算法的抗干擾能力。在噪聲異常情況下，相比于上述自適應算法，Sage-Husa 自適應濾波（Sage Husa Adaptive Kalman Filter, SHAKF）不僅可以提高濾波精度，還能夠實時估計出系統的噪聲參數[9,10]，文獻[11]基于CKF，結合帶有衰減指數漸消因子的SHAKF，提高了縱向距離的濾波精度，文獻[12]結合聯邦濾波和SHAKF，提出了動態加權因子將兩種算法進行融合，有效地應用于多源導航系統，文獻[13]依據當前時刻的殘差信息，在濾波的量測更新中加入漸消因子，提高了濾波的自適應能力，同時引入可變滑窗提高對量測噪聲的估計精度，文獻[14]根據各導航狀態量的可觀測度設置漸消因子，調整濾波增益矩陣，降低了狀態估計誤差。

上述算法在SHAKF 的基礎上做出了改進，取得了更好的濾波效果，但都沒有針對SHAKF 中的遺忘因子b 展開深入研究，遺忘因子的大小會影響對量測噪聲估計的靈敏度和精度，文獻[15]通過滑動窗口檢測量測噪聲方差變化，提出了基于滑動遺忘因子的自適應濾波算法（Sliding Attenuating Factor Adaptive Kalman Filter, SAFAKF），相比于固定遺忘因子SHAKF，該方法可以兼顧跟蹤精度和靈敏度，但其收斂速度有待進一步提高，且存在一定的拖尾效應，實際仿真及應用中，導航精度提升不明顯。基于此，本文提出了基于指數漸消遺忘因子的自適應濾波算法（Exponential Attenuating Factor Adaptive Kalman Filter, EAFAKF）。首先設計了基于滑動窗口的χ2故障檢測方法，當檢測到量測信息故障時，設計了基于指數動態變化的遺忘因子，使得算法在快速估計量測噪聲矩陣的同時，能夠保證其準確性，從而提高濾波精度。

1 組合導航模型

捷聯慣性導航系統通過數學平臺解算姿態轉移矩陣，從而獲取姿態信息并進行坐標系的變換。這里給出本文中出現的幾種坐標系及其定義。

(2)地球坐標系Oxeyeze（e 系）：原點位于地心，z軸沿地球自轉軸方向，x 軸在赤道平面內，與零度子午線相交，y 軸與x 軸、z 軸構成右手坐標系；

(3)載體坐標系Oxbybzb（b 系）：原點與載體質心重合，x 軸沿載體橫軸向右，y 軸沿載體縱軸向前，z軸沿載體豎軸向上，即“右前上”坐標系；

(4)導航坐標系Oxnynzn（ n 系）：當地地理坐標系，原點位于載體質心，x 軸指向東，y 軸指向北，z 軸垂直于當地水平面向上，即“東北天”坐標系。

1.1 狀態方程

SINS/GNSS 組合導航系統的狀態變量為

則狀態方程為

式中，狀態轉移矩陣A 由式(3)所示的微分方程導出。

1.2 量測方程

SINS/GNSS 組合導航系統量測方程為

式中， Zk為量測向量，Hk為量測系數矩陣， Vk為量測噪聲向量，近似為白噪聲。量測信息選擇為捷聯慣導系統和全球衛星導航系統的速度和位置差值，具體如下：

其中，vE,I、vN,I和vU,I是捷聯慣導計算得到的東向、北向和天向速度，vE,GNSS、vN,GNSS和vU,GNSS是GNSS輸出的東向、北向和天向速度； LI、λI和 hI是捷聯慣導計算得到的緯度、經度和高度，LGNSS、λGNSS和 hGNSS是GNSS 輸出的緯度、經度和高度。

2 基于指數漸消遺忘因子的自適應濾波算法

2.1 基于滑動窗口的χ 2故障檢測方法

卡爾曼濾波實際測量值 Zk與量測預測值之間的殘差為 εk，其方差為

在濾波工作正常且GNSS 沒有發生故障的情況下，殘差服從均值為零的高斯分布，即

式中，m 為量測向量的維度。

當GNSS 量測信息異常時，故障檢測函數 λk不再服從上述分布，因此，可以通過χ2分布上分位點性質進行故障檢測。例如，當量測維度m=6 時，選取上分位點ζ =18.548，通過查表可知P{χ2( m)＞ ζ} =0.5%，在量測信息正常時， λk＞ ζ的概率只有0.5%，則在99.5%的置信度下可認為系統無故障。

上分位點選取過高或過低都會產生誤判，因此本文設計了一種基于滑動窗口的故障檢測方法，新的故障檢測函數表達式如下：

式中，N 為窗口寬度。窗口寬度不宜設置過大，否則會產生較大的時延，檢測能力將變弱，經過權衡考慮，窗口寬度選取3～5 為宜。經優化后的故障檢測函數能夠更加準確地判斷出量測信息是否出現異常。

2.2 Sage-Husa 自適應濾波器改進

SHAKF 可以在進行狀態估計的同時根據量測輸出實時估計出系統的噪聲參數，包括系統噪聲均值和量測噪聲，一般認為系統噪聲具有穩定性，且在實際應用中，量測噪聲方差陣對濾波影響較為顯著，因此通常僅對量測噪聲進行估計[16]。

系統狀態空間模型如下：

其中

假設濾波過程中量測噪聲方差矩陣 Rk未知或發生變化，常規卡爾曼濾波難以保證濾波精度，若能夠實時地估計出量測噪聲方差矩陣 Rk，可以提高濾波精度。在濾波過程中，量測預測誤差公式為： 測噪聲 Vk均值為零，可知的均值也為零，且

將上式移項，可得k 時刻量測噪聲方差矩陣為

式中，b 為遺忘因子，易知，越陳舊的數據，其加權系數越小，根據式(15)可構建加權系數如下

對量測噪聲矩陣 Rk的估計如下：

對 Rk進行漸消記憶指數加權后，可保持一定的自適應能力，且遺忘因子取值越小，對新量測噪聲變化的自適應能力越強，但估計結果會跳變得較為劇烈，遺忘因子取值越大，其自適應能力變弱。因此，為了能夠在保證較強自適應能力的同時使得對量測噪聲矩陣的估計較為平穩，本文提出了可變遺忘因子的自適應濾波方法。

在濾波過程中，當量測噪聲發生變化，通過故障檢測函數對濾波狀態進行判斷，若判斷出濾波異常，遺忘因子取較小的值以提高對量測噪聲矩陣的跟蹤速度，隨著時間的推移，為了能夠更精確地估計出量測噪聲矩陣，遺忘因子的取值應適當增大。考慮到遺忘因子的取值應滿足0

式中，t0為量測噪聲發生變化的時刻，可由故障檢測函數得到，m 決定了遺忘因子的初始值，n 可以控制遺忘因子的變化速度。從式(18)可以看出，當量測噪聲變化時，遺忘因子取較小的值，隨后遺忘因子的值逐漸增大，使得對 Rk的估計更加精確。

假設系統500 s 后發生故障，量測噪聲擴大為原來的10 倍，圖1 展示了傳統SHAKF、SAFAKF 和EAFAKF 遺忘因子取值的變化曲線。遺忘因子的選取會影響對量測噪聲的估計，圖2 展示了不同算法對量測噪聲的估計結果?？梢钥闯?，SAFAKF 存在較為嚴重的拖尾效應，而EAFAKF 在快速估計出噪聲變化的同時，還能保證其準確性。

圖1 不同算法遺忘因子變化曲線Fig.1 Attenuating factor variation curve of different algorithms

圖2 不同算法對量測噪聲矩陣估計結果對比Fig.2 Estimation of measurement noise matrixof different algorithms

濾波算法整體流程如圖3 所示。

圖3 算法流程圖Fig.3 Algorithm flow chart

3實驗與分析

3.1 仿真實驗

仿真參數設置如下：初始經度λ =118.79 °E，緯度L=31.94° N ，水平姿態角為0°，航向角為30°，初始失準角為[0.1′ 0.2′ 3′] ，參考慣導級IMU 性能指標，設置仿真系統傳感器精度如表1所示。

表1 仿真參數Tab.1 Simulation parameters

濾波器參數設置如下：

本文設置了包括起飛、爬升、平飛、降落等階段的飛行軌跡，飛行時間為1940s。飛行過程中，假設在500～600s以及1200～1500s時間段內，GNSS量測噪聲擴大10倍。分別采用常規卡爾曼濾波算法、SHAKF、SAFAKF以及EAFAKF進行濾波估計，位置誤差曲線如圖4所示。

圖4 位置誤差曲線Fig.4 Position error curves

在500～600s以及1200～1500s時間段內，GNSS量測信息發生故障，從圖4中可以看出，EAFAKF的估計精度更高，且當故障結束后，其收斂過程更加迅速。本文針對上述算法各進行了100次蒙特卡洛仿真驗證，并比較了上述算法的均方根誤差（Root Mean SquareError,RMSE）。

式中，n表示信號序列， Xi表示導航信息真值，由航跡發生器得到，表示對導航信息的估計值。均方根誤差可以很好地反映出量測精度，圖5給出了100次蒙特卡洛仿真驗證均方根誤差的箱形圖。

圖5 位置均方根誤差Fig.5 RMSE of position

從圖5可以看出，本文提出算法的均方根誤差最小，在位置精度方面，EAFAKF相比于KF、SHAKF、SAFAKF分別平均提高了28.33%，21.99%，20.33%。

3.2 跑車實驗

為了驗證本文算法的有效性，選取實際的系統進行跑車試驗，實驗裝置如圖6所示。

圖6 跑車測試平臺Fig.6 Experimental test platform

根據實測數據，分析了慣性組件（IMU）和GPS的性能指標，具體參數如表2所示。

表2 傳感器參數Tab.2 Sensor paramete rs

車載系統搭載RTK，可以提供高精度的位置、速度信息，并將其作為參考值驗證組合導航精度。測試路線在校園內進行，歷時1450s，運動軌跡如圖7所示。

圖7 跑車軌跡Fig.7 Trajectory of experimental test

為了驗證本文算法的導航精度，設置了兩個衛星信號故障區域，在500s-600s以及1100s-1300s兩個時間段內對衛星量測信息加入如下故障：誤差增大10倍，測速誤差從0.2m/s變化為2m/s，定位誤差從10m變化為100m。以RTK輸出的速度、位置作為參考值，對比了常規卡爾曼濾波算法、SHAKF、SAFAKF以及EAFAKF的導航精度。圖8(a)對比了本文算法導航軌跡與參考軌跡的對比結果，圖中選取起始點為坐標原點，x 軸正向為正東，y軸正向為正北，圖8(b)、圖8(c)分別對比了四種算法在500s-600s（圖8(a)中AB段）、1100s-1300s（圖8(a)中CD段）兩段量測信息異常區域的軌跡結果。

在量測信息發生異常時，本文算法可以快速、準確地估計出量測噪聲矩陣，從圖8可以看出，相比于常規卡爾曼濾波、SHAKF、SAFAKF，本文算法提供的位置信息更加精確。為了量化算法的性能指標，表3給出了整個導航階段四種算法的位置均方根誤差。

圖8 軌跡對比Fig.8 Comparison of trajectory

由表3可以看出，整個導航過程，本文算法的位置均方根誤差最小，定位精度相比于KF、SHAKF、SAFAKF分別提高了29.50%、38.82%、26.92%。相比于其他三種方法，本文算法能夠在量測信息異常時提供更高精度的導航結果。

表3 四種算法位置均方根誤差Tab.3 RMSEof position of four algorithms

4 結 論

為了提高量測信息異常時組合導航精度，本文提出了基于指數漸消遺忘因子的自適應濾波算法。在構建故障檢測算法判斷量測信息是否出現異常的基礎上，引入了基于指數函數變化的遺忘因子，有助于實現對量測噪聲統計特性的快速準確評估。通過與常規卡爾曼濾波、SHAKF、SAFAKF 的對比分析可知，本文算法在量測異常的仿真和跑車實驗中，導航精度均提高20%以上，具有較好的工程應用潛力。