基于EKF的地磁／陀螺信息融合姿態(tài)測(cè)量算法研究

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

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

中圖分類號(hào)：TN919.5 文獻(xiàn)標(biāo)志碼：A文章編號(hào)1674-5124（2019）05-0010-07

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

0 引言

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

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

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

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

1.1 坐標(biāo)系的定義及坐標(biāo)系轉(zhuǎn)換

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

彈體坐標(biāo)系OxbybZb如圖1所示。彈體坐標(biāo)系與彈體固連，隨彈體轉(zhuǎn)動(dòng)，其原點(diǎn)位于彈體質(zhì)心。Ox_b軸與彈軸重合，方向指向彈軸前方，Oy_b軸垂直于Ox_b軸方向向右，而Oz_b軸垂直于x_bOy_b平面向下。

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

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

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

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

1.3 地磁/陀螺信息融合的擴(kuò)展卡爾曼濾波器設(shè)計(jì)

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

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

H_m^b=C_n^bH_eⁿ（1）其中C_n^b是從地理坐標(biāo)系到彈體坐標(biāo)系的轉(zhuǎn)換矩陣，即：

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

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

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

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

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

將其離散化，假設(shè)一個(gè)周期內(nèi)的陀螺儀的角速度為一固定值，令：

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

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

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

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

2 試驗(yàn)驗(yàn)證

2.1 計(jì)算機(jī)仿真

依據(jù)彈道質(zhì)心運(yùn)動(dòng)學(xué)方程和彈丸繞質(zhì)心轉(zhuǎn)動(dòng)的運(yùn)動(dòng)學(xué)方程，由于質(zhì)心運(yùn)動(dòng)分析可以忽略其他星球?qū)χ茖?dǎo)彈的影響，導(dǎo)彈僅僅受到地球引力作用，不考慮地球自轉(zhuǎn)及其繞太陽(yáng)的公轉(zhuǎn)，并且認(rèn)為地球?yàn)橐粋€(gè)質(zhì)量分布均勻的圓球體，因此導(dǎo)彈軌跡可以簡(jiǎn)化，不影響炮彈運(yùn)動(dòng)要求的精度[13]。設(shè)定初始條件，t=0時(shí)，彈丸初速ν₀=870m/s，彈道初始傾角θ₀=7.5°，即可計(jì)算得到彈丸飛行過(guò)程的速度三分量，進(jìn)一步解算出彈丸任意時(shí)刻的偏航角φ和俯仰角。為：

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

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

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

2.2 半物理試驗(yàn)驗(yàn)證

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

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

由表1可得月：KF解算滾轉(zhuǎn)角相比于純地磁解算滾轉(zhuǎn)角精度提高一個(gè)數(shù)量級(jí)以上，且標(biāo)準(zhǔn)差也減小為原來(lái)的1/10左右，噪聲水平提高近一個(gè)數(shù)量級(jí)。

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

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

結(jié)束語(yǔ)

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

參考文獻(xiàn)

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

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

[3]鮑亞琪，陳國(guó)光，吳坤，等.基于磁強(qiáng)計(jì)和MEMS陀螺的彈箭全姿態(tài)探測(cè)[J].兵工學(xué)報(bào)，2008，29（10）：1227-1231.

[4]趙鑫爐，張曉明，龍達(dá)峰，等.旋轉(zhuǎn)彈用滾轉(zhuǎn)角磁測(cè)系統(tǒng)設(shè)計(jì)[J].傳感技術(shù)學(xué)報(bào)，2013，26（9）：1309-1313.

[5]龍達(dá)峰，劉俊，張曉明，等.高速旋轉(zhuǎn)彈載體干擾磁場(chǎng)補(bǔ)償算法[J].彈箭與制導(dǎo)學(xué)報(bào)，2016，36（2）：151-156.

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

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

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

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

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

[11]趙鑫爐，張曉明，白諸銼，等.基于磁阻傳感器的航姿測(cè)量系統(tǒng)羅差補(bǔ)償技術(shù)研究[J].傳感技術(shù)學(xué)報(bào)，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

基金項(xiàng)目：國(guó)家自然科學(xué)基金（61873247）

作者簡(jiǎn)介：張曉明（1976-），男，山西新絳縣人，教授，博士，研究方向?yàn)閯?dòng)態(tài)測(cè)試與組合導(dǎo)航。