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

1 煙臺(tái)南山學(xué)院工學(xué)院，山東省龍口市大學(xué)路12號(hào)，265713 2 航天晨光股份有限公司，南京市天元中路188號(hào)，211100 3 北京理工大學(xué)化學(xué)與化工學(xué)院，北京市中關(guān)村南大街5號(hào)，100081

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

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

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

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

(1)

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

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

(2)

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

(3)

同理，選取GNSS輸出的三維位置、三維速度作為觀測(cè)量時(shí)，有相應(yīng)的量測(cè)方程：

(4)

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

2 組合導(dǎo)航UKF算法的簡(jiǎn)化

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

2.1 組合導(dǎo)航標(biāo)準(zhǔn)UKF算法存在的問題

在標(biāo)準(zhǔn)UKF算法中，通常將系統(tǒng)狀態(tài)、過程噪聲和量測(cè)噪聲擴(kuò)展成增廣狀態(tài)向量Xa：

(5)

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

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

同樣，在標(biāo)準(zhǔn)UKF算法中，量測(cè)更新過程通過對(duì)2n+1個(gè)樣點(diǎn)進(jìn)行非線性計(jì)算后，將運(yùn)算結(jié)果線性疊加，該過程的計(jì)算量較大。當(dāng)量測(cè)方程是線性且量測(cè)噪聲是加性時(shí)，可以對(duì)標(biāo)準(zhǔn)UKF算法進(jìn)行簡(jiǎn)化，無需對(duì)狀態(tài)量進(jìn)行增廣，以減少運(yùn)算量[9]。

2.2 簡(jiǎn)化UKF的設(shè)計(jì)

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

(6)

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

1)初始化：

(7)

2)樣點(diǎn)計(jì)算：

(8)

(9)

3)時(shí)間更新：

(10)

(11)

4)量測(cè)更新：

(12)

(13)

5)濾波更新：

(14)

3 多傳感器組合導(dǎo)航序貫UKF算法

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

為此，本文將序貫卡爾曼濾波算法與簡(jiǎn)化UKF算法進(jìn)行融合，進(jìn)而得到適用于多傳感器組合導(dǎo)航系統(tǒng)的序貫UKF算法。具體步驟如下。

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

2)樣點(diǎn)計(jì)算：與式(8)、式(9)相同。

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

4)量測(cè)更新：

(15)

(16)

5)濾波更新：

(17)

4 仿真結(jié)果及分析

4.1 仿真條件

以飛行器作為仿真對(duì)象，飛行器航跡包括加速、爬升、旋轉(zhuǎn)、降落等階段，飛行時(shí)間為3 600 s，初始經(jīng)度、緯度和高度分別為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°，設(shè)置傳感器精度如表1所示。

表1 仿真參數(shù)

4.2 組合導(dǎo)航系統(tǒng)簡(jiǎn)化UKF性能分析

采用協(xié)方差分析法來評(píng)估標(biāo)準(zhǔn)UKF算法及簡(jiǎn)化UKF算法的估計(jì)精度，圖1給出部分導(dǎo)航參數(shù)誤差對(duì)應(yīng)的估計(jì)誤差協(xié)方差曲線?？梢钥闯?，基于標(biāo)準(zhǔn)UKF算法與簡(jiǎn)化UKF算法的各導(dǎo)航參數(shù)估計(jì)誤差的協(xié)方差曲線基本重合，即二者具有相同的濾波精度。

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

為比較標(biāo)準(zhǔn)UKF算法與簡(jiǎn)化UKF算法計(jì)算的復(fù)雜度，圖2列出二者的濾波時(shí)間對(duì)比。經(jīng)統(tǒng)計(jì)，簡(jiǎn)化UKF算法最大濾波時(shí)間及平均濾波時(shí)間分別為0.016 0 s和0.011 0 s；而標(biāo)準(zhǔn)UKF算法最大濾波時(shí)間及平均濾波時(shí)間分別為0.030 0 s和0.013 5 s。考慮到捷聯(lián)解算周期為0.02 s，簡(jiǎn)化UKF算法更容易保證系統(tǒng)的實(shí)時(shí)性，其計(jì)算時(shí)間比標(biāo)準(zhǔn)UKF算法減少約18%，運(yùn)算量更小。

圖2 標(biāo)準(zhǔn)UKF與簡(jiǎn)化UKF的濾波時(shí)間對(duì)比Fig.2 Filtering time comparison between standard UKF and simplified UKF

4.3 多傳感器組合導(dǎo)航序貫UKF仿真分析

在以上分析的基礎(chǔ)上，對(duì)GNSS/CNS/SINS多傳感器組合導(dǎo)航系統(tǒng)進(jìn)行序貫UKF算法實(shí)驗(yàn)。

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

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

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

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

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

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

5 結(jié) 語(yǔ)

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