曾 成，李文霄

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

0 引言

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

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

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

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

1 磁強(qiáng)計輸出模型

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

B′=K·B+b，

(1)

補(bǔ)償后磁強(qiáng)計的輸出為

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

(2)

本文方法針對磁場干擾即矢量b進(jìn)行求解，對磁強(qiáng)計進(jìn)行補(bǔ)償為

B=B′-b。

(3)

2 磁強(qiáng)計干擾補(bǔ)償方法

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

2.1 三軸磁強(qiáng)計解算航向角

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

圖1 三軸磁強(qiáng)計的測量原理圖

Fig.1 The measuring principle of three-axis magnetometer

(4)

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

(5)

2.2 陀螺儀解算航向角

與磁強(qiáng)計解算航向角的方法不同，陀螺儀利用四元數(shù)原理和四階龍格-庫塔算法對姿態(tài)角進(jìn)行解算，導(dǎo)航坐標(biāo)系相對于陀螺儀載體坐標(biāo)系的轉(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 磁場干擾估計與補(bǔ)償方法

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

(12)

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

(13)

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

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

(14)

tan(ψt 2-ψt1)可以通過陀螺儀進(jìn)行求解，令A(yù)=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)

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

3 磁場干擾的估計與補(bǔ)償步驟

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

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

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

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

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

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

圖2 干擾補(bǔ)償方法流程圖

Fig2 Flow chart of the interference compensation

4 實驗分析

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

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

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

Fig3 High precision turntable and installation of sensors

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

圖4 未校正的磁強(qiáng)計誤差圖

Fig.4 Error of uncorrected magnetometer

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

圖5 兩種方法補(bǔ)償后磁強(qiáng)計誤差的對比

Fig.5 Comparison of magnetometer errors corrected by two methods

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

圖6 磁強(qiáng)計未補(bǔ)償前的航向角的對比

Fig.6 Comparison of heading angle before uncompensated magnetometer

5 結(jié)論

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

圖7 磁場干擾補(bǔ)償后磁強(qiáng)計航向角的對比

Fig.7 Comparison of heading angle corrected by interference compensation