基于指數(shù)漸消遺忘因子的組合導(dǎo)航自適應(yīng)濾波算法

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

（1. 南京航空航天大學(xué)導(dǎo)航研究中心，南京 210016；2. 南京航空航天大學(xué)金城學(xué)院，南京 211156）

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

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

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

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

1 組合導(dǎo)航模型

捷聯(lián)慣性導(dǎo)航系統(tǒng)通過數(shù)學(xué)平臺解算姿態(tài)轉(zhuǎn)移矩陣，從而獲取姿態(tài)信息并進行坐標(biāo)系的變換。這里給出本文中出現(xiàn)的幾種坐標(biāo)系及其定義。

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

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

(4)導(dǎo)航坐標(biāo)系Oxnynzn（ n 系）：當(dāng)?shù)氐乩碜鴺?biāo)系，原點位于載體質(zhì)心，x 軸指向東，y 軸指向北，z 軸垂直于當(dāng)?shù)厮矫嫦蛏?，即“東北天”坐標(biāo)系。

1.1 狀態(tài)方程

SINS/GNSS 組合導(dǎo)航系統(tǒng)的狀態(tài)變量為

則狀態(tài)方程為

式中，狀態(tài)轉(zhuǎn)移矩陣A 由式(3)所示的微分方程導(dǎo)出。

1.2 量測方程

SINS/GNSS 組合導(dǎo)航系統(tǒng)量測方程為

式中， Zk為量測向量，Hk為量測系數(shù)矩陣， Vk為量測噪聲向量，近似為白噪聲。量測信息選擇為捷聯(lián)慣導(dǎo)系統(tǒng)和全球衛(wèi)星導(dǎo)航系統(tǒng)的速度和位置差值，具體如下：

其中，vE,I、vN,I和vU,I是捷聯(lián)慣導(dǎo)計算得到的東向、北向和天向速度，vE,GNSS、vN,GNSS和vU,GNSS是GNSS輸出的東向、北向和天向速度； LI、λI和 hI是捷聯(lián)慣導(dǎo)計算得到的緯度、經(jīng)度和高度，LGNSS、λGNSS和 hGNSS是GNSS 輸出的緯度、經(jīng)度和高度。

2 基于指數(shù)漸消遺忘因子的自適應(yīng)濾波算法

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

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

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

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

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

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

式中，N 為窗口寬度。窗口寬度不宜設(shè)置過大，否則會產(chǎn)生較大的時延，檢測能力將變?nèi)?，?jīng)過權(quán)衡考慮，窗口寬度選取3～5 為宜。經(jīng)優(yōu)化后的故障檢測函數(shù)能夠更加準(zhǔn)確地判斷出量測信息是否出現(xiàn)異常。

2.2 Sage-Husa 自適應(yīng)濾波器改進

SHAKF 可以在進行狀態(tài)估計的同時根據(jù)量測輸出實時估計出系統(tǒng)的噪聲參數(shù)，包括系統(tǒng)噪聲均值和量測噪聲，一般認(rèn)為系統(tǒng)噪聲具有穩(wěn)定性，且在實際應(yīng)用中，量測噪聲方差陣對濾波影響較為顯著，因此通常僅對量測噪聲進行估計[16]。

系統(tǒng)狀態(tài)空間模型如下：

其中

假設(shè)濾波過程中量測噪聲方差矩陣 Rk未知或發(fā)生變化，常規(guī)卡爾曼濾波難以保證濾波精度，若能夠?qū)崟r地估計出量測噪聲方差矩陣 Rk，可以提高濾波精度。在濾波過程中，量測預(yù)測誤差公式為： 測噪聲 Vk均值為零，可知的均值也為零，且

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

式中，b 為遺忘因子，易知，越陳舊的數(shù)據(jù)，其加權(quán)系數(shù)越小，根據(jù)式(15)可構(gòu)建加權(quán)系數(shù)如下

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

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

在濾波過程中，當(dāng)量測噪聲發(fā)生變化，通過故障檢測函數(shù)對濾波狀態(tài)進行判斷，若判斷出濾波異常，遺忘因子取較小的值以提高對量測噪聲矩陣的跟蹤速度，隨著時間的推移，為了能夠更精確地估計出量測噪聲矩陣，遺忘因子的取值應(yīng)適當(dāng)增大?？紤]到遺忘因子的取值應(yīng)滿足0

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

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

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

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

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

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

3實驗與分析

3.1 仿真實驗

仿真參數(shù)設(shè)置如下：初始經(jīng)度λ =118.79 °E，緯度L=31.94° N ，水平姿態(tài)角為0°，航向角為30°，初始失準(zhǔn)角為[0.1′ 0.2′ 3′] ，參考慣導(dǎo)級IMU 性能指標(biāo)，設(shè)置仿真系統(tǒng)傳感器精度如表1所示。

表1 仿真參數(shù)Tab.1 Simulation parameters

濾波器參數(shù)設(shè)置如下：

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

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

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

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

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

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

3.2 跑車實驗

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

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

根據(jù)實測數(shù)據(jù)，分析了慣性組件（IMU）和GPS的性能指標(biāo)，具體參數(shù)如表2所示。

表2 傳感器參數(shù)Tab.2 Sensor paramete rs

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

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

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

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

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

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

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

4 結(jié) 論

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