王齊賢，李東光

(北京理工大學(xué)機(jī)電動態(tài)控制重點(diǎn)實(shí)驗(yàn)室，北京 100081)

0 引言

近年來，地磁傳感器因其體積小、成本低、抗高過載等特點(diǎn)而廣泛應(yīng)用于導(dǎo)航定位系統(tǒng)中。然而地磁場為弱磁場，地磁傳感器極易受到周圍鐵磁材料的干擾，測量數(shù)據(jù)不能直接使用，此外傳感器由于安裝、生產(chǎn)工藝等問題也會引入測量誤差，因此在使用時需要對地磁傳感器進(jìn)行誤差校正以提高姿態(tài)解算精度。

地磁傳感器誤差校正方法按照是否需要外部姿態(tài)基準(zhǔn)分為非獨(dú)立姿態(tài)的校正方法和獨(dú)立姿態(tài)的校正方法。其中，非獨(dú)立姿態(tài)的校正方法需要三軸轉(zhuǎn)臺或陀螺儀等外部儀器提供較精確的實(shí)時姿態(tài)，而在大多數(shù)應(yīng)用中磁傳感器就是為了進(jìn)行姿態(tài)估計，因此該方法僅可在實(shí)驗(yàn)室條件下實(shí)現(xiàn)。國內(nèi)外學(xué)者也提出了很多獨(dú)立姿態(tài)的校正方法，如TWOSTEP方法[1]、橢球擬合方法[2-4]、粒子群算法[5-7]以及遺傳算法[8-9]等。這些方法實(shí)際上都是基于地磁場模值估計的標(biāo)量校正，由此引入了一個旋轉(zhuǎn)模糊性問題[10]，即地磁傳感器校正后的傳感器坐標(biāo)系相對于載體坐標(biāo)系旋轉(zhuǎn)了一個固定角度，該角度即為非對準(zhǔn)誤差[11]。為了解決旋轉(zhuǎn)模糊性問題，Ali A等人只考慮了靈敏度誤差和偏置誤差，將12個誤差參數(shù)減少到6個[7]。而龐學(xué)亮等人只考慮了非正交誤差、靈敏度誤差和偏置誤差，將誤差系數(shù)矩陣假設(shè)為上三角矩陣[8]。通常采用的橢球擬合校正方法中會假設(shè)誤差系數(shù)矩陣的形式(對角矩陣、三角矩陣或?qū)ΨQ矩陣)，消除旋轉(zhuǎn)模糊性問題，但實(shí)際誤差系數(shù)矩陣的形式并不可知。Wu Z等人[5-6]和章雪挺等人[9]則引入一個正交矩陣將誤差系數(shù)矩陣簡化為上三角矩陣，誤差參數(shù)變?yōu)?個，但并沒有考慮到正交矩陣對校正效果的影響。雖然洪琪璐[11]等人提出了一種利用磁傳感器自身姿態(tài)信息的優(yōu)化補(bǔ)償方法對非對準(zhǔn)誤差進(jìn)行校正，但磁干擾過大時磁傳感器自身姿態(tài)信息就不可信，依舊無法準(zhǔn)確估計出非對準(zhǔn)誤差。李翔等人[12-13]在校正過程引入大地坐標(biāo)系下的常矢量(如重力矢量)或其他傳感器(陀螺儀等)數(shù)據(jù)構(gòu)造的常矢量，提出了一種基于矢量點(diǎn)積不變原理的校正方法，有效解決了非對準(zhǔn)誤差問題，該方法需要重力加速度計或其他傳感器輔助，不適合純地磁姿態(tài)測量系統(tǒng)。

為了提高彈丸滾轉(zhuǎn)角測量精度，本文在橢球擬合校正方法的基礎(chǔ)上考慮了旋轉(zhuǎn)模糊性問題，提出了一種基于粒子群優(yōu)化的地磁傳感器非對準(zhǔn)誤差自主校正方法，以求解橢球擬合校正方法引入的非對準(zhǔn)誤差。在獲取地磁數(shù)據(jù)時，需要將傳感器繞其至少兩軸旋轉(zhuǎn)，先采用橢球擬合方法計算誤差校正矩陣，然后應(yīng)用粒子群算法估計傳感器三個非對準(zhǔn)誤差。

1 傳感器誤差模型與橢球擬合校正方法

1.1 傳感器誤差模型

磁傳感器在使用過程中會受安裝誤差、自身誤差、環(huán)境干擾誤差以及隨機(jī)偏差的影響，參考文獻(xiàn)[3]中的線性誤差模型，再補(bǔ)充安裝誤差因素，則一個完整的捷聯(lián)式地磁傳感器的誤差模型可以寫為：

(1)

(2)

式(2)中，A=CsfCNCACsi，是一個3×3的誤差系數(shù)矩陣；b=CsfCNCAbhi+bso，是一個3×1的固定誤差偏置。

如果誤差系數(shù)矩陣A可逆，那么變換式(2)就可得到地磁傳感器的誤差校正模型，如式(3)所示。

(3)

若能確定校正系數(shù)矩陣A-1和誤差偏置b，就能由地磁傳感器的測量值計算出載體坐標(biāo)系下的地磁分量，以此解算載體姿態(tài)。因此，誤差校正的目的就是求解A-1和b。

1.2 橢球擬合校正方法

一定時間內(nèi)地磁矢量在以探測點(diǎn)為中心的100 km范圍內(nèi)保持不變[12]，即可認(rèn)為某地地磁矢量的模值為一個常量，那么理想情況下傳感器輸出的地磁數(shù)據(jù)應(yīng)分布在一個以原點(diǎn)為中心地磁強(qiáng)度為半徑的球面上，根據(jù)式(2)該球面因各種誤差影響產(chǎn)生平移、旋轉(zhuǎn)和伸縮變換而形成一個橢球面。而橢球擬合校正方法的思想就是通過擬合橢球面將校正系數(shù)矩陣的求解轉(zhuǎn)化為橢球系數(shù)的求解。

根據(jù)式(3)有：

(4)

式(4)中，H為地磁場強(qiáng)度。令：

M=(A-1)TA-1/H2

(5)

因?yàn)镸正定矩陣，則式(4)表示為橢球的廣義方程即：

(6)

式(6)中，M、b即為待求解的橢球系數(shù)組成的矩陣。

將橢球的廣義方程(6)改寫成一般形式為：

(7)

(8)

式(8)中，中間變量u=[a7,a8,a9]T，m=[a1,a4,a5;a4,a2,a6;a5,a6,a3]T。

定義地磁傳感器輸出的n組地磁分量的運(yùn)算組合向量X=[x1,x2,…,xn]T，則橢球擬合問題轉(zhuǎn)化為F(a,X)函數(shù)的最小化問題，即：

(9)

利用Li Q等人[15]提出的最小二乘橢球擬合方法即可完成M和b的求解。最后通過式(5)分解得到校正系數(shù)矩陣A-1。

1.3 橢球擬合校正方法的局限性

2 基于PSO算法的非對準(zhǔn)誤差校正方法

2.1 非對準(zhǔn)誤差校正方法分析

橢球校正后的傳感器坐標(biāo)系依次繞z軸、y軸、x軸旋轉(zhuǎn)角度u1，u2，u3后變換到載體坐標(biāo)系，u1，u2，u3稱為非對準(zhǔn)誤差角。則存在正交矩陣R(u)：

(10)

式(10)中，u=[u1,u2,u3]，使得：

(11)

(12)

圖1 橢球校正后地磁數(shù)據(jù)點(diǎn)在xy平面投影Fig.1 Projection of geomagnetic data points in thexy plane after calibration with ellipsoid fitting

2.2 基于PSO算法的非對準(zhǔn)誤差校正方法

1) 構(gòu)造適應(yīng)度函數(shù)：

(13)

2) 初始化粒子群，設(shè)置群體規(guī)模m，粒子維度l，粒子范圍[-umax,umax]，粒子最大速度vmax，慣性權(quán)重wmax和wmin，最大迭代次數(shù)tmax，學(xué)習(xí)因子c1，c2，并對粒子位置ui和速度vi在限制范圍里隨機(jī)初始化，i=1,2,…,m。

3) 根據(jù)式(13)計算每個粒子的適應(yīng)度值L(ui)。

4) 根據(jù)下式選擇個體極值pbest和全局極值gbest，其中pbest取個體最優(yōu)適應(yīng)度值Pbest對應(yīng)的位置，gbest取全局最優(yōu)適應(yīng)度值Gbest對應(yīng)的位置；

(14)

5) 根據(jù)下式更新粒子的慣性權(quán)重、速度和位置；

(15)

式(15)中，t表示當(dāng)前迭代的次數(shù)，r1和r2是[0,1]范圍內(nèi)的均勻隨機(jī)數(shù)。

6) 判斷迭代結(jié)束條件：t=t+1，當(dāng)t>tmax時，迭代結(jié)束，返回gbest即為所求未對準(zhǔn)誤差角；否則返回步驟3)繼續(xù)執(zhí)行。

7) 由式(11)更正傳感器誤差校正矩陣。

至此，準(zhǔn)確的誤差校正矩陣A-1和固定誤差偏置b都已求出來，利用步驟3)對地磁傳感器輸出數(shù)據(jù)進(jìn)行校正。

3 實(shí)驗(yàn)驗(yàn)證與分析

為了驗(yàn)證基于PSO算法的非對準(zhǔn)誤差校正方法的可行性和有效性，本文分別進(jìn)行了計算機(jī)仿真驗(yàn)證和轉(zhuǎn)臺實(shí)驗(yàn)驗(yàn)證，對比了橢球擬合校正方法和改進(jìn)后校正方法之間的姿態(tài)解算精度。

3.1 仿真實(shí)驗(yàn)驗(yàn)證與分析

參考WMM世界地磁模型中北京地區(qū)的地磁場參數(shù)，地磁場強(qiáng)度H=54 641 nT，磁傾角59.15°，磁偏角-6.82°。人為設(shè)置靈敏度系數(shù)、非正交誤差角、安裝誤差角、軟磁系數(shù)、硬磁誤差以及偏置誤差，得到地磁傳感器誤差系數(shù)矩陣A、固定誤差偏置b和誤差校正矩陣A-1如下：

地磁傳感器每個軸上疊加均值為零，標(biāo)準(zhǔn)差為250 nT的高斯白噪聲，根據(jù)式(2)仿真得到三組受誤差干擾的傳感器輸出數(shù)據(jù)，如圖2所示，可以看出理想地磁數(shù)據(jù)受各種誤差因素影響由圓畸變成了橢圓。

圖2 地磁數(shù)據(jù)仿真圖Fig.2 Geomagnetic dataset simulation

分別采用上文中提到的橢球擬合校正方法和PSO校正方法對傳感器輸出數(shù)據(jù)進(jìn)行誤差校正。PSO算法的參數(shù)設(shè)置如表1所示。

表1 PSO算法參數(shù)設(shè)置

得到的誤差校正矩陣A-1和固定誤差偏置b如表2所示，經(jīng)過對比可以發(fā)現(xiàn)，橢球擬合校正方法可以很準(zhǔn)確地估計出固定誤差偏置b，但無法準(zhǔn)確估計出誤差校正矩陣A-1，而基于PSO的校正方法可以較準(zhǔn)確的估計出A-1。

校正效果對比如圖3所示。由圖3(a)可以看出經(jīng)橢球擬合校正后的地磁數(shù)據(jù)點(diǎn)并沒有校正到理想數(shù)據(jù)點(diǎn)上，而是相對旋轉(zhuǎn)了某個固定角度，而本文提出的基于PSO算法的非對準(zhǔn)誤差校正方法能夠很準(zhǔn)確的估計出橢球擬合校正未解決的非對準(zhǔn)誤差，校正后的數(shù)據(jù)點(diǎn)能夠很好地吻合理論數(shù)據(jù)點(diǎn)。同時，圖3(b)也證實(shí)了PSO校正方法思想的準(zhǔn)確性。

表2 兩種校正方法得到的A-1和b

圖3 兩種方法校正后數(shù)據(jù)對比圖Fig.3 Comparison of dataset with calibration of two methods

分別利用橢球擬合校正后和PSO校正后的地磁數(shù)據(jù)計算載體的滾轉(zhuǎn)角，滾轉(zhuǎn)角誤差對比如圖4所示，橢球擬合校正后計算得到的滾轉(zhuǎn)角精度為8°，而PSO校正后計算得到的滾轉(zhuǎn)角精度為1°。因此，本文所提出的方法的效果明顯優(yōu)于橢球擬合校正方法。

圖4 兩種方法校正后滾轉(zhuǎn)角誤差對比圖Fig.4 Comparison of roll angle errors with calibration of two methods

3.2 轉(zhuǎn)臺實(shí)驗(yàn)驗(yàn)證與分析

為了驗(yàn)證本文提出的改進(jìn)算法的實(shí)際性能，利用實(shí)驗(yàn)室二自由度無磁轉(zhuǎn)臺和HMC1053磁阻傳感器制作的純地磁滾轉(zhuǎn)角測量模塊進(jìn)行轉(zhuǎn)臺實(shí)驗(yàn)，收集滾轉(zhuǎn)角測量模塊在三組不同俯仰角下進(jìn)行滾轉(zhuǎn)運(yùn)動時和在固定滾轉(zhuǎn)角下進(jìn)行俯仰運(yùn)動時的地磁數(shù)據(jù)。不失一般性，轉(zhuǎn)臺進(jìn)行滾轉(zhuǎn)運(yùn)動時選擇固定俯仰角θ分別為0°，30°，90°，進(jìn)行俯仰運(yùn)動時選擇固定滾轉(zhuǎn)角γ分別為0°，地磁原始數(shù)據(jù)如圖5所示。

圖5 地磁原始數(shù)據(jù)Fig.5 Raw Geomagnetic dataset

采集得到的地磁數(shù)據(jù)分別經(jīng)過橢球擬合校正和PSO校正，校正后的數(shù)據(jù)點(diǎn)如圖6所示。由圖6(a)和圖6(b)可以看出經(jīng)橢球擬合校正后的數(shù)據(jù)點(diǎn)在x軸上存在非對準(zhǔn)誤差角，轉(zhuǎn)臺進(jìn)行俯仰運(yùn)動時采集的數(shù)據(jù)點(diǎn)集經(jīng)橢球擬合校正方法處理后在yz平面的投影近似為一條傾斜的直線段，經(jīng)過基于PSO算法的改進(jìn)方法校正后地磁矢量在載體坐標(biāo)系z軸上的分量基本不變，近似為一條水平的直線段。

圖6 兩種方法校正后數(shù)據(jù)對比圖Fig.6 Comparison of dataset with calibration of two methods

分別利用橢球擬合校正后和PSO校正后的地磁數(shù)據(jù)計算載體的滾轉(zhuǎn)角，滾轉(zhuǎn)角誤差對比如圖7所示，同一組地磁數(shù)據(jù)經(jīng)過兩種校正方法校正后計算得到的滾轉(zhuǎn)角精度基本一致。但是當(dāng)俯仰角為0°時，橢球擬合校正后計算得到的滾轉(zhuǎn)角誤差絕對值均值為0.434 1°，經(jīng)PSO校正后計算得到的滾轉(zhuǎn)角誤差絕對值均值為0.426 2°；當(dāng)俯仰角為30°時，橢球擬合校正后計算得到的滾轉(zhuǎn)角誤差絕對值均值為0.363 6°，經(jīng)PSO校正后計算得到的滾轉(zhuǎn)角誤差絕對值均值為0.362 4°；當(dāng)俯仰角為90°時，橢球擬合校正后計算得到的滾轉(zhuǎn)角誤差絕對值均值為0.703 0°，經(jīng)PSO校正后計算得到的滾轉(zhuǎn)角誤差絕對值均值為0.637 7°。

本次實(shí)驗(yàn)由于地磁場畸變程度較小，橢球擬合校正方法和基于PSO算法的改進(jìn)方法校正后得到的滾轉(zhuǎn)角測量精度整體上一致，但是從誤差對比圖中可以看出在實(shí)時精度上基于PSO算法的改進(jìn)方法優(yōu)于橢球擬合校正方法。

圖7 兩種方法校正后滾轉(zhuǎn)角誤差對比圖Fig.7 Comparison of roll angle errors with calibration of two methods

4 結(jié)論

本文在橢球擬合校正方法的基礎(chǔ)上進(jìn)一步改進(jìn)，根據(jù)捷聯(lián)地磁傳感器繞載體坐標(biāo)系某一軸旋轉(zhuǎn)時該軸地磁分量為常量這一想法，提出了一種基于粒子群優(yōu)化算法的非對準(zhǔn)誤差校正方法，解決了橢球擬合校正方法沒有考慮的旋轉(zhuǎn)模糊性問題，并分別進(jìn)行了計算機(jī)仿真實(shí)驗(yàn)和轉(zhuǎn)臺實(shí)驗(yàn)，從滾轉(zhuǎn)角測量精度的角度對比了兩種方法，從仿真結(jié)果上看基于粒子群優(yōu)化算法的改進(jìn)方法將測量精度從8°提高到1°，從本次轉(zhuǎn)臺實(shí)驗(yàn)結(jié)果上看兩種方法測量精度無明顯差別，但改進(jìn)算法的實(shí)時精度更好一些。此外，對比仿真實(shí)驗(yàn)和轉(zhuǎn)臺實(shí)驗(yàn)的結(jié)果，當(dāng)?shù)卮艌龌兂潭容^大即誤差干擾較大時，本文提出的改進(jìn)算法將明顯優(yōu)于橢球擬合校正方法。