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

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

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

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

1 組合導航系統(tǒng)非線性模型

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

(1)

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

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

(2)

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

(3)

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

(4)

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

2 組合導航UKF算法的簡化

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

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

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

(5)

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

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

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

2.2 簡化UKF的設計

增廣狀態(tài)向量可簡化為：

(6)

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

1)初始化：

(7)

2)樣點計算：

(8)

(9)

3)時間更新：

(10)

(11)

4)量測更新：

(12)

(13)

5)濾波更新：

(14)

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

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

為此，本文將序貫卡爾曼濾波算法與簡化UKF算法進行融合，進而得到適用于多傳感器組合導航系統(tǒng)的序貫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，方向正北，水平姿態(tài)角為0°，捷聯(lián)解算周期為0.02 s，濾波周期為1 s。濾波初始參數(shù)如下：三維位置誤差均為5 m，三維速度誤差均為0.1 m/s，三維姿態(tài)角誤差均為0.5°，設置傳感器精度如表1所示。

表1 仿真參數(shù)

4.2 組合導航系統(tǒng)簡化UKF性能分析

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

圖1 導航參數(shù)估計誤差的協(xié)方差曲線Fig.1 Covariance curve of parameter estimation error

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

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

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

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

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

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

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

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

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

圖5 姿態(tài)誤差曲線對比Fig.5 Attitude error curve comparison

5 結 語

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