基于序貫UKF的GNSS/CNS/SINS組合導航最優融合算法

林雪原 王 萍 許家龍 劉立寧 陳祥光,3

1 煙臺南山學院工學院，山東省龍口市大學路12號，265713 2 航天晨光股份有限公司，南京市天元中路188號，211100 3 北京理工大學化學與化工學院，北京市中關村南大街5號，100081

無跡卡爾曼濾波(unscented Kalman filter，UKF)采用不敏變換原理，是一種基于確定性樣點計算的非線性濾波方法，相對于擴展卡爾曼濾波(extended Kalman filter，EKF)及粒子濾波(particle filter，PF)算法，具有計算精度高、算法便于實現的優點，在目標跟蹤、組合導航等領域得到較深入的研究[1-2]。目前，對于UKF算法的研究大多局限于線性濾波的范疇[3-4]，并未發揮出UKF非線性濾波的優勢。

GNSS/CNS/SINS組合導航系統作為一種輔助信息源全面的多傳感器組合導航系統[5-6]，以狀態方程線性化為基礎，常用的濾波方法為聯邦卡爾曼濾波[7]。本文針對GNSS/CNS/SINS組合導航系統，引入序貫卡爾曼濾波思想，提出一種多傳感器組合導航系統的序貫UKF最優融合算法。

1 組合導航系統非線性模型

一般情況下，非線性系統的離散模型表示為：

(1)

式中，f(·)、h(·)為非線性函數；Xk為系統狀態向量，其方差矩陣為Pk；k為當前離散時刻；uk為系統確定性控制項或輸入項；Zk為根據輔助導航傳感器而確定的量測向量；Wk和Vk分別為過程噪聲向量和量測噪聲向量，其方差矩陣分別為Qk和Rk。

基于非線性濾波模型的GNSS/CNS/SINS組合導航系統直接選取導航參數作為被估計的狀態，采用捷聯慣性導航系統的力學方程作為狀態方程，選取N、E、U地理坐標系作為導航坐標系，其狀態方程可表示為[8]：

(2)

選取CNS輸出的三維姿態作為觀測量，相應的量測方程可表示為：

(3)

同理，選取GNSS輸出的三維位置、三維速度作為觀測量時，有相應的量測方程：

(4)

假設系統的先驗狀態、過程噪聲及測量噪聲相互獨立，測量噪聲向量Vj(t)的方差矩陣為Rj(t)(j=1,2)。對式(2)～(4)進行離散化，即可得到其離散化的非線性模型。

2 組合導航UKF算法的簡化

標準UKF算法針對的是狀態方程及量測方程均為非線性的系統模型，而式(2)建立了非線性狀態方程，式(3)、式(4)建立了線性量測方程，且量測噪聲是加性的。下面以Z(t)=H(t)X(t)+V(t)涵蓋式(3)、式(4)兩個量測方程，以方便問題的討論。

2.1 組合導航標準UKF算法存在的問題

在標準UKF算法中，通常將系統狀態、過程噪聲和量測噪聲擴展成增廣狀態向量Xa：

(5)

式中，χa為Xa的樣點向量。假設χX、χW和χV(Xk、Wk和Vk)的維數分別為LX、Lw、Lv，則χa(Xa)的維數n=Lx+Lw+Lv，且La=Lx+Lw。

標準UKF算法的樣點個數為2n+1，復雜度為O[(2n+1)3]；另，標準UKF算法將不敏變換(unsensitive transformation，UT)應用于狀態方程與量測方程，使計算量進一步加大。顯然，當Lx、Lw、Lv的值較高時，標準UKF運算量的增加非常顯著，對于系統的實時性不利[9]。

同樣，在標準UKF算法中，量測更新過程通過對2n+1個樣點進行非線性計算后，將運算結果線性疊加，該過程的計算量較大。當量測方程是線性且量測噪聲是加性時，可以對標準UKF算法進行簡化，無需對狀態量進行增廣，以減少運算量[9]。

2.2 簡化UKF的設計

增廣狀態向量可簡化為：

(6)

式中，χa的維數定義為La，即La=Lx+Lw。具體計算步驟如下。

1)初始化：

(7)

2)樣點計算：

(8)

(9)

3)時間更新：

(10)

(11)

4)量測更新：

(12)

(13)

5)濾波更新：

(14)

3 多傳感器組合導航序貫UKF算法

為了將UKF算法應用于多傳感器組合導航系統，最直觀的方法是借鑒集中式卡爾曼濾波器，即將所有傳感器的量測信息在一步量測更新中同時加以處理。該方法的優點是便于工程實現，但缺點在于一旦某個傳感器失效，錯誤數據將污染整個濾波器，同時引起計算量增加、容錯性差及通信負擔重等新問題。為了解決該問題，可借鑒分布式濾波、序貫濾波等思想。

為此，本文將序貫卡爾曼濾波算法與簡化UKF算法進行融合，進而得到適用于多傳感器組合導航系統的序貫UKF算法。具體步驟如下。

1)初始化：與式(7)相同。

2)樣點計算：與式(8)、式(9)相同。

3)時間更新：與式(10)、式(11)相同。

4)量測更新：

(15)

(16)

5)濾波更新：

(17)

4 仿真結果及分析

4.1 仿真條件

以飛行器作為仿真對象，飛行器航跡包括加速、爬升、旋轉、降落等階段，飛行時間為3 600 s，初始經度、緯度和高度分別為118°E、29°N、50 m，方向正北，水平姿態角為0°，捷聯解算周期為0.02 s，濾波周期為1 s。濾波初始參數如下：三維位置誤差均為5 m，三維速度誤差均為0.1 m/s，三維姿態角誤差均為0.5°，設置傳感器精度如表1所示。

表1 仿真參數

4.2 組合導航系統簡化UKF性能分析

采用協方差分析法來評估標準UKF算法及簡化UKF算法的估計精度，圖1給出部分導航參數誤差對應的估計誤差協方差曲線。可以看出，基于標準UKF算法與簡化UKF算法的各導航參數估計誤差的協方差曲線基本重合，即二者具有相同的濾波精度。

圖1 導航參數估計誤差的協方差曲線Fig.1 Covariance curve of parameter estimation error

為比較標準UKF算法與簡化UKF算法計算的復雜度，圖2列出二者的濾波時間對比。經統計，簡化UKF算法最大濾波時間及平均濾波時間分別為0.016 0 s和0.011 0 s；而標準UKF算法最大濾波時間及平均濾波時間分別為0.030 0 s和0.013 5 s。考慮到捷聯解算周期為0.02 s，簡化UKF算法更容易保證系統的實時性，其計算時間比標準UKF算法減少約18%，運算量更小。

圖2 標準UKF與簡化UKF的濾波時間對比Fig.2 Filtering time comparison between standard UKF and simplified UKF

4.3 多傳感器組合導航序貫UKF仿真分析

在以上分析的基礎上，對GNSS/CNS/SINS多傳感器組合導航系統進行序貫UKF算法實驗。

為驗證本文算法的有效性，基于相同導航傳感器仿真原始數據，分別進行3種濾波方案的處理：1) 集中式線性卡爾曼濾波方法(簡稱集中式KF算法)；2)基于文獻[3-4，8]的集中UKF(簡稱集中線性UKF算法)；3)本文算法(簡稱序貫UKF算法)。圖3～5為采用上述3種濾波算法求解得到的位置、速度及姿態誤差曲線。

根據圖3～5得到各導航參數的均方根誤差(RMSE)統計結果如表2所示，可以看出，相對于集中線性UKF算法及集中式KF算法，序貫UKF算法可分別降低32%和40%的位置誤差，13.5%、19%的速度誤差及20.3%、25.8%的姿態誤差。

表2 組合導航各參數均方根誤差對比

圖3 位置誤差曲線對比Fig.3 Position error curve comparison

圖4 速度誤差曲線對比Fig.4 Speed error curve comparison

圖5 姿態誤差曲線對比Fig.5 Attitude error curve comparison

5 結 語

針對GNSS/CNS/SINS多傳感器組合導航系統，在建立非線性狀態方程及線性量測方程的基礎上，設計簡化UKF算法，降低標準UKF算法的計算量；通過引入序貫卡爾曼濾波原理，提出多傳感器組合導航系統的序貫UKF最優融合算法。仿真實驗結果表明，該算法不僅具有較低的計算量，同時具有較高的濾波精度，可為動態環境下多傳感器組合導航系統的非線性估計提供一種有效的解決方法。