一種新的電子羅盤校準(zhǔn)算法研究*

晁正正,張曉明,馬喜宏,呂 辰,陳 雷,李小燕

(中北大學(xué)電子測(cè)試技術(shù)科技重點(diǎn)實(shí)驗(yàn)室,太原 030051)

在地磁導(dǎo)航中,能夠準(zhǔn)確的獲取地磁場(chǎng)信息至關(guān)重要,通常采用MEMS三軸磁強(qiáng)計(jì)(TAM)進(jìn)行地磁場(chǎng)矢量測(cè)量,將采集到的地磁場(chǎng)矢量信息進(jìn)行姿態(tài)解算或者以偏航角形式輔助慣性導(dǎo)航。由于TAM通常具有刻度系數(shù)、零點(diǎn)和三軸不正交性等制造誤差,這些因素最終導(dǎo)致了電子羅盤具有大約1°～2°的誤差。此外,TAM不可避免的受到周圍鐵磁材料干擾,嚴(yán)重影響電子羅盤航向角解算精度。針對(duì)這些誤差,常用的TAM標(biāo)定補(bǔ)償技術(shù)有多傳感器融合方法、橢圓擬合方法、橢球擬合方法等。多傳感器融合方法融合了多個(gè)傳感器獲得的量測(cè)數(shù)據(jù)(含有TAM),但是標(biāo)定結(jié)果嚴(yán)重受其他傳感器數(shù)據(jù)影響;橢圓擬合只適用于二維平面運(yùn)動(dòng)下的標(biāo)定,具有一定的局限性;而橢球擬合方法是一種新穎的標(biāo)定方法,它將誤差模型與橢球方程系數(shù)聯(lián)系起來,從而將其系數(shù)標(biāo)定出來進(jìn)行磁補(bǔ)償。傳統(tǒng)的基于迭代遞推思想的橢球擬合方法受初始狀態(tài)影響易陷入局部最優(yōu)解,從而導(dǎo)致磁補(bǔ)償不準(zhǔn)確。

針對(duì)上述問題,退火算法作為一種模擬進(jìn)化算法,可用來誤差模型參數(shù)的最優(yōu)辨識(shí),在跳出局部參數(shù)最優(yōu)情況下實(shí)現(xiàn)全局參數(shù)最優(yōu)估計(jì),實(shí)現(xiàn)參數(shù)的準(zhǔn)確估計(jì)[1-2]。因此本文提出了基于模擬退火算法的橢球擬合算法來提高電子羅盤測(cè)量精度,仿真和試驗(yàn)結(jié)果驗(yàn)證了本方法的有效性。

1 TAM的誤差分析和建模

TAM的誤差分為傳感器誤差、磁材料干擾誤差和安裝誤差[3]。其中傳感器誤差由刻度系數(shù)、零點(diǎn)和不正角三種誤差組成。磁材料干擾誤差由軟磁干擾和硬磁干擾誤差組成。軟磁干擾主要影響TAM刻度系數(shù)矩陣,硬磁干擾主要影響TAM零點(diǎn)偏移矩陣。

本文建立了所有誤差源在內(nèi)的TAM模型為:

(1)

考慮到這些誤差之間相互作用對(duì)TAM的干擾[5],本文根據(jù)橢球擬合方法簡(jiǎn)化誤差模型為:

(2)

式中:A為安裝誤差與不正交角誤差矩陣;K為TAM的軟磁干擾和刻度系數(shù)誤差矩陣;P為TAM的硬磁干擾和零點(diǎn)偏移矩陣。通過標(biāo)定出以上三個(gè)誤差矩陣即可實(shí)現(xiàn)對(duì)TAM測(cè)量值的補(bǔ)償[6-8]。

1.1 橢球體模型擬合

在軸間存在不正交角的情況下,TAM誤差模型為:

(3)

當(dāng)TAM在某一固定地理位置作各種姿態(tài)變化時(shí),將地磁場(chǎng)矢量視為一常矢量,其磁場(chǎng)強(qiáng)度為一常值,因此有:

(4)

而橢球的一般方程為:

(5)

對(duì)比上式,設(shè):

(6)

C2=-2KKTP=[lmn]T

(7)

用9個(gè)未知變量表示K矩陣和P矩陣則有:

(8)

K=VDV-1

(9)

(10)

式中:D=diag(d1d2d3)的元素是矩陣C1特征值的平方根,V是矩陣C1特征向量組成矩陣。因此TAM制造誤差補(bǔ)償步驟為:首先,由采集的原始數(shù)據(jù)確定式(6)中的橢球參數(shù)。然后用式(8)和式(9)確定矩陣K和P。最后通過式(10)補(bǔ)償輸出,得到了地磁場(chǎng)矢量測(cè)量信息的校準(zhǔn)。

1.2 偏差模型

偏差模型中主要包括TAM軸間不正交角誤差和系統(tǒng)安裝誤差。如圖1所示,實(shí)線表示地心地固坐標(biāo)系的軸線,虛線表示TAM實(shí)際三軸軸線。在地心地固坐標(biāo)系中,z軸與傳感器坐標(biāo)系三軸之間夾角分別表示為a,b,c。當(dāng)傳感器在地心地固坐標(biāo)系內(nèi)繞z軸旋轉(zhuǎn)時(shí),TAM的測(cè)量值滿足:

(11)

(12)

同理可得Rx和Ry,因此可以通過式(13)求出A,通過式(14)進(jìn)行不正交角和系統(tǒng)安裝誤差角補(bǔ)償?shù)玫礁鼮榫_地磁場(chǎng)矢量測(cè)量信息[9-12]。

A=[RxRyRz]

(13)

(14)

圖1 三軸磁強(qiáng)計(jì)的非正交性和安裝誤差

2 模型參數(shù)的最優(yōu)估計(jì)

模擬退火算法的原理和固體退火的原理近似:將熱力學(xué)的理論套用到數(shù)據(jù)統(tǒng)計(jì)學(xué)上,隨著固體被加熱至最高溫,其內(nèi)部粒子變?yōu)殡s亂無(wú)序狀態(tài),內(nèi)能變大,在其漸漸冷卻過程中,固體內(nèi)部粒子逐漸趨于恒定狀態(tài),使其處于每個(gè)溫度時(shí)都能達(dá)到平衡狀態(tài),最后在常溫時(shí)達(dá)到基態(tài),內(nèi)能減為最小的狀況類比為參數(shù)尋優(yōu)中的收斂過程[13]。

根據(jù)Metropolis準(zhǔn)則,模擬退火算法接受新解的概率為:

(15)

將內(nèi)能E類比為評(píng)價(jià)函數(shù)f,溫度T的下降類比為算法迭代次數(shù)k的增加,即得到求解參數(shù)優(yōu)化問題的模擬退火算法:由初始解θ0和初始k開始,對(duì)當(dāng)前解重復(fù)“產(chǎn)生參數(shù)新狀態(tài)—計(jì)算評(píng)價(jià)函數(shù)差—接受或舍棄”的迭代,并逐步緩慢衰減T值,算法終止時(shí)的當(dāng)前狀態(tài)即為所得近似全局最優(yōu)解。模擬退火過程由參數(shù)的初值T0,衰減因子R,每個(gè)T值時(shí)的迭代次數(shù)L和停止條件Tf參數(shù)控制。其總體流程圖如圖2所示。

圖2 模擬退火算法流程圖

2.1 評(píng)價(jià)函數(shù)

要準(zhǔn)確的估計(jì)TAM軟磁干擾和刻度系數(shù)誤差矩陣,硬磁干擾和零點(diǎn)偏移向量有關(guān)的10個(gè)參數(shù),利用最小二乘法構(gòu)造評(píng)價(jià)函數(shù):

(16)

2.2 模擬退火算法具體步驟

Step 1 狀態(tài)初始化:初始溫度T0,初始狀態(tài)參數(shù)θ0,每個(gè)溫度T下的迭代次數(shù)L。

Step 2 外循環(huán):判斷T

Step 3 內(nèi)循環(huán)

Step 3.1 初始化內(nèi)循環(huán)次數(shù)L=0。

Step 3.3L=L+1,判斷L是否達(dá)到設(shè)定次數(shù),若是則退出內(nèi)循環(huán),否則轉(zhuǎn)Step 3.1。

Step 4 降溫處理,令T=RT,本文取R=0.99,轉(zhuǎn)Step 2。

3 試驗(yàn)及結(jié)果分析

3.1 計(jì)算機(jī)仿真實(shí)驗(yàn)

利用MATLAB進(jìn)行仿真設(shè)置:利用IGRF模型及當(dāng)?shù)亟?jīng)緯度、海拔高度參數(shù),得到當(dāng)?shù)氐卮艌?chǎng)三分量(單位:nT)為:He=[29 061.7 -2 684.3 45 662.8]T。仿真中令俯仰角、滾轉(zhuǎn)角、偏航角分別做1°,300轉(zhuǎn)弧度,0～360°;300轉(zhuǎn)弧度,1°,0～360°;300轉(zhuǎn)弧度,91°,0～360°角度變化,模擬標(biāo)定實(shí)驗(yàn)過程。設(shè)置誤差干擾參數(shù)a=1,b=1.5,c=2,d=1,e=1.5,f=2,l=1,m=1.5,n=2,生成磁干擾數(shù)據(jù)。設(shè)置初始參數(shù)θ0=[3 3 3 3 3 3 3 3 3 3]T,內(nèi)循環(huán)設(shè)置為L(zhǎng)=5,進(jìn)行模擬退火橢球方程參數(shù)估計(jì),估計(jì)結(jié)果如圖3所示。

如圖3所示,退火算法在初始溫度較大情況下,參數(shù)收斂波動(dòng)較大,隨著溫度T0呈指數(shù)下降,每一時(shí)刻溫度T下接收新狀態(tài)參數(shù)的概率越低,磁干擾誤差參數(shù)趨于穩(wěn)定,最后θopt=[1.009 3 1.500 7 2.003 3 1.002 4 1.498 4 2.0017 0.999 3 1.495 8 2.006 7 0.310 3]T,與初始設(shè)置的干擾誤差系數(shù)基本一致,驗(yàn)證了此算法的有效性。

圖3 模擬退火參數(shù)估計(jì)

將估計(jì)好的參數(shù)代入式(10),進(jìn)行地磁場(chǎng)測(cè)量矢量的校準(zhǔn),由圖4所示,橢圓體被校正成球形,軟磁干擾和刻度系數(shù)誤差得到了補(bǔ)償校準(zhǔn);球型歸于原點(diǎn),硬磁干擾和零點(diǎn)偏移得到了補(bǔ)償校準(zhǔn)。補(bǔ)償前后三軸誤差均值為1 000 nT以內(nèi),方差為500 nT以內(nèi),此算法具有明顯補(bǔ)償效果,校準(zhǔn)前后對(duì)比圖如圖4所示。

圖4 磁場(chǎng)測(cè)量值補(bǔ)償前后對(duì)比圖

表1 三軸補(bǔ)償前后誤差均值與方差

3.2 半物理試驗(yàn)驗(yàn)證

本文使用的電子羅盤系統(tǒng),其硬件系統(tǒng)包括基于ARM公司開發(fā)的Cortex-M4F內(nèi)核的STM32F405RG主控芯片,具有32位的微處理器最高時(shí)鐘可達(dá)168MHz,指令處理速度大210DMIPS,高主頻可以保證信號(hào)采集中的快速獲取。IMU模塊使用MPU9255中的三軸加速度計(jì)和三軸陀螺儀,其中加速度計(jì)具有可編程參數(shù)選擇±2gn,±4gn,±8gn,±16gn,本系統(tǒng)令A(yù)FS_SEL=1選擇±4gn量程;陀螺儀具有可編程參數(shù)選擇±250 °/s,±500 °/s,±1 000 °/s,±2 000 °/s,本系統(tǒng)令FS_SEL=1選擇±500 °/s量程,分別具有16位AD輸出數(shù)字量使其免去ADC采集精度的下降,而且陀螺儀和加速度計(jì)一體化結(jié)構(gòu)可以避免繁瑣的芯片選擇和外設(shè)成本,保證最佳性能。TAM模塊選擇霍尼韋爾公司的HMC5983芯片,該芯片表面封裝12位AD與地干擾AMR傳感器,能在±8 gauss的磁場(chǎng)中實(shí)現(xiàn)2 mgauss的分辨率。主控芯片通過SPI模式將采集回來的加速度計(jì),陀螺儀和磁強(qiáng)計(jì)數(shù)據(jù)存儲(chǔ)到SD卡中進(jìn)行數(shù)據(jù)存儲(chǔ)。

本文實(shí)驗(yàn)地點(diǎn)在開闊地段任意旋轉(zhuǎn)電子羅盤,以獲得不同姿態(tài)下的地磁場(chǎng)測(cè)量數(shù)據(jù)。利用該算法準(zhǔn)確擬合出估計(jì)參數(shù),并繪制出藍(lán)色原始數(shù)據(jù)和漸變紫色擬合橢球面的對(duì)照?qǐng)D。如圖5所示,每個(gè)原始數(shù)據(jù)點(diǎn)幾乎位于擬合橢球面上,表明橢球體擬合良好,即K和P可由式(8)和式(9)確定。

圖5 原始數(shù)據(jù)和擬合橢球面

然后分別將系統(tǒng)的x、y和z軸調(diào)整為垂直向上,旋轉(zhuǎn)電子羅盤系統(tǒng),通過系統(tǒng)獲得測(cè)量數(shù)據(jù),由式(13)求得A。則原始數(shù)據(jù)可以通過式(10)和式(14)來修正。

如圖6所示,紅色虛線所在的橢圓體表示為原始數(shù)據(jù)擬合所得。底部的紅色粗實(shí)線表示由z軸垂直方向旋轉(zhuǎn)的原始數(shù)據(jù)所組成的圓。藍(lán)色實(shí)線描繪了符合修正數(shù)據(jù)的球體。橢圓體明顯地平移和拉伸,z軸最大值明顯大于地磁場(chǎng)模值(54 193 nT)。但球體幾乎是中心對(duì)稱的,它的半徑大約等于地磁場(chǎng)強(qiáng)度。虛線所在的平面明顯不平行于x-y平面,也不正交于z軸。修正后的實(shí)線所在平面幾乎與z軸正交。由圖6可以看出,經(jīng)過標(biāo)定過程后,地磁場(chǎng)測(cè)量矢量的位置、方向和大小都得到了很好的校正。

圖6 補(bǔ)償前和補(bǔ)償后數(shù)據(jù)

最后,我們將電子羅盤固定在無(wú)磁轉(zhuǎn)臺(tái)上,手動(dòng)將轉(zhuǎn)臺(tái)打到俯仰角30°,保持水平旋轉(zhuǎn)轉(zhuǎn)臺(tái)一周,每隔10°取點(diǎn),共取36個(gè)點(diǎn),做出補(bǔ)償前后航向角誤差曲線,如圖7所示。由圖7可知,通過補(bǔ)償前后航向角與理論值的對(duì)比,實(shí)驗(yàn)表明電子羅盤受干擾模型中的誤差對(duì)航向角的解算有很大影響,補(bǔ)償前航向角最大誤差為4.5°,補(bǔ)償后的航向角誤差為0.4°,補(bǔ)償后航向角的精度提高了一個(gè)數(shù)量級(jí)。

圖7 補(bǔ)償前后航向角的誤差曲線

4 結(jié)論

本文通過對(duì)TAM誤差源詳細(xì)分析,建立了誤差模型,提出了基于模擬退火算法的橢球參數(shù)擬合標(biāo)定補(bǔ)償方法,提出了一套完整的TAM標(biāo)定補(bǔ)償方法,并將其應(yīng)用于某電子羅盤系統(tǒng)中。試驗(yàn)結(jié)果表明,電子羅盤系統(tǒng)的航向解算精度得到了顯著提高,表明該方法對(duì)TAM在導(dǎo)航系統(tǒng)中的應(yīng)用具有一定的參考價(jià)值。