基于擴(kuò)展H∞濾波的SINS/AMM機(jī)載組合導(dǎo)航技術(shù)

申杰亮, 王 宇, 朱欣華, 蘇 巖

(南京理工大學(xué)機(jī)械工程學(xué)院, 江蘇 南京 210094)

?

基于擴(kuò)展H∞濾波的SINS/AMM機(jī)載組合導(dǎo)航技術(shù)

申杰亮, 王 宇, 朱欣華, 蘇 巖

(南京理工大學(xué)機(jī)械工程學(xué)院, 江蘇 南京 210094)

研究了飛機(jī)運(yùn)動(dòng)模型(aircraft motion model, AMM)與中、低精度捷聯(lián)慣導(dǎo)系統(tǒng)(strapdown inertial navigation system, SINS)相融合的組合導(dǎo)航技術(shù)。針對(duì)由于氣動(dòng)系數(shù)的不準(zhǔn)確所導(dǎo)致的系統(tǒng)方程的不確定性以及觀測(cè)方程和噪聲的不確定性問題,提出采用基于極小極大準(zhǔn)則的擴(kuò)展H∞濾波(extendedH∞filter, EHF)方法用于組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合。以某小型固定翼無人機(jī)為研究對(duì)象,分別從時(shí)域、頻域的角度進(jìn)行仿真實(shí)驗(yàn)的驗(yàn)證與分析。實(shí)驗(yàn)結(jié)果表明,在SINS/AMM組合導(dǎo)航過程中,與擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)相比,EHF具有更好的魯棒性,并且可以提高35%左右的導(dǎo)航精度。

飛機(jī)運(yùn)動(dòng)模型; 捷聯(lián)慣導(dǎo)系統(tǒng); 系統(tǒng)和噪聲的不確定性; 擴(kuò)展H∞濾波

0 引 言

捷聯(lián)慣導(dǎo)系統(tǒng)(strapdown inertial navigation system, SINS)與全球定位系統(tǒng)(global positioning system, GPS)的慣性/衛(wèi)星組合導(dǎo)航是目前最主要的機(jī)載導(dǎo)航系統(tǒng)。SINS輸出的導(dǎo)航參數(shù)與GPS信號(hào)融合后,實(shí)時(shí)補(bǔ)償SINS的累積誤差,可以實(shí)現(xiàn)高精度的導(dǎo)航。但GPS信號(hào)在某些情況下不穩(wěn)定,如GPS在上電的初始階段,由于無法立即搜索到足夠數(shù)量的衛(wèi)星,組合系統(tǒng)無法完成精確的數(shù)據(jù)融合;在有較大較多障礙物的環(huán)境下,GPS會(huì)因遮蔽而無法使用;另外,GPS也會(huì)受到自然界或人為的電磁干擾等。

飛機(jī)運(yùn)動(dòng)模型(aircraft motion model, AMM)基于速度、角速度和姿態(tài)微分方程,利用已知的飛機(jī)油門、舵機(jī)等控制輸入量和初始運(yùn)動(dòng)狀態(tài),可實(shí)時(shí)推算出機(jī)體的姿態(tài)、速度和位置。在機(jī)載導(dǎo)航中,SINS/AMM的組合可以在某種程度上替代SINS/GPS組合導(dǎo)航,且無需增加外部傳感器,具有低成本、自主性強(qiáng)的優(yōu)點(diǎn)。Koifman M最早于1999年提出了利用飛機(jī)運(yùn)動(dòng)模型輔助低精度慣導(dǎo)系統(tǒng)的方法,利用擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)仿真驗(yàn)證其可行性,并分析了飛行機(jī)動(dòng)特性、氣動(dòng)參數(shù)的不準(zhǔn)確性對(duì)導(dǎo)航精度的影響[1];文獻(xiàn)[2]將飛機(jī)運(yùn)動(dòng)模型和慣導(dǎo)系統(tǒng)的組合運(yùn)用到直升飛機(jī)的導(dǎo)航中,通過仿真和飛行實(shí)驗(yàn),驗(yàn)證了該組合方案可以達(dá)到較好的導(dǎo)航精度;載體運(yùn)動(dòng)模型和慣導(dǎo)系統(tǒng)組合的方法還成功地運(yùn)用到車載導(dǎo)航[3]、潛艇導(dǎo)航[4-6]以及炮彈制導(dǎo)中[7-9]。以上研究均采用EKF來進(jìn)行數(shù)據(jù)融合,但EKF無法解決由于載體運(yùn)動(dòng)模型和噪聲的不確定性所致導(dǎo)航誤差的問題,且EKF自身也包含一定的線性化誤差。

本文以某小型固定翼無人機(jī)為研究對(duì)象,設(shè)計(jì)了中、低精度SINS與AMM的組合導(dǎo)航方案。針對(duì)SINS/AMM組合導(dǎo)航中系統(tǒng)方程、量測(cè)方程以及噪聲的不確定性問題,設(shè)計(jì)了魯棒性更強(qiáng)的擴(kuò)展H∞濾波(extendedH∞filter, EHF) 算法。仿真實(shí)驗(yàn)結(jié)果表明,與EKF算法相比較,EHF可以達(dá)到更高的導(dǎo)航精度。

1 SINS/AMM組合導(dǎo)航系統(tǒng)的建立

1.1 SINS誤差模型

SINS誤差模型表示如式(1)所示,地理坐標(biāo)系n系指向北、東、地。

(1)

式中

1.2 AMM及線性化誤差模型

AMM包括動(dòng)力學(xué)模型和運(yùn)動(dòng)學(xué)模型。飛機(jī)動(dòng)力學(xué)模型以速度、角速度微分方程的形式描述了飛機(jī)在外力和外力矩作用下的運(yùn)動(dòng)規(guī)律;而飛機(jī)運(yùn)動(dòng)學(xué)模型則通過姿態(tài)、位置微分方程來描述機(jī)體繞質(zhì)心的轉(zhuǎn)動(dòng)和質(zhì)心的位移[11]。

以某常規(guī)布局的小型螺旋槳式固定翼無人機(jī)為例,速度微分方程在b系中投影為

(2)

(3)

角速度微分方程在b系中投影為

(4)

(5)

用于姿態(tài)計(jì)算的姿態(tài)角微分方程為

(6)

(7)

基于小擾動(dòng)原理,在平衡點(diǎn)處對(duì)式(7)做泰勒級(jí)數(shù)展開并僅保留線性項(xiàng),得到的包含狀態(tài)量XAMM=[δu,δw,δq,δθ]T的線性縱向運(yùn)動(dòng)誤差微分方程為

(8)

式中,FAMM為線性化過程中由式(7)計(jì)算的Jacobian矩陣,飛機(jī)縱向氣動(dòng)系數(shù)CX,CZ,Cm的不準(zhǔn)確會(huì)導(dǎo)致不確定性ΔFAMM的產(chǎn)生;WAMM=[wδu,wδw,wδq]T表示因線性化而產(chǎn)生的不確定性噪聲,可近似為白噪聲。而高度h和俯仰角θ等縱向參數(shù)的保持與控制,由基于飛機(jī)縱向通道線性化模型設(shè)計(jì)的經(jīng)典PID控制實(shí)現(xiàn),其原理如圖1所示。

圖1 飛機(jī)縱向通道控制原理圖Fig.1 Principle for aircraft’s longitudinal control

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

建立如式(9)所示的線性化的系統(tǒng)方程和觀測(cè)方程:

(9)

系統(tǒng)方程由式(1)、式(8)組成:

,

(10)

那么,SINS/AMM組合導(dǎo)航系統(tǒng)設(shè)計(jì)如圖2所示,經(jīng)過濾波器估計(jì)得到的狀態(tài)量實(shí)時(shí)閉環(huán)補(bǔ)償?shù)絊INS和AMM中。

圖2 SINS/AMM組合導(dǎo)航系統(tǒng)原理圖Fig.2 Principle for SINS/AMM integrated navigation

2 擴(kuò)展H∞濾波器的設(shè)計(jì)

由SINS/AMM組合導(dǎo)航系統(tǒng)建立過程可以看出,系統(tǒng)狀態(tài)方程、觀測(cè)方程以及系統(tǒng)噪聲和觀測(cè)噪聲均存在一定的不確定性。飛機(jī)初始?xì)鈩?dòng)系數(shù)分析誤差以及飛行條件和飛行環(huán)境的變化,使得氣動(dòng)系數(shù)C存在一定的不準(zhǔn)確性和時(shí)變性,導(dǎo)致系統(tǒng)狀態(tài)方程的不確定性ΔF;而觀測(cè)方程矩陣H由于近似計(jì)算會(huì)產(chǎn)生不確定性ΔH;系統(tǒng)和觀測(cè)噪聲同樣存在統(tǒng)計(jì)特性的不確定性,但均可視為能量有界噪聲。針對(duì)系統(tǒng)和噪聲的不確定性,本文采用魯棒性更強(qiáng)的EHF算法實(shí)現(xiàn)信息的融合。

(11)

與最小方差估計(jì)理論不同,H∞濾波旨在保證系統(tǒng)由輸入噪聲至估計(jì)誤差的傳遞函數(shù)的H∞范數(shù)最小,等價(jià)于輸入信號(hào)至輸出信號(hào)能量最大增益的最小化,使得濾波器具有更強(qiáng)的魯棒性。其相應(yīng)的代價(jià)函數(shù)[12-14]為

(12)

(13)

則EHF的遞推過程[14-15]為

(14)

(15)

(16)

(17)

(18)

θ的選取應(yīng)該滿足:

(19)

(20)

3 仿真驗(yàn)證與分析

本文仿真包含3個(gè)模塊。

(1) AMM的仿真

選取某型號(hào)的小型螺旋槳式固定翼無人機(jī),其結(jié)構(gòu)參數(shù)、氣動(dòng)參數(shù)以及螺旋槳參數(shù)標(biāo)稱值可以較精確獲得[16],如表1所示。飛機(jī)飛行軌跡設(shè)定為定高定直線無側(cè)滑勻速飛行。飛行初始狀態(tài)量、螺旋槳轉(zhuǎn)速、舵機(jī)的輸入量可經(jīng)過配平得到,如表2所示。利用飛行的初始狀態(tài)、結(jié)構(gòu)和氣動(dòng)參數(shù)以及AMM可以逐步解算出飛機(jī)的各項(xiàng)運(yùn)動(dòng)參數(shù)。仿真中忽略風(fēng)速的影響。

表1 飛機(jī)參數(shù)標(biāo)稱值設(shè)定

表2 配平狀態(tài)設(shè)定

(2) 機(jī)載SINS的仿真

取中等精度的慣性測(cè)量單元,設(shè)定陀螺的常值漂移為1(°)/h,隨機(jī)噪聲為0.5(°)/h;加速度計(jì)的常值偏置為0.1 mg,隨機(jī)噪聲為0.05 mg。不考慮慣導(dǎo)解算初始誤差。

(3) EKF、EHF濾波器的仿真

Q=diag{(0.5°/h)2(0.5°/h)2(0.5°/h)2(0.05mg)2(0.05mg)2

(0.05mg)2(1e-4m/s)2(1e-4m/s)2(1e-5rad/s)2}

R=diag{ (0.01m/s)2(0.01m/s)2(0.01m/s)2}

P0=diag{(0.2°)2(0.2°)2(0.2°)2(0.1m/s)2

(0.1m/s)2(0.1m/s)2(2e-5°)2(2e-5°)2(0.1m)2

(0.5°/h)2(0.5°/h)2(0.5°/h)2(0.1mg)2(0.1mg)2

(0.1mg)2(0.1m/s)2(0.1m/s)2(0.001rad/s)2(0.15°)2}

EHF的加權(quán)矩陣以及相應(yīng)的參數(shù)設(shè)定:

Sk=Pk;QH=5Q;RH=5R,θ=5

仿真過程中,SINS和AMM均以0.01 s為采樣周期,濾波器的濾波周期為1 s。其中,AMM的縱向氣動(dòng)參數(shù)(包括阻力系數(shù)CX、升力系數(shù)CZ、俯仰力矩系數(shù)Cm)分別使用表1中的標(biāo)稱值以及分別增加0.1%、1%的隨機(jī)誤差的不準(zhǔn)確值。

在這3種情況下,分別利用EKF和EHF算法,對(duì)比各個(gè)導(dǎo)航參數(shù)的閉環(huán)補(bǔ)償效果,如圖3～圖6所示。表3為各縱向?qū)Ш絽?shù)誤差均值絕對(duì)值的對(duì)比。

圖5 AMM x軸速度誤差比較圖6 AMM俯仰角誤差比較Fig.5 Comparison of AMM velocity error alone x-axisFig.6 Comparison of AMM pitch error

導(dǎo)航參數(shù)標(biāo)稱氣動(dòng)系數(shù)EKFEHF增加0．1%隨機(jī)誤差EKFEHF增加1%隨機(jī)誤差EKFEHF?D/(°)0．01200．01220．01200．01230．01220．0121δVnN/(m/s)0．00120．00100．00570．00350．16860．1541δVnD/(m/s)0．00260．00260．00690．00690．06590．0186δL/(°)4．3992e?67．0608e?71．8013e?51．4276e?54．7550e?44．1785e?4δh/m0．46120．19511．84111．581520．93676．8476δu/(m/s)0．00157．1524e?40．00540．00310．17390．1570δw/(m/s)3．3013e?41．4276e?40．00430．00380．00788．4641e?4δθ/(°)0．00155．9080e?40．00480．00270．29840．1516

由圖表分析可得:①隨著氣動(dòng)系數(shù)誤差的增加,導(dǎo)航誤差總體增大,主要是因?yàn)锳MM誤差的增大;②采用標(biāo)稱氣動(dòng)系數(shù)或者包含隨機(jī)誤差的氣動(dòng)系數(shù),EHF濾波的精度整體高于EKF濾波的精度,主要是由于EHF對(duì)于系統(tǒng)方程、量測(cè)方程以及噪聲的不確定性都具有更高的魯棒性;③對(duì)于采用標(biāo)稱氣動(dòng)系數(shù)的組合系統(tǒng),與EKF相比,EHF濾波后的SINS的位置誤差和AMM的速度、位置誤差均有60%左右精度的提升;④氣動(dòng)系數(shù)分別加入0.1%、1%的隨機(jī)誤差后,除了SINS的俯仰角誤差φD外,兩種情況下的導(dǎo)航精度分別有25%和45%左右的提高。即氣動(dòng)系數(shù)誤差越大,越能體現(xiàn)EHF的魯棒性;⑤相比于EKF,利用EHF算法使得SINS/AMM組合導(dǎo)航精度平均提高了35%左右。

(21)

(22)

在圖7、圖8中,藍(lán)色實(shí)線簇表示采用EKF時(shí)的傳遞函數(shù)幅頻特性曲線,紅色虛線簇則表示采用EHF時(shí)的幅頻特性。局部放大圖可以看出,在EKF或EHF的作用下,隨著縱向氣動(dòng)系數(shù)隨機(jī)誤差大小的遞增,幅頻特性曲線上移,即幅值增加。EHF的幅值總體比EKF小約25dB左右,即由輸入噪聲至狀態(tài)估計(jì)誤差的增益小于EKF。此外,EHF濾波器幅頻特性曲線的包線相對(duì)于EKF更為集中,說明在系統(tǒng)和噪聲存在不確定性的情況下,EHF具有更好的魯棒性。

圖7 輸入至輸出δh的幅頻特性Fig.

圖8 輸入至輸出δu的幅頻特性Fig.

4 結(jié) 論

本文主要研究了AMM與中、低精度的SINS的組合導(dǎo)航技術(shù)。針對(duì)SINS/AMM在融合過程中系統(tǒng)方程、觀測(cè)方程和噪聲統(tǒng)計(jì)特性的不確定性,提出了基于極小極大準(zhǔn)則的EHF組合導(dǎo)航系統(tǒng)信息融合方法,可以在衛(wèi)星信號(hào)不穩(wěn)定的情況下,實(shí)現(xiàn)低成本、自主性強(qiáng)的組合導(dǎo)航。仿真結(jié)果表明,與EKF算法相比,EHF具有更好的魯棒性,并且可提高35%左右的導(dǎo)航精度。在后續(xù)的研究中,需要根據(jù)不同的飛行環(huán)境、飛行機(jī)動(dòng)條件,針對(duì)飛機(jī)氣動(dòng)系數(shù)的不確定性問題,開展相應(yīng)的補(bǔ)償技術(shù)研究,從而進(jìn)一步提高SINS/AMM的導(dǎo)航精度。

[1]KoifmanM,Bar-ItzhackIY.Inertialnavigationsystemaidedbyaircraftdynamics[J]. IEEE Trans.on Control Systems Technology, 1999,7(4):487-493.

[2]VasconcelosJF,SilvestreC,OliveiraP,etal.EmbeddedUAVmodelandLASERaidingtechniquesforinertialnavigationsystems[J].Control Engineering Practice,2010, 18:262-278.

[3]WangJH,GaoY.Landvehicledynamics-aidedinertialnavigation[J]. IEEE Trans.on Aerospace and Electronic Systems, 2010, 46(4): 1638-1653.

[4]MorgadoM,OliveiraP,SilvestreC,etal.Embeddedvehicledyna-micsaidingforUSBL/INSunderwaternavigationsystem[J]. IEEE Trans.on Control Systems Technology, 2014, 22(1): 322-330.

[5]LiN,DingW.Model-aidedstrapdowninertialnavigationintegratedmethodforAUVbasedonH∞filtering[C]∥Proc.of the IEEE International Conference on Computational and Information Sciences, 2013: 1088-1092.

[6] ?yvindH,EinarB,OddvarH.Model-aidedinertialnavigationforunderwatervehicles[C]∥Proc.of the IEEE International Conference on Robotics and Automation, 2008:1069-1076.

[7]FresconiF,HarkinsT.Aerodynamiccharacterizationsofasymmetricandmaneuvering105mm, 120mm,and155mmfin-stabilizedprojectilesderivedfromtelemetryexperiments[C]∥Proc.of the AIAA Atmospheric Flight Mechanics Conference, 2011: 1-30.

[8]FairfaxLD,FresconiF.Affordablestateestimationusinginertialnavigationsensorarrays[C]∥Proc.of the AIAA Atmospheric Flight Mechanics Conference, 2011:1-8.

[9]FairfaxLD,FresconiFE.Cost-efficientstateestimationforprecisionprojectiles[C]∥Proc.of the AIAA Aerospace Sciences Meeting Including the New Horizons Forum and Aerospace Exposition, 2011:1-11.

[10]LuoJJ,MaWH,YuanJP,etal. Principle and application of the integrated navigation[M].Xi’an:NorthwesternPolytechnicalUniversityPublisher,2012.(羅建軍,馬衛(wèi)華,袁建平,等.組合導(dǎo)航原理與應(yīng)用[M].西安:西北工業(yè)大學(xué)出版社,2012.)

[11]WuST,FeiYH. Flight control system[M].Beijing:BeijingUniversityofAeronauticsandAstronauticsPublisher,2005.(吳森堂,費(fèi)玉華.飛行控制系統(tǒng)[M].北京:北京航空航天大學(xué)出版社,2005.)

[12]ShakedU,TheodorY.H∞-optimalestimation:atutorial[C]∥Proc.of the 31th IEEE Conference on Decision and Control, 1992:2278-2286.

[13]GaoYN,ChenJB,YangTP.Initialalignmentonmovingbaseforintegratednavigationsystembasedonrobustfilterapproach[J].Systems Engineering and Electronics, 2005,27(7):1260-1263. (高亞楠,陳家斌,楊亭鵬.基于魯棒濾波的組合導(dǎo)航系統(tǒng)動(dòng)基座對(duì)準(zhǔn)[J].系統(tǒng)工程與電子技術(shù),2005,27(7):1260-1263.)

[14]QinYY,ZhangHY,WangSH. Kalman filter and principle of integrated navigation[M].2nded.Xi’an:NorthwesternPolytechnicalUniversityPublisher,2011. (秦永元,張洪鉞,汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].2版.西安:西北工業(yè)大學(xué)出版社,2012.)

[15]WangX,ZhangSL,LiuMQ.ComparisonofKalmanfilter,H∞filterandrobustmixedKalman/H∞filter[C]∥Proc.of the 30th IEEE Chinese Control Conference,2011:3277-3281.

[16]GuillaumeJ,DucardJ. Fault-tolerant flight control and guidance systems-practical methods for small unmanned aerial vehicles[M].ChenZL,XieZGtrans.Beijing:NationalDefendIndustryPress,2012. (GuillaumeJ,DucardJ. 容錯(cuò)飛行控制與導(dǎo)航系統(tǒng)——小型無人機(jī)實(shí)用方法[M]. 陳自力,謝志剛譯. 北京: 國(guó)防工業(yè)出版社,2012.)

[17]YiDJ.Robustfilteringforintegratednavigationsystem[D].Changsha:NationalUniversityofDefenseTechnology, 2008.(易大江.組合導(dǎo)航中的魯棒濾波研究[D].長(zhǎng)沙：國(guó)防科技大學(xué)，2008.)

Airborne integrated navigation technology of SINS/AMM based on extendedH∞filter

SHEN Jie-liang, WANG Yu, ZHU Xin-hua, SU Yan

(SchoolofMechanicalEngineering,NanjingUniversityofScienceandTechnology,Nanjing210094,China)

The integrated navigation technology of aircraft motion model (AMM) with medium and low accuracy strapdown inertial navigation system (SINS) is studied. Considering the uncertainty of the system equation caused by inaccurate aerodynamic coefficient and the uncertainty of the observation equation as well as the noise, the extendedH∞filter (EHF) algorithm is proposed, which is abided by the min-max principle. The simulation, which is tested on a small fixed-wing UAV in both time and frequency domain, shows that for the SINS/AMM integrated navigation system, EHF performs better than the extended Kalman filter (EKF) in robustness and EHF brings approximately a 35% increasing in precision.

aircraft motion model (AMM); strapdown inertial navigation system (SINS); uncertainty of the system and the noise; extendedH∞filter (EHF)

2015-10-18;

2016-10-13;網(wǎng)絡(luò)優(yōu)先出版日期:2016-10-27。

國(guó)家自然科學(xué)基金(61374215)資助課題

V 249.3

A

10.3969/j.issn.1001-506X.2016.12.23

申杰亮(1989-),男,博士研究生,主要研究方向?yàn)榻M合導(dǎo)航技術(shù)。

E-mail:perfect.sjlchg2008@163.com

王 宇(1978-),男,講師,博士,主要研究方向?yàn)镸EMS慣性導(dǎo)航、衛(wèi)星導(dǎo)航、組合導(dǎo)航與控制。

E-mail:wangyu.njust@gmail.com

朱欣華(1963-),男,教授,博士,主要研究方向?yàn)閷?dǎo)航制導(dǎo)與控制、嵌入式系統(tǒng)與應(yīng)用、MEMS慣性技術(shù)。

E-mail:zhuxinhua@mail.njust.edu.cn

蘇 巖(1967-),男,教授,博士,主要研究方向?yàn)镸EMS傳感技術(shù)。

E-mail:suyan@mail.njust.edu.cn

網(wǎng)絡(luò)優(yōu)先出版地址:http://www.cnki.net/kcms/detail/11.2422.TN.20161027.1550.004.html