改進Sage-Husa 算法在飛機組合導航中的應用

孫淑光，溫啟新

(中國民航大學 電子信息與自動化學院，天津 300300)

0 引 言

全球衛星導航定位技術在航空領域中的應用日益廣泛.以美國的全球定位系統(GPS)、俄羅斯的格洛納斯系統(GLONASS)、歐盟的伽利略系統(Galileo)和中國的北斗衛星導航系統(BDS)為典型代表，共同組成了全球衛星導航系統(GNSS)[1].GNSS具有全天候、導航定位精度高等優點，但其信號易受遮蔽與干擾，對精確導航定位產生影響.機載慣性導航技術具有完全自主、無源性、實時性強，輸出參數全面等優點，已成為現代民航飛機、軍用飛機航電系統的重要組成部分，但其也有導航誤差隨時間快速發散、初始對準時間長等缺點[2].捷聯慣性導航系統(SINS)具有結構簡單、重量輕、可靠性高等優點，是目前最常使用的慣性導航系統.GNSS/SINS 組合導航系統結合二者的優點，使組合后系統定位精度、連續性及有效性優于單系統工作狀態[3-4].

GNSS/SINS 組合導航系統一般采用卡爾曼濾波來實現[5].傳統卡爾曼濾波需通過建立精確的系統模型與噪聲模型來保持良好的濾波精度，但在實際應用中系統狀態與干擾噪聲統計特性難以預先進行準確判斷和建模[6]，會出現濾波精度低甚至濾波發散等情況.為解決上述問題，文獻[7]應用了傳統Sage-Husa自適應濾波算法，但算法計算量大，實際應用中難以滿足濾波實時性的要求.文獻[8]和文獻[9]中對傳統Sage-Husa 自適應濾波算法進行簡化，并對系統量測噪聲進行優化處理，提高了濾波的準確性，但算法并未考慮濾波異常的情況，使其無法在濾波出現異常情況時做出及時調整.文獻[10]結合濾波異常判據的條件，對Sage-Husa 自適應濾波算法進行改進，使其適用于特定的系統工作環境，提高了濾波精度.

針對飛機全飛行過程，本文提出一種改進的Sage-Husa算法，引入飛機氣壓高度作為濾波異常判斷條件，結合GNSS/SINS 組合導航，提高定位精度，增強組合導航濾波算法自適應性.

1 GNSS/SINS 組合導航系統模型

GNSS/SINS 組合導航系統整體架構如圖1 所示.其中GNSS 接收機通過接收導航衛星信號，解算出用戶的位置與速度，SINS 通過慣性測量元件測量出用戶的比力與角速度，經過慣性導航解算得到用戶位置與速度.最后將上述兩種導航系統求得用戶位置與速度通過組合導航系統進行數據融合，并利用融合后的數據進行SINS 誤差補償，最終得到最優導航輸出信息.

圖1 GNSS/SINS 組合導航架構圖

1.1 GNSS/SINS 組合導航系統狀態方程

GNSS/SINS 組合導航系統中，選取的狀態變量包括經度誤差 δLI、緯度誤差δ λI、高度誤差δhI、東(E)、北(N)、天(U)坐標系下的E 方向速度誤差 δvIe、N 方向速度誤差 δvIn、U 方向速度誤差 δvIu、E 方向姿態角誤差 δφe、N 方向姿態角誤差 δφn、U 方向姿態角誤差 δφu、機體坐標系下三個坐標軸方向的陀螺儀漂移誤差 εx、εy、εz以及加速度計漂移誤差?x、?y、?z.狀態變量的微分方程如下：

G(t) 表達式為

W(t) 表達式為

F(t) 為系統狀態轉移矩陣，表達式為

矩陣F1(t) 各元素可由系統狀態變量的微分方程推導出,F2(t) 表達式為

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

GNSS/SINS 組合導航系統采用松組合方式，故將GNSS 解算的位置和速度與SINS 測量得到的位置和速度之差，作為組合導航系統量測信息.將飛機在導航坐標系下的真實位置設為(L,λ,h)，真實速度設為(ve,vn,vu)，SINS 解 算的位置與速度可分別為(LI,λI,hI)、(vIe,vIn,vIu).GNSS 解算的位置與速度可分別為(LG,λG,hG)、(vGe,vGn,vGu)，可得GNSS/SINS 組合導航系統量測方程為

式中：H(t) 為

量測噪聲矩陣V(t) 為

將式(18)與式(27)進行離散化，可得GNSS/SINS組合導航離散時間的系統模型為

式中：Φk,k?1為離散化后的系 統狀態一步轉移 矩陣；Γk?1為離散系統噪聲分配矩陣；Wk?1為系統噪聲矩陣；Hk為離散系統量測轉移矩陣；Vk為量測噪聲矩陣.

2 改進的Sage-Husa 自適應濾波算法

2.1 Sage-Husa 自適應濾波算法特性分析

針對離散化后的GNSS/SINS 組合導航系統模型，其系統噪聲Wk?1與 量測噪聲Vk滿足以下統計特性 ：E (Wk)=qk，D (Wk)=Qk，E (Vk)=rk，D (Vk)=Rk且.即系統噪聲Wk的均值為qk，方 差為Qk，系統量測噪聲Vk的均值為rk，方差為Rk，且兩者相互獨立.傳統卡爾曼濾波算法中，要求系統噪聲Wk?1與 量測噪聲Vk均為均值是零的高斯白噪聲(WGN)，針對噪聲對象單一，實際應用中噪聲類型復雜，噪聲統計特性難以判斷，卡爾曼濾波算法難以獲得系統狀態最優估計.

Sage-Husa 算法對系統狀態進行最優估計過程中，對系統噪聲Wk?1與 量測噪聲Vk的均值與方差進行實時估計與校正，確保系統濾波處于正常狀態.然而在飛機實際飛行過程中，當其飛行環境平穩，系統濾波正常時，無需對系統噪聲的統計特性進行校正，因此文獻[11]中引入濾波異常的判據條件：

式中：γ 為調節因子，表示濾波異常判斷的嚴格程度；vk表示量測新息序列，表示新息序列方差的理論預計值.當式(31)成立時，說明新息序列的實際方差大于理論預計值的 γ 倍，系統濾波出現異常，需對量測噪聲做出調整，保證系統正常濾波.

2.2 改進的Sage-Husa 自適應濾波算法

民用航空飛機的飛行階段可劃分為：起飛、爬升、巡航、進近、著陸階段，其中起飛和著陸時間約占飛機總飛行時間的6%，但是卻有近一半的飛行事故發生在該階段[12].另外，飛機的起飛和著陸階段，相比于爬升與巡航階段，距離地面高度較低，導航定位精度對飛行安全的影響增大.

因此，當飛機飛行高度越低時，組合導航系統濾波結果要求更加精確，保證飛行所需的定位精度.通常情況下，在WN 模型已知的前提下，卡爾曼濾波輸出為最優線性無偏估計，但是在復雜的飛行環境中，飛機受到的干擾噪聲(如：空間中的電子干擾，機場終端區由于多路徑效應產生的反射疊加噪聲等)并不能全面提前預測，對于未知噪聲建立精確數學模型并掌握其數理統計特性是非常困難的.當飛機實際飛行所受到的干擾噪聲與提前建立的卡爾曼濾波算法中的噪聲模型不匹配甚至相差特別大時，卡爾曼濾波輸出將會出現跳變、發散等濾波異常情況.

為滿足飛機在不同飛行階段中GNSS/SINS 組合導航系統濾波的精確性與自適應性，本文利用飛機氣壓高度hp，對Sage-Husa 自適應濾波算法進行改進.飛機在巡航階段，氣壓高度的參考基準面為國際標準氣壓海平面(101.325 kPa)，而當飛機進入機場管制區域時，氣壓高度的參考基準面為當地修正海平面[13]，在特定飛行階段中，氣壓高度的參考基準面具有確定性與統一性.因此，引入氣壓高度來對式(31)中的調節因子 γ進行優化，令

式中：B為大于0 的常數，a與C也為常數，a的取值范圍為1

其中，b稱為遺忘因子，一般在0.95～0.99 選取，調節因子 γ=Blogahp+C.改進后的自適應濾波算法，濾波異常判斷嚴格程度隨著氣壓高度的變化而變化.當飛機氣壓高度變低時，調節因子 γ 隨之變低，系統濾波異常判斷嚴格程度變高[14]，當其氣壓高度變高，飛機在相應飛行高度層間隔增大，可進行調節飛行狀態的空間與時間比在起飛與著陸階段更加充足，此時調節因子 γ 增大，系統濾波異常判斷嚴格程度降低，由此來增強飛機在不同飛機階段組合導航系統濾波的自適應性.改進后的Sage-Husa 自適應濾波算法流程如圖2 所示

圖2 改進后的Sage-Husa 自適應算法流程圖

3 改進Sage-Husa 自適應濾波算法仿真與結果分析

3.1 仿真模塊與仿真條件設置

本文利用從我國西南某城市到北方某城市真實機載快速數據記錄器(QAR)的數據對改進后Sage-Husa 自適應濾波算法有效性進行驗證.驗證平臺包含多個仿真模塊：SINS 模塊、GNSS 模塊、組合導航濾波模塊.QAR 數據可提供飛機位置、氣壓高度等信息，SINS 模塊提供慣性元件測量的角速度、比力信息以及慣導系統解算的、位置信息，GNSS 模塊提供導航衛星系統解算的用戶速度與位置信息，組合導航濾波模塊對兩種導航信息進行數據融合，輸出組合導航數據信息.

系統仿真時間為500 s，濾波頻率為1 Hz；陀螺儀誤差為均值為0 °/h，方差為0.03 °/h 的WGN；加速度計誤差為均值為0 g，方差為1×10?5g 的WGN；GNSS 測量偽距誤差為均值為10 m，方差為5 m 的WGN.SINS 輸出頻率為20 Hz，GNSS 輸出頻率為1 Hz.在200～210 s 內與350～360 s 內使其量測噪聲擴大到原來的5 倍與10 倍，用來模擬實際飛行環境中飛機受到噪聲干擾及量測噪聲發生突變的情況.

3.2 仿真結果分析

利用真實QAR 數據獲取驗證用飛行軌跡，如圖3所示，初始位置為北緯30.56°，東經103.94°，氣壓高度為489.51 m.

圖3 飛機實際飛行軌跡

在本文仿真條件下，SINS 與GNSS 在導航坐標系下的定位誤差如圖4、圖5 所示.

圖4 SINS 定位誤差

圖5 GNSS 定位誤差

由圖4 可知，SINS 定位誤差隨著時間增長迅速積累，在東(E)、北(N)、天(U)三個方向都存在不同程度發散，其中在系統仿真時間結束時，E 方向定位誤差發散到77.50 m，N 方向定位誤差發散到91.68 m，U 方向定位誤差發散到140.92 m.由圖5 可知，在本文條件下GNSS 水平方向定位精度要優于垂直方向的定位精度，其中東向定位誤差范圍為?20.40～24.33 m，N 方向定位誤差范圍為?24.01～25.59 m，U 方向定位誤差范圍為?33.21～31.59 m.通過仿真結果可以看出，SINS 定位誤差隨著時間的積累而發散，故不適合長時間依靠該系統進行定位，GNSS 定位誤差雖不會出現發散現象，但當可見星數量減少或出現導航信號受到遮蔽、干擾時，GNSS 定位誤差也會出現增大的現象.因此不管是SINS 還是GNSS，其單系統定位精度都不能滿足飛機飛行時定位精度要求，故需對不同導航方式進行組合，來提高飛機導航定位精度.

改進Sage-Husa 自適應濾波算法的遺忘因子選取b=0.98，調節因子 γ 為

圖6 為改進前后的調節因子變化曲線對比圖.通過引入QAR 中氣壓高度數據，對調節因子進行改進.從改進后的曲線可以看出，飛機起飛(0～50 s)、進近著陸(420～500 s)階段調節因子數值偏小，此時濾波異常判斷標準嚴格程度高.飛機在航路階段(51～419 s)時，調節因子數值偏大，濾波異常判斷標準嚴格程度變低，符合高空飛機之間間距要求較大的預期.根據氣壓高度調整濾波異常判斷標準的嚴格程度，在確保飛行安全的前提下，提高了算法的自適應性.而改進前的調節因子為常值，難以根據氣壓高度的變化對系統濾波異常判斷標準的嚴格程度進行動態調整.

圖6 調節因子變化曲線圖

在前述仿真條件下，分別進行卡爾曼濾波組合計算、改進前Sage-Husa 自適應濾波算法、改進后Sage-Husa 自適應濾波算法的組合導航解算的仿真，定位誤差的仿真結果如圖7～圖9 所示.

圖7 E 方向定位誤差

圖8 N 方向定位誤差

圖9 U 方向定位誤差

由圖7、圖8、圖9 以及表1 可知，在文中仿真條件下，在200～210 s 與350～360 s 時間段內增大量測噪聲后，卡爾曼濾波算法在E、N、U 方向定位誤差發生明顯跳變，最大值分別為32.63 m、26.32 m、78.37 m，誤差的均方根(RMS)值分別為11.25 m、12.58 m、13.32 m，且誤差值隨時間增長呈發散趨勢，定位精度較低且誤差波動范圍較大，難以滿足飛機起降階段高精度定位的要求.改進前Sage-Husa 自適應濾波算法在E、N、U 三個方向的定位誤差最大值分別為6.83 m、9.35 m、20.42 m，誤差的RMS 值下降到4.39 m、4.72 m、4.58 m，提高系統定位精度的同時，減小了誤差波動范圍.改進后的Sage-Husa 自適應濾波算法在E、N、U 三個方向定位誤差最大值分別為3.86 m、6.42 m、12.97 m，誤差的RMS 值分別為3.05 m、3.98 m、3.62 m.可以看出，Sage-Husa 自適應濾波算法在量測噪聲發生突變時，仍能使系統保持良好的濾波輸出，且對誤差發散現象起到良好的抑制效果.改進后的算法不僅降低了定位誤差，使誤差波動更加平滑，提高了定位精度，且隨著飛行氣壓高度變化，借助調節因子的改變，使其適用于復雜的飛行環境與不同飛行階段，增強了組合導航算法自適應性，可以更好地確保飛機安全平穩飛行.

表1 導航系下定位誤差比較 m

4 結束語

本文針對飛機起飛、爬升、巡航、進近、著陸不同飛行階段，利用氣壓高度對Sage-Husa 自適應濾波算法進行改進，同時對SINS 與GNSS 單系統的工作以及通過傳統卡爾曼濾波算法進行組合導航工作時的定位誤差特性進行仿真與結果分析，可獲得以下結論：

1) SINS 定位誤差隨時間而積累，長時間工作會導致其誤差發散嚴重，定位精度隨系統時間增加嚴重降低.利用GNSS 定位時，其定位誤差波動較大且導航信號易受遮蔽與干擾，不適用于在復雜電磁環境中工作，定位精度易受影響；

2) 卡爾曼濾波將SINS 與GNSS 進行組合導航，在一定程度上能夠提高定位精度，但傳統卡爾曼濾波算法對系統過程噪聲與量測噪聲的統計特性規定較嚴格，在實際復雜噪聲環境應用中存在局限性，難以保證飛機在受到干擾時，仍保持良好濾波效果；

3) 利用氣壓高度對Sage-Husa 自適應濾波算法進行改進，將判斷濾波異常嚴格程度的調節因子從原來固定不變的常數，轉變為結合飛機實際飛行氣壓高度的動態變化值，以此來滿足飛機不同飛行階段對濾波標準的不同要求，效果良好.

在本文的仿真條件下，相比于改進前的算法，改進后的Sage-Husa 自適應濾波算法在E 方向定位誤差降低43.48%，N 方向定位誤差降低31.33%，U 方向定位誤差降低36.48%，且使誤差波動更加平緩，增強自適應性的同時，提高了系統定位精度.