基于EKF的地磁／陀螺信息融合姿態測量算法研究

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

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

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

關鍵詞：四元數;全姿態角解算;地磁/陀螺信息融合：EKF

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

1.3.1 基于四元數的地磁姿態測量原理

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

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

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

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

1.3.2 基于四元數的陀螺姿態測量原理

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

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

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

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

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

1.3.3 EKF狀態方程和觀測方程的建立

以姿態四元數Q為狀態量，建立如下的狀態方程為：

2 試驗驗證

2.1 計算機仿真

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

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

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

根據圖7～圖9對于導彈飛行模型仿真結果顯示，制導彈飛行60s落地，全程導彈軸向正轉，無反轉。對比EKF融合算法和純地磁解算算法，EKF解算滾轉角的誤差均值為0.1133°，標準差為0.0585°，而純地磁解算滾轉角的誤差標準差為一3-3776°，標準差為2.1295°;所以EKF融合算法的滾轉角解算精度提高一個數量級以上，此外純地磁解算滾轉角誤差最大為-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個姿態角，彌補了地磁傳感器滾轉角、俯仰角測量受偏航影響的缺陷，而且解算得到的姿態角均具有較高的精度和較低的噪聲水平。

2.2 半物理試驗驗證

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

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

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

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

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

結束語

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

參考文獻

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

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

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

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

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

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

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

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

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

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

[11]趙鑫爐，張曉明，白諸銼，等.基于磁阻傳感器的航姿測量系統羅差補償技術研究[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-），男，山西新絳縣人，教授，博士，研究方向為動態測試與組合導航。