基于最小二乘法和卡爾曼濾波算法的平面電子羅盤誤差補償方法研究

李 蓉，張一帆,楊薇薇

1.淮南職業(yè)技術(shù)學(xué)院信息與電氣工程系，安徽淮南,232001；2.安徽理工大學(xué)電氣與信息工程學(xué)院，安徽淮南,232001

相較于傳統(tǒng)指針式羅盤和平衡架結(jié)構(gòu)羅盤，電子羅盤(又稱數(shù)字指南針)具有能耗低、體積小、重量輕、精度高、可微型化等優(yōu)點，被廣泛運用于汽車、航海、無人機導(dǎo)航等領(lǐng)域[1]。電子羅盤在使用時，易受外界硬、軟磁干擾產(chǎn)生測量誤差，硬磁干擾通常會導(dǎo)致電子羅盤產(chǎn)生零點漂移；軟磁干擾則會導(dǎo)致隨機誤差的產(chǎn)生，硬磁干擾與軟磁干擾產(chǎn)生的誤差互相獨立。

目前對電子羅盤受到的硬、軟磁干擾產(chǎn)生的誤差進行補償?shù)姆椒ㄓ泻芏唷＠钪恰⑶貪热颂岢龌跈E圓假設(shè)的誤差補償方法[2-4]，可同時對電子羅盤的硬、軟磁干擾進行補償，其特點是精度較高，但計算量過大導(dǎo)致應(yīng)用場景有限。胡超等人提出的羅差補償系數(shù)法[5]只能對電子羅盤的軟磁干擾進行補償，無法補償電子羅盤受到的硬磁干擾，應(yīng)用場景單一。Fang J提出的基于最小二乘法(Least squares,LS)的橢圓擬合的誤差補償方法[6]，對應(yīng)用場景的要求較為苛刻，不具有廣泛性。Bonnet S提出了標(biāo)定傳感器采集數(shù)據(jù)，利用橢球擬合對誤差進行補償?shù)姆椒╗7]，但該方法忽略了傳感器自身噪聲的影響，從而導(dǎo)致精度較低。

針對現(xiàn)有電子羅盤誤差補償方法中存在計算量過大、應(yīng)用場景有限、補償精度較低等問題，提出了一種基于最小二乘法和卡爾曼濾波算法(LS-KF)的誤差補償方法，分別對電子羅盤的硬、軟磁干擾誤差進行補償，并結(jié)合Allan方差分析法對實驗結(jié)果進行定量分析。該方法可有效補償電子羅盤測量誤差，同時具有計算量小，應(yīng)用場景豐富的特點。

1 電子羅盤LS-KF誤差補償方法

1.1 電子羅盤的LS硬磁干擾誤差補償

電子羅盤在實際使用過程中會受到外部環(huán)境的硬磁干擾產(chǎn)生零點漂移，從而導(dǎo)致測量誤差的產(chǎn)生。可使用最小二乘法對硬磁干擾導(dǎo)致的誤差進行補償。

(1)

(2)

式(2)中，Sx，Sy分別表示電子羅盤真實值S的X軸方向和Y軸方向的向量。

(3)

式(3)中，Ox，Oy分別表示電子羅盤零點漂移量O的X軸方向和Y軸方向的數(shù)值，三個變量之間的關(guān)系可由一個數(shù)學(xué)模型來表示：

(4)

在理想狀態(tài)下，電子羅盤實測數(shù)據(jù)即為真實值，沒有測量誤差存在,Ox，Oy的值為零。STS為一常數(shù)U2，是為本地磁場強度平方

(5)

在實際測量環(huán)境中，Ox，Oy的值不為零，因此，實測電子羅盤第i個測量值誤差

(6)

可將式(6)簡寫為：

(7)

式(7)中

(8)

式(8)中,D2為電子羅盤任意一點真實值Sx(i)和Sy(i)平方的和。

假設(shè)電子羅盤任意一點測量值的坐標(biāo)到以電子羅盤零點漂移量Ox、Oy的數(shù)值為坐標(biāo)點的距離都相等。

由式(8)可知實測電子羅盤軌跡形如以電子羅盤零點漂移量Ox、Oy的數(shù)值為圓心的，半徑為D的標(biāo)準(zhǔn)圓。

將測量所得n組樣本帶入(6)式中并寫成矩陣形式有

r=Y-Fβ

(9)

式(9)中:

(10)

(11)

(12)

根據(jù)最小二乘法理論有

rTr=(Y-Fβ)T(Y-Fβ)

(13)

令J=rTr，設(shè)存在β，使得J=min對β求偏導(dǎo)，則有

(14)

可得

β=(FTF)-1(FTY)

(15)

將測量數(shù)據(jù)代入式(10)(11)中，可得矩陣Y、F的值；將Y、F代入式(15)中，可進一步求得矩陣β的值，從而可得電子羅盤零點漂移量Ox、Oy。

1.2 電子羅盤的KF軟磁干擾誤差補償

電子羅盤在實際使用過程中，受外部環(huán)境的軟磁干擾會產(chǎn)生隨機噪聲，隨機噪聲是導(dǎo)致測量誤差存在的主要原因。電子羅盤受軟磁干擾產(chǎn)生的隨機噪聲屬于高斯白噪聲。

卡爾曼濾波算法可對帶有高斯白噪聲的線性系統(tǒng)進行估計[8-10]，該濾波算法是目前應(yīng)用最為廣泛的一種濾波方法，在通信、導(dǎo)航、制導(dǎo)與控制等領(lǐng)域都得到了較好的應(yīng)用。其基本思想是以最小均方誤差準(zhǔn)則作為最佳估計準(zhǔn)則，通過遞推理論實現(xiàn)對系統(tǒng)未來狀態(tài)量的估計，使得估計值盡可能接近真實值。卡爾曼濾波算法的數(shù)學(xué)模型如下：

(16)

式中，X(k)為k時刻電子羅盤狀態(tài)量；Z(k)為k時刻電子羅盤的測量值；假設(shè)W(k)、V(k)是互相獨立的高斯白噪聲；其協(xié)方差分別為Q、R，W(k)為輸入噪聲；V(k)為觀測噪聲；B為狀態(tài)轉(zhuǎn)移矩陣。H是系統(tǒng)測量矩陣。

卡爾曼濾波具體遞推過程如下：

(1)利用系統(tǒng)k-1時刻的狀態(tài)預(yù)測系統(tǒng)k時刻的狀態(tài)：

X(k|k-1)=BX(k-1|k-1)

(17)

式(17)中，X(k|k-1)是利用系統(tǒng)k-1時刻的狀態(tài)預(yù)測得到的系統(tǒng)k時刻的狀態(tài)，X(k-1|k-1)是系統(tǒng)k-1時刻狀態(tài)的最優(yōu)結(jié)果。

(2)更新系統(tǒng)的協(xié)方差矩陣：

P(k|k-1)=BP(k-1|k-1)BT+Q

(18)

式中,P(k|k-1)是X(k|k-1)對應(yīng)的協(xié)方差，P(k-1|k-1)是X(k-1|k-1)對應(yīng)的協(xié)方差。

(3)計算系統(tǒng)k時刻狀態(tài)的最優(yōu)估算

X(k|k)=X(k|k-1)+Kg(k)(Z(K)-HX(k|k-1))

(19)

式中，Kg為系統(tǒng)的卡爾曼增益

(20)

(4)更新系統(tǒng)k時刻下系統(tǒng)狀態(tài)的協(xié)方差

P(k|k)=(1-Kg(k)H)P(k|k-1)

(21)

當(dāng)系統(tǒng)進入k+1狀態(tài)時，P(k|k)就是式(18)的P(k-1|k-1)。這樣，系統(tǒng)就可以通過不斷迭代的計算，更新各時刻的系統(tǒng)狀態(tài)量，從而得到系統(tǒng)狀態(tài)的最優(yōu)估計值。

在該研究模型中，將系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣B和系統(tǒng)的測量矩陣H設(shè)置為1，同樣，將系統(tǒng)的卡爾曼增益Kg以及系統(tǒng)的協(xié)方差P的初始值也設(shè)置為1，即可進行卡爾曼濾波。

2 基于Allan方差的誤差分析

Allan方差分析法是由David Allan[11]提出的一種基于時域的誤差分析方法，具有便于計算、易于分離等優(yōu)點，能夠定量地對傳感器各項不同的誤差進行細(xì)致的表征和辨識[12-14]。因此本文采用Allan方差分析法對經(jīng)過LS-KF誤差補償前后的電子羅盤隨機誤差進行分析，并將其作為LS-KF誤差補償方法的性能評價指標(biāo)。

2.1 Allan方差基本定義

樣本信號長度為N的電子羅盤采集數(shù)據(jù)，其采樣周期為T。將樣本分成L組，每組含M(MN-1/2)個樣本點,則每組的相關(guān)時間τ=M·T，計算每一組的平均值[15]：

(22)

(23)

其中，Allan方差的平方根即為Allan標(biāo)準(zhǔn)差。

2.2 電子羅盤隨機噪聲的Allan方差

電子羅盤在受到外部磁場干擾的情況下，所產(chǎn)生的隨機噪聲主要有量化噪聲QN、零偏不穩(wěn)定性噪聲BI;可用Allan方差分析法對這兩項噪聲進行辨識分析[16-18]，各噪聲源互相獨立。量化噪聲的Allan方差為:

(24)

零偏不穩(wěn)定性噪聲的Allan方差為:

(25)

鑒于電子羅盤的量化噪聲和零偏不穩(wěn)定性噪聲互相獨立，所以電子羅盤噪聲的Allan方差分析結(jié)果如式(26)所示

(26)

(27)

(28)

由式可以求得各Allan方差的估計值

3 實驗數(shù)據(jù)及結(jié)果分析

3.1 數(shù)據(jù)采集

實驗使用HMC5883L電子羅盤采集數(shù)據(jù)，采樣時間為0.1 s，通過Arduino UNO的I2C通信總線讀取測量數(shù)據(jù)。通過串口轉(zhuǎn)接模塊將所得數(shù)據(jù)發(fā)送至筆記本，筆記本使用MATLAB接收數(shù)據(jù)并保存。數(shù)據(jù)采集環(huán)境如圖1所示。

圖1 電子羅盤數(shù)據(jù)采集環(huán)境

3.2 實驗結(jié)果分析

3.2.1 硬磁干擾誤差補償結(jié)果分析

圖2 實測數(shù)據(jù)軌跡與標(biāo)準(zhǔn)圓對比

假設(shè)實測電子羅盤軌跡為標(biāo)準(zhǔn)圓是基于最小二乘法對電子羅盤硬磁干擾誤差進行補償?shù)那疤幔虼诵枰獙崪y電子羅盤軌跡與標(biāo)準(zhǔn)圓對比。

實測電子羅盤數(shù)據(jù)軌跡與標(biāo)準(zhǔn)圓的對比如圖2所示，測算后可得，實測電子羅盤數(shù)據(jù)軌跡與標(biāo)準(zhǔn)圓最大相對誤差為4%，在誤差允許的范圍內(nèi)；因此，可使用最小二乘法對電子羅盤硬磁干擾誤差進行補償。

由圖3、圖4可知，補償后的電子羅盤數(shù)據(jù)軌跡的圓心從(-42.092 8,419.273)處轉(zhuǎn)移到了(0,0)處。結(jié)果顯示，最小二乘法可對電子羅盤硬磁干擾誤差進行有效補償。

圖3 對硬磁干擾補償前電子羅盤數(shù)據(jù)軌跡

圖4 對硬磁干擾補償后電子羅盤軌跡

3.2.2 軟磁干擾補償結(jié)果分析

在對電子羅盤進行硬磁干擾補償后，進一步利用卡爾曼濾波對電子羅盤實測數(shù)據(jù)進行軟磁干擾誤差補償。

電子羅盤x軸原始數(shù)據(jù)如圖5所示，對原始數(shù)據(jù)進行測算，其均值為0.002 2，方差為1.371 7。

圖5 卡爾曼濾波前電子羅盤X軸數(shù)據(jù)

對電子羅盤x軸原始數(shù)據(jù)進行卡爾曼濾波誤差補償，結(jié)果如圖6所示。對補償后的數(shù)據(jù)進行測算，其均值為-0.007 5，方差為0.012 9。相較于原始數(shù)據(jù)，經(jīng)由卡爾曼濾波補償后，電子羅盤x軸數(shù)據(jù)的均值和方差均有明顯下降。

圖6 卡爾曼濾波后電子羅盤x軸數(shù)據(jù)

使用Allan方差分析法對實驗結(jié)果進行分析計算，并繪制Allan方差曲線，Allan方差曲線對比如圖7所示。

由圖7可以看出，經(jīng)由卡爾曼濾波補償后，電子羅盤x軸數(shù)據(jù)隨機誤差的Allan方差值有明顯的下降；與圖5、圖6的表現(xiàn)相符。

圖7 卡爾曼濾波前后Allan方差對比

使用最小二乘法對實驗所得的Allan方差曲線進行擬合，可得到各誤差項的值。濾波后的量化噪聲和零偏不穩(wěn)定性噪聲的Allan方差值分別由0.000 935和0.160 291，降到了0.000 161和0.035 448。經(jīng)過補償后量化噪聲和零偏不穩(wěn)定性噪聲分別減小了82.8%和77.9%，實驗結(jié)果表明，基于LS-KF的誤差補償方法能對電子羅盤受硬、軟磁干擾產(chǎn)生的誤差進行有效補償。

4 結(jié) 語

為對電子羅盤在實際使用過程中的硬、軟磁干擾誤差進行補償，論文提出了一種基于LS-KF的誤差補償方法。該方法可先后對電子羅盤硬、軟磁干擾誤差進行補償。同時，運用Allan方差分析法對補償結(jié)果進行定量分析。

實驗結(jié)果表明，LS-KF誤差補償方法可對電子羅盤硬、軟磁干擾誤差進行有效的補償，使用Allan方差分析法對誤差補償前后電子羅盤信號進行定量分析，可以看出各項隨機誤差均大幅降低。該方法可有效提高電子羅盤測量精度。