李 蓉,張一帆,楊薇薇
1.淮南職業(yè)技術(shù)學(xué)院信息與電氣工程系,安徽淮南,232001;2.安徽理工大學(xué)電氣與信息工程學(xué)院,安徽淮南,232001
相較于傳統(tǒng)指針式羅盤和平衡架結(jié)構(gòu)羅盤,電子羅盤(又稱數(shù)字指南針)具有能耗低、體積小、重量輕、精度高、可微型化等優(yōu)點(diǎn),被廣泛運(yùn)用于汽車、航海、無人機(jī)導(dǎo)航等領(lǐng)域[1]。電子羅盤在使用時(shí),易受外界硬、軟磁干擾產(chǎn)生測量誤差,硬磁干擾通常會導(dǎo)致電子羅盤產(chǎn)生零點(diǎn)漂移;軟磁干擾則會導(dǎo)致隨機(jī)誤差的產(chǎn)生,硬磁干擾與軟磁干擾產(chǎn)生的誤差互相獨(dú)立。
目前對電子羅盤受到的硬、軟磁干擾產(chǎn)生的誤差進(jìn)行補(bǔ)償?shù)姆椒ㄓ泻芏?。李智、秦濤等人提出基于橢圓假設(shè)的誤差補(bǔ)償方法[2-4],可同時(shí)對電子羅盤的硬、軟磁干擾進(jìn)行補(bǔ)償,其特點(diǎn)是精度較高,但計(jì)算量過大導(dǎo)致應(yīng)用場景有限。胡超等人提出的羅差補(bǔ)償系數(shù)法[5]只能對電子羅盤的軟磁干擾進(jìn)行補(bǔ)償,無法補(bǔ)償電子羅盤受到的硬磁干擾,應(yīng)用場景單一。Fang J提出的基于最小二乘法(Least squares,LS)的橢圓擬合的誤差補(bǔ)償方法[6],對應(yīng)用場景的要求較為苛刻,不具有廣泛性。Bonnet S提出了標(biāo)定傳感器采集數(shù)據(jù),利用橢球擬合對誤差進(jìn)行補(bǔ)償?shù)姆椒╗7],但該方法忽略了傳感器自身噪聲的影響,從而導(dǎo)致精度較低。
針對現(xiàn)有電子羅盤誤差補(bǔ)償方法中存在計(jì)算量過大、應(yīng)用場景有限、補(bǔ)償精度較低等問題,提出了一種基于最小二乘法和卡爾曼濾波算法(LS-KF)的誤差補(bǔ)償方法,分別對電子羅盤的硬、軟磁干擾誤差進(jìn)行補(bǔ)償,并結(jié)合Allan方差分析法對實(shí)驗(yàn)結(jié)果進(jìn)行定量分析。該方法可有效補(bǔ)償電子羅盤測量誤差,同時(shí)具有計(jì)算量小,應(yīng)用場景豐富的特點(diǎn)。
電子羅盤在實(shí)際使用過程中會受到外部環(huán)境的硬磁干擾產(chǎn)生零點(diǎn)漂移,從而導(dǎo)致測量誤差的產(chǎn)生??墒褂米钚《朔▽τ泊鸥蓴_導(dǎo)致的誤差進(jìn)行補(bǔ)償。
(1)
(2)
式(2)中,Sx,Sy分別表示電子羅盤真實(shí)值S的X軸方向和Y軸方向的向量。
(3)
式(3)中,Ox,Oy分別表示電子羅盤零點(diǎn)漂移量O的X軸方向和Y軸方向的數(shù)值,三個變量之間的關(guān)系可由一個數(shù)學(xué)模型來表示:
(4)
在理想狀態(tài)下,電子羅盤實(shí)測數(shù)據(jù)即為真實(shí)值,沒有測量誤差存在,Ox,Oy的值為零。STS為一常數(shù)U2,是為本地磁場強(qiáng)度平方
(5)
在實(shí)際測量環(huán)境中,Ox,Oy的值不為零,因此,實(shí)測電子羅盤第i個測量值誤差
(6)
可將式(6)簡寫為:
(7)
式(7)中
(8)
式(8)中,D2為電子羅盤任意一點(diǎn)真實(shí)值Sx(i)和Sy(i)平方的和。
假設(shè)電子羅盤任意一點(diǎn)測量值的坐標(biāo)到以電子羅盤零點(diǎn)漂移量Ox、Oy的數(shù)值為坐標(biāo)點(diǎn)的距離都相等。
由式(8)可知實(shí)測電子羅盤軌跡形如以電子羅盤零點(diǎn)漂移量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)中,可進(jìn)一步求得矩陣β的值,從而可得電子羅盤零點(diǎn)漂移量Ox、Oy。
電子羅盤在實(shí)際使用過程中,受外部環(huán)境的軟磁干擾會產(chǎn)生隨機(jī)噪聲,隨機(jī)噪聲是導(dǎo)致測量誤差存在的主要原因。電子羅盤受軟磁干擾產(chǎn)生的隨機(jī)噪聲屬于高斯白噪聲。
卡爾曼濾波算法可對帶有高斯白噪聲的線性系統(tǒng)進(jìn)行估計(jì)[8-10],該濾波算法是目前應(yīng)用最為廣泛的一種濾波方法,在通信、導(dǎo)航、制導(dǎo)與控制等領(lǐng)域都得到了較好的應(yīng)用。其基本思想是以最小均方誤差準(zhǔn)則作為最佳估計(jì)準(zhǔn)則,通過遞推理論實(shí)現(xiàn)對系統(tǒng)未來狀態(tài)量的估計(jì),使得估計(jì)值盡可能接近真實(shí)值??柭鼮V波算法的數(shù)學(xué)模型如下:
(16)
式中,X(k)為k時(shí)刻電子羅盤狀態(tài)量;Z(k)為k時(shí)刻電子羅盤的測量值;假設(shè)W(k)、V(k)是互相獨(dú)立的高斯白噪聲;其協(xié)方差分別為Q、R,W(k)為輸入噪聲;V(k)為觀測噪聲;B為狀態(tài)轉(zhuǎn)移矩陣。H是系統(tǒng)測量矩陣。
卡爾曼濾波具體遞推過程如下:
(1)利用系統(tǒng)k-1時(shí)刻的狀態(tài)預(yù)測系統(tǒng)k時(shí)刻的狀態(tài):
X(k|k-1)=BX(k-1|k-1)
(17)
式(17)中,X(k|k-1)是利用系統(tǒng)k-1時(shí)刻的狀態(tài)預(yù)測得到的系統(tǒng)k時(shí)刻的狀態(tài),X(k-1|k-1)是系統(tǒng)k-1時(shí)刻狀態(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)計(jì)算系統(tǒng)k時(shí)刻狀態(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時(shí)刻下系統(tǒng)狀態(tài)的協(xié)方差
P(k|k)=(1-Kg(k)H)P(k|k-1)
(21)
當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí),P(k|k)就是式(18)的P(k-1|k-1)。這樣,系統(tǒng)就可以通過不斷迭代的計(jì)算,更新各時(shí)刻的系統(tǒng)狀態(tài)量,從而得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)值。
在該研究模型中,將系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣B和系統(tǒng)的測量矩陣H設(shè)置為1,同樣,將系統(tǒng)的卡爾曼增益Kg以及系統(tǒng)的協(xié)方差P的初始值也設(shè)置為1,即可進(jìn)行卡爾曼濾波。
Allan方差分析法是由David Allan[11]提出的一種基于時(shí)域的誤差分析方法,具有便于計(jì)算、易于分離等優(yōu)點(diǎn),能夠定量地對傳感器各項(xiàng)不同的誤差進(jìn)行細(xì)致的表征和辨識[12-14]。因此本文采用Allan方差分析法對經(jīng)過LS-KF誤差補(bǔ)償前后的電子羅盤隨機(jī)誤差進(jìn)行分析,并將其作為LS-KF誤差補(bǔ)償方法的性能評價(jià)指標(biāo)。
樣本信號長度為N的電子羅盤采集數(shù)據(jù),其采樣周期為T。將樣本分成L組,每組含M(MN-1/2)個樣本點(diǎn),則每組的相關(guān)時(shí)間τ=M·T,計(jì)算每一組的平均值[15]:
(22)
(23)
其中,Allan方差的平方根即為Allan標(biāo)準(zhǔn)差。
電子羅盤在受到外部磁場干擾的情況下,所產(chǎn)生的隨機(jī)噪聲主要有量化噪聲QN、零偏不穩(wěn)定性噪聲BI;可用Allan方差分析法對這兩項(xiàng)噪聲進(jìn)行辨識分析[16-18],各噪聲源互相獨(dú)立。量化噪聲的Allan方差為:
(24)
零偏不穩(wěn)定性噪聲的Allan方差為:
(25)
鑒于電子羅盤的量化噪聲和零偏不穩(wěn)定性噪聲互相獨(dú)立,所以電子羅盤噪聲的Allan方差分析結(jié)果如式(26)所示
(26)
(27)
(28)
由式可以求得各Allan方差的估計(jì)值
實(shí)驗(yàn)使用HMC5883L電子羅盤采集數(shù)據(jù),采樣時(shí)間為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.1 硬磁干擾誤差補(bǔ)償結(jié)果分析
圖2 實(shí)測數(shù)據(jù)軌跡與標(biāo)準(zhǔn)圓對比
假設(shè)實(shí)測電子羅盤軌跡為標(biāo)準(zhǔn)圓是基于最小二乘法對電子羅盤硬磁干擾誤差進(jìn)行補(bǔ)償?shù)那疤?,因此需要將?shí)測電子羅盤軌跡與標(biāo)準(zhǔn)圓對比。
實(shí)測電子羅盤數(shù)據(jù)軌跡與標(biāo)準(zhǔn)圓的對比如圖2所示,測算后可得,實(shí)測電子羅盤數(shù)據(jù)軌跡與標(biāo)準(zhǔn)圓最大相對誤差為4%,在誤差允許的范圍內(nèi);因此,可使用最小二乘法對電子羅盤硬磁干擾誤差進(jìn)行補(bǔ)償。
由圖3、圖4可知,補(bǔ)償后的電子羅盤數(shù)據(jù)軌跡的圓心從(-42.092 8,419.273)處轉(zhuǎn)移到了(0,0)處。結(jié)果顯示,最小二乘法可對電子羅盤硬磁干擾誤差進(jìn)行有效補(bǔ)償。
圖3 對硬磁干擾補(bǔ)償前電子羅盤數(shù)據(jù)軌跡
圖4 對硬磁干擾補(bǔ)償后電子羅盤軌跡
3.2.2 軟磁干擾補(bǔ)償結(jié)果分析
在對電子羅盤進(jìn)行硬磁干擾補(bǔ)償后,進(jìn)一步利用卡爾曼濾波對電子羅盤實(shí)測數(shù)據(jù)進(jìn)行軟磁干擾誤差補(bǔ)償。
電子羅盤x軸原始數(shù)據(jù)如圖5所示,對原始數(shù)據(jù)進(jìn)行測算,其均值為0.002 2,方差為1.371 7。
圖5 卡爾曼濾波前電子羅盤X軸數(shù)據(jù)
對電子羅盤x軸原始數(shù)據(jù)進(jìn)行卡爾曼濾波誤差補(bǔ)償,結(jié)果如圖6所示。對補(bǔ)償后的數(shù)據(jù)進(jìn)行測算,其均值為-0.007 5,方差為0.012 9。相較于原始數(shù)據(jù),經(jīng)由卡爾曼濾波補(bǔ)償后,電子羅盤x軸數(shù)據(jù)的均值和方差均有明顯下降。
圖6 卡爾曼濾波后電子羅盤x軸數(shù)據(jù)
使用Allan方差分析法對實(shí)驗(yàn)結(jié)果進(jìn)行分析計(jì)算,并繪制Allan方差曲線,Allan方差曲線對比如圖7所示。
由圖7可以看出,經(jīng)由卡爾曼濾波補(bǔ)償后,電子羅盤x軸數(shù)據(jù)隨機(jī)誤差的Allan方差值有明顯的下降;與圖5、圖6的表現(xiàn)相符。
圖7 卡爾曼濾波前后Allan方差對比
使用最小二乘法對實(shí)驗(yàn)所得的Allan方差曲線進(jìn)行擬合,可得到各誤差項(xiàng)的值。濾波后的量化噪聲和零偏不穩(wěn)定性噪聲的Allan方差值分別由0.000 935和0.160 291,降到了0.000 161和0.035 448。經(jīng)過補(bǔ)償后量化噪聲和零偏不穩(wěn)定性噪聲分別減小了82.8%和77.9%,實(shí)驗(yàn)結(jié)果表明,基于LS-KF的誤差補(bǔ)償方法能對電子羅盤受硬、軟磁干擾產(chǎn)生的誤差進(jìn)行有效補(bǔ)償。
為對電子羅盤在實(shí)際使用過程中的硬、軟磁干擾誤差進(jìn)行補(bǔ)償,論文提出了一種基于LS-KF的誤差補(bǔ)償方法。該方法可先后對電子羅盤硬、軟磁干擾誤差進(jìn)行補(bǔ)償。同時(shí),運(yùn)用Allan方差分析法對補(bǔ)償結(jié)果進(jìn)行定量分析。
實(shí)驗(yàn)結(jié)果表明,LS-KF誤差補(bǔ)償方法可對電子羅盤硬、軟磁干擾誤差進(jìn)行有效的補(bǔ)償,使用Allan方差分析法對誤差補(bǔ)償前后電子羅盤信號進(jìn)行定量分析,可以看出各項(xiàng)隨機(jī)誤差均大幅降低。該方法可有效提高電子羅盤測量精度。