張曉明 陳雷 張鶯鶯 檀杰 朱孟龍

摘要：針對智能彈藥機動飛行中僅利用地磁信息制導時無法實現(xiàn)全姿態(tài)角解算的問題，提出一種采用三軸陀螺儀角速率信息輔助三軸磁傳感器信息進行彈體姿態(tài)角解算的EKF融合算法。算法利用磁傳感器測量模型和四元數(shù)微分方程建立觀測方程和狀態(tài)方程，并分別對非線性的系統(tǒng)進行線性化得到卡爾曼濾波方程。通過在高速飛行仿真轉臺上進行半物理仿真試驗，最終全姿態(tài)角的解算實現(xiàn)對地磁/陀螺信息的融合。經(jīng)過對仿真信號的處理，在彈體俯仰角士300變化的情況下，該EKF融合算法解算滾轉角和俯仰角比傳統(tǒng)單純依靠地磁信息進行滾轉角和俯仰角解算的精度提高近一個數(shù)量級，并且解算偏航角誤差在10以內。

中圖分類號：TN919.5 文獻標志碼：A文章編號1674-5124（2019）05-0010-07

關鍵詞：四元數(shù);全姿態(tài)角解算;地磁/陀螺信息融合：EKF

0 引言

現(xiàn)代戰(zhàn)爭要求進攻武器具備精確打擊能力和快速響應能力，傳統(tǒng)的常規(guī)彈藥無法滿足這種要求，必須對常規(guī)彈藥進行制導化改造，而改造的核心為彈體姿態(tài)的準確測量。由于受常規(guī)彈藥高旋轉、高沖擊、小體積的應用環(huán)境限制，傳統(tǒng)的彈載姿態(tài)測量方案中，單一使用磁傳感器無法實現(xiàn)全姿態(tài)角的實時解算，并且其精度易受彈體俯仰方向變化和干擾磁場的影響[1-2]。而常規(guī)組合導航算法計算量大，數(shù)據(jù)更新率低，信息融合精度低，不能滿足制導彈藥的實時性、可靠性和精度要求[3-5]。因此需要一種高精度的信息融合姿態(tài)測試方案，以滿足全姿態(tài)角高精度實時解算和適應復雜的彈載環(huán)境要求。

對于上述問題，文獻[6]提出了由單軸陀螺和三軸地磁傳感器組合姿態(tài)測量方案，利用單軸陀螺積分得到軸向滾轉角，進而由滾轉角結合地磁傳感器信息解算得到俯仰角和偏航角;該解算雖然方法簡單，但是滾轉角初值不易獲取，并且精度受到限制。文獻[7]采用的方法由于基于實際彈道可以較好匹配模版彈道，從而獲得一些飛行特征參數(shù)，在發(fā)射前將其裝訂供彈載飛行使用，發(fā)射后將彈道參數(shù)與三軸磁傳感器信息結合從而解算姿態(tài)角，最后用一個擴展卡爾曼濾波器融合磁傳感器和陀螺儀的信息，提高解算精度和系統(tǒng)穩(wěn)定性;這種方法在實際彈道模型準確的情況下具有較高精度，但是其過程復雜，實際彈道與匹配彈道總有偏差，而無法獲得較高的精度。文獻[8]采用權系數(shù)和卡爾曼濾波算法對地磁/陀螺組合測姿中的仿真信號進行處理，得到較好的結果，證明其算法的適用性;但卡爾曼濾波算法是針對線性高斯的情況，僅用于線性系統(tǒng)處理。以上方案分別從傳感器組合、彈道匹配、算法處理3個方面對信息融合姿態(tài)進行了測試，但均不能滿足復雜彈載環(huán)境全姿態(tài)角實時解算要求。

本文提出一種基于EKF的地磁/陀螺信息融合姿態(tài)測量方法。利用三軸陀螺儀和三軸磁傳感器對彈體姿態(tài)角進行解算，避免了精度受限的問題且滾轉角易獲取，利用EKF將非線性方程線性化，進而對仿真信號進行處理，避免了彈道匹配的偏差且可以處理非線性系統(tǒng)。

1 基于EKF的地磁/陀螺信息融合姿態(tài)測量算法設計

1.1 坐標系的定義及坐標系轉換

發(fā)射坐標系Ox_nv_nz_n如圖1所示。發(fā)射系原點位于載體上與載體固連，發(fā)射系不隨載體轉動。X_nOy_n與地理水平面重合，Ox_n軸水平指向發(fā)射方向，Oz_n軸垂直向下指向地心，Oy_n軸與之形成右手坐標系。

彈體坐標系OxbybZb如圖1所示。彈體坐標系與彈體固連，隨彈體轉動，其原點位于彈體質心。Ox_b軸與彈軸重合，方向指向彈軸前方，Oy_b軸垂直于Ox_b軸方向向右，而Oz_b軸垂直于x_bOy_b平面向下。

為了得到彈體的姿態(tài)角，需要研究發(fā)射坐標系與載體坐標系之間的轉換關系。載體坐標系固聯(lián)在載體上，當彈體在飛行時載體坐標系也會發(fā)生變化，載體坐標系與發(fā)射坐標系之間的夾角就是彈體運動的姿態(tài)角[9]。設φ為偏航角，θ為俯仰角，γ為滾轉角，轉換關系如圖2所示。

在圖2中，發(fā)射坐標系經(jīng)三次變化即可得到載體坐標系。在初始狀態(tài)下，載體坐標系與發(fā)射坐標系完全重合。第一次轉動后，發(fā)射坐標系OxnYnZn繞Z軸旋轉航向角φ，得到Ox₁y₁z_n;第二次轉動后，坐標系Ox₁y₁zn繞Y軸旋轉俯仰角θ，得到Ox_by₁z₁;第三次轉動后，坐標系Ox_by₁z₁繞x軸旋轉滾轉角γ，得到Ox_by_bz_b。

1.2 基于EKF的地磁/陀螺信息融合姿態(tài)測量原理

地磁/陀螺信息融合姿態(tài)測量原理如圖3所示，其核心是設計一個擴展卡爾曼濾波器對姿態(tài)四元數(shù)進行估計，最終再由姿態(tài)四元數(shù)換算得到姿態(tài)角。首先依載體姿態(tài)角作為原始輸入，由陀螺儀的測量可以得到載體坐標系相對發(fā)射坐標系的三軸角速率，再利用四元數(shù)微分方程得到姿態(tài)四元數(shù)信息。圖3中的卡爾曼濾波器中，采用四元數(shù)微分方程作為狀態(tài)方程，以姿態(tài)四元數(shù)為系統(tǒng)觀測量，ω_x、ω_y、ω_z作為狀態(tài)方程的輸入量。姿態(tài)輸入的另一個路徑是，直接由姿態(tài)角換行到姿態(tài)四元數(shù)，即系統(tǒng)狀態(tài)量，經(jīng)地磁測量得到載體坐標系下的地磁三分量Hx、Hy、Hz，以此作為對系統(tǒng)狀態(tài)量的觀測反饋到EKF，系統(tǒng)觀測方程以發(fā)射坐標系和載體坐標系下的地磁三分量轉化關系建立的。由于系統(tǒng)觀測方程是非線性的，因此采用擴展卡爾曼濾波器進行迭代估計。

1.3 地磁/陀螺信息融合的擴展卡爾曼濾波器設計

1.3.1 基于四元數(shù)的地磁姿態(tài)測量原理

地磁傳感器的測量值是地磁場在載體坐標系下的三分量H_m^b=[HxHyHz]^T，它與地磁場在發(fā)射坐標下三分量H_eⁿ=[H_xⁿH_yⁿH_zⁿ]^T可以通過姿態(tài)矩陣進行轉換，即有：

H_m^b=C_n^bH_eⁿ（1）其中C_n^b是從地理坐標系到彈體坐標系的轉換矩陣，即：

由于轉換矩陣C_n^b線性相關，因此利用純地磁信息無法完成全姿態(tài)解算，必須由其他傳感器輔助給出其中一個姿態(tài)角，才能利用地磁信息得到另外兩個姿態(tài)角[10]。

用四元數(shù)代替姿態(tài)角表示從地理坐標系到彈體坐標系的轉換矩陣[11]，有：

1.3.2 基于四元數(shù)的陀螺姿態(tài)測量原理

根據(jù)載體坐標系和發(fā)射坐標系的定義，可以得到描述角速率和角位置關系的四元數(shù)微分方程：式（4）可以轉化為：

Q=1/2M^*（ω_nb^b）Q（5）取Q（t）=[q₀（t）q₁（t）q₂（t）q₃（t）]^T，則式（5）所示方程連續(xù)，求解齊次線性方程解為：

將其離散化，假設一個周期內的陀螺儀的角速度為一固定值，令：

則得到四元數(shù)離散化的齊次線性方程解為：

已知載體初始四元數(shù)和任何時刻下陀螺儀的輸出角速度數(shù)據(jù)，就能求得更新后的四元數(shù)。

1.3.3 EKF狀態(tài)方程和觀測方程的建立

以姿態(tài)四元數(shù)Q為狀態(tài)量，建立如下的狀態(tài)方程為：

2 試驗驗證

2.1 計算機仿真

依據(jù)彈道質心運動學方程和彈丸繞質心轉動的運動學方程，由于質心運動分析可以忽略其他星球對制導彈的影響，導彈僅僅受到地球引力作用，不考慮地球自轉及其繞太陽的公轉，并且認為地球為一個質量分布均勻的圓球體，因此導彈軌跡可以簡化，不影響炮彈運動要求的精度[13]。設定初始條件，t=0時，彈丸初速ν₀=870m/s，彈道初始傾角θ₀=7.5°，即可計算得到彈丸飛行過程的速度三分量，進一步解算出彈丸任意時刻的偏航角φ和俯仰角。為：

此外彈體軸向變化根據(jù)實際某型號彈設定參數(shù)，將其積分得到滾轉角的姿態(tài)信息。理想姿態(tài)角的變化曲線如圖4所示，由姿態(tài)角生成的仿真三軸磁場值和三軸陀螺如圖5和圖6所示。

根據(jù)生成的三軸磁場信息和三軸陀螺信息運用EKF融合算法對其信息融合進行偏航角、俯仰角、滾轉角的計算，并且與地磁解算的俯仰角和滾轉角對比，得到如圖7一圖9所示誤差曲線。

根據(jù)圖7～圖9對于導彈飛行模型仿真結果顯示，制導彈飛行60s落地，全程導彈軸向正轉，無反轉。對比EKF融合算法和純地磁解算算法，EKF解算滾轉角的誤差均值為0.1133°，標準差為0.0585°，而純地磁解算滾轉角的誤差標準差為一3-3776°，標準差為2.1295°;所以EKF融合算法的滾轉角解算精度提高一個數(shù)量級以上，此外純地磁解算滾轉角誤差最大為-9.158°，而EKF算法的滾轉角誤差在最后階段最大僅為0.2671°，滾轉角解算精度較高。此外EKF解算俯仰角的誤差均值為-0.2536°，標準差為0.1042°，純地磁解算俯仰角的誤差均值為0.1744°，標準差為0.3652°，所以EKF融合算法的噪聲水平降低約1/3，在57-5s地磁解算俯仰角誤差最大達到2.013°，而EKF解算俯仰角誤差均不超過-0.6°。純地磁無法解算偏航角，而EKF解算偏航角的誤差均值為0.2767°，標準差為0.1717°，全程最大不超過1°，解算偏航角精度較高。此融合算法不僅可以解算彈載3個姿態(tài)角，彌補了地磁傳感器滾轉角、俯仰角測量受偏航影響的缺陷，而且解算得到的姿態(tài)角均具有較高的精度和較低的噪聲水平。

2.2 半物理試驗驗證

把集成了EIMC 1043地磁傳感器、ITG-3701陀螺儀（兩個傳感器的測量精度均在1mV以內）的系統(tǒng)捷聯(lián)安裝于轉臺上（見圖10），在轉臺的零點位置上（內框、中框、外框均為轉臺零位）預先裝載地磁場初始三分量，上電內框在is內加速到5r/s，外框在1s內轉動到30°，之后內框以5r/s的速度轉動，中框和外框均按照30sin（2πt）的規(guī)律運動，此階段20s，實驗全程共21s。以三軸高速飛行仿真轉臺的反饋信息為基準信號，每組試驗后分別對試驗數(shù)據(jù)進行讀取、保存、處理，通過轉臺反饋數(shù)據(jù)對比EKF融合算法和純地磁解算姿態(tài)角的精度，誤差如圖11、圖12和圖13所示。

由圖11可以看出，EKF解算滾轉角相比于純地磁解算滾轉角，精度得到明顯改善，抗偏航和俯仰擾動能力提升，機動性較好。具體比較數(shù)據(jù)如表1所示。

由表1可得月：KF解算滾轉角相比于純地磁解算滾轉角精度提高一個數(shù)量級以上，且標準差也減小為原來的1/10左右，噪聲水平提高近一個數(shù)量級。

此外，由圖12可以得出純地磁解算俯仰角和EFK解算俯仰角均值相差不多，但從標準差和最大偏差來看，融合算法的俯仰角精度提高一個數(shù)量級以上，如表2所示。

從圖13可以得出，EKF解算偏航角的誤差均值為0.2807°，標準差為0.2228°，誤差均在1°以內，進一步證明了EKF對于偏航角解算的準確度，可以融合地磁和陀螺信息實現(xiàn)全姿態(tài)角高精度解算。

結束語

本文提出一種基于EKF的簡單、高效、實時性、自適應的地磁/陀螺信息融合姿態(tài)測量方法。得到主要結論為：1）方法解算精度比地磁解算的滾轉角和俯仰角提高一個數(shù)量級以上，而偏航角的解算誤差在1°以內;2）采用信息融合后，外界干擾和全姿態(tài)機動的情況對解算精度影響較小，且解算姿態(tài)角誤差較為穩(wěn)定，變化幅度更小。

參考文獻

[1]趙鑫爐.基于MARL傳感器的車輛姿態(tài)測量系統(tǒng)設計[D].太原：中北大學，2014.

[2]韓艷.制導炮彈飛行姿態(tài)的陀螺/磁阻傳感器組合測量方法研究[D].南京：南京理工大學，2010.

[3]鮑亞琪，陳國光，吳坤，等.基于磁強計和MEMS陀螺的彈箭全姿態(tài)探測[J].兵工學報，2008，29（10）：1227-1231.

[4]趙鑫爐，張曉明，龍達峰，等.旋轉彈用滾轉角磁測系統(tǒng)設計[J].傳感技術學報，2013，26（9）：1309-1313.

[5]龍達峰，劉俊，張曉明，等.高速旋轉彈載體干擾磁場補償算法[J].彈箭與制導學報，2016，36（2）：151-156.

[6]曹紅松，馮順山，趙捍東，等.地磁陀螺組合彈藥姿態(tài)探測技術研究[J].彈箭與制導學報，2006，26（3）：142-145.

[7]王嘉雨.地磁/陀螺信息融合的姿態(tài)解算算法研究[D].太原：中北大學，2015

[8]馬秀麗.基于磁阻月沱螺組合測量彈體飛行姿態(tài)的數(shù)據(jù)融合方法研究[D].南京：南京理工大學，2012.

[9]代剛，李枚，蘇偉，等.自旋導彈捷聯(lián)式陀螺/地磁姿態(tài)測量方法[J].中國慣性技術學報，2010，18（6）：702-705.

[10]劉凱，梁曉庚.基于陀螺儀和磁強計的姿態(tài)解算方法研究[J].計算機仿真，2014（5）：39-41.

[11]趙鑫爐，張曉明，白諸銼，等.基于磁阻傳感器的航姿測量系統(tǒng)羅差補償技術研究[J].傳感技術學報，2013（11）：1504-1507.

[12]GRANDVALLET B，ZEMOUCHE A，BOUTAYEB M，et al.Real-time attitude-independent three-axis magnetometercalibration for spinning projectiles：A sliding windowapproach[J].IEEE Transactions on Control SystemsTechnology，2013，22（1）：255-264.

[13]JULIER S J，UHLMANN J K，DURRANT-WHYTE H F.Anew method of the nonlinear transformation of means andcovariance in filters and estimation[J].IEEE Transactions onAutomatic Control，2000，45（3）：477-428.

（編輯：李剛）

收稿日期：2018-10-07;收到修改稿日期：2018-11-13

基金項目：國家自然科學基金（61873247）

作者簡介：張曉明（1976-），男，山西新絳縣人，教授，博士，研究方向為動態(tài)測試與組合導航。