基于多磁力計融合的電子羅盤研究

葉 鑫,趙忠華,金昱冏,羅忠渝

(上海交通大學(xué) 電子信息與電氣工程學(xué)院,上海 200240)

0 引言

隨著導(dǎo)航技術(shù)在無人機(jī)、智能汽車等領(lǐng)域的不斷發(fā)展和應(yīng)用,航向(方向)的精準(zhǔn)測量成為保證導(dǎo)航精度的關(guān)鍵因素之一。電子羅盤通過敏感地磁場來獲取載體航向信息,是一種重要的導(dǎo)航儀器,具有環(huán)境適應(yīng)性廣、不依賴外部信息等特點(diǎn)[1],應(yīng)用較廣泛。然而,電子羅盤在測量過程中易受到自身以及周圍環(huán)境磁場等影響,導(dǎo)致精度變差,穩(wěn)定性降低,這也成為制約電子羅盤在高精領(lǐng)域應(yīng)用的關(guān)鍵因素。針對此問題,國內(nèi)外研究者做了相關(guān)研究和創(chuàng)新,較為經(jīng)典的有差分磁羅盤技術(shù)[2-3]。其利用兩顆三軸磁力計同軸擺放,通過差分得到的軸向干擾磁場信息較少,對提升精度有限。文獻(xiàn)[4]提出基于磁力計和慣性測量單元(IMU)組合以增強(qiáng)羅盤對動態(tài)誤差補(bǔ)償效果,提升環(huán)境適應(yīng)能力,但是陀螺儀容易產(chǎn)生漂移、累積誤差,需要初始標(biāo)定對準(zhǔn)。文獻(xiàn)[5]提出了一種利用磁線圈有源補(bǔ)償?shù)牧_盤設(shè)計,能夠有效補(bǔ)償載體強(qiáng)磁干擾,并通過了仿真實(shí)驗驗證,但其本身設(shè)計較復(fù)雜,實(shí)現(xiàn)難度大。因此,本文提出了一種基于多磁力計融合的電子羅盤測量模型,該模型依靠8顆磁力計和1顆加速度計對地磁場和重力加速度進(jìn)行測量,并采用遞推最小二乘(RLS)方法聯(lián)合卡爾曼濾波(KF)進(jìn)行磁場誤差補(bǔ)償與數(shù)據(jù)融合。其簡化了補(bǔ)償操作,實(shí)現(xiàn)了航向角的精準(zhǔn)測量,提升了羅盤的測量精度和可靠性。

1 多磁力計系統(tǒng)模型與誤差分析

為方便分析各傳感器之間的約束關(guān)系,對所用坐標(biāo)系做如下規(guī)定:以三軸指向羅盤前(x)、左(y)、上(z)方向的載體坐標(biāo)系(b系),傳感器自身測量坐標(biāo)系(m系),與當(dāng)?shù)乇?、西、天方向重合的?dǎo)航坐標(biāo)系(n系)。

1.1 多磁力計系統(tǒng)模型

多磁力計結(jié)構(gòu)如圖1所示,其中同一平面內(nèi)包括8顆三軸磁力計M1-M8,均勻分布于圓周的8個方向,相鄰之間夾角為45°。另有一顆三軸加速度計A1固定在圓心,與磁力計M1同軸,負(fù)責(zé)提供姿態(tài)信息。

圖1 多磁力計系統(tǒng)模型結(jié)構(gòu)圖

根據(jù)圖1所示的空間關(guān)系,當(dāng)以羅盤載體坐標(biāo)系作為參考系,8顆磁力計的Z軸具有相同的指向,即共Z軸。因此,相鄰兩磁力計測量值之間存在如下轉(zhuǎn)換關(guān)系:

(1)

根據(jù)式(1)可知多磁力計一組測量值的分布特征,但是其姿態(tài)關(guān)系還需根據(jù)加速度計測量值進(jìn)行分析。當(dāng)加速度計A1測量值為

(2)

由于同軸關(guān)系,由此可計算得到磁力計M1的姿態(tài)角[6]為

(3)

式中:α1為俯仰角;β1為橫滾角。關(guān)于姿態(tài)角定義如圖2所示,圖中箭頭指示方向定義為俯仰橫滾角的正方向。

圖2 載體姿態(tài)角定義示意圖

(4)

(5)

式中:n=1,2,…,8(0);Acc2-Acc8為虛擬加速度計測量值。

結(jié)合式(3)、(5)進(jìn)行推導(dǎo)計算,最終可得相鄰磁力計之間姿態(tài)角關(guān)系為

(6)

式中:αn∈[-90°,+90°];βn∈[-180°,+180°];n=1,2,…,8(0)。

根據(jù)式(6)的姿態(tài)關(guān)系,即可由加速度計A1測量值計算得到所有磁力計的姿態(tài)角信息,進(jìn)一步結(jié)合磁力計測量值可解算得到航向角信息。深入研究發(fā)現(xiàn),當(dāng)載體處于水平狀態(tài)時,磁力計的俯仰橫滾角均為0°,相鄰磁力計航向角相差45°。而當(dāng)載體處于傾斜狀態(tài)時,此時磁力計姿態(tài)各不同,姿態(tài)關(guān)系呈現(xiàn)如式(6)所示的非線性。

1.2 多磁力計誤差分析

磁力計測量誤差主要分為傳感器自身誤差、裝配誤差、磁干擾誤差等[7]。其中,傳感器自身的誤差主要包括三軸不正交、刻度系數(shù)、零點(diǎn)偏移3個方面。裝配誤差表現(xiàn)為載體坐標(biāo)系與傳感器坐標(biāo)系之間的對準(zhǔn)偏差。磁干擾誤差主要包括硬磁干擾誤差和軟磁干擾誤差,因此可建立磁力計誤差的數(shù)學(xué)模型:

(7)

對于多磁力計的誤差,各磁力計既具有相同的誤差項,也有一些獨(dú)自的誤差項。一方面,各磁力計處于同樣的磁干擾環(huán)境中,即具有相同的軟硬磁誤差矩陣。另一方面,各磁力計具有不同的安裝誤差及三軸不正交度、刻度系數(shù)、零偏等自身誤差,各自的測量噪聲方差也并不一致。根據(jù)以上分析可以構(gòu)建多磁力計誤差模型:

(8)

通過式(8)即可表征多磁力計的誤差特征,與式(7)在形式上保持了一致,便于分析,但式(8)包含有更多隱藏的誤差信息,復(fù)雜度更高,數(shù)據(jù)計算量更大。

2 基于RLS-KF的誤差補(bǔ)償與融合算法

與傳統(tǒng)單磁力計羅盤不同,多磁力計測量誤差得到補(bǔ)償后,還有多傳感器的數(shù)據(jù)融合問題,這在理論上能夠進(jìn)一步提升航向角的測量精度和穩(wěn)定性,增強(qiáng)羅盤抗干擾能力。因此,在多磁力計羅盤測量過程中,如何準(zhǔn)確獲取磁力計誤差參數(shù),并對測量數(shù)據(jù)融合是最重要的兩方面。

磁力計誤差補(bǔ)償?shù)幕驹硎抢美硐氲拇艌鰷y量數(shù)據(jù)球面分布這一特征對實(shí)測數(shù)據(jù)進(jìn)行橢球擬合,從而得到橢球方程各項系數(shù),再進(jìn)一步轉(zhuǎn)化得到誤差補(bǔ)償參數(shù)。常用的橢球擬合算法有最小二乘法[8]、高斯-牛頓法[9]等經(jīng)典算法。但此類算法涉及到矩陣求逆運(yùn)算,計算量大且非實(shí)時處理,內(nèi)存占用大。因為8顆磁力計同時產(chǎn)生的數(shù)據(jù)量較大,并不適用,因此選擇了能夠迭代在線更新、計算速度更快的RLS方法。在數(shù)據(jù)融合方面,對于同構(gòu)多傳感器融合選用了基于卡爾曼濾波器的融合方案。所使用到的算法整體結(jié)構(gòu)框圖如圖3所示。

圖3 補(bǔ)償融合算法結(jié)構(gòu)流程圖

2.1 RLS在線誤差參數(shù)估計

(9)

式中bn為已知磁力計Mn自身零偏,n=1,2,…,8。

當(dāng)?shù)氐卮艌鰪?qiáng)度參考值為href,在固定地理位置下,可將其地磁場矢量模值視為常值,構(gòu)建誤差方程為

(10)

根據(jù)磁力計地磁測量誤差特征,可將誤差模型表示為橢球一般方程[10]:

ax2+by2+cz2+2fyz+2gzx+2hxy+
2ux+2vy+2wz+d+vk=0

(11)

式中:x,y,z代表地磁場測量三軸分量;vk為噪聲。

為了進(jìn)行線性化處理,規(guī)定:

(12)

式中k=1,2,…表示測量次數(shù)。

將式(11)轉(zhuǎn)為RLS求解形式:

Zk=GkXk+vk

(13)

式中Zk為0,此處vk可忽略。

根據(jù)RLS算法計算步驟如下:

(14)

式中:λ為遺忘因子;Jk為增益系數(shù)。

隨著式(14)計算步驟的不斷迭代更新,橢球參數(shù)Xk的估計誤差逐漸減小,并不斷接近真實(shí)值。同時,通過引入遺忘因子λ可增強(qiáng)迭代過程中新數(shù)據(jù)信息量,防止數(shù)據(jù)飽和。

進(jìn)一步將橢球參數(shù)轉(zhuǎn)化為磁力計誤差補(bǔ)償參數(shù)。首先將式(11)化為

hkTDhk+(2E)Thk+U=0

(15)

對式(15)進(jìn)一步變換形式:

(hk+D-1E)TD(hk+D-1E)=ETDE-U

(16)

(17)

聯(lián)合式(16)、(17)結(jié)果,有

(18)

最后,通過矩陣分解即可得到軟硬磁誤差矩陣Km和bm。

2.2 多磁力計數(shù)據(jù)融合

在完成誤差補(bǔ)償后,還需要對8顆磁力計的測量數(shù)據(jù)進(jìn)行融合解算,得到最終的航向角結(jié)果。數(shù)據(jù)融合按照融合層次可以分為:數(shù)據(jù)層融合、特征層融合和決策層融合[11]。隨著融合深度的增加,對多源異構(gòu)數(shù)據(jù)容納能力增強(qiáng),數(shù)據(jù)處理速度和實(shí)時性提高,但是也面臨著融合信息丟失增多,精度變差的問題。由于多磁力計模型中8顆磁力計屬于同構(gòu)傳感器,數(shù)據(jù)之間關(guān)聯(lián)性強(qiáng),適合在數(shù)據(jù)層融合。因此可將磁力計M1作為參考基準(zhǔn)來構(gòu)建卡爾曼濾波器數(shù)據(jù)融合模型:

(19)

式中:F為狀態(tài)轉(zhuǎn)移矩陣;C為狀態(tài)觀測矩陣;xk為系統(tǒng)狀態(tài)向量,代表磁力計M1的測量值;zk為系統(tǒng)觀測向量,代表8顆磁力計測量值;wk-1為過程噪聲;vk為測量噪聲。

由式(19)可知,C為24維矩陣,融合過程中會涉及高維矩陣的逆運(yùn)算,導(dǎo)致計算速度緩慢,占用內(nèi)存大。因此,本研究基于同構(gòu)多傳感器融合特征對算法模型進(jìn)行改進(jìn),提出了迭代融合方法,其單次融合過程如下:

預(yù)測:

(20)

更新1:

(21)

更新2:

(22)

按傳感器順序,一直到更新8:

(23)

將x8作為卡爾曼濾波器最終迭代計算結(jié)果輸出,代表一組多磁力計數(shù)據(jù)的融合結(jié)果。由于多顆磁力計之間屬于同構(gòu)傳感器,具有統(tǒng)一的融合空間,故融合順序并不影響最終結(jié)果。

通過上述改進(jìn)算法,矩陣運(yùn)算從高維降階到三維,迭代過程中的計算量減小,運(yùn)算速度得到提高。

3 實(shí)驗工作

3.1 仿真驗證

根據(jù)第1.1節(jié)和第1.2節(jié)中所構(gòu)建的系統(tǒng)模型和誤差模型設(shè)計了仿真實(shí)驗,以模擬多磁力計靜態(tài)采集地磁場數(shù)據(jù)。根據(jù)WMM-2020地磁模型獲取當(dāng)?shù)氐乩碜鴺?biāo)系下的三軸地磁場強(qiáng)度參考值Href=[33.279 9,-3.679 4,35.919 1]TμT,初始設(shè)置磁力計誤差矩陣與測量噪聲方差為

(24)

基于式(24)模型參數(shù)獲取仿真數(shù)據(jù),一組8顆磁力計的測量數(shù)據(jù)分布如圖4所示,分別展示了三維空間和XY平面兩個視角,數(shù)據(jù)呈現(xiàn)在同一平面的圓周分布特征,符合模型分析結(jié)果。

圖4 一組仿真磁場數(shù)據(jù)的分布特征

進(jìn)一步獲取多種姿態(tài)下的仿真數(shù)據(jù),對其進(jìn)行RLS在線誤差參數(shù)估計,得到的估計結(jié)果為

(25)

由式(25)可以看出,算法較好地估計出模型的誤差參數(shù),與初始設(shè)置的真實(shí)值十分接近。圖5為補(bǔ)償前后的仿真數(shù)據(jù)分布對比。由圖可見,補(bǔ)償后的數(shù)據(jù)較好地分布在球面上,驗證了補(bǔ)償算法的有效性。當(dāng)然,對于最終的航向角解算精度能夠有多少提升還待進(jìn)一步的測試。

圖5 補(bǔ)償前后的仿真磁場數(shù)據(jù)空間分布

在誤差補(bǔ)償完成后進(jìn)行多磁力計的數(shù)據(jù)融合以及航向角解算,測試航向角精度,具體結(jié)果如表1所示。

表1 仿真數(shù)據(jù)航向角解算結(jié)果對比與誤差水平

由表1可見,補(bǔ)償融合后的航向角解算值誤差基本在±0.5°內(nèi),航向角標(biāo)準(zhǔn)差在0.2°內(nèi),符合預(yù)期效果,表明通過誤差補(bǔ)償與融合算法能有效降低誤差影響,較為精準(zhǔn)地輸出航向角信息。

3.2 實(shí)際測試與結(jié)果分析

第3.1節(jié)已通過仿真實(shí)驗驗證了多磁力計模型和算法的有效性,本節(jié)將進(jìn)一步在真實(shí)環(huán)境下測試所提出補(bǔ)償融合算法的效果。

3.2.1 實(shí)驗設(shè)備與標(biāo)定工作

本實(shí)驗選用自主設(shè)計的多磁力計電子羅盤模塊,它主要包括8顆三軸磁力計(LSM303DLHC),一顆六軸慣性器件(BMI088,目前僅使用加速度計),一顆MCU芯片(STM32F405RGT6),設(shè)備實(shí)物圖如圖6所示。使用時,采用計算機(jī)作為上位機(jī)接收測量數(shù)據(jù)并進(jìn)行預(yù)處理,之后基于MATLAB軟件進(jìn)行誤差補(bǔ)償、數(shù)據(jù)融合及航向角解算工作。

圖6 多磁力計電子羅盤實(shí)物圖

在正式測量前還需要完成各傳感器的標(biāo)定,以補(bǔ)償自身零偏和刻度系數(shù)。對于磁力計而言,主要是由于載體自身磁材料所攜帶磁場導(dǎo)致磁力計三軸產(chǎn)生的零偏,因此,采用在屏蔽桶環(huán)境下(可屏蔽地磁場)進(jìn)行測量標(biāo)定。而加速度計需要標(biāo)定出各軸的零偏以及刻度系數(shù)偏差,故依靠水平轉(zhuǎn)臺對各軸以此進(jìn)行標(biāo)定[12]。標(biāo)定設(shè)備如圖7所示。

圖7 磁力計和加速度計初始標(biāo)定設(shè)備

標(biāo)定完成后,同步開展多項實(shí)驗對所提出的模型和算法進(jìn)行實(shí)際測試驗證。

3.2.2 良好磁場環(huán)境下地磁場測量實(shí)驗

首先選擇在空曠地帶,良好磁場環(huán)境下進(jìn)行數(shù)據(jù)采集,驗證多磁力計實(shí)測地磁場數(shù)據(jù)是否符合模型分析特征,并與仿真結(jié)果進(jìn)行比對,進(jìn)一步驗證仿真實(shí)驗的合理性。

其中單組實(shí)測數(shù)據(jù)分布如圖8、9所示。從三維空間和XY平面看其分布特征均與仿真結(jié)果一致,其三軸測量值關(guān)系也符合多磁力計數(shù)學(xué)模型。

圖8 一組實(shí)測地磁場數(shù)據(jù)的分布

圖9 一組磁力計的三軸地磁測量數(shù)據(jù)

圖10為多組不同姿態(tài)下地磁場測量數(shù)據(jù)的三維空間分布圖。由圖可見,數(shù)據(jù)基本分布在參考球面上,符合無干擾條件下地磁場測量值球面分布特征。

圖10 多組不同姿態(tài)地磁場測量數(shù)據(jù)空間分布

通過觀察良好環(huán)境下測量數(shù)據(jù)的分布特征,可以初步確定多磁力計實(shí)際測量結(jié)果與理論分析、仿真結(jié)果基本一致,設(shè)備實(shí)測數(shù)據(jù)的有效性得到了保證。

3.2.3 干擾磁場環(huán)境下地磁場測量實(shí)驗

選取存在干擾磁場的環(huán)境進(jìn)行實(shí)驗,從而對提出的補(bǔ)償與融合算法效果進(jìn)行驗證。首先采集多個不同姿態(tài)下的磁場數(shù)據(jù),用于磁場誤差補(bǔ)償。補(bǔ)償完成后,基于轉(zhuǎn)臺和高精度基準(zhǔn)塊進(jìn)行相對角度測量,以此分析比較設(shè)備輸出航向角的精度。

圖11為RLS補(bǔ)償前后的多磁力計實(shí)測數(shù)據(jù)。由圖可見,補(bǔ)償后的數(shù)據(jù)能夠很好地分布在參考球面上,效果較理想。RLS算法迭代過程中誤差收斂情況如圖12所示,在迭代初期快速收斂,并降低到較低的水平。誤差補(bǔ)償完成后,對單個磁力計原始測量數(shù)據(jù)、單個磁力計補(bǔ)償后測量數(shù)據(jù)、多磁力計補(bǔ)償融合后測量數(shù)據(jù)分別進(jìn)行航向角解算,并做誤差分析,對應(yīng)解算的航向角分別用agl1、agl2、agl3表示。實(shí)驗結(jié)果如表2-4所示,分別展示了在多組測量實(shí)驗下3個航向角解算結(jié)果及其誤差與標(biāo)準(zhǔn)差。

表2 航向角agl1解算結(jié)果對比與誤差水平

圖11 補(bǔ)償前后的實(shí)測磁場數(shù)據(jù)空間分布

圖12 RLS誤差收斂曲線

由表2-4分析可知,原始單磁力計測量數(shù)據(jù)解算航向角存在較大誤差,無法直接作為導(dǎo)航數(shù)據(jù)使用,而在經(jīng)過誤差補(bǔ)償后,表3中展示的航向角誤差水平大幅度減小,在±1.5°以內(nèi),航向角標(biāo)準(zhǔn)差基本在0.3°以內(nèi),可以較為穩(wěn)定地輸出準(zhǔn)確的航向信息。表4經(jīng)過多磁力計補(bǔ)償融合后,航向角誤差進(jìn)一步降低,均在±0.5°以內(nèi),航向角標(biāo)準(zhǔn)差在0.2°以內(nèi),達(dá)到較高的精度和穩(wěn)定性。圖13為不同航向角在各組測試中誤差和標(biāo)準(zhǔn)差對比。由圖可見,多磁力計測量模型相較于單個磁力計具有更低的誤差和更穩(wěn)定的輸出,性能得到大幅提升。

表3 航向角agl2解算結(jié)果對比與誤差水平

表4 航向角agl3解算結(jié)果對比與誤差水平

圖13 不同測量航向角誤差與標(biāo)準(zhǔn)差對比

4 結(jié)束語

本文提出了基于多磁力計的測量模型,分析其數(shù)學(xué)關(guān)系與誤差模型,并根據(jù)模型特征設(shè)計了基于RLS-KF的多磁力計補(bǔ)償融合算法,實(shí)現(xiàn)了在迭代過程中實(shí)時更新補(bǔ)償參數(shù)并完成數(shù)據(jù)融合解算。設(shè)計仿真實(shí)驗驗證了多磁力計模型的合理性與補(bǔ)償融合算法的有效性。通過實(shí)際環(huán)境測試實(shí)驗證明,在良好的磁場環(huán)境下,實(shí)際測量得到的數(shù)據(jù)分布符合預(yù)分析模型特征。在存在干擾磁場的環(huán)境中,經(jīng)過誤差補(bǔ)償與融合算法后,航向角誤差在±0.5°以內(nèi),標(biāo)準(zhǔn)差在0.2°以內(nèi),相較于同型號單顆磁力計測量結(jié)果,精度提升了3倍左右。本文所提出的多磁力計模型及其補(bǔ)償融合算法能有效提升航向角的測量精度和穩(wěn)定性,帶來更高的可靠性。