發射系下的SINS/CNS/GNSS組合導航UKF濾波算法

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

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

1 引言

目前，組合導航系統已廣泛應用于航空、航天和航海等領域，必須以特定的坐標系作為測量載體導航參數的基礎。近地空間中，如機載導航系統建立在地理坐標系下，也是目前研究較多的一種情況[1]。文獻[2-3]針對彈載系統的導航需求，建立基于發射慣性坐標系的SINS/CNS/GNSS組合導航模型。文獻[4]針對極區范圍經線變化速度加快并聚于極點的情況，建立了基于地球坐標系的SINS/GNSS組合導航模型。隨著需求的增加，組合導航系統的研究廣度進一步擴展。

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

基于上述原因，本文直接將彈載系統在發射慣性坐標系下的三維姿態角、三維位置、三維速度構成狀態向量，并給出了三維姿態角的樣本點生成、均值預測和方差預測的生成過程，進而建立了基于發射慣性坐標系下的SINS/CNS/GNSS組合導航UKF數學模型。

2 發射系下SINS/CNS/GNSS組合導航系統數學模型

2.1 狀態方程

由于導航坐標系采用的是發射慣性坐標系，捷聯安裝的加速度計的比力方程可以描述為[2-3]

(1)

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

(2)

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

采用四元數描述載體相對于發射慣性坐標系的姿態運動，避免了姿態運算過程中的奇異點，基于陀螺角速度測量值積分推算下時刻的載體姿態可表述為

(3)

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

(4)

(5)

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

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

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

(6)

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

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

設a=[γθφ]T，根據方程(1)(2)(4)(5)及姿態角構成系統的狀態向量：

2.2 量測方程的建立

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

(7)

(8)

對式(8)進行姿態解算，即可獲得由CNS測量的本體坐標系相對于發射慣性坐標系的姿態角Z2：

Z2=a+va

(9)

式中：va為姿態測量噪聲向量。

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

(10)

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

3 組合導航系統的UKF設計

將系統噪聲W、量測噪聲V與狀態向量組成增廣向量Xa：

(11)

式中：Pa為Xa的方差陣。

3.1 UKF算法步驟

(1)步驟1：初始化

(2)步驟2：采樣點計算

i=1,…,n

(12)

i=n+1,…,2n

(13)

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

(3)步驟3：狀態均值和方差的一步預測

以式(12)(13)所描述的采樣點為初值，基于陀螺、加速度計的原始測量數據，由SINS遞推計算得到k時刻的位置、速度和姿態進而構成系統狀態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 姿態采樣點及統計量的計算[10-11]

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

其中：s和c分別代表正弦和余弦函數。由方向余弦矩陣的特性，可得姿態角各采樣點所對應的方向余弦矩陣：

(19)

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

4 仿真試驗與分析

為了驗證本文算法的性能，設計了一條導航飛行軌跡，如圖1所示。

圖1 飛行軌跡Fig.1 Flight path

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

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

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

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

圖4 姿態誤差曲線對比Fig.4 Comparison of attitude error curve

根據仿真試驗結果，可以獲得基于UKF和EKF兩種算法的各導航參數誤差的統計結果如表1所示。

表1 組合導航系統EKF、UKF算法誤差對比

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

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

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

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

5 結論

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