發(fā)射系下的SINS/CNS/GNSS組合導(dǎo)航UKF濾波算法

喬玉新，林雪原,*，張吉松，陳祥光,2

1. 煙臺南山學(xué)院，煙臺 265713 2. 北京理工大學(xué)，北京 100081

1 引言

目前，組合導(dǎo)航系統(tǒng)已廣泛應(yīng)用于航空、航天和航海等領(lǐng)域，必須以特定的坐標(biāo)系作為測量載體導(dǎo)航參數(shù)的基礎(chǔ)。近地空間中，如機(jī)載導(dǎo)航系統(tǒng)建立在地理坐標(biāo)系下，也是目前研究較多的一種情況[1]。文獻(xiàn)[2-3]針對彈載系統(tǒng)的導(dǎo)航需求，建立基于發(fā)射慣性坐標(biāo)系的SINS/CNS/GNSS組合導(dǎo)航模型。文獻(xiàn)[4]針對極區(qū)范圍經(jīng)線變化速度加快并聚于極點(diǎn)的情況，建立了基于地球坐標(biāo)系的SINS/GNSS組合導(dǎo)航模型。隨著需求的增加，組合導(dǎo)航系統(tǒng)的研究廣度進(jìn)一步擴(kuò)展。

針對基于發(fā)射慣性坐標(biāo)系下的SINS/CNS/ GNSS組合導(dǎo)航系統(tǒng)，文獻(xiàn)[2-3]將姿態(tài)四元數(shù)中的矢量誤差項(xiàng)、位置誤差、速度誤差構(gòu)成狀態(tài)向量，進(jìn)而建立EKF(擴(kuò)展卡爾曼濾波)模型，然而EKF因?yàn)槟Ｐ途€性化展開將會導(dǎo)致模型不準(zhǔn)確。為此，文獻(xiàn)[5]將粒子濾波引入該組合導(dǎo)航系統(tǒng)，但粒子濾波算法存在復(fù)雜度高和粒子退化的缺陷。文獻(xiàn)[6]建立該組合導(dǎo)航系統(tǒng)的UKF(無跡卡爾曼濾波)模型，其核心思想是將四元數(shù)中的矢量項(xiàng)、位置、速度構(gòu)成狀態(tài)向量，并利用CNS和GNSS提供的姿態(tài)與位置測量值直接構(gòu)成量測向量。然而直接控制四元數(shù)難度較大，且有可能發(fā)散[7]，該文獻(xiàn)沒有給出四元數(shù)的樣本采樣點(diǎn)、均值預(yù)測和方差預(yù)測的生成過程。

基于上述原因，本文直接將彈載系統(tǒng)在發(fā)射慣性坐標(biāo)系下的三維姿態(tài)角、三維位置、三維速度構(gòu)成狀態(tài)向量，并給出了三維姿態(tài)角的樣本點(diǎn)生成、均值預(yù)測和方差預(yù)測的生成過程，進(jìn)而建立了基于發(fā)射慣性坐標(biāo)系下的SINS/CNS/GNSS組合導(dǎo)航UKF數(shù)學(xué)模型。

2 發(fā)射系下SINS/CNS/GNSS組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型

2.1 狀態(tài)方程

由于導(dǎo)航坐標(biāo)系采用的是發(fā)射慣性坐標(biāo)系，捷聯(lián)安裝的加速度計的比力方程可以描述為[2-3]

(1)

基于方程(1)，位置可由速度直接積分得到

(2)

式中：p為載體在發(fā)射系中的位置；υ為載體在發(fā)射系中的速度。

采用四元數(shù)描述載體相對于發(fā)射慣性坐標(biāo)系的姿態(tài)運(yùn)動，避免了姿態(tài)運(yùn)算過程中的奇異點(diǎn)，基于陀螺角速度測量值積分推算下時刻的載體姿態(tài)可表述為

(3)

由于在發(fā)射慣性坐標(biāo)系中飛行器的導(dǎo)航工作時間均較短，陀螺與加速度計的測量誤差可簡化建模成“隨機(jī)游走+白噪聲”的形式[5]，即陀螺與加速度計的實(shí)際輸出ωc、fc為

(4)

(5)

式中：fb、ωb分別為加速度計、陀螺輸出的真值；fr、ωr分別為加速度計、陀螺隨機(jī)游走誤差；fn、ωn分別為加速度計、陀螺隨機(jī)游走驅(qū)動噪聲；fε、ωε分別為加速度計、陀螺測量噪聲，可看作白噪聲。

對四元數(shù)方程(3)求解，可得[8-9]

qk=e0.5[Δθ]qk-1=[λ0λ1λ2λ3]T

(6)

式中：λ0為四元數(shù)的標(biāo)量部分，λ1、λ2、λ3為四元數(shù)的矢量分量；[Δθ]可表示為

由方程(6)及姿態(tài)方向余弦矩陣的關(guān)系，可求載體相對于發(fā)射慣性坐標(biāo)系的姿態(tài)角：γ(橫滾角)、θ(俯仰角)、φ(航向角)。

設(shè)a=[γθφ]T，根據(jù)方程(1)(2)(4)(5)及姿態(tài)角構(gòu)成系統(tǒng)的狀態(tài)向量：

2.2 量測方程的建立

由于GNSS位置測量值pe一般描述在地球固連坐標(biāo)系中，經(jīng)過如下變換可得到在發(fā)射慣性坐標(biāo)系中的位置測量值Z1：

(7)

(8)

對式(8)進(jìn)行姿態(tài)解算，即可獲得由CNS測量的本體坐標(biāo)系相對于發(fā)射慣性坐標(biāo)系的姿態(tài)角Z2：

Z2=a+va

(9)

式中：va為姿態(tài)測量噪聲向量。

結(jié)合方程(7)(9)，則有如下的量測方程：

(10)

式中：H=[I6×606×9]；V為量測噪聲向量。

3 組合導(dǎo)航系統(tǒng)的UKF設(shè)計

將系統(tǒng)噪聲W、量測噪聲V與狀態(tài)向量組成增廣向量Xa：

(11)

式中：Pa為Xa的方差陣。

3.1 UKF算法步驟

(1)步驟1：初始化

(2)步驟2：采樣點(diǎn)計算

i=1,…,n

(12)

i=n+1,…,2n

(13)

式中：n為Xa的維數(shù)；λ=α2(n+κ)-n，α、β和κ均為比例因子。

(3)步驟3：狀態(tài)均值和方差的一步預(yù)測

以式(12)(13)所描述的采樣點(diǎn)為初值，基于陀螺、加速度計的原始測量數(shù)據(jù)，由SINS遞推計算得到k時刻的位置、速度和姿態(tài)進(jìn)而構(gòu)成系統(tǒng)狀態(tài)Xi,k/k-1，i=0,1,…,2n，則

(14)

(15)

(4)步驟4：量測更新

Zi,k/k-1=HXi,k/k-1+V，i=0,1,…,2n

定義：

(16)

(17)

則

Kk=Pxz(Pzz)-1

(18)

3.2 姿態(tài)采樣點(diǎn)及統(tǒng)計量的計算[10-11]

第2.1小節(jié)步驟3中，位置、速度及慣性測量器件(IMU)噪聲的采樣點(diǎn)可以通過式(12)(13)線性疊加到各自均值的方式得到。而姿態(tài)角的計算是典型的非線性計算，SINS中姿態(tài)角的計算必須通過四元數(shù)與姿態(tài)矩陣得到以避免奇異值的發(fā)生，為此姿態(tài)角采樣點(diǎn)也必須通過四元數(shù)與姿態(tài)矩陣得到，并進(jìn)一步計算其均值和方差。

其中：s和c分別代表正弦和余弦函數(shù)。由方向余弦矩陣的特性，可得姿態(tài)角各采樣點(diǎn)所對應(yīng)的方向余弦矩陣：

(19)

根據(jù)式(19)，求得分布于a0=[γ0θ0φ0]T周邊的另外2n個姿態(tài)采樣點(diǎn)。

4 仿真試驗(yàn)與分析

為了驗(yàn)證本文算法的性能，設(shè)計了一條導(dǎo)航飛行軌跡，如圖1所示。

圖1 飛行軌跡Fig.1 Flight path

仿真環(huán)境為Matlab7.01，發(fā)射原點(diǎn)設(shè)定為(118°(E),32°(N),100 m)，發(fā)射初始方位角為30°，發(fā)射時刻設(shè)定為2020-03-23T02-03-00，仿真時長為900 s。捷聯(lián)解算周期設(shè)定為0.02 s，濾波周期為1 s[2-3, 5-6]。陀螺隨機(jī)游走驅(qū)動噪聲及陀螺白噪聲均設(shè)定為0.01 (°)/s，加速度計隨機(jī)游走驅(qū)動噪聲及加速度計白噪聲均設(shè)定為0.000 1g(g=9.8 m/s2)；GNSS定位誤差設(shè)定為10 m，CNS測姿誤差設(shè)定為20"。

對于相同的IMU、GNSS和CNS原始數(shù)據(jù)，分別應(yīng)用UKF算法及EKF算法[2-3]對數(shù)據(jù)進(jìn)行處理，得到位置誤差曲線、速度誤差曲線、姿態(tài)誤差曲線分別如圖2～圖4所示。

圖2 位置誤差曲線對比Fig.2 Comparison of position error curve

圖3 速度誤差曲線對比Fig.3 Comparison of velocity error curve

圖4 姿態(tài)誤差曲線對比Fig.4 Comparison of attitude error curve

根據(jù)仿真試驗(yàn)結(jié)果，可以獲得基于UKF和EKF兩種算法的各導(dǎo)航參數(shù)誤差的統(tǒng)計結(jié)果如表1所示。

表1 組合導(dǎo)航系統(tǒng)EKF、UKF算法誤差對比

從表1可以看出，在相同的仿真條件下，相對于EKF算法，利用UKF求得的各個導(dǎo)航參數(shù)所對應(yīng)的誤差無論是在誤差最大值、誤差最小值、誤差均值及誤差均方差方面都有較大的改善。例如對于誤差均方差統(tǒng)計項(xiàng)，UKF可提高精度約20%～30%。其中的主要原因是，組合導(dǎo)航系統(tǒng)是一種典型的非線性系統(tǒng)，且載體處于高速、高機(jī)動狀態(tài)時，UKF因?yàn)槭且环N非線性算法，可以避免因EFK線性化展開而引起的模型失真現(xiàn)象?？梢哉fUKF的高精度特性是以較大的計算量為代價的。

為了考證UKF算法的實(shí)時性，在Intel Core i5-7200U CPU 2.70 Hz處理器環(huán)境下，在相同的原始數(shù)據(jù)條件下，UKF和EKF算法在每次濾波過程中的計算量進(jìn)行對比，如圖5所示。

如前所述，濾波周期為1 s，捷聯(lián)解算的周期為0.02 s，由圖5可以看出EKF算法每次濾波的耗時小于0.005 s(小于捷聯(lián)解算周期)，滿足系統(tǒng)實(shí)時性的要求；而UKF算法每次濾波的耗時小于0.015 s(小于捷聯(lián)解算周期)，故UKF算法也能滿足系統(tǒng)實(shí)時性的需求。

圖5 算法計算量對比Fig.5 Comparison of algorithm calculation amount

5 結(jié)論

彈載等飛行器的導(dǎo)航系統(tǒng)經(jīng)常以發(fā)射慣性坐標(biāo)系為基準(zhǔn)，且導(dǎo)航系統(tǒng)是典型的非線性系統(tǒng)，其常用的濾波方法為EKF。為了避免EKF濾波算法因線性化展開而引起的導(dǎo)航精度下降問題，本文在設(shè)計了發(fā)射慣性坐標(biāo)系下的SINS/CNS/GNSS組合導(dǎo)航系統(tǒng)模型的基礎(chǔ)上，采用UKF算法對導(dǎo)航參數(shù)進(jìn)行直接估計，該算法避免對姿態(tài)四元數(shù)進(jìn)行直接估計而引起的對姿態(tài)四元數(shù)控制過于復(fù)雜的問題。仿真結(jié)果表明，相較于EKF算法，UKF算法能獲得更高精度的導(dǎo)航結(jié)果，且姿態(tài)誤差穩(wěn)定，同時算法能滿足系統(tǒng)實(shí)時性的需求。