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

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

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

0 引言

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

1 多磁力計系統模型與誤差分析

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

1.1 多磁力計系統模型

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

圖1 多磁力計系統模型結構圖

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

(1)

根據式(1)可知多磁力計一組測量值的分布特征,但是其姿態關系還需根據加速度計測量值進行分析。當加速度計A1測量值為

(2)

由于同軸關系,由此可計算得到磁力計M1的姿態角[6]為

(3)

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

圖2 載體姿態角定義示意圖

(4)

(5)

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

結合式(3)、(5)進行推導計算,最終可得相鄰磁力計之間姿態角關系為

(6)

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

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

1.2 多磁力計誤差分析

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

(7)

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

(8)

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

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

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

磁力計誤差補償的基本原理是利用理想的磁場測量數據球面分布這一特征對實測數據進行橢球擬合,從而得到橢球方程各項系數,再進一步轉化得到誤差補償參數。常用的橢球擬合算法有最小二乘法[8]、高斯-牛頓法[9]等經典算法。但此類算法涉及到矩陣求逆運算,計算量大且非實時處理,內存占用大。因為8顆磁力計同時產生的數據量較大,并不適用,因此選擇了能夠迭代在線更新、計算速度更快的RLS方法。在數據融合方面,對于同構多傳感器融合選用了基于卡爾曼濾波器的融合方案。所使用到的算法整體結構框圖如圖3所示。

圖3 補償融合算法結構流程圖

2.1 RLS在線誤差參數估計

(9)

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

當地地磁場強度參考值為href,在固定地理位置下,可將其地磁場矢量模值視為常值,構建誤差方程為

(10)

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

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

(11)

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

為了進行線性化處理,規定:

(12)

式中k=1,2,…表示測量次數。

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

Zk=GkXk+vk

(13)

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

根據RLS算法計算步驟如下:

(14)

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

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

進一步將橢球參數轉化為磁力計誤差補償參數。首先將式(11)化為

hkTDhk+(2E)Thk+U=0

(15)

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

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

(16)

(17)

聯合式(16)、(17)結果,有

(18)

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

2.2 多磁力計數據融合

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

(19)

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

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

預測:

(20)

更新1:

(21)

更新2:

(22)

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

(23)

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

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

3 實驗工作

3.1 仿真驗證

根據第1.1節和第1.2節中所構建的系統模型和誤差模型設計了仿真實驗,以模擬多磁力計靜態采集地磁場數據。根據WMM-2020地磁模型獲取當地地理坐標系下的三軸地磁場強度參考值Href=[33.279 9,-3.679 4,35.919 1]TμT,初始設置磁力計誤差矩陣與測量噪聲方差為

(24)

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

圖4 一組仿真磁場數據的分布特征

進一步獲取多種姿態下的仿真數據,對其進行RLS在線誤差參數估計,得到的估計結果為

(25)

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

圖5 補償前后的仿真磁場數據空間分布

在誤差補償完成后進行多磁力計的數據融合以及航向角解算,測試航向角精度,具體結果如表1所示。

表1 仿真數據航向角解算結果對比與誤差水平

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

3.2 實際測試與結果分析

第3.1節已通過仿真實驗驗證了多磁力計模型和算法的有效性,本節將進一步在真實環境下測試所提出補償融合算法的效果。

3.2.1 實驗設備與標定工作

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

圖6 多磁力計電子羅盤實物圖

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

圖7 磁力計和加速度計初始標定設備

標定完成后,同步開展多項實驗對所提出的模型和算法進行實際測試驗證。

3.2.2 良好磁場環境下地磁場測量實驗

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

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

圖8 一組實測地磁場數據的分布

圖9 一組磁力計的三軸地磁測量數據

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

圖10 多組不同姿態地磁場測量數據空間分布

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

3.2.3 干擾磁場環境下地磁場測量實驗

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

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

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

圖11 補償前后的實測磁場數據空間分布

圖12 RLS誤差收斂曲線

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

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

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

圖13 不同測量航向角誤差與標準差對比

4 結束語

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