一種簡化的發(fā)射系下SINS/GPS/CNS組合導(dǎo)航系統(tǒng)無跡卡爾曼濾波算法

潘加亮，熊智，王麗娜，郁豐，趙慧，林愛軍

(1.南京航空航天大學(xué) 自動(dòng)化學(xué)院，江蘇 南京210016;2.北京航天自動(dòng)控制研究所，北京100854;3.南京航空航天大學(xué) 航天學(xué)院，江蘇 南京210016)

0 引言

構(gòu)建發(fā)射慣性系下的捷聯(lián)慣導(dǎo)系統(tǒng)/導(dǎo)航星全球定位系統(tǒng)/天文尋航系統(tǒng)(SINS/GPS/CNS)多信息組合導(dǎo)航系統(tǒng)是實(shí)現(xiàn)未來高超、空天高動(dòng)態(tài)飛行器高精度導(dǎo)航的有效手段之一［1-2］，而發(fā)射慣性系下組合導(dǎo)航系統(tǒng)的核心技術(shù)是多信息融合處理方法。針對(duì)未來高超、空天飛行器飛行軌跡高動(dòng)態(tài)性和導(dǎo)航系統(tǒng)狀態(tài)強(qiáng)非線性等特點(diǎn)，傳統(tǒng)的擴(kuò)展卡爾曼濾波(EKF)算法因系統(tǒng)模型線性化展開的要求而會(huì)影響估計(jì)的精度［3］。為此，Julier 等提出的無損卡爾曼濾波(UKF)方法可以有效解決模型線性化展開所帶來的模型不準(zhǔn)確這一問題［4-9］，其核心思想是通過利用一系列近似高斯分布的采樣點(diǎn)，通過無跡變換(UT)進(jìn)行狀態(tài)和誤差協(xié)方差陣的遞推和更新，不需要對(duì)系統(tǒng)非線性方程進(jìn)行線性化，因而可以有效避免引入線性化誤差，提高導(dǎo)航精度。

但對(duì)于高階擴(kuò)維的多組合導(dǎo)航系統(tǒng)，由于組合系統(tǒng)狀態(tài)維數(shù)過高，傳統(tǒng)的UKF 算法存在大量采樣粒子計(jì)算過程，遞推濾波計(jì)算步驟繁瑣、運(yùn)算復(fù)雜等問題，影響系統(tǒng)的實(shí)時(shí)性，使其不便于工程實(shí)現(xiàn)。為此，針對(duì)發(fā)射慣性系下SINS/GPS/CNS 組合導(dǎo)航系統(tǒng)狀態(tài)模型非線性而觀測(cè)模型線性的特點(diǎn)，本文設(shè)計(jì)了一種簡化UKF 算法(SUKF)直接進(jìn)行導(dǎo)航參數(shù)的估計(jì)，在算法模型等效原則下可以簡化計(jì)算步驟，降低計(jì)算的復(fù)雜度，提高算法的實(shí)時(shí)性，同時(shí)SUKF算法由于繼承了UKF 算法無需狀態(tài)模型一階線性化展開的優(yōu)點(diǎn)，能獲得比EKF 算法更高精度的導(dǎo)航結(jié)果，十分適合算法的工程應(yīng)用。

1 發(fā)射系下組合導(dǎo)航系統(tǒng)SUKF 算法

1.1 組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型

本文選取發(fā)射慣性系作為導(dǎo)航坐標(biāo)系，SINS/GPS/CNS 組合導(dǎo)航系統(tǒng)相關(guān)解算方程如下:

式中:q 為姿態(tài)四元數(shù)(包括一維標(biāo)量部分q0和三維矢量部分q1、q2、q3);p 為發(fā)射慣性系下的位置值;v 為發(fā)射慣性系下的速度值;g 為地球的萬有引力;為發(fā)射慣性系相對(duì)于載體系的姿態(tài)矩陣;為發(fā)射慣性系相對(duì)于地心慣性系的姿態(tài)矩陣;fc為加速度實(shí)際測(cè)量值;fr為加速度計(jì)隨機(jī)游走誤差;ωc為角速度實(shí)際測(cè)量值;ωr為陀螺隨機(jī)游走誤差;ωε為陀螺測(cè)量噪聲;ωn、fn分別為陀螺和加速度計(jì)隨機(jī)游走驅(qū)動(dòng)噪聲。

考慮到姿態(tài)的變化主要反映在四元數(shù)的矢量部分，同時(shí)為了減少濾波器的計(jì)算量，本文選取方程(1)式中的姿態(tài)四元數(shù)三維矢量部分(q1、q2、q3)、三維位置矢量p、三維速度矢量v、三維陀螺隨機(jī)游走誤差ωr、三維加速度計(jì)隨機(jī)游走誤差fr作為狀態(tài)量構(gòu)建濾波器狀態(tài)方程，狀態(tài)量定義:X =［q1q2q3pxpypzvxvyvzωrxωryωrzfrxfryfrz］T，系統(tǒng)白噪聲矢量為

W=［ωεxωεyωεzωnxωnyωnzfnxfnyfnz］T.

選取GPS 和星敏感器分別輸出的位置信息和姿態(tài)信息(矢量部分)作為觀測(cè)量，則有觀測(cè)方程:

式中:qc，13為姿態(tài)觀測(cè)量的矢量部分;qI，13為慣導(dǎo)系統(tǒng)姿態(tài)參數(shù)的矢量部分;qε，13為姿態(tài)量測(cè)噪聲;pc為位置觀測(cè)量;pI為慣導(dǎo)系統(tǒng)的位置參數(shù);pε為位置量測(cè)噪聲;H=［I6×606×9］;V 為量測(cè)噪聲陣。

由上述分析，可以獲得發(fā)射系下組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測(cè)方程一般形式表示為

式中:狀態(tài)量X 維數(shù)為15 維;系統(tǒng)噪聲向量W 為9 維;系統(tǒng)量測(cè)噪聲向量V 為6 維。

1.2 SUKF 算法實(shí)現(xiàn)方案

發(fā)射慣性系下的SINS/GPS/CNS 組合導(dǎo)航系統(tǒng)狀態(tài)方程具有非線性，基于UT 的UKF 濾波算法是處理非線性問題的一種有效方法［10-14］，傳統(tǒng)UKF算法需要將系統(tǒng)噪聲和量測(cè)噪聲都增廣為系統(tǒng)狀態(tài)，不失一般性，假設(shè)系統(tǒng)狀態(tài)維數(shù)為n 維，系統(tǒng)狀態(tài)噪聲陣為w 維，系統(tǒng)量測(cè)噪聲陣為v 維，則增廣后的系統(tǒng)狀態(tài)維數(shù)將由原來的n 維擴(kuò)展為n +w +v維，擴(kuò)維后的系統(tǒng)狀態(tài)向量表示如下:

在我國計(jì)劃生育的變遷過程中，政策專家發(fā)揮了不可替代的作用。傳統(tǒng)媒體報(bào)道進(jìn)一步放大了某些代表學(xué)者的聲音，甚至使之成為一類政策主張的“符號(hào)”；新媒體則賦予了更多專家以話語權(quán)，進(jìn)一步拓展了他們的話語空間，使得專家學(xué)者共同體的話語影響力得到強(qiáng)化。

式中:Xa表示擴(kuò)維后的系統(tǒng)狀態(tài)向量;χa為擴(kuò)維后的狀態(tài)對(duì)應(yīng)的采樣點(diǎn)向量;^xa0表示擴(kuò)維后系統(tǒng)狀態(tài)初始均值;Pa0為增廣后的狀態(tài)向量初始協(xié)方差陣;P0為原狀態(tài)向量初始協(xié)方差陣;Q 為系統(tǒng)噪聲陣;R為量測(cè)噪聲陣。

從擴(kuò)維后的系統(tǒng)狀態(tài)向量可以看出，如果系統(tǒng)自身的狀態(tài)噪聲和量測(cè)噪聲維數(shù)較高，則通過相應(yīng)的狀態(tài)量增廣處理后，進(jìn)一步增大了狀態(tài)維數(shù)，從而較大增加了濾波算法運(yùn)算量，如以本文研究的發(fā)射慣性系組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型為例，按照傳統(tǒng)UKF算法狀態(tài)擴(kuò)維后，其狀態(tài)維數(shù)將由15 維增加到30 維，從而極大增加了算法的計(jì)算量。

為此，在系統(tǒng)狀態(tài)自身維數(shù)較高的情況下，為減少UKF 算法計(jì)算量，結(jié)合發(fā)射系下組合導(dǎo)航系統(tǒng)量測(cè)方程線性的特點(diǎn)，本文設(shè)計(jì)了一種降維的SUKF實(shí)現(xiàn)算法，主要對(duì)傳統(tǒng)UKF 算法中^zk/k-1、Pzz和Pxz的計(jì)算方法進(jìn)行簡化，具體簡化部分對(duì)應(yīng)的簡化原理如下:

1.2.1 初始化

基于發(fā)射慣性系下系統(tǒng)狀態(tài)方程非線性而量測(cè)方程線性這一特點(diǎn)，將系統(tǒng)噪聲增廣為狀態(tài)向量，而不把量測(cè)噪聲增廣為狀態(tài)向量，這樣可以有效降低擴(kuò)維后系統(tǒng)狀態(tài)的維數(shù)，設(shè)簡化后的UKF 算法的增廣狀態(tài)向量記為Xa，相應(yīng)的采樣點(diǎn)向量記為χa，則可得

1.2.2 系統(tǒng)量測(cè)方程

由傳統(tǒng)的UKF 算法公式，則有

基于發(fā)射慣性系下SINS/GPS/CNS 組合導(dǎo)航系統(tǒng)線性量測(cè)方程，將(4)式代入(5)式、(6)式得

1.2.3 系統(tǒng)估計(jì)誤差方差陣

基于傳統(tǒng)UKF 算法模型有

式中:R 表示系統(tǒng)量測(cè)噪聲陣;PA1k/k-1表示由Pk/k-1的前六行與前六列構(gòu)成的分塊矩陣;表示由Pk/k-1的前十五行與前六列構(gòu)成的分塊矩陣，

根據(jù)上述分析，傳統(tǒng)UKF 算法中的計(jì)算式(6)式、(9)式和(10)式可分別由(8)式、(11)式和(12)式代替，簡化了矩陣運(yùn)算維數(shù)，從而大大降低了計(jì)算的復(fù)雜度。由此可以得到改進(jìn)后的SUKF 算法基本計(jì)算流程如圖1所示，其中n 為增廣后的系統(tǒng)狀態(tài)向量維數(shù)，λ =α2(n +k)-n，α 決定采樣點(diǎn)距均值的遠(yuǎn)近程度，通常被賦一個(gè)較小的正值，本文取α2=0.002，k≥0 保證方差陣的半正定性，本文取0，β 用于包含狀態(tài)量分布的高階成分信息，本文取2.

圖1 SUKF 算法計(jì)算流程Fig.1 Calculation procedure of simplified unscented Kalman filter algorithm

2 仿真實(shí)驗(yàn)與分析

為有效驗(yàn)證本文算法的性能，在MATLAB 2009Ra 環(huán)境下，對(duì)分別采用UKF、SUKF 和EKF 濾波方法的發(fā)射慣性系下的SINS/GPS/CNS 組合導(dǎo)航系統(tǒng)性能進(jìn)行對(duì)比分析，同時(shí)還將UKF 與SUKF 的計(jì)算量進(jìn)行了對(duì)比分析。

2.1 仿真條件

導(dǎo)彈發(fā)射初始經(jīng)度、緯度、高度為:118°、32°、0 m，初始航向角為90°，發(fā)射時(shí)間為2014年6月1日0 時(shí)0 分0 秒，飛行時(shí)間為600 s;捷聯(lián)解算周期為0.02 s，濾波周期為1 s;仿真中設(shè)置捷聯(lián)慣性傳感器仿真參數(shù)為:陀螺隨機(jī)游走驅(qū)動(dòng)噪聲0.2°/h，陀螺白噪聲0.2°/h，加速度計(jì)隨機(jī)游走驅(qū)動(dòng)噪聲0.000 1 g，加速度計(jì)白噪聲0.000 1 g;衛(wèi)星接收機(jī)和星敏感器仿真參數(shù)為:衛(wèi)星導(dǎo)航位置誤差15 m，星光跟蹤儀誤差21″.

2.2 仿真分析

2.2.1 標(biāo)準(zhǔn)彈道飛行航跡

為有效驗(yàn)證本文算法性能，設(shè)計(jì)了一條標(biāo)準(zhǔn)導(dǎo)彈飛行軌跡如圖2所示。仿真誤差曲線如下所示，圖3為采用UKF、SUKF 與EKF 算法求解得到的姿態(tài)誤差曲線對(duì)比圖，圖4為采用UKF、SUKF 與EKF算法求解得到的位置誤差曲線對(duì)比圖，圖5為采用UKF、SUKF 與EKF 算法求解得到的速度誤差曲線對(duì)比圖。并根據(jù)相應(yīng)的仿真數(shù)據(jù)，可以獲得各類導(dǎo)航參數(shù)的均方誤差RMS 統(tǒng)計(jì)結(jié)果如表1所示。

圖2 導(dǎo)彈航跡圖Fig.2 Missile track

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

同時(shí)估計(jì)誤差協(xié)方差陣曲線如圖6所示，由于姿態(tài)、位置和速度的三軸誤差參數(shù)規(guī)律大體一致，在此給出滾動(dòng)角誤差、Y 軸位置誤差和Z 軸速度誤差對(duì)應(yīng)的估計(jì)誤差協(xié)方差參數(shù)。從圖6中可以看出，達(dá)到穩(wěn)態(tài)時(shí)，采用UKF、SUKF 與EKF 算法求解得到的滾動(dòng)角估計(jì)誤差協(xié)方差參數(shù)為2.362 rad，2.362 rad，3.774 rad;采用UKF、SUKF 與EKF 算法求解得到的Y 軸位置估計(jì)誤差協(xié)方差參數(shù)為3.038 m，3.038 m，4.166 m;采用UKF、SUKF 與EKF 算法求解得到的Z 軸速度估計(jì)誤差協(xié)方差參數(shù)為0.078 m/s，0.078 m/s，0.206 m/s.

從以上誤差曲線對(duì)比圖、誤差RMS 統(tǒng)計(jì)表以及估計(jì)誤差協(xié)方差參數(shù)可以看出，SUKF 繼承了UKF 精度高這一優(yōu)點(diǎn)，其濾波精度較EKF 有顯著的提升。

圖4 位置誤差曲線對(duì)比圖Fig.4 Position error curves

圖5 速度誤差曲線對(duì)比圖Fig.5 Velocity error curves

對(duì)SUKF 和UKF 算法的計(jì)算量進(jìn)行對(duì)比分析，如表2所示。

從表2中還可以看出在同樣的仿真計(jì)算條件下，SUKF 算法的計(jì)算時(shí)間大約比傳統(tǒng)UKF 算法減少了18%，有效降低了計(jì)算的復(fù)雜度。

表1 組合導(dǎo)航系統(tǒng)UKF、SUKF 與EKF 算法誤差對(duì)比表Tab.1 UKF，SUKF and EKF RMS errors of integrated navigation system

表2 算法計(jì)算量分析(低動(dòng)態(tài))Tab.2 Calculated amount (low dynamic range)

圖6 估計(jì)誤差協(xié)方差參數(shù)(低動(dòng)態(tài))Fig.6 Estimation error covariance matrix parameters(low dynamic range)

2.2.2 高動(dòng)態(tài)飛行航跡

為有效驗(yàn)證本文算法的性能，還設(shè)計(jì)了一條高動(dòng)態(tài)環(huán)境下導(dǎo)彈飛行軌跡如圖7所示，包含了高速姿態(tài)機(jī)動(dòng)等過程。仿真誤差曲線如下所示，圖8為采用UKF、SUKF 與EKF 算法求解得到的姿態(tài)誤差曲線對(duì)比圖，圖9為采用UKF、SUKF 與EKF 算法求解得到的位置誤差曲線對(duì)比圖，圖10 為采用UKF、SUKF 與EKF 算法求解得到的速度誤差曲線對(duì)比圖。并根據(jù)相應(yīng)的仿真數(shù)據(jù)，可以獲得各類導(dǎo)航參數(shù)的均方誤差RMS 統(tǒng)計(jì)結(jié)果，如表3所示。

圖7 導(dǎo)彈航跡圖(高動(dòng)態(tài))Fig.7 Missile track(high dynamic range)

圖8 姿態(tài)誤差曲線對(duì)比圖(高動(dòng)態(tài))Fig.8 Attitude error curves(high dynamic range)

估計(jì)誤差協(xié)方差陣曲線如圖11 所示，從圖11中可以看出，達(dá)到穩(wěn)態(tài)時(shí)，采用UKF、SUKF 與EKF算法求解得到的滾動(dòng)角估計(jì)誤差協(xié)方差參數(shù)為2.362 rad，2.362 rad，3.773 rad;采用UKF、SUKF 與EKF 算法求解得到的Y 軸位置估計(jì)誤差協(xié)方差參數(shù)為3.028 m，3.028 m，4.158 m;采用UKF、SUKF 與EKF 算法求解得到的Z 軸速度估計(jì)誤差協(xié)方差參數(shù)為0.079 m/s，0.079 m/s，0.206 m/s.

表3 組合導(dǎo)航系統(tǒng)UKF、SUKF 與EKF 算法誤差對(duì)比表(高動(dòng)態(tài))Tab.3 UKF，SUKF and EKF RMS errors of integrated navigation system

圖9 位置誤差曲線對(duì)比圖(高動(dòng)態(tài))Fig.9 Position error curves(high dynamic range)

從以上誤差曲線對(duì)比圖、誤差RMS 統(tǒng)計(jì)表以及估計(jì)誤差協(xié)方差參數(shù)可以看出，在高動(dòng)態(tài)環(huán)境下，SUKF 依然具備UKF 精度高這一優(yōu)點(diǎn)，其濾波精度較EKF 有顯著的提升。

對(duì)SUKF 和UKF 算法的計(jì)算量進(jìn)行對(duì)比分析如表4所示。

從表4中還可以看出，在高動(dòng)態(tài)的仿真計(jì)算條件下，SUKF 算法的計(jì)算時(shí)間大約比傳統(tǒng)UKF 算法減少了17.7%，有效降低了計(jì)算的復(fù)雜度。

圖10 速度誤差曲線對(duì)比圖(高動(dòng)態(tài))Fig.10 Velocity error curves(high dynamic range)

表4 算法計(jì)算量分析(高動(dòng)態(tài))Tab.4 Calculated amount (high dynamic range)

3 結(jié)論

彈載等高動(dòng)態(tài)環(huán)境下組合導(dǎo)航系統(tǒng)狀態(tài)方程具有強(qiáng)非線性，且各狀態(tài)相互耦合影響，傳統(tǒng)的EKF算法由于需要對(duì)模型進(jìn)行線性化展開從而影響導(dǎo)航系統(tǒng)精度，而傳統(tǒng)的UKF 算法又因組合導(dǎo)航系統(tǒng)維數(shù)過大而存在濾波過程計(jì)算復(fù)雜和計(jì)算量大的不足，不利于工程實(shí)現(xiàn)。為此，本文針對(duì)發(fā)射慣性系下SINS/GPS/CNS 組合導(dǎo)航系統(tǒng)狀態(tài)模型非線性而觀測(cè)模型線性的特點(diǎn)，設(shè)計(jì)了一種SUKF 算法對(duì)組合導(dǎo)航系統(tǒng)狀態(tài)參數(shù)直接進(jìn)行估計(jì)，從而簡化了UKF算法計(jì)算步驟，降低了計(jì)算的復(fù)雜度。算法仿真結(jié)果表明SUKF 算法具有比EKF 算法更高的導(dǎo)航精度，同時(shí)也有效減少了傳統(tǒng)UKF 算法的計(jì)算量，從而為解決高動(dòng)態(tài)環(huán)境下非線性濾波估計(jì)問題提供了一種有效的方法。

圖11 估計(jì)誤差協(xié)方差參數(shù)(高動(dòng)態(tài))Fig.11 Estimation error covariance matrix parameters(high dynamic range)

References)

［1］Wang R，Xiong Z，Liu J，et al. SINS/GPS/CNS information fusion system based on improved Huber filter with classified adaptive factors for high-speed UAVs［C］∥Proceeding of the 2012 IEEE/ION Position，Location and Navigation Symposium (PLANS).Myrtle Beach，South Cardina:IEEE，2012:441 -446.

［2］Hu H D，Huang X L.SINS/CNS/GPS integrated navigation algorithm based on UKF［J］. Journal of Systems Engineering and Electronics，2010，21(1):102 -109.

［3］江曉東，謝京穩(wěn)，郭軍海. 基于UKF 的再入彈道高精度估計(jì)方法研究［J］. 航天控制，2011，29(3):28 -32.JIANG Xiao-dong，XIE Jing-wen，GUO Jun-hai.The precision estimation method of reentry target trajectory based on the UKF［J］.Aerospace Control，2011，29(3):28 -32.(in Chinese)

［4］Wang Q T，Xiao D. The research and application of robust UKF algorithm for GPS/SINS integrated system［J］. Journal of Convergence Information Technology，2011，6(6):202 -211.

［5］Majeed M，Kar I N. Aerodynamic parameter estimation using adaptive unscented Kalman filter［J］. Aircraft Engineering and Aerospace Technology，2013，85(4):267 -279.

［6］羅楠，許錄平，張華. 基于UKF 和信息融合的航天器自主導(dǎo)航方法［J］. 中國空間科學(xué)技術(shù)，2012，32(2):1 -9.LUO Nan，XU Lu-ping，ZHANG Hua. Method of autonomous celestial navigation based on UKF and information fusion［J］. Chinese Space Science and Technology，2012，32(2):1 -9.(in Chinese)

［7］Zhang H T. Unscented Kalman filter and its nonlinear application for tracking a moving target［J］. Optik-International Journal for Light and Electron Optics，2013，13(3):4468 -4471.

［8］Kol?s S，F(xiàn)oss B A，Schei T S. Constrained nonlinear state estimation based on the UKF approach［J］. Computers and Chemical Engineering，2009，33(8):1386 -1401.

［9］周丕森，鮑其蓮. 組合導(dǎo)航系統(tǒng)UKF 濾波算法設(shè)計(jì)［J］. 上海交通大學(xué)學(xué)報(bào)，2009，43(3):389 -392.ZHOU Pei-sen，BAO Qi-lian. Design of filter in micro-integrated navigation system［J］. Journal of Shanghai Jiaotong University，2009，43(3):389 -392.(in Chinese)

［10］Luo Z，F(xiàn)ang H J. Modified state prediction algorithm based on UKF［J］. Journal of Systems Engineering and Electronics，2013，24(1):135 -140.

［11］Liu J，Ma J. Pulsar/CNS integrated navigation based on federated UKF［J］. Journal of Systems Engineering and Electronics，2010，21(4):675 -681.

［12］Ge Z X，Yang Y M，Zheng H. A new UKF based fault detection method in non-linear systems［J］. International Journal of Plant Engineering and Management，2006，11(3):179 -183.

［13］Rhudy M，Gu Y，Jason V，et al. Evaluation of matrix square root operations for UKF within a UAV GPS/INS sensor fusion application［J］. International Journal of Navigation and Observation，2011:1 -11.

［14］Nowak T，Eidloth A. Dynamic multipath mitigation applying unscented Kalman filters in local positioning systems［J］. International Journal of Microwave and Wireless Technologies，2011，3(3):365 -372.