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

趙捍東 ，李志鵬，王　芳

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

?

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

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

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

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

關(guān)鍵詞:姿態(tài)估計；組合測姿；聯(lián)邦濾波；無跡濾波；強跟蹤濾波

0引言

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

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

1組合測姿原理

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

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

ε=εb+εr+ωg

(1)

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

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

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

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

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

從算法結(jié)構(gòu)上看，聯(lián)邦濾波結(jié)構(gòu)中的各個子濾波器從主濾波器中分享最優(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

對于一般的離散系統(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稱為信息分配系數(shù)，并且符合以下約束條件：

(10)

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

實際計算時，對于每一個子濾波器，主濾波器到子濾波器的信息反饋方式一般取為[5-6]：

(11)

(12)

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

1.3子濾波器設(shè)計

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

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

(13)

其中λ=α2(n+κ)-n，同時定義對應(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)誤差和量測噪聲，k表示第k次觀測。那么基于UKF的濾波過程可表示為：

1)初始化過程：

(17)

(18)

2)預(yù)測更新：

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

(19)

(20)

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

(21)

(22)

(23)

3)量測更新：

(24)

(25)

(26)

(27)

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

(28)

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

2改進的組合測姿

2.1改進的強跟蹤無跡卡爾曼濾波

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

(29)

(30)

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

(31)

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

(32)

(33)

(34)

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

(35)

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

強跟蹤改進的濾波器因為采用了強迫正交原理，可以有效的削弱建模誤差和不確定性噪聲帶來的不利影響[13-14]，這對實際應(yīng)用具有十分重要的意義。

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

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

圖2　改進的INS/地磁組合測量模型Fig.2　novel INS/magnetometer based integrated estimation model

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

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

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

3仿真與分析

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

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

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

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

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

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

4結(jié)論

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

參考文獻:

[1]趙捍東, 曹紅松, 朱基智, 等. 基于磁強計和陀螺的姿態(tài)測量方法[J]. 中北大學(xué)學(xué)報, 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].探測與控制學(xué)報, 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)測量[J]. 探測與控制學(xué)報, 2010,32(6):10-14.

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

[13]王小旭, 趙琳, 夏全喜, 等. 基于unscented變換的強跟蹤濾波勃起[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

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

中圖分類號:TJ714

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

文章編號：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