基于EKF算法的三軸磁力儀一體化補(bǔ)償方法*

孫 歡，楊賓峰，吳 昊，李 馳，王 潤

(空軍工程大學(xué)信息與導(dǎo)航學(xué)院， 西安 710077)

0 引言

地磁場作為一個(gè)天然的物理場，具有全地域、全天候、全天時(shí)等特點(diǎn)，以地磁場為基礎(chǔ)的地磁導(dǎo)航，相比于傳統(tǒng)無線電導(dǎo)航和衛(wèi)星導(dǎo)航，具有隱蔽性好，自主性高，抗干擾能力強(qiáng)的特點(diǎn)；相比于地形匹配導(dǎo)航和天文導(dǎo)航，地磁導(dǎo)航受天氣影響小，地磁場隨時(shí)間變化緩慢，因此導(dǎo)航定位更穩(wěn)定[1-2]。但是在實(shí)際測量中，在諸多因素的影響下，磁場測量結(jié)果將會(huì)受到多方面的干擾。首先，三軸磁力儀制造及在安裝載體過程中存在工差；其次，載體內(nèi)部設(shè)備結(jié)構(gòu)將形成硬磁誤差和軟磁誤差；再者，對于頻繁做姿態(tài)變換的載體，比如戰(zhàn)斗機(jī)，機(jī)體姿態(tài)轉(zhuǎn)動(dòng)會(huì)產(chǎn)生較強(qiáng)的渦流，進(jìn)而產(chǎn)生渦流磁場，也會(huì)極大干擾磁力儀的測量[3-9]。

針對三軸磁力儀的標(biāo)定和誤差補(bǔ)償問題，國內(nèi)外學(xué)者進(jìn)行了諸多研究，Geber-Egziabher提出two-step算法補(bǔ)償三軸磁力儀刻度因子誤差、零偏誤差和硬磁誤差[10]；Vasconcelos對三軸磁力儀誤差和安裝誤差進(jìn)行一體化建模，并提出用橢球面旋轉(zhuǎn)、尺度變換、平移變換的方法進(jìn)行估計(jì)，并設(shè)計(jì)了基于最大似然算法的自校正算法[11]；文獻(xiàn)[12-13]對三軸磁力儀的儀表誤差、安裝誤差和軟硬磁誤差分別進(jìn)行建模，后者采用ANSYS軟件仿真建模，并采用最小二乘法進(jìn)行補(bǔ)償；文獻(xiàn)[14]對三軸磁力儀本身誤差和軟硬磁誤差進(jìn)行一體化建模，并考慮到觀測向量和數(shù)據(jù)矩陣均存在誤差，因此用總體最小二乘進(jìn)行了補(bǔ)償；針對載體姿態(tài)變換的問題，文獻(xiàn)[15] 采用COMSOL Multiphysics仿真軟件建立了載體渦流干擾場的仿真模型，并進(jìn)行定性分析；文獻(xiàn)[16-18]建立等效磁強(qiáng)計(jì)誤差模型，并采用兩步估計(jì)算法和圓約束非對準(zhǔn)誤差估計(jì)算法實(shí)現(xiàn)了補(bǔ)償。

通過分析，以往的研究存在以下問題：1)模型中沒有考慮渦流磁場的影響，對于低速、姿態(tài)穩(wěn)定的載體尚可補(bǔ)償，但對于高速、頻繁進(jìn)行姿態(tài)變換的載體，其渦流磁場的影響將會(huì)降低補(bǔ)償精度；2)在以往研究所普遍采用的補(bǔ)償方法中，最小二乘算法解算依賴大量先采數(shù)據(jù)，對數(shù)據(jù)量敏感度高，且不能做到實(shí)時(shí)補(bǔ)償；橢圓約束法和橢球約束法只適用于以軟磁硬磁為核心的簡單建模方式，沒有考慮儀表、安裝、渦流所帶來的誤差。文中對三軸磁力儀的儀表誤差和安裝誤差進(jìn)行了分析，結(jié)合硬磁誤差和軟磁誤差進(jìn)行統(tǒng)一處理，考慮到飛行器載體進(jìn)行頻繁姿態(tài)變換的現(xiàn)狀，引進(jìn)渦流磁場進(jìn)行一體化建模，并采用EKF(擴(kuò)展卡爾曼濾波)算法進(jìn)行補(bǔ)償，最后通過模擬仿真和實(shí)驗(yàn)來檢驗(yàn)建模方法和補(bǔ)償方法的實(shí)用性和有效性。

1 地磁場觀測值誤差分析

在磁力儀標(biāo)定過程中，三軸磁力儀的三軸x′、y′、z′和真實(shí)三軸x、y、z的不完全重合，將會(huì)造成非正交誤差，從而影響磁力儀的測量精度。令z軸和z′軸重合，yoz和y′oz′共面， 記oy軸和oy′軸之間的夾角為β，ox軸和面x′oy′之間的夾角為γ，ox軸和面x′oz′之間的夾角為α，其對應(yīng)關(guān)系如圖1所示。

圖1 三軸非正交誤差

則非正交誤差矩陣CNO可以表示為：

(1)

在磁力儀安裝過程中，原則上傳感器坐標(biāo)系應(yīng)該與載體坐標(biāo)系嚴(yán)格一致，但是在安裝過程中，將不可避免的出現(xiàn)安裝誤差，從而造成安裝誤差，設(shè)磁力儀三軸與載體坐標(biāo)系三軸的誤差角為εx、εy、εz，則安裝誤差CM為：

(2)

由于制作工藝精度的問題，三軸磁力儀三軸存在細(xì)微差異，由此產(chǎn)生刻度因子誤差，設(shè)三軸的刻度因子誤差分別為sx、sy、sz，則刻度因子誤差矩陣CSF為：

CSF=diag(sx,sy,sz)

(3)

制作工藝精度和安裝誤差同樣也會(huì)造成零刻度漂移誤差w和剩磁誤差bs，矩陣表達(dá)式分別為：

w=[δ1,δ2,δ3]T，bs=[bx1,by1,bz1]T

(4)

硬磁誤差是載體內(nèi)部硬磁材料產(chǎn)生的磁場所造成的誤差，常見的硬磁材料有永磁體和高碳鋼，硬磁材料的磁場隨時(shí)間變化十分緩慢，在短時(shí)間內(nèi)可以認(rèn)為是時(shí)不變的，因此在3個(gè)軸上的投影為常值，記硬磁誤差Bh為：

Bh=[bh1,bh2,bh3]T

(5)

軟磁誤差是軟磁性材料被外部磁場激勵(lì)所產(chǎn)生的誤差，外部磁場包括地磁場和載體內(nèi)部所產(chǎn)生的磁場，在載體內(nèi)部磁場中，由于集成電路和磁場擾動(dòng)所產(chǎn)生的干擾磁場遠(yuǎn)遠(yuǎn)小于硬磁磁場和地磁場，因此只考慮硬磁磁場Bh和地磁場H的激勵(lì)效應(yīng)。軟磁材料被激勵(lì)產(chǎn)生的激勵(lì)磁場強(qiáng)度和外部激勵(lì)強(qiáng)度呈線性比例關(guān)系，比例系數(shù)CSI取決于軟磁材料的磁化特性，當(dāng)外部激勵(lì)磁場方向發(fā)生變化時(shí)，所產(chǎn)生的激勵(lì)磁場方向也會(huì)隨之變化，因此軟磁誤差BSI可以表示為：

BSI=CSI(H+Bh)

(6)

綜上所述，地磁場的觀測值B可以表示為：

B=CMCNOCSFCSI(H+Bh)+w+bs+ε

(7)

式中：ε為零均值高斯地磁觀測噪聲，其方差為∑。將其化簡，則可以表示為：

B=CH+b+ε

(8)

式中：

C=CMCNOCSFCSI

(9)

b=CMCNOCSFCSIBh+w+bs

(10)

載體做姿態(tài)變換的過程中，由于自身做切割地球磁感線運(yùn)動(dòng)，根據(jù)法拉第電磁感應(yīng)定律，將會(huì)在載體內(nèi)部產(chǎn)生渦流，進(jìn)而產(chǎn)生渦流磁場，對磁力儀的測量產(chǎn)生影響，渦流磁場的強(qiáng)度與感應(yīng)渦流的強(qiáng)度成正比，而渦流的強(qiáng)度又與載體磁場變化率成正比，設(shè)該比例系數(shù)矩陣為P， 將其考慮在內(nèi)，可得到地磁場測量值的數(shù)學(xué)模型為：

B=CH+b+P(ΔB/Δt)+ε

(11)

2 補(bǔ)償算法研究

2.1 EKF算法

在利用卡爾曼濾波進(jìn)行誤差補(bǔ)償?shù)臅r(shí)候，如果出現(xiàn)了二次及二次以上的高次項(xiàng)，整個(gè)過程呈非線性，就不能用線性方法進(jìn)行處理。對于非線性系統(tǒng)，理論上很難推導(dǎo)嚴(yán)格的遞推濾波公式，因此常用近似的方法進(jìn)行處理。非線性系統(tǒng)的線性化濾波就是利用近似的方法研究非線性問題的途徑之一。EKF(擴(kuò)展卡爾曼濾波)就是一種典型的方法。采用EKF進(jìn)行處理時(shí)，先將非線性的傳遞函數(shù)f(·)和觀測函數(shù)h(·)進(jìn)行泰勒展開，并略去二階及以上的高次項(xiàng)，從而得到非線性系統(tǒng)的線性化模型，再用標(biāo)準(zhǔn)卡爾曼濾波進(jìn)行線性化處理。

由系統(tǒng)方程

(12)

Zk=h(Xk,k)+Vk

(13)

(14)

令

(15)

(16)

則狀態(tài)方程為:

(17)

同理，對非線性函數(shù)h(·)進(jìn)行處理，最終得觀測方程為：

Zk=HkXk+yk+Vk

(18)

利用卡爾曼基本方程可得:

(19)

(20)

(21)

(22)

Pk=[I-KkHk]Pk,k-1

(23)

其中，濾波初值和濾波誤差的協(xié)方差矩陣為：

(24)

(25)

2.2 EKF模型及校正原理

根據(jù)上文，地磁場觀測值模型如下：

B=CH+b+P(ΔB/Δt)+ε

(26)

由于在進(jìn)行姿態(tài)變換的過程中，B的不規(guī)則度較大，而地磁場真實(shí)值H的模值‖H‖不隨姿態(tài)變換而變化，且以B作為自變量，H作為因變量的模型解算出的參數(shù)在實(shí)際問題中可以不經(jīng)過變換直接使用，因此在研究中以‖H‖2作為研究對象，將上述模型進(jìn)行變換處理，得地磁場校正模型為：

Zk=‖H‖2=(Q(B-b-P(ΔB/Δt))+ε)2=
hk(Xk)+vk

(27)

式中：Q=C-1，則變量X為：

(28)

hk(Xk)=(B-b-P(ΔB/Δt))TQTQ(B-b-P(ΔB/Δt))

(29)

可見校正模型Zk是關(guān)于變量X的二次系統(tǒng)，而并非常規(guī)線性系統(tǒng)，采用EKF進(jìn)行降階處理。

vk=(2Q(B-b-P(ΔB/Δt)))Tε+‖ε‖2

(30)

觀測噪聲vk統(tǒng)計(jì)模型服從高斯分布，其均值和方差分別為：

μk=E[vk]=-tr(Σ)

(31)

Σ(Q(B-b-P(ΔB/Δt)))+2trΣ2)

(32)

Σ={εεT}

(33)

由于在此模型中，不含過程噪聲，只有觀測噪聲，因此EKF模型可以簡化為：

(34)

(35)

(36)

3 模擬仿真分析

在仿真實(shí)驗(yàn)中，根據(jù)上文地磁場觀測值的模型

B=CH+b+P(ΔB/Δt)+ε

其中ε為均值為0、方差為30的高斯觀測噪聲。設(shè)地磁場真實(shí)值H總量為50 000 nT，為了得到各個(gè)方向的磁場分量，設(shè)參數(shù)p=(-180:0.3:510)×π/180，q=(250:0.5:1400)×π/180，則地磁場分量值為：

根據(jù)上文校正模型利用EKF算法進(jìn)行校正， 為保證較好的權(quán)值收斂性，令所求參數(shù)初始值X0全為0，初始協(xié)方差矩陣P0為：

P0=107×I21×21

表1 參數(shù)估計(jì)值

各個(gè)參數(shù)均能收斂，由于參數(shù)較多，選取部分參數(shù)收斂過程，如圖2～圖7所示。

圖2 q11收斂過程

圖3 q12收斂過程

圖4 q13收斂過程

圖5 p31收斂過程

圖6 p32收斂過程

圖7 p33收斂過程

經(jīng)過補(bǔ)償，從數(shù)據(jù)值隨機(jī)抽取10組數(shù)據(jù)進(jìn)行比較，結(jié)果如表2所示。

由表2中數(shù)據(jù)可以看出，對地磁場觀測值進(jìn)行補(bǔ)償之后，誤差降低兩個(gè)數(shù)量級(jí)，測量精度得到明顯提高。經(jīng)過補(bǔ)償，數(shù)據(jù)的標(biāo)準(zhǔn)差明顯減小且趨于穩(wěn)定，如圖8所示，表明用EKF算法補(bǔ)償有效且穩(wěn)定。

采用不考慮渦流磁場的模型，同樣用EKF算法進(jìn)行補(bǔ)償，將所得結(jié)果與文中所得結(jié)果進(jìn)行比較，如圖9所示，可以看到，不考慮渦流磁場模型可以對地磁場實(shí)現(xiàn)補(bǔ)償，但效果欠佳，存在400 nT左右的誤差，并且會(huì)上下波動(dòng)，考慮渦流磁場的一體化模型將誤差降至80 nT以下，從而實(shí)現(xiàn)更高精度的補(bǔ)償。

表2 補(bǔ)償前后結(jié)果比較

圖8 補(bǔ)償前后標(biāo)準(zhǔn)差比較

圖9 不同模型補(bǔ)償結(jié)果比較

4 實(shí)驗(yàn)結(jié)果

在該實(shí)驗(yàn)中，采用Bartington公司的Mag-03MSL70三軸磁通門傳感器進(jìn)行補(bǔ)償，該磁通門傳感器能夠精確測量靜態(tài)或交變磁場，采用National Instruments軟件進(jìn)行數(shù)據(jù)采集。為了模擬飛行載體在飛行過程中的不同姿態(tài)，該實(shí)驗(yàn)采用3FHT30C三軸手動(dòng)無磁轉(zhuǎn)臺(tái)來提供姿態(tài)變換信息。后期用Matlab進(jìn)行數(shù)據(jù)處理。采用標(biāo)準(zhǔn)磁圖確定地磁場真實(shí)值。

選取一處磁場穩(wěn)定的郊外地點(diǎn)作為試驗(yàn)點(diǎn)，將磁通門傳感器置于無磁轉(zhuǎn)臺(tái)，快速轉(zhuǎn)動(dòng)并進(jìn)行全方位姿態(tài)信息測量。通過對比磁圖得到所處位置地磁場真實(shí)值總量為42 587 nT，將NI軟件所采數(shù)據(jù)套用文中所提出的模型，并采用EKF算法進(jìn)行誤差補(bǔ)償，補(bǔ)償結(jié)果如圖11所示，在補(bǔ)償前，由于各種因素的影響，誤差幅值達(dá)到1 800 nT左右，在用不考慮渦流磁場的模型補(bǔ)償后，誤差幅值明顯減小，但峰值誤差仍能達(dá)到500 nT，并且補(bǔ)償結(jié)果不穩(wěn)定，存在波動(dòng)趨勢，在用考慮渦流磁場的一體化模型補(bǔ)償之后，補(bǔ)償結(jié)果明顯趨于穩(wěn)定，峰值誤差減小到120 nT，證明了該補(bǔ)償模型和補(bǔ)償方法在實(shí)際應(yīng)用中的有效性。

圖10 三軸磁通門傳感器和無磁轉(zhuǎn)臺(tái)

圖11 三軸磁通門傳感器校正結(jié)果

5 結(jié)論

文中采用EKF算法對地磁場觀測值進(jìn)行補(bǔ)償，在建模過程中，在考慮磁力儀測量誤差和軟硬磁誤差的基礎(chǔ)上，結(jié)合飛行載體頻繁進(jìn)行姿態(tài)變換的實(shí)際情況，加入渦流磁場誤差進(jìn)行一體化建模，相比于以往研究，提出的方法具有以下優(yōu)點(diǎn)：1)一體化建模方式對于進(jìn)行頻繁姿態(tài)變換的載體，能夠?qū)崿F(xiàn)更高精度的補(bǔ)償；2)卡爾曼濾波算法相比于最常用的最小二乘算法，無須事先采集大量數(shù)據(jù)進(jìn)行解算，對數(shù)據(jù)量敏感度低，具有較強(qiáng)的魯棒性，并且可以實(shí)現(xiàn)即時(shí)補(bǔ)償；3)EKF相比于其他非線性算法，原理簡單，易于實(shí)現(xiàn)，降低了解算難度，有效縮短了補(bǔ)償時(shí)間。

在實(shí)驗(yàn)中，相比于不考慮渦流磁場的模型，一體化建模的模型在有效減小誤差的基礎(chǔ)上，濾除了姿態(tài)變換所引起的渦流磁場對于補(bǔ)償結(jié)果的波動(dòng)干擾，極大增強(qiáng)了補(bǔ)償結(jié)果的穩(wěn)定性。補(bǔ)償精度的提升和穩(wěn)定性的增加使得地磁導(dǎo)航在飛行載體的導(dǎo)航中有了更加廣闊的前景?？紤]到EKF算法不適用于強(qiáng)非線性問題，下一步將針對于在其線性化過程中忽略的二次及更高項(xiàng)所引起的誤差進(jìn)行研究。