曾 成，李文霄

(1.河北工業(yè)大學 電子信息工程學院，天津 300401；2.天津市電子材料與器件重點實驗室，天津 300401)

0 引言

三軸磁強計具有體積小、質(zhì)量輕、功耗低的優(yōu)點，被廣泛用于導航、航空磁測、衛(wèi)星定姿等領(lǐng)域[1]。在組合導航定位中，三軸磁強計作為敏感器件用來測量磁場的大小和方向，常與陀螺儀結(jié)合使用約束航向角誤差。但因其器件特性，受自身誤差和外界干擾的影響較大，導致航向角偏差較大[2]。航向角偏差嚴重時會導致重大事故發(fā)生，造成不可預計的后果。因此在設(shè)備使用過程中需要對三軸磁強計進行誤差與干擾的估計，并對其進行補償，從而使航向角更加精準[3-4]。

目前，三軸磁強計誤差補償常用方法有極大極小值法、橢球擬合校正法和矢量點積不變法。文獻[5]提出一種極大極小值校正方法，能校正靈敏度誤差和零偏誤差，但補償后航向角的精度偏低。文獻[6]提出一種橢球擬合校正方法對三軸磁強計誤差標定補償，降低了計算量及磁強計誤差，提高了航向角精度。文獻[7]針對外界磁場對橢球擬合的影響，在補償中采用分段優(yōu)化，進一步提高了航向角精度。但橢球擬合需要本地地磁信息的準確值且要采集大量不同姿態(tài)下磁場矢量數(shù)據(jù)，過程復雜、不易實現(xiàn)。文獻[8-9]采用一種矢量點積不變性校正方法，無需本地地磁場精確數(shù)據(jù)，可實現(xiàn)三軸磁強計的坐標系對準。但該方法輔助矢量的選取仍然難以實現(xiàn)。

針對三軸磁強計誤差補償常用方法中補償精度較低、過程復雜和輔助矢量較難選取等缺點，在組合導航中，常利用多種傳感器相結(jié)合的方法對磁強計誤差校正。文獻[10]提出磁強計與加速度計相結(jié)合的方法，利用極大極小值對磁強計初步校正，再利用矢量點積不變性對磁強計進一步校正。但該方法需保證處于零速區(qū)間，并且算法復雜。文獻[11]將3種測量儀器優(yōu)勢互補，利用陀螺儀作為估計值預測姿態(tài)角，加速度計和磁強計的數(shù)據(jù)作為觀測值進行修正，從而解算出高精度的姿態(tài)數(shù)據(jù)。但該方法在磁強計受到較大干擾時，未進行干擾補償，導致此種情況下磁強計誤差過大，航向角的精度明顯下降。

本文利用陀螺儀短時精度較好、長期誤差較大，而磁強計本身器件誤差較大、長期誤差較小的特點，提出一種磁強計的磁場干擾補償方法。利用磁強計受干擾情況下陀螺儀在卡爾曼濾波算法中可信度較高的條件，建立磁強計的干擾模型。將磁強計的三軸磁場干擾轉(zhuǎn)化為水平面上的兩軸磁場干擾并對其進行估計，從而補償磁強計的輸出值，提高航向角精度。該方法無需采集大量不同姿態(tài)下的磁場信息，方法簡單，實時性強，適用于磁場干擾較大的場景。

1 磁強計輸出模型

磁強計的輸出主要受自身誤差影響和外界磁場的軟硬磁干擾影響[12]，可建立磁強計輸出模型為

B′=K·B+b，

(1)

補償后磁強計的輸出為

B=K-1·(B′-b)，

(2)

本文方法針對磁場干擾即矢量b進行求解，對磁強計進行補償為

B=B′-b。

(3)

2 磁強計干擾補償方法

在文獻[11]提出的卡爾曼濾波算法中，當磁強計受到干擾時，其解算出的航向角失去準確性，此時陀螺儀可信度較高。而在無磁場干擾情況下，磁強計解算的航向角角度差與陀螺儀解算的航向角角度差相等。將磁強計三軸輸出量通過陀螺儀解算的橫滾角與俯仰角轉(zhuǎn)化為水平面上的兩軸輸出量，建立干擾影響后磁強計的輸出模型，并對干擾進行估計與補償，并求解補償后的航向角。

2.1 三軸磁強計解算航向角

三軸磁強計的坐標系與載體坐標系重合。三軸磁強計的x軸沿載體縱軸向前，y軸沿載體橫軸向右，z軸與兩水平軸垂直，方向向下。通過三軸磁強計測量地磁矢量，當三軸磁強計處于水平狀態(tài)時，可通過三軸磁強計的兩水平磁軸的測量值計算載體的航向角[13]。磁強計在導航中的測量原理如圖1所示。

圖1 三軸磁強計的測量原理圖

Fig.1 The measuring principle of three-axis magnetometer

(4)

由于地球的自轉(zhuǎn)軸和磁軸不完全重合，故地磁的北向和地理的北向之間存在一個磁偏角α。將磁偏角補償?shù)胶较蚪铅字?，可求得航向角ψm為

(5)

2.2 陀螺儀解算航向角

與磁強計解算航向角的方法不同，陀螺儀利用四元數(shù)原理和四階龍格-庫塔算法對姿態(tài)角進行解算，導航坐標系相對于陀螺儀載體坐標系的轉(zhuǎn)動四元數(shù)為[14-15]

Q=q0+q1i+q2j+q3k，

(6)

利用轉(zhuǎn)動四元數(shù)表示捷聯(lián)矩陣為

(7)

根據(jù)式(7)求解橫滾角θ、俯仰角γ、航向角ψ為

(8)

根據(jù)四元數(shù)的相關(guān)理論[16]，得到轉(zhuǎn)動四元數(shù)的微分方程為

(9)

式中，?表示四元數(shù)乘法，ω=0+ωxi+ωyj+ωzk，ωx、ωy和ωz分別為陀螺儀輸出的三軸角速度。

微分方程式(9)的矩陣形式為

(10)

利用四階龍格庫塔法解四元數(shù)微分方程式(10)，通過式(11)實現(xiàn)四元數(shù)實時更新：

(11)

式中，h=tk+1-tk，為四元數(shù)的更新周期。

將四元數(shù)代入式(7)，得到捷聯(lián)矩陣，再根據(jù)式(8)即可得到由陀螺儀解算的3個姿態(tài)角，其中ψ為所求航向角。

2.3 磁場干擾估計與補償方法

通過2.2節(jié)中陀螺儀解算的橫滾角和俯仰角，對磁強計的三軸分量進行轉(zhuǎn)換，獲得磁強計在水平面的投影Mx，My為

(12)

根據(jù)式(12)得到磁強計解算的航向角公式為

(13)

令ψt1和ψt2分別為陀螺儀在t1和t2兩個時刻解算的航向角，ψm1和ψm2分別為磁強計在無干擾情況下在t1和t2兩個時刻解算的航向角。為保證航向角變化足夠大，采取時間間隔為t，進行數(shù)據(jù)采集。根據(jù)陀螺儀與磁強計解算出的航向角差值相同，建立等式如下:

tan(ψt 2-ψt1)=tan(ψm2-ψm1)，

(14)

tan(ψt 2-ψt1)可以通過陀螺儀進行求解，令A=tan(ψt 2-ψt1)，由式(13)和式(14)可得

(15)

(16)

將式(16)化簡可得

(17)

式中

k1=M1y-M2y-AM2x-AM1x，

k2=M2x-M1x-AM2y-AM1y。

將式(17)轉(zhuǎn)化為

F=k1b′x+k2b′y+k3-R。

(18)

當樣本點超過3個(耗時超過3 s)時，利用最小二乘擬合法使F最小，求出磁強計的兩軸磁場干擾b′x、b′y。根據(jù)式(3)可知兩軸干擾補償與三軸干擾補償原理相同。對磁強計進行兩軸補償，得到補償后的磁場強度，再利用補償后的數(shù)據(jù)對航向角進行解算。

3 磁場干擾的估計與補償步驟

對磁場干擾估計與補償流程如圖2所示。

第一步，輸入t時刻陀螺儀與磁強計原始數(shù)據(jù)以及t-1時刻陀螺儀解算的航向角；

第二步，根據(jù)式(8)對陀螺儀的數(shù)據(jù)進行航向角、橫滾角與俯仰角解算；

第三步，將第二步中解算的航向角與t-1時刻的航向角作差并根據(jù)式(12)將磁強計的三軸分量轉(zhuǎn)化為兩軸分量；

第四步，利用兩個時刻航向角的差值,每隔1 s進行一次計算，并將解算的差值與磁強計兩軸分量代入到干擾模型式(18)中；

第五步，利用最小二乘法對干擾求解，并對磁強計進行補償，根據(jù)式(13)求解經(jīng)干擾補償后的航向角。

圖2 干擾補償方法流程圖

Fig2 Flow chart of the interference compensation

4 實驗分析

實驗采用低成本MEMS傳感器 MPU9250，該傳感器包含三軸磁強計、三軸陀螺儀和三軸加速度計三類傳感器。并在高精度轉(zhuǎn)臺上進行驗證。高精度轉(zhuǎn)臺與傳感器的固定如圖3所示。

實驗選取在磁場干擾較大的室內(nèi)，當?shù)氐卮艔姸葹锽=53.934 μT。實驗分為兩步采集數(shù)據(jù)。第一步，將mpu9250傳感器固定在高精度轉(zhuǎn)臺上，采樣率設(shè)定為100 Hz。在轉(zhuǎn)臺邊放置磁場干擾源，對傳感器通電后開啟轉(zhuǎn)臺，令轉(zhuǎn)臺分別水平轉(zhuǎn)動兩周和豎直轉(zhuǎn)動一周，采集磁強計的數(shù)據(jù)。第二步，任意轉(zhuǎn)動傳感器采集數(shù)據(jù)，采集20 s的數(shù)據(jù)用來驗證校正效果。由于橢球擬合法需采集大量不同姿態(tài)下的磁場信息，矢量點積不變法需獲取靜止區(qū)間的重力矢量，兩種方法步驟復雜，實時性差，不適用于磁場干擾較大的場景。因此，分別利用文獻[5]中極大極小值法和本文方法對采集數(shù)據(jù)進行解算，補償磁強計。實驗通過比較磁場強度的最大最小絕對誤差和均方根誤差以及陀螺儀解算出的航向角驗證補償效果。

圖3 高精度轉(zhuǎn)臺與傳感器的固定圖

Fig3 High precision turntable and installation of sensors

通過上述實驗進行數(shù)據(jù)采集，并與當?shù)氐卮艔姸葘Ρ?，誤差結(jié)果如圖4所示，磁強計未經(jīng)補償前測得的地磁場強度值的最大最小絕對值誤差為43.622 μT，均方值誤差為17.248 μT，磁場干擾對磁強計的作用較大。經(jīng)文獻[5]所述的極大極小值法補償后，磁強計最大最小誤差降到至14.731 μT，均方誤差降至7.416 μT，說明極大極小值法對受到干擾后的磁強計補償?shù)男Ч焕硐搿?/p>

圖4 未校正的磁強計誤差圖

Fig.4 Error of uncorrected magnetometer

根據(jù)圖5可知，經(jīng)過干擾補償方法后，磁強計的最大最小絕對值誤差降至0.499 μT，均方誤差降至0.519 μT，比極大極小值法提高了6.897 μT，有效去除了干擾對磁強計的影響。

圖5 兩種方法補償后磁強計誤差的對比

Fig.5 Comparison of magnetometer errors corrected by two methods

圖6和圖7進一步展示了補償算法對磁強計的補償效果。由于未補償干擾前磁強計的誤差較大，導致磁強計解算的航向角與陀螺儀解算的差值也較大，航向角變化角度僅為50°到150°左右，而補償后的磁強計解算出的航向角大致與陀螺儀解算出的航向角相似，可以看出磁強計干擾補償方法是一種有效的磁強計干擾補償方法，提高了磁強計解算航向角的精度。

圖6 磁強計未補償前的航向角的對比

Fig.6 Comparison of heading angle before uncompensated magnetometer

5 結(jié)論

本文針對組合導航定位中磁場干擾較大導致磁強計解算航向角嚴重失準的問題，利用陀螺儀與無干擾時磁強計解算的航向角角度差相吻合的特點，提出一種磁強計干擾補償方法。通過建立干擾模型并采用最小二乘擬合法估計磁場干擾值，用來補償磁強計的輸出，提高航向角精度。實驗結(jié)果表明，該方法運算簡單，無需大量采集不同姿態(tài)的數(shù)據(jù)，補償后磁場強度的絕對誤差降至0.499 μT，均方根誤差降至0.519 μT，有效提高了航向角準確度，補償效果明顯，適用于磁場干擾場景。

圖7 磁場干擾補償后磁強計航向角的對比

Fig.7 Comparison of heading angle corrected by interference compensation