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

趙捍東 ，李志鵬，王　芳

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

?

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

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

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

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

關鍵詞:姿態估計；組合測姿；聯邦濾波；無跡濾波；強跟蹤濾波

0引言

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

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

1組合測姿原理

1.1姿態測量原理

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

ε=εb+εr+ωg

(1)

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

地磁傳感器測姿的原理是三軸地磁傳感器敏感載體在地磁場中的傾角，再轉化到慣性坐標系內姿態角[1]。地磁場是地球本身的固有物理屬性，穩定性好，不易人為干擾。基于對地磁的測量進行測姿，已經在低軌衛星、無人機、艦船上得到了廣泛的應用[2-3]。地磁測姿誤差不累積，但受到地磁建模誤差，異磁場干擾，載體干擾的影響，總的誤差比較復雜。

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

1.2基于聯邦濾波的融合原理

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

從算法結構上看，聯邦濾波結構中的各個子濾波器從主濾波器中分享最優融合后的狀態信息[4-5]，并將從主濾波器分配到的信息作為下一步濾波融合的基準值，這就可以在最大程度上抑制陀螺誤差的累積。聯邦濾波的經典結構如圖1所示[4,6,9]。

圖1　聯邦濾波融合結構Fig.1　the federated filter fusing frame

對于一般的離散系統，我們可以將其系統狀態模型描述為：

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

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

上式中的βi稱為信息分配系數，并且符合以下約束條件：

(10)

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

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

(11)

(12)

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

1.3子濾波器設計

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

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

(13)

其中λ=α2(n+κ)-n，同時定義對應的權值為：

(14)

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

(15)

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

(16)

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

1)初始化過程：

(17)

(18)

2)預測更新：

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)

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

2改進的組合測姿

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

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

(29)

(30)

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

(31)

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

(32)

(33)

(34)

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

(35)

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

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

2.2改進的聯邦濾波融合結構

由于采用了信息反饋機制，可以使得聯邦濾波獲得最優的融合效果，所以信息分配系數便在一定程度上決定了最后的融合精度。傳統的信息分配系數的求法，如式(14)，僅考慮了子濾波器方差的特性。由于每個子系統獲得的觀測量不同，那么對不同子系統的可觀測度就不相同。可觀測性越好，子系統對應的估計值就越精確，于是，文獻[9]提出了一種將系統可觀測度信息引入求解分配系數的過程中，對分配系數進行改進后，本文中基于聯邦濾波的組合測姿原理可表示為圖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；地磁姿態誤差1°，并每隔30s添加一次幅值為5°的異常干擾。陀螺初始姿態誤差分別為，1.5°，1°，-2°，基于某火箭彈彈道進行仿真，并設定角運動狀態方程的系數誤差為2%。分別以傳統方法和本文方法進行姿態估計的仿真對比，其中傳統方法為經典UKF作為子濾波器的一般聯邦濾波方法，這也是工程上較為常用的一種方法。仿真時間600s，誤差統計如圖3、圖4所示。

圖3　姿態角誤差統計Fig.3　attitude error statistics

圖4　姿態角速率誤差統計Fig. 4　attitude angle velocity error statistics

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

表1　姿態角估計的誤差統計

在工程應用中，使用陀螺和磁強計的組合，可以充分利用陀螺和磁強計的優點，從而提供更精確的姿態測量數據。

4結論

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

參考文獻:

[1]趙捍東, 曹紅松, 朱基智, 等. 基于磁強計和陀螺的姿態測量方法[J]. 中北大學學報, 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]張海龍, 郝靜如, 李啟光. 聯邦卡爾曼濾波在捷聯慣導/ 全球定位/里程儀組合系統中的優化設計[J].探測與控制學報, 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 陀螺/ 磁傳感器復合彈丸姿態測量[J]. 探測與控制學報, 2010,32(6):10-14.

[12]蘇宛新. 自適應 UKF 濾波在 SINS初始對準中的應用[J]. 中國慣性技術學報, 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

文獻標志碼: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