基于長航時(shí)飛行器的SINS/GNSS自適應(yīng)組合導(dǎo)航算法*

陳 灃,楊鵬翔

(中國兵器工業(yè)第203研究所,西安 710065)

?

基于長航時(shí)飛行器的SINS/GNSS自適應(yīng)組合導(dǎo)航算法*

陳 灃,楊鵬翔

(中國兵器工業(yè)第203研究所,西安 710065)

當(dāng)GNSS出現(xiàn)故障時(shí),SINS/GNSS組合導(dǎo)航系統(tǒng)的輸出誤差就會(huì)增加,影響導(dǎo)航系統(tǒng)的性能。在此情況下,為了使組合導(dǎo)航系統(tǒng)的輸出更加準(zhǔn)確,文中論述了一種量測(cè)修正算法。該算法通過檢查卡爾曼濾波殘差,調(diào)整量測(cè)更新過程中觀測(cè)量的權(quán)值,對(duì)狀態(tài)量進(jìn)行修正,減少了觀測(cè)量異常對(duì)輸出結(jié)果的影響。最后對(duì)量測(cè)修正算法以及簡化的Sage-Husa自適應(yīng)算法進(jìn)行了仿真。仿真結(jié)果表明,兩種方法得到的誤差均優(yōu)于標(biāo)準(zhǔn)卡爾曼濾波。

長航時(shí)導(dǎo)航;無人飛行器;自適應(yīng)組合導(dǎo)航;算法仿真;卡爾曼濾波器

0 引言

無人機(jī)、巡飛彈等飛行器要求的導(dǎo)航時(shí)間大于30 min,而低成本的捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)無法滿足如此長時(shí)間的導(dǎo)航需求;單獨(dú)使用衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)又有被干擾或遮擋造成失效的風(fēng)險(xiǎn)。所以,一般長航時(shí)飛行器采用SINS/GNSS組合導(dǎo)航系統(tǒng)。當(dāng)GNSS誤差增加甚至失效時(shí),不同的組合導(dǎo)航算法策略對(duì)于導(dǎo)航系統(tǒng)最終輸出的誤差影響很大。文中介紹了一種簡化的Sage-Husa自適應(yīng)卡爾曼濾波和一種量測(cè)修正卡爾曼濾波,并仿真分析這兩種算法對(duì)隔離GNSS故障的效果。

1 航行軌跡設(shè)置

文中針對(duì)的是長航時(shí)飛行器的仿真,所以航行軌跡時(shí)間設(shè)置為1 049 s。航行軌跡的參數(shù)如表1所示。

設(shè)定慣性器件采樣周期Ts=10 ms,慣性導(dǎo)航算法更新周期Tm=20 ms。

利用龍格-庫塔數(shù)值解法解微分方程,可以由上表得出軌跡中各個(gè)時(shí)刻的姿態(tài)、位置、速度、陀螺角增量、加速度計(jì)比力增量信息[1]。

2 簡化的Sage-Husa卡爾曼濾波

線性條件下,系統(tǒng)的狀態(tài)方程和量測(cè)方程的簡化形式為:

(1)

Zk=HkXk+Vk

(2)

式中:Xk為k時(shí)刻系統(tǒng)狀態(tài);Zk為k時(shí)刻測(cè)量值;Φk/k-1為tk-1時(shí)刻至tk時(shí)刻的一步轉(zhuǎn)移陣;Γk-1為系統(tǒng)噪聲驅(qū)動(dòng)陣;Hk為量測(cè)陣;Vk為量測(cè)噪聲序列;Wk-1為系統(tǒng)噪聲序列。

時(shí)間更新與傳統(tǒng)卡爾曼濾波相同。

量測(cè)噪聲均方差陣遞推式為[2]:

(3)

表1 航行軌跡及參數(shù)的選取

系統(tǒng)噪聲方差陣遞推式為:

(4)

其中:

(5)

dk=(1-b)/(1-bk), 0

(6)

遺忘因子b需要根據(jù)情況設(shè)定,一般取0.95

上述流程可以估計(jì)出量測(cè)噪聲協(xié)方差陣R和過程噪聲協(xié)方差陣Q。

但實(shí)際上量測(cè)噪聲協(xié)方差陣R和系統(tǒng)噪聲協(xié)方差陣Q并不能同時(shí)被估計(jì)出來。Sage-Husa自適應(yīng)濾波算法只能在Q已知時(shí)估計(jì)出R,或者在R已知時(shí)估計(jì)出Q。一般認(rèn)為組合導(dǎo)航系統(tǒng)中系統(tǒng)噪聲具有較好的穩(wěn)定性,所以通常只對(duì)量測(cè)噪聲進(jìn)行估計(jì)[5]。

3 基于殘差 χ2檢驗(yàn)的量測(cè)修正卡爾曼濾波數(shù)據(jù)融合算法

假設(shè)卡爾曼濾波線性系統(tǒng)的輸入輸出均服從高斯分布,則其正常工作時(shí)的殘差也應(yīng)當(dāng)服從高斯分布。而當(dāng)故障發(fā)生時(shí),系統(tǒng)性能發(fā)生改變進(jìn)而造成殘差序列不服從白噪聲的特性,例如均值偏離零點(diǎn)或者方差陣偏離濾波理論值。因此可以根據(jù)殘差序列的噪聲統(tǒng)計(jì)特性來判定系統(tǒng)級(jí)的故障。

(7)

第k時(shí)刻的預(yù)測(cè)值為:

(8)

(9)

且:

(10)

而εk的方差為:

當(dāng)系統(tǒng)發(fā)生故障時(shí),殘差εk就不再是零均值白噪聲序列。因此通過檢查εk的統(tǒng)計(jì)特性就可以判別系統(tǒng)是否發(fā)生了故障。

故障檢測(cè)統(tǒng)計(jì)量:

(12)

與上節(jié)類似,λk,ε服從自由度為n的χ2分布[7],即λk,ε～χ2(n)。則故障判定準(zhǔn)則為:

(13)

式中：TD為預(yù)先設(shè)定的門限,可以由適當(dāng)?shù)奶摼怕矢鶕?jù)χ2分布函數(shù)表查出,在工程應(yīng)用中可以根據(jù)先驗(yàn)知識(shí)得出。

這里引入一個(gè)量測(cè)值調(diào)整函數(shù)f(λk,ε),利用殘差χ2檢驗(yàn)的故障檢測(cè)統(tǒng)計(jì)量λk,ε,自適應(yīng)的降低量測(cè)信息的權(quán)重,以防止局部子系統(tǒng)造成信息損失,還可以隔離故障子系統(tǒng)。

常規(guī)的卡爾曼濾波量測(cè)更新過程增加了該調(diào)整函數(shù)以后可以變?yōu)?

(14)

式中f(λk,ε)為k時(shí)刻對(duì)量測(cè)量具有修正功能的調(diào)整函數(shù)。采用“徑向基”函數(shù)構(gòu)造的調(diào)整函數(shù)為:

(15)

式中：radbas(x)=e-x2為徑向基函數(shù),其取值在[0,1]區(qū)間連續(xù)光滑。

上述量測(cè)修正卡爾曼濾波器的融合算法工作流程是,在量測(cè)更新周期內(nèi),利用殘差χ2檢驗(yàn)統(tǒng)計(jì)量λk,ε對(duì)量測(cè)量的可信度進(jìn)行檢測(cè);當(dāng)量測(cè)量不正常時(shí),依據(jù)λk,ε對(duì)量測(cè)結(jié)果進(jìn)行修正,以抑制故障對(duì)卡爾曼濾波器的影響;當(dāng)量測(cè)量恢復(fù)正常,殘差檢測(cè)結(jié)果λk,ε滿足正常要求時(shí),調(diào)整函數(shù)取值為1,卡爾曼濾波退化為常規(guī)狀態(tài)。

4 自適應(yīng)組合導(dǎo)航仿真分析

為了考察上文所述簡化的Sage-Husa自適應(yīng)濾波以及量測(cè)修正卡爾曼濾波,利用已經(jīng)生成的航行軌跡分別對(duì)χ2檢驗(yàn)、Sage-Husa、標(biāo)準(zhǔn)卡爾曼濾波進(jìn)行仿真與分析,傳感器初始誤差設(shè)置如表2。

表2 仿真誤差設(shè)置

濾波更新周期為0.1s。選擇速度和位置同時(shí)作為量測(cè)量。門限TD設(shè)定為60。為了便于比較,在本節(jié)的 χ2檢驗(yàn)、Sage-Husa、標(biāo)準(zhǔn)卡爾曼濾波仿真中,對(duì)量測(cè)量施加的白噪聲是同一組隨機(jī)數(shù)。白噪聲強(qiáng)度設(shè)置如表3所示。在以上仿真條件下,位置誤差如圖1所示,速度誤差如圖2所示,數(shù)學(xué)平臺(tái)的姿態(tài)誤差如圖3所示。

在衛(wèi)星導(dǎo)航系統(tǒng)出現(xiàn)故障的時(shí)間段(120 s、310 s、530 s左右),標(biāo)準(zhǔn)卡爾曼濾波所得的誤差曲線出現(xiàn)了較大的波動(dòng),表現(xiàn)出對(duì)于故障有較強(qiáng)的敏感性。而簡化的Sage-Husa濾波性能優(yōu)于標(biāo)準(zhǔn)卡爾曼濾波,在故障時(shí)間內(nèi)表現(xiàn)平穩(wěn)。文中所述的殘差卡方檢測(cè)量測(cè)修正卡爾曼濾波對(duì)于故障的耐受性最好,位置、速度、角度誤差均低于以上兩種方法。

表3 仿真故障設(shè)置

圖1 位置誤差

圖2 速度誤差

5 結(jié)論

標(biāo)準(zhǔn)卡爾曼濾波結(jié)構(gòu)簡單,計(jì)算速度快,在組合導(dǎo)航子系統(tǒng)均正常工作時(shí)能給出定位定速的最優(yōu)解。然而,當(dāng)子系統(tǒng)出現(xiàn)故障時(shí),這種基礎(chǔ)算法會(huì)受到較大影響。簡化的Sage-Husa濾波通過檢查殘差的情況來不停修正量測(cè)噪聲協(xié)方差陣,對(duì)于隔離子系統(tǒng)的偶然誤差有一定的作用。而殘差卡方檢測(cè)量測(cè)修正也是首先檢查殘差的情況,以此為依據(jù)構(gòu)造一個(gè)調(diào)整函數(shù),直接對(duì)量測(cè)值進(jìn)行修正,仿真結(jié)果優(yōu)于以上兩種方法。

圖3 角度誤差

[1] 嚴(yán)恭敏. 捷聯(lián)慣導(dǎo)算法及車載組合導(dǎo)航算法研究 [D]. 西安: 西北工業(yè)大學(xué), 2004.

[2] 薛麗, 高社生, 胡高歌. 自適應(yīng)Sage-Husa粒子濾波及其在組合導(dǎo)航中的應(yīng)用 [J]. 中國慣性技術(shù)學(xué)報(bào), 2013, 21(01): 84-88.

[3] 張海, 常艷紅, 車歡. 基于GPS/INS不同測(cè)量特性的自適應(yīng)卡爾曼濾波算法 [J]. 中國慣性技術(shù)學(xué)報(bào), 2010, 18(6): 696-701.

[4] 田海, 朱新巖. 一種簡化的SAGE-HUSA卡爾曼濾波 [J]. 彈箭與制導(dǎo)學(xué)報(bào), 2011, 31(1): 75-77.

[5] 魯平, 趙龍, 陳哲. 改進(jìn)的Sage-Husa自適應(yīng)濾波及應(yīng)用 [J]. 系統(tǒng)仿真學(xué)報(bào), 2007, 19(15): 3503-3505.

[6] 李旦, 秦永元, 梅春波. 組合導(dǎo)航自適應(yīng)卡爾曼濾波改進(jìn)算法研究 [J]. 測(cè)控技術(shù), 2011, 30(3): 114-116.

[7] 吳楓, 谷叢, 朱啟舉. SINS輔助GPS檢測(cè)技術(shù)在彈載組合導(dǎo)航中的應(yīng)用 [J]. 彈箭與制導(dǎo)學(xué)報(bào), 2014, 34(5): 17-20.

Self-adaptive SINS/GNSS Integrated Arithmetic Based on Long-endurance Aerial Vehicle

CHEN Feng,YANG Pengxiang

(No.203 Research Institute of China Ordnance Industries, Xi’an 710065, China)

global navigation satellite system (GNSS) breakdown makes accretion of output error of SINS (strap-down inertial navigation system)/GNSS integrated navigation system, and then degrade system performance. In order to achieve more accurate output under these circumstances, in this article, a measurement correcting arithmetic was discussed. This arithmetic checks residual error of Kalman filter, modifies status value, and minimizes negative consequence of abnormal observed quantity. Finally, simulation was made based on the measurement correcting arithmetic and a simplified Sage-Husa self-adaptive arithmetic was made. The result of the simulation shows that two methods above have better performance compared with standard Kalman filter.

long-endurance navigation; self-adaptive integrated navigation; arithmetic simulation; Kalman filter

2015-08-09

陳灃(1990-),男,陜西西安人,助理工程師,碩士,研究方向:慣性導(dǎo)航與組合導(dǎo)航。

V241.62

A