朱璐瑛 林雪原 王 萍 許家龍 陳祥光,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

CNS抗干擾能力強(qiáng)，誤差不隨時(shí)間積累，精度高達(dá)角秒級(jí)[1]，在航天飛機(jī)、載人飛船及深空探測等領(lǐng)域得到廣泛應(yīng)用[2]；同時(shí)，SINS和CNS都是完全自主的導(dǎo)航系統(tǒng)，將二者組合也是目前的研究重點(diǎn)。組合導(dǎo)航系統(tǒng)的濾波方法主要分為線性和非線性，其中線性濾波方法以常規(guī)卡爾曼濾波為代表，而非線性濾波方法包括擴(kuò)展卡爾曼濾波(EKF)、不敏卡爾曼濾波(UKF)及粒子濾波(PF)等。相對(duì)于擴(kuò)展卡爾曼濾波和粒子濾波，不敏卡爾曼濾波因具有精度高及易于實(shí)現(xiàn)的特點(diǎn)而得到廣泛應(yīng)用[3]。

目前，基于UKF的GNSS/SINS組合導(dǎo)航系統(tǒng)得到深入研究，主要原因是其速度和位置的樣本點(diǎn)、均值及方差較容易生成，且可通過線性疊加產(chǎn)生[4-6]；由于姿態(tài)角不是普通的矢量，故對(duì)基于UKF的CNS/SINS組合導(dǎo)航系統(tǒng)的研究相對(duì)繁瑣[7-8]。本文將UKF方法運(yùn)用于CNS/SINS組合導(dǎo)航系統(tǒng)中，建立其非線性狀態(tài)方程及線性量測方程，并提出了一種基于UKF的CNS/SINS組合導(dǎo)航系統(tǒng)濾波算法。

1 基于UKF的組合導(dǎo)航濾波模型

1.1 標(biāo)準(zhǔn)UKF簡介

考慮如下形式的n維離散時(shí)間非線性系統(tǒng)：

(1)

式中，f(·)、h(·)為非線性函數(shù)；k為離散時(shí)刻；Xk為k時(shí)刻的系統(tǒng)狀態(tài)，假設(shè)其方差矩陣為Pk；uk為系統(tǒng)確定性輸入項(xiàng)；Zk為量測值；Wk和Vk分別為系統(tǒng)噪聲和量測噪聲，其方差矩陣分別為Qk和Rk。

由式(1)可知，標(biāo)準(zhǔn)UKF算法包含初始化、樣點(diǎn)計(jì)算、時(shí)間更新、量測更新及濾波更新等過程。因式(1)中狀態(tài)方程及量測方程均為典型的非線性方程，故UKF中的時(shí)間更新及量測更新過程也是典型的非線性運(yùn)算，算法復(fù)雜度高。同時(shí)，標(biāo)準(zhǔn)UKF算法將系統(tǒng)狀態(tài)向量、系統(tǒng)噪聲和量測噪聲增廣為系統(tǒng)狀態(tài)，也進(jìn)一步增大了濾波算法的運(yùn)算量。

1.2 CNS/SINS組合導(dǎo)航系統(tǒng)濾波模型

結(jié)合式(1)的非線性系統(tǒng)模型，基于CNS/SINS組合導(dǎo)航系統(tǒng)的UKF算法，以導(dǎo)航參數(shù)直接作為被估計(jì)的狀態(tài)，采用捷聯(lián)慣性導(dǎo)航系統(tǒng)的機(jī)械編排方程作為狀態(tài)方程，且狀態(tài)方程無需進(jìn)行線性化處理。選取N、E、U地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，慣導(dǎo)系統(tǒng)采取指北方位機(jī)械編排的形式，其狀態(tài)方程可表示為：

(2)

依據(jù)SINS工作原理，展開式如下：

(3)

CNS的輸出可轉(zhuǎn)換為地理坐標(biāo)系下的橫滾角、俯仰角及航向角，故選取CNS輸出作為觀測量，則系統(tǒng)的量測方程可表示為：

(4)

對(duì)式(2)和式(4)進(jìn)行離散化處理，可以得到離散化的狀態(tài)方程及量測方程，且假設(shè)導(dǎo)航系統(tǒng)的先驗(yàn)狀態(tài)、過程噪聲方差和測量噪聲方差相互獨(dú)立。

1.3 CNS/SINS組合導(dǎo)航的簡化UKF算法

基于式(2)和式(4)可以看出，CNS/SINS組合導(dǎo)航系統(tǒng)具有狀態(tài)方程非線性而量測方程線性的特點(diǎn)，為此僅將系統(tǒng)狀態(tài)向量和系統(tǒng)噪聲增廣為狀態(tài)向量[9]，則簡化的系統(tǒng)增廣狀態(tài)向量為：

(5)

式中，χa、χX和χW分別為Xa、X和W的樣點(diǎn)，其維數(shù)分別定義為La、Lx和Lw，且La=Lx+Lw。

簡化后的UKF具體計(jì)算步驟如下[10-11]。

步驟1)：初始化。

(6)

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

(7)

(8)

其中，λ=α2(n+κ)-La，α、β和κ為比例因子。

步驟3)：時(shí)間更新。

(9)

(10)

步驟4)：量測更新。

(11)

而

(12)

同理，有：

PXZ=Pk|k-1HT

(13)

步驟5)：濾波更新。

對(duì)比標(biāo)準(zhǔn)UKF算法，簡化UKF算法的計(jì)算復(fù)雜度得到了降低。

2 組合導(dǎo)航系統(tǒng)中姿態(tài)融合的設(shè)計(jì)

與位置和速度不同，計(jì)算姿態(tài)與真實(shí)姿態(tài)之間的誤差不能通過簡單的線性減法來實(shí)現(xiàn)，必須借助于捷聯(lián)姿態(tài)誤差分析原理進(jìn)行。在捷聯(lián)式慣性導(dǎo)航系統(tǒng)中，用數(shù)字平臺(tái)坐標(biāo)系模擬導(dǎo)航坐標(biāo)系，由于平臺(tái)有誤差，故數(shù)字平臺(tái)坐標(biāo)系(n′)和導(dǎo)航坐標(biāo)系(n)之間存在著平臺(tái)角誤差，將這種平臺(tái)角誤差寫成列向量，可表示為[12]：

(15)

式中，φx、φy和φz分別為x、y和z方向平臺(tái)角誤差。則2個(gè)坐標(biāo)系之間的轉(zhuǎn)換矩陣為：

(16)

對(duì)捷聯(lián)式系統(tǒng)計(jì)算的姿態(tài)陣為：

(17)

即

(18)

2.1 系統(tǒng)狀態(tài)向量協(xié)方差矩陣的設(shè)計(jì)

UKF算法中，位置、速度及IMU噪聲的樣點(diǎn)可以通過式(7)進(jìn)行簡單的線性疊加得到，并且位置和速度對(duì)應(yīng)的協(xié)方差矩陣也可通過式(10)計(jì)算得到。根據(jù)捷聯(lián)導(dǎo)航原理可知，為避免姿態(tài)計(jì)算過程中出現(xiàn)奇點(diǎn)(即方程退化的問題)，需要通過四元數(shù)乘法或方向余弦矩陣等算法來計(jì)算姿態(tài)角，是典型的非線性計(jì)算。為此，本文以方向余弦矩陣為工具，計(jì)算姿態(tài)角樣點(diǎn)、均值和方差，具體過程如下。

(19)

2.2 量測值一步預(yù)測誤差的確定

(20)

根據(jù)式(16)的定義，可以得到量測值一步預(yù)測誤差為：

(21)

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

3.1 仿真條件

設(shè)定飛行器初始經(jīng)度、緯度和高度分別為118°、29°、50 m，初始航向角、俯仰角和橫滾角分別為90°、0°和0°，飛行時(shí)間為3 600 s，捷聯(lián)解算周期為0.02 s，濾波周期為1 s。濾波初始參數(shù)設(shè)置如下：三維位置誤差均為5 m，三維速度誤差均為0.1 m/s，三維姿態(tài)角誤差均為0.5°；陀螺隨機(jī)游走驅(qū)動(dòng)噪聲及陀螺白噪聲均為1°/h，加速度計(jì)隨機(jī)游走驅(qū)動(dòng)噪聲及陀螺白噪聲均為10-4g；CNS測角誤差為300″，其采樣周期為1 s。系統(tǒng)仿真在MATLAB7.1環(huán)境下進(jìn)行，仿真軌跡如圖1所示。

圖1 載體飛行軌跡Fig.1 Flight trajectory of carrier

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

根據(jù)仿真初始條件的設(shè)定，對(duì)于相同導(dǎo)航傳感器仿真的原始數(shù)據(jù)，本文分別進(jìn)行了基于下述濾波模型的仿真實(shí)驗(yàn)：1)基于常規(guī)卡爾曼濾波[7-8]；2)基于簡化UKF算法；3)基于EKF算法[13]。圖2～4分別給出3組仿真實(shí)驗(yàn)結(jié)果的位置誤差、速度誤差及姿態(tài)誤差對(duì)比曲線，可以看出，3種算法均可有效抑制位置及速度的發(fā)散，使姿態(tài)角處于收斂狀態(tài)，且本文簡化UKF算法的效果要優(yōu)于常規(guī)卡爾曼濾波及EKF算法。

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

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

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

為更加具體形象地說明3種濾波算法對(duì)姿態(tài)角的濾波效果，表1給出實(shí)驗(yàn)結(jié)果數(shù)據(jù)統(tǒng)計(jì)的對(duì)比分析?？梢钥闯觯喕疷KF算法的姿態(tài)角濾波精度比EKF算法提高了約9%，比常規(guī)卡爾曼濾波算法提高了約14%。

表1 姿態(tài)均方誤差統(tǒng)計(jì)對(duì)比

為研究本文算法對(duì)濾波器初始三維姿態(tài)角誤差的敏感度，分別在初始三維姿態(tài)角誤差為1°及300″的情況下，對(duì)三維姿態(tài)角的均方差(即平臺(tái)角誤差)進(jìn)行仿真，時(shí)長為600 s。圖5給出了平臺(tái)角誤差的協(xié)方差仿真曲線，可以看出，在2種濾波器初始三維姿態(tài)角誤差的條件下，本文算法得到的平臺(tái)角誤差協(xié)方差曲線完全重合，即本文算法對(duì)濾波器初始三維姿態(tài)角誤差的敏感度極低。為對(duì)濾波器初始階段協(xié)方差曲線的細(xì)節(jié)有更完全的描述，僅取圖5中1～4 s時(shí)間段進(jìn)行對(duì)比，具體如圖6所示。可以看出，在2種初始條件下，經(jīng)過3次濾波后本文算法得到的平臺(tái)角誤差協(xié)方差曲線完全重合，進(jìn)一步說明了本文算法對(duì)濾波器初始三維姿態(tài)角誤差的魯棒性極高。

圖5 不同濾波器初始姿態(tài)誤差角條件下的平臺(tái)誤差角協(xié)方差對(duì)比曲線Fig.5 Platform angle error covariance contrast curve under the condition of different filter initial attitude error

圖6 圖5中起始時(shí)間段內(nèi)的平臺(tái)角誤差協(xié)方差對(duì)比曲線Fig.6 Platform angle error covariance contrast curve during the starting period of Fig.5

基于圖2～4的仿真實(shí)驗(yàn)，對(duì)標(biāo)準(zhǔn)UKF算法及本文簡化UKF算法的計(jì)算量進(jìn)行對(duì)比分析。從表2可以看出，簡化UKF算法的計(jì)算時(shí)間比標(biāo)準(zhǔn)UKF算法減少了18.5%，有效地降低了算法的復(fù)雜度。

表2 算法計(jì)算量分析

4 結(jié) 語

CNS/SINS組合導(dǎo)航系統(tǒng)采用姿態(tài)融合的方式，其狀態(tài)方程具有強(qiáng)非線性、量測方程具有線性的特點(diǎn)。當(dāng)采用UKF算法對(duì)系統(tǒng)進(jìn)行濾波時(shí)，由于姿態(tài)角樣點(diǎn)、樣點(diǎn)均值和樣點(diǎn)方差的生成不同于以往GNSS/SINS組合導(dǎo)航系統(tǒng)模式，本文首先對(duì)標(biāo)準(zhǔn)UKF算法進(jìn)行簡化，在生成姿態(tài)角樣點(diǎn)的基礎(chǔ)上，借助于平臺(tái)角誤差這一概念設(shè)計(jì)了姿態(tài)角樣點(diǎn)均值及方差的生成過程；同樣，借助于平臺(tái)角誤差，設(shè)計(jì)了量測更新過程中的量測一步預(yù)測誤差生成過程。仿真結(jié)果表明，本文算法具有比常規(guī)卡爾曼濾波及EKF算法更高的導(dǎo)航精度，同時(shí)本文算法對(duì)濾波器初始三維姿態(tài)角誤差具有更高的魯棒性。