基于容積規則的組合導航系統容錯濾波算法

沈華健SHEN Hua-jian

（無錫機電高等職業技術學校，無錫214000）

0 引言

考慮到日趨復雜的應用環境，單獨的導航系統無法滿足高精度需求，通過組合GNSS 系統和INS 系統可以提高導航精度，組合導航系統已經成為了當今導航領域的主流技術[1，2]。

組合導航系統需要采用濾波算法進行解算，卡爾曼濾波（Kalman Filter，KF）算法是經典的導航濾波算法，但其只適用于線性系統[3]。組合導航系統具有一定的非線性特性，KF 算法會極大地降低解算精度，所以，非線性濾波算法逐漸受到學者的青睞。有學者采用了無跡卡爾曼濾波（Unscented Kalman Filter，UKF）算法進行導航濾波解算，通過無跡變換得到的2n+1 個采樣點，能夠不斷逼近濾波均值和協方差，有效地提高了導航解算精度[4]。此外，容積卡爾曼濾波（Cubature Kalman Filter，CKF）算法在進行組合導航系統解算時[5，6]，不僅提高了精度，還保證了狀態協方差的正定性，改善了濾波過程的數值穩定性。

狀態χ2檢測法和殘差χ2檢測法均有各自的不足，而雙狀態χ2檢測法能夠彌補兩者的不足，提升故障檢測的靈敏度，在導航系統容錯濾波算法領域得到較為廣泛的應用[7，8]。綜合考慮非線性導航系統模型和故障信息對導航解算精度的影響，本文設計了基于容積規則的組合導航系統容錯濾波算法。該算法采用了雙狀態χ2檢測法對導航系統進行故障診斷，根據故障檢測值分配濾波權重，降低了故障影響；同時利用了CKF 算法的容積規則，降低了非線性濾波誤差，進一步改善了容錯濾波算法的精度。

1 組合導航系統濾波模型

GNSS 系統中應用最為廣泛的是全球定位系統（Global Positioning System，GPS），我國自主研發的北斗二代（BD2）導航系統也具有很好的應用性能，為了保證高可靠性，本文采用了SINS/GPS/BD2 組合導航模式，其濾波結構如圖1 所示。

圖1 組合導航系統聯邦濾波結構

組合導航系統以SINS 為主子導航系統，通過航位推算得到SINS 的姿態、速度和位置信息，與GPS、BD2 輸出的位置信息分別組成濾波子系統，得到各自的狀態均值和誤差協方差。然后，通過故障檢測得到兩個濾波子系統的故障檢測值，根據檢測值實時分配權重信息，反饋至SINS進行校正，降低導航解算誤差，提高系統的容錯性能。

2 結合雙狀態χ2 檢測法的容錯濾波算法

無人機在空中飛行時，電磁干擾、電源失效以及機體振動等異常情況會導致故障的產生，影響量測系統的量測值，進而降低導航系統的解算值。因此，為了準確地檢測故障信息，本文引入了雙狀態χ2檢測法，結合前述章節的內容，設計了容錯濾波算法。

SINS/GPS 子系統的故障檢測結構如下圖所示：

根據圖2，故障檢測過程描述如下：

步驟1、選擇合適的時間間隔T，兩個狀態遞推器中設置相同的初始狀態值和誤差協方差。

步驟2、當濾波運行至時間t1時，開關K1打到1 處，復位狀態遞推器1，狀態遞推器2 與非線性濾波解算值開始進行雙狀態χ2檢測過程。

步驟3、當濾波運行時間在t1至t1+T 時，開關K1打到0 處，狀態遞推器1 和狀態遞推器2 都不復位，在這個過程中，狀態遞推器2 與濾波結果和完成故障檢測過程。

圖2 子系統的故障檢測結構

步驟4、當濾波運行至時間t1+T 時，開關K1打到2處，復位狀態遞推器2，狀態遞推器1 與非線性濾波解算值開始進行雙狀態χ2檢測過程。

步驟5、當濾波運行時間在t1+T 至t1+2T 時，開關K1打到0 處，狀態遞推器1 和狀態遞推器2 都不復位，在這個過程中，狀態遞推器1 與濾波結果和完成故障檢測過程。

步驟6、返回步驟2。

可以設計聯邦系統的分配系數公式如下：

上述公式中，β1和β2分別為兩個子系統的對應權重值。

Step1：初始化聯邦結構主濾波器和子濾波器的狀態估計值，誤差協方差以及雙狀態χ2檢測法的時間間隔。

Step2：SINS/GPS 和SINS/BD2 兩個子系統分別采用容積變換規則進行非線性濾波，得到x^gps，Pgps和x^bd2，Pbd2。

Step3：進行雙狀態χ2故障檢測過程，得到兩個子系統的故障檢測值qgps和qbd2。

Step4：采用權重分配算法計算兩個子系統的聯邦權重值β1和β2。

Step5：在主濾波器中進行數據融合，得到聯邦狀態估計值和誤差協方差Pm，計算公式如下：

Step6：將主濾波器估計值反饋至SINS，進行輸出校正。

通過上述循環，可實現SINS/GPS/BD2 組合導航系統的容錯濾波過程，能夠提高故障情況下的導航解算精度。

3 仿真分析

為了驗證本文所提的算法，采用了導航工具箱進行數值仿真試驗。導航工具箱可以生成無人機的空中飛行軌跡數據。此外，導航工具箱還有基準導航數據可用于進行對比分析。空中飛行軌跡如圖3 所示，主要包括起飛，爬升、加速、轉彎等一系列機動過程，總共時間為1000 秒。為了驗證故障情況下的容錯算法性能，在400～410 秒，人為增加GPS 的量測誤差，各個方向的位置誤差分別增加50m，圖3～圖4 為本文所提算法與CKF 算法的解算性能對比圖。

圖3 仿真飛行軌跡

圖4 位置誤差對比

從圖4（b）看出，經過主濾波器融合后，濾波曲線沒有很大的波動，趨于正常濾波結果，在故障消失后，二者的濾波曲線均趨于平穩，因此，采取本文所提的組合導航系統容錯算法，能夠減少故障對導航解算過程的影響，在一定程度上提高了導航精度。

4 結論

在無人機長航時飛行過程中，組合導航系統的可靠性是至關重要的。本文提出了一種基于容積規則的組合導航系統容錯濾波算法。將雙狀態χ2檢測法應用與聯邦結構中，通過兩個狀態遞推器交替校正進行故障檢測，利用容積規則進一步提高了非線性導航系統的濾波精度，有助于提高故障檢測的靈敏度；通過權重分配公式可以合理地計算兩個導航子系統的聯邦濾波分配系數，并在主濾波器中進行信息融合。仿真結果表明，本文所提算法能夠實現了導航系統的實時故障診斷和重構過程，有效地減少了故障信息對導航解算精度的影響。