趙捍東 ，李志鵬，王　芳

(1.中北大學(xué)機(jī)電工程學(xué)院,山西 太原 030051；2.北京理工大學(xué)機(jī)電學(xué)院，北京 100081；3.江南工業(yè)集團(tuán)有限公司工藝技術(shù)研究所，湖南 湘潭 411207)

?

基于慣性/地磁的彈體組合測(cè)姿方法

趙捍東1，李志鵬2，王芳3

(1.中北大學(xué)機(jī)電工程學(xué)院,山西 太原 030051；2.北京理工大學(xué)機(jī)電學(xué)院，北京 100081；3.江南工業(yè)集團(tuán)有限公司工藝技術(shù)研究所，湖南 湘潭 411207)

摘要:針對(duì)單一傳感器測(cè)姿精度有限和容錯(cuò)性不佳的問(wèn)題，提出了基于慣性/地磁傳感器的彈體姿態(tài)組合測(cè)量方法。該方法使用矢量因子聯(lián)邦濾波算法對(duì)測(cè)量數(shù)據(jù)進(jìn)行融合，并針對(duì)系統(tǒng)中存在的模型誤差和噪聲不嚴(yán)格符合高斯分布的問(wèn)題，使用了改進(jìn)的強(qiáng)跟蹤無(wú)跡卡爾曼濾波算法作為聯(lián)邦濾波的子濾波器。仿真結(jié)果表明，本文方法可以獲得比任何單一傳感器都高的估計(jì)精度，并比傳統(tǒng)聯(lián)邦濾波算法的融合精度更高。

關(guān)鍵詞:姿態(tài)估計(jì)；組合測(cè)姿；聯(lián)邦濾波；無(wú)跡濾波；強(qiáng)跟蹤濾波

0引言

隨著作戰(zhàn)形式的變化，簡(jiǎn)易制導(dǎo)彈藥因其較低的使用成本和較高的打擊精度成為戰(zhàn)場(chǎng)新寵，而彈體姿態(tài)的準(zhǔn)確測(cè)量又對(duì)簡(jiǎn)易制導(dǎo)彈藥的有效控制具有重要作用，所以提高彈體姿態(tài)的測(cè)量精度具有十分重要的意義。

慣性傳感器和地磁傳感器都是重要的彈體姿態(tài)測(cè)量?jī)x器，但使用單一傳感器進(jìn)行姿態(tài)測(cè)量時(shí)，測(cè)姿精度和容錯(cuò)性都不佳，為了獲得更高的姿態(tài)測(cè)量精度，文獻(xiàn)[1]提出了使用陀螺和地磁傳感器的彈丸姿態(tài)測(cè)量方法，但是這種方法僅僅利用陀螺數(shù)據(jù)作為地磁解算的初值，對(duì)數(shù)據(jù)的利用并不充分。文獻(xiàn)[2-3]基于融合算法給出了多傳感器組合測(cè)量方法，對(duì)各傳感器的輸出信息利用的比較充分，也取得了很高的測(cè)量精度，但其使用傳感器較多而不適合低成本簡(jiǎn)易制導(dǎo)彈藥使用。同時(shí)，文獻(xiàn)[2-3]中的聯(lián)邦濾波融合算法作為廣泛應(yīng)用的分散式融合方法[4-9]，其標(biāo)量式因子分配方法并不能最大限度的發(fā)揮這一融合算法的優(yōu)勢(shì)。同時(shí)就子濾波器而言，噪聲不嚴(yán)格符合高斯分布或系統(tǒng)模型存在誤差也會(huì)影響濾波精度。本文針對(duì)此問(wèn)題，提出了基于慣性傳感器和地磁傳感器的組合測(cè)姿方法，并在融合算法上做了改進(jìn)。

1組合測(cè)姿原理

1.1姿態(tài)測(cè)量原理

在慣性傳感器中，進(jìn)行姿態(tài)測(cè)量的器件主要是陀螺儀，陀螺儀通過(guò)敏感載體的角速率，再進(jìn)行積分，便可得到載體的當(dāng)前姿態(tài)角。統(tǒng)計(jì)規(guī)律表明，陀螺的輸出誤差主要由一階馬爾可夫隨機(jī)漂移、隨機(jī)白噪聲、常值漂移誤差組成。其誤差模型為[2-3,11]：

ε=εb+εr+ωg

(1)

式(1)中,εb為隨機(jī)常數(shù)，εr為一階馬爾科夫過(guò)程噪聲，ωg為陀螺隨機(jī)噪聲漂移。由此可以看出，陀螺的輸出噪聲是十分復(fù)雜的，這也給應(yīng)用中的濾波估計(jì)帶來(lái)了難度。

地磁傳感器測(cè)姿的原理是三軸地磁傳感器敏感載體在地磁場(chǎng)中的傾角，再轉(zhuǎn)化到慣性坐標(biāo)系內(nèi)姿態(tài)角[1]。地磁場(chǎng)是地球本身的固有物理屬性，穩(wěn)定性好，不易人為干擾?；趯?duì)地磁的測(cè)量進(jìn)行測(cè)姿，已經(jīng)在低軌衛(wèi)星、無(wú)人機(jī)、艦船上得到了廣泛的應(yīng)用[2-3]。地磁測(cè)姿誤差不累積，但受到地磁建模誤差，異磁場(chǎng)干擾，載體干擾的影響，總的誤差比較復(fù)雜。

考慮到MEMS陀螺的角速率測(cè)量精度較高，且價(jià)格功耗等都較低，同時(shí)磁強(qiáng)計(jì)的角度測(cè)量誤差不隨時(shí)間累積，本文便利用磁強(qiáng)計(jì)和MEMS陀螺進(jìn)行姿態(tài)測(cè)量，使其優(yōu)勢(shì)互補(bǔ)?；诖艔?qiáng)計(jì)和陀螺的姿態(tài)解算原理，在文獻(xiàn)[1-2]中有詳細(xì)的描述，本文主要在文獻(xiàn)[1]的現(xiàn)有工作的基礎(chǔ)上，考慮將磁強(qiáng)計(jì)和陀螺的測(cè)量數(shù)據(jù)進(jìn)行融合，以便充分利用二者的信息，提高彈體姿態(tài)測(cè)量的精度。在成本上，本文所提出的方法只是在算法上實(shí)現(xiàn)，硬件上與文獻(xiàn)[1]相比并沒(méi)有新增，所以本文方法是一種提高精度、控制成本的方法。

1.2基于聯(lián)邦濾波的融合原理

MEMS陀螺和磁強(qiáng)計(jì)是彈體姿態(tài)測(cè)量的常用傳感器，但目前為止只能進(jìn)行分散式融合，而無(wú)法將陀螺儀和磁強(qiáng)計(jì)做成一個(gè)傳感器，實(shí)現(xiàn)深度組合。而聯(lián)邦濾波又是最常用最有效的分散式融合模式，所以本文中使用聯(lián)邦濾波作為融合框架。

從算法結(jié)構(gòu)上看，聯(lián)邦濾波結(jié)構(gòu)中的各個(gè)子濾波器從主濾波器中分享最優(yōu)融合后的狀態(tài)信息[4-5]，并將從主濾波器分配到的信息作為下一步濾波融合的基準(zhǔn)值，這就可以在最大程度上抑制陀螺誤差的累積。聯(lián)邦濾波的經(jīng)典結(jié)構(gòu)如圖1所示[4,6,9]。

圖1　聯(lián)邦濾波融合結(jié)構(gòu)Fig.1　the federated filter fusing frame

對(duì)于一般的離散系統(tǒng)，我們可以將其系統(tǒng)狀態(tài)模型描述為：

X(k)=Φ(k/k-1)X(k-1)+W(k)

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

上式中的βi稱(chēng)為信息分配系數(shù)，并且符合以下約束條件：

(10)

其中：0≤βi≤1, i=1,2,…,n，βm為主濾波器上的信息分配系數(shù)，也可稱(chēng)為分配因子[7-8]。從式(3)—式(9)中可以看出，在聯(lián)邦濾波融合中，每個(gè)子濾波器的信息都得到了有效的利用。

實(shí)際計(jì)算時(shí)，對(duì)于每一個(gè)子濾波器，主濾波器到子濾波器的信息反饋方式一般取為[5-6]：

(11)

(12)

那么在MEMS陀螺/磁強(qiáng)計(jì)組合測(cè)量彈體姿態(tài)的應(yīng)用中，陀螺和磁強(qiáng)計(jì)分別敏感彈體的姿態(tài)，并分別對(duì)二傳感器的輸出量進(jìn)行濾波，之后根據(jù)二傳感器對(duì)應(yīng)的子濾波器的方差估計(jì)的大小在主濾波器中進(jìn)行姿態(tài)融合。

1.3子濾波器設(shè)計(jì)

在聯(lián)邦濾波融合算法中，主濾波器進(jìn)行信息融合和分配，而真正的濾波過(guò)程在子濾波器中完成，所以子濾波器的性能對(duì)最終的融合結(jié)果有一定的決定性作用。

雖然卡爾曼濾波器的使用十分廣泛，但狀態(tài)方程具有一定的非線性并且噪聲并不嚴(yán)格符合高斯分布，將會(huì)嚴(yán)重影響經(jīng)典卡爾曼濾波器的性能，所以工程中常使用無(wú)跡卡爾曼濾波器(Unscented Kalman Filter, UKF)進(jìn)行濾波的實(shí)現(xiàn)，本文中便使用UKF作為子濾波器。

(13)

其中λ=α2(n+κ)-n，同時(shí)定義對(duì)應(yīng)的權(quán)值為：

(14)

X(k)=f(X(k-1))+W(k-1)

(15)

Z(k)=h(X(k))+V(k)

(16)

上式中，W,V分別為狀態(tài)誤差和量測(cè)噪聲，k表示第k次觀測(cè)。那么基于UKF的濾波過(guò)程可表示為：

1)初始化過(guò)程：

(17)

(18)

2)預(yù)測(cè)更新：

xi(k/k-1)=f(χi(k-1))

(19)

(20)

zi(k/k-1)=h(xi(k/k-1))

(21)

(22)

(23)

3)量測(cè)更新：

(24)

(25)

(26)

(27)

P(k)=P*(k)-K(k)PZ(k)Z(k)KT(k)

(28)

子濾波器完成狀態(tài)估計(jì)和方差估計(jì)后，將信息輸入主濾波器，進(jìn)行最優(yōu)融合，以提高最終的姿態(tài)估計(jì)精度，所以說(shuō)子濾波器的估計(jì)精度和主濾波器的融合精度，都對(duì)最終的姿態(tài)估計(jì)有影響。本文中根據(jù)現(xiàn)有研究成果，對(duì)子濾波器和主濾波器都進(jìn)行了改進(jìn)。

2改進(jìn)的組合測(cè)姿

2.1改進(jìn)的強(qiáng)跟蹤無(wú)跡卡爾曼濾波

UKF在進(jìn)行非線性問(wèn)題的濾波時(shí)具有獨(dú)特的優(yōu)點(diǎn)，是當(dāng)前應(yīng)用最為廣泛的非線性濾波方法。但模型的不確定性和異常擾動(dòng)也會(huì)在很大程度上影響UKF的性能。為此，我們引入了強(qiáng)跟蹤UKF方法，并進(jìn)行了簡(jiǎn)化的改進(jìn)。強(qiáng)跟蹤原理原本是用在擴(kuò)展卡爾曼濾波器中的一項(xiàng)改進(jìn)，文獻(xiàn)[13]將這一原理應(yīng)用到UKF中，形成強(qiáng)跟蹤UKF，即STUKF，但該強(qiáng)跟蹤UKF變換次數(shù)多，所以本文對(duì)文獻(xiàn)[13]中的強(qiáng)跟蹤UKF進(jìn)行了簡(jiǎn)化改進(jìn)。改進(jìn)過(guò)程主要針對(duì)上文中的式(26)—式(28)，改進(jìn)過(guò)程如下：

(29)

(30)

P(k)=λ(k)P*(k)-K(k)PZ(k)Z(k)KT(k)

(31)

上式中的λ(k)為對(duì)應(yīng)于k時(shí)刻的漸消因子，其計(jì)算方法為：

(32)

(33)

(34)

N(k)=Vr(k)-βR(k) ，R(k) 為觀測(cè)系統(tǒng)噪聲，r為觀測(cè)量預(yù)測(cè)殘差。

(35)

其中0<ρ≤1為遺忘因子，β為弱化因子[13-16]。

強(qiáng)跟蹤改進(jìn)的濾波器因?yàn)椴捎昧藦?qiáng)迫正交原理，可以有效的削弱建模誤差和不確定性噪聲帶來(lái)的不利影響[13-14]，這對(duì)實(shí)際應(yīng)用具有十分重要的意義。

2.2改進(jìn)的聯(lián)邦濾波融合結(jié)構(gòu)

由于采用了信息反饋機(jī)制，可以使得聯(lián)邦濾波獲得最優(yōu)的融合效果，所以信息分配系數(shù)便在一定程度上決定了最后的融合精度。傳統(tǒng)的信息分配系數(shù)的求法，如式(14)，僅考慮了子濾波器方差的特性。由于每個(gè)子系統(tǒng)獲得的觀測(cè)量不同，那么對(duì)不同子系統(tǒng)的可觀測(cè)度就不相同?？捎^測(cè)性越好，子系統(tǒng)對(duì)應(yīng)的估計(jì)值就越精確，于是，文獻(xiàn)[9]提出了一種將系統(tǒng)可觀測(cè)度信息引入求解分配系數(shù)的過(guò)程中，對(duì)分配系數(shù)進(jìn)行改進(jìn)后，本文中基于聯(lián)邦濾波的組合測(cè)姿原理可表示為圖2所示。

圖2　改進(jìn)的INS/地磁組合測(cè)量模型Fig.2　novel INS/magnetometer based integrated estimation model

其中，Ai=diag(ai1,ai2,…,ain) ，

i=1,2,…,N;j=1,2,…n,

σij為可觀測(cè)度矩陣的奇異值[9]。

3仿真與分析

為了驗(yàn)證本文方法的有效性，基于以下較大的誤差信息進(jìn)行仿真試驗(yàn)。MEMS陀螺測(cè)量誤差0.5°，常值漂移為5(°)/h，隨機(jī)漂移5(°)/h；地磁姿態(tài)誤差1°，并每隔30s添加一次幅值為5°的異常干擾。陀螺初始姿態(tài)誤差分別為，1.5°，1°，-2°，基于某火箭彈彈道進(jìn)行仿真，并設(shè)定角運(yùn)動(dòng)狀態(tài)方程的系數(shù)誤差為2%。分別以傳統(tǒng)方法和本文方法進(jìn)行姿態(tài)估計(jì)的仿真對(duì)比，其中傳統(tǒng)方法為經(jīng)典UKF作為子濾波器的一般聯(lián)邦濾波方法，這也是工程上較為常用的一種方法。仿真時(shí)間600s，誤差統(tǒng)計(jì)如圖3、圖4所示。

圖3　姿態(tài)角誤差統(tǒng)計(jì)Fig.3　attitude error statistics

圖4　姿態(tài)角速率誤差統(tǒng)計(jì)Fig. 4　attitude angle velocity error statistics

從圖中可以直觀的看出，本文方法比傳統(tǒng)的聯(lián)合濾波算法具有更高的濾波精度，并且誤差初期誤差收斂更快。這主要是因?yàn)楸疚乃褂玫膹?qiáng)跟蹤改進(jìn)可以有效解決模型誤差、異常擾動(dòng)和非高斯噪聲的問(wèn)題。并且從誤差曲線中可以看出，最終的姿態(tài)角誤差小于任何一個(gè)單傳感器的輸出誤差，也說(shuō)明了多傳感組合測(cè)姿的優(yōu)越性。統(tǒng)計(jì)解算過(guò)程中的姿態(tài)角誤差，如表1所示。

表1　姿態(tài)角估計(jì)的誤差統(tǒng)計(jì)

在工程應(yīng)用中，使用陀螺和磁強(qiáng)計(jì)的組合，可以充分利用陀螺和磁強(qiáng)計(jì)的優(yōu)點(diǎn)，從而提供更精確的姿態(tài)測(cè)量數(shù)據(jù)。

4結(jié)論

本文提出了基于MEMS陀螺和磁強(qiáng)計(jì)的彈體姿態(tài)組合測(cè)量方法，該方法使用了矢量分配因子型的聯(lián)邦濾波算法對(duì)測(cè)量數(shù)據(jù)進(jìn)行融合，并引入了改進(jìn)的強(qiáng)跟蹤無(wú)跡卡爾曼濾波器作為子濾波器，提高了復(fù)雜噪聲環(huán)境下的融合精度，仿真結(jié)果表明，本文方法在三軸方向上的姿態(tài)估計(jì)精度均高于任何一個(gè)傳感器的測(cè)量精度，并且比傳統(tǒng)估計(jì)方法精度更高。因?yàn)樽訛V波器的估計(jì)精度很大程度上決定了最終的融合精度，采用適用范圍更廣的粒子濾波器作為子濾波器將是本課題進(jìn)行進(jìn)一步研究的方向。

參考文獻(xiàn):

[1]趙捍東, 曹紅松, 朱基智, 等. 基于磁強(qiáng)計(jì)和陀螺的姿態(tài)測(cè)量方法[J]. 中北大學(xué)學(xué)報(bào), 2010,31(6): 631-635.

[2]WangXinlong,ZhangQing,LiHengnian.Anautonomousnavigationschemebasedonstarlight,geomagneticandgyroswithinformationfusionforsmallsatellites[J].ActaAstronautica, 2014,94(1)：708-717.

[3]GrovesPaulD.PrinciplesofGNSS,Inertial,andMultisensorIntegrated,NavigationSystems[M].publishedbyartechhouse, 2008: 280-300.

[4]BenjaminNoack,JulierSimonJ,MarcReinhardt,etal.NonlinearFederatedFiltering[C]//Turkey,16thInternationalConferenceonInformationFusionIstanbul, 2013:350-356.

[5]JongheeBae,YoudanKim.AttitudeEstimationforSatelliteFaultTolerantSystemUsingFederatedUnscentedKalmanFilter[J].InternationalJournalofAeronautical&SpaceSci, 2010, 11(2): 80-86.

[6]張海龍, 郝靜如, 李啟光. 聯(lián)邦卡爾曼濾波在捷聯(lián)慣導(dǎo)/ 全球定位/里程儀組合系統(tǒng)中的優(yōu)化設(shè)計(jì)[J].探測(cè)與控制學(xué)報(bào), 2009,31(6):66-73.

[7]GU Qitai, FANG Jing. Global optimality for generalized federated filter[J]. Acta automatica sinica, 2009,35(10)：1310-1316.

[8]Zhou Benchuan, Cheng Xianghong. Federated filtering algorithm based on fuzzy adaptive UKF for marine SINS/GPS/DVL integrated system[C]//2010 Chinese Control and Decision Conference, 2010: 2082-2085.

[9]Xiong Zhi, Chen Jihui, Wang Rong, et al. A new dynamic vector formed information sharing algorithm in federated filter [J]. Aerospace Science and Technology, 2013, 29(1): 37-46.

[10]Wan E A, Van Der Merwe R.The unscented Kalman filter[J]. Kalman filtering and neural networks, 2001,11(1): 221-280.

[11]周翔, 霍鵬飛, 祁克玉. MEMS 陀螺/ 磁傳感器復(fù)合彈丸姿態(tài)測(cè)量[J]. 探測(cè)與控制學(xué)報(bào), 2010,32(6):10-14.

[12]蘇宛新. 自適應(yīng) UKF 濾波在 SINS初始對(duì)準(zhǔn)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2011, 19(5):533-536.

[13]王小旭, 趙琳, 夏全喜, 等. 基于unscented變換的強(qiáng)跟蹤濾波勃起[J]. 控制與決策, 2010,25(7): 1063-1068.

[14]Chen Xiyuan, Shen Chong, Zhang Weibin,et al. Novel hybrid of strong tracking Kalman filter and wavelet neural network for GPS/INS during GPS outages[J]. Measurement, 2013,46(1): 3847-3854.

[15]M Kiani, Seid H Pourtakdoust. Concurrent orbit and attitude estimation using minimum sigma point unscented Kalman filter[J]. Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 2014, 228(6): 801-819.

[16]Dah Jing Jwo, Chi Fan Yang, Chih Hsun Chuang, et al. Performance enhancement for ultra-tight GPS/INS integration using a fuzzy adaptive strong tracking unscented Kalman filter[J]. Nonlinear Dynamics, 2013, 73(1):377-395.

*收稿日期:2015-11-26

作者簡(jiǎn)介:趙捍東(1960—),男，吉林長(zhǎng)春人，博士，教授，研究方向：彈箭飛行與控制。E-mail:nuc_zhd@163.com。

中圖分類(lèi)號(hào):TJ714

文獻(xiàn)標(biāo)志碼:A

文章編號(hào)：1008-1194(2016)03-0047-05

Projectile Attitude Estimation Based on Inertial & Magneto Integrated Measurement

ZHAO Handong1, LI Zhipeng2, WANG Fang3

(1. Mechanical and Electrical Engineering School of North University of China, Taiyuan 030051,China;2. Mechanical Engineering School, Beijing Institute of Technology, Beijing 100081, China;3. Institute of Process Technology, Jiangnan Industries Group Co.LTD, Xiangtan 411207,China)

Abstract:Aiming at the problem of attitude detection accuracy is limited and the fault tolerance is low when using single sensor, a inertial sensor and magnetometer based attitude estimation approach was proposed. Measure data was integrated via the vector formed information share federated filter. As there exist the problem of system model bias and the Non-Gaussian noise, proposed a novel strong tracking unscented kalman filter algorithm as the local filter. Simulation result shows, this method could get higher estimation accuracy than using any single sensor, and which was higher than traditional integration approach.

Key words:attitude estimation; integrated attitude detecting; federated filter; unscented kalman filter; strong tracking kalman filter