李文寬，蔡浩原，趙晟霖，劉春秀

(1.中國科學(xué)院空天信息研究院，北京 100190；2.中國科學(xué)院大學(xué)，北京 100049)

0 引言

在導(dǎo)航定位中，確定機(jī)器人、飛行器等載體的姿態(tài)是十分重要的，姿態(tài)可以通俗地使用歐拉角(俯仰角、橫滾角和姿態(tài)角)來表示，目前通常使用六軸IMU來計算歐拉角，其中的陀螺儀感知三軸角速度值，由于重力加速度的存在，加速度計可以準(zhǔn)確的感知橫滾角和俯仰角，但六軸IMU只能獲得航向角的相對變化值，并且由于陀螺儀存在不同程度的漂移現(xiàn)象，所以得到的航向角也會漂移，存在不穩(wěn)定的現(xiàn)象。因此往往需要添加磁力計，組成九軸IMU來計算航向角。由于磁力計極易受到周圍環(huán)境磁場的干擾，所以在使用過程中，往往需要先對磁力計進(jìn)行校準(zhǔn)，得到軟鐵干擾和硬鐵干擾參數(shù)，磁力計數(shù)據(jù)在經(jīng)過這些參數(shù)的處理后，才能準(zhǔn)確的指示航向[1-3]。

磁力計校準(zhǔn)方法可以根據(jù)是否需要外部設(shè)備分為兩種類型。一種是基于磁場數(shù)據(jù)的約束，對磁場建模并僅使用磁力計數(shù)據(jù)來計算校準(zhǔn)參數(shù)[4-5]。最常用的方法是橢圓擬合方法[6]和最大和最小值方法。朱建良等[7]首先線性化橢球表面方程，然后分別用最小二乘法和總體最小二乘法擬合得到橢球的參數(shù)；最大和最小值的方法是在3個軸上收集磁力計的最大和最小測量值，然后計算橢球面參數(shù)，其原理類似于橢球擬合法。雖然這種方法可以取得很好的效果，但對磁力計數(shù)據(jù)要求很高，因此往往需要用戶進(jìn)行特定的操作(如繞“8”)來收集數(shù)據(jù)，這對用戶的使用來說非常不友好，而且在某些情況下(如機(jī)器人、無人機(jī))很難實(shí)現(xiàn)。不僅如此，由于這種方法不能實(shí)時運(yùn)行，所以需要在磁力計使用環(huán)境改變后再次校準(zhǔn)。

另一種方法是使用外部設(shè)備來進(jìn)行校準(zhǔn)，最常用的是慣性測量單元(IMU)。M.Kok等[8]將磁力計和六軸IMU的數(shù)據(jù)建模為最大似然問題并進(jìn)行求解，該方法使用來自2個不同的傳感器單元實(shí)現(xiàn)了不錯的校準(zhǔn)效果，并且避免了某些操作的執(zhí)行，但卻無法實(shí)時運(yùn)行。K.Han等[9]使用EKF(擴(kuò)展卡爾曼濾波器)將磁力計和陀螺儀的數(shù)據(jù)融合實(shí)現(xiàn)了實(shí)時校準(zhǔn)，但由于陀螺儀的漂移，得到的磁力計數(shù)據(jù)也不是很穩(wěn)定。M.Zhu等[10]通過求解均勻最小二乘問題，在陀螺儀的幫助下提出了一種有效的磁力計校準(zhǔn)算法。該算法能夠在一個步驟中完成磁力計與陀螺儀固有誤差和磁力計本身干擾誤差的計算。

本文先通過互補(bǔ)濾波使用加速度修正陀螺儀數(shù)據(jù)，然后使用修正后的陀螺儀數(shù)據(jù)對磁力計進(jìn)行旋轉(zhuǎn)，以更準(zhǔn)確地預(yù)測磁力計數(shù)據(jù)，完成擴(kuò)展卡爾曼濾波中的狀態(tài)預(yù)測過程，接著使用擴(kuò)展卡爾曼濾波融合預(yù)測值和磁力計測量值，實(shí)現(xiàn)磁力計的動態(tài)校準(zhǔn)。實(shí)現(xiàn)了一種穩(wěn)定、高精度的實(shí)時磁力計校準(zhǔn)。

1 磁力計量測模型

磁力計測量的磁場是使用環(huán)境中的磁場，包含了可以用于計算航向的地磁場和傳感器附近的一些鐵磁材料產(chǎn)生的磁場，所以需要對磁力計的測量值進(jìn)行校準(zhǔn)，保留地磁場信息，并以此來計算航向。

如果沒有鐵磁性材料的干擾，磁力計在t時刻的量測值hm,t與當(dāng)?shù)氐牡卮艌鰉n的關(guān)系為[6，11-12]：

(1)

由此可知，理想情況下，磁力計的量測值應(yīng)該位于一個球體表面，該球體的球心是原點(diǎn)，半徑是當(dāng)?shù)氐卮艌鰪?qiáng)度。

鐵磁材料對磁力計測量值產(chǎn)生的影響可以分為硬鐵干擾和軟鐵干擾，硬鐵干擾是鐵磁材料的永久性磁化引起的，通常它會在磁力計的量測值上產(chǎn)生一個3×1的零漂ohi。軟鐵干擾是由于鐵磁材料受到外部磁場的磁化而引起的，因此取決于材料相對于局部磁場的方向，通常用一個3×3的矩陣Csi來表示。結(jié)合公式(1)，可以推導(dǎo)出一般情況下的磁力計量測公式為：

(2)

同時，由于加工工藝等的影響，磁力計傳感器的軸和IMU的軸不能保證完全重合，因此IMU的載體坐標(biāo)系b與磁力計的載體坐標(biāo)系bm之間仍然有一個旋轉(zhuǎn)誤差Rbmb，因此公式(2)可以寫成：

(3)

另外，還有一些磁力計傳感器本身的固有誤差，這些誤差也是在傳感器的加工過程中確定的，每個傳感器的誤差值也不盡相同，這些固有誤差分別為：

(1)磁力計傳感器三軸沒有嚴(yán)格正交引起的非正交(non-orthogonality)誤差，使用3×3的矩陣Cno來表示；

(2)零漂(zero bias)的存在會在3個測量值上產(chǎn)生一個固定值，使用3×1的矢量ozb來表示；

(3)磁力計傳感器3個軸的靈敏度不同，會對測量值產(chǎn)生縮放(scale)的影響，使用3×3的矩陣Csc來表示。

結(jié)合公式(3)，可以得到完整的磁力計量測模型：

(4)

對其進(jìn)行化簡并添加量測噪聲，得到磁力計讀數(shù)的表達(dá)式為：

(5)

D=CscCnoCsiRbmb

(6)

o=CscCnoohi+ozb

(7)

2 IMU數(shù)據(jù)處理

慣性測量單元(IMU)是一種可以測量物體3個軸的角速度和加速度的傳感器，通常用于感知載體的姿態(tài)以及慣性導(dǎo)航。 加速度計和陀螺儀都可以計算姿態(tài)，但是各有優(yōu)點(diǎn)和缺點(diǎn)：加速度計長期使用時計算出的姿態(tài)可信度比較高，沒有累計誤差，但是它對振動等干擾十分敏感，并且不能感知航向角的變化； 陀螺儀對振動不敏感也可以感知航向角的變化，但是長期使用陀螺儀會產(chǎn)生很嚴(yán)重的漂移。鑒于加速度計和陀螺儀各自的特性，使用互補(bǔ)濾波對二者數(shù)據(jù)進(jìn)行融合[13-15]。

(8)

之后使用歸一化后的加速度計的實(shí)際量測值a，和理論量測值v進(jìn)行向量叉乘，得到陀螺儀的補(bǔ)償校準(zhǔn)值e。

e=v×a

(9)

然后使用PI控制器進(jìn)行濾波，對陀螺儀消除漂移誤差。只要存在誤差，控制器便會持續(xù)作用，直至誤差為0。控制的效果取決于P和I的參數(shù)，分別對應(yīng)比例控制和積分控制的參數(shù)。

(10)

3 磁力計校準(zhǔn)

3.1 狀態(tài)轉(zhuǎn)移方程的建模

由于磁力計和陀螺儀測量的是同一載體的姿態(tài)信息，所以二者感知到的姿態(tài)變化理論上應(yīng)該是相同的，可以利用這一關(guān)系建立角速度和磁力計量測值之間的關(guān)系。

根據(jù)旋轉(zhuǎn)矩陣的相關(guān)知識，其關(guān)于時間的導(dǎo)數(shù)可以寫為：

(11)

該式被稱為泊松公式(Possion’s equation)，其中∧為反對稱矩陣算子：

(12)

將式(1)和式(12)相結(jié)合，就可以得到陀螺儀數(shù)據(jù)與磁力計數(shù)據(jù)二者之間的聯(lián)系：

(13)

(14)

之后選取磁場數(shù)據(jù)hm,t、畸變矩陣D和偏移矢量o中的各個元素作為t時刻的狀態(tài)量，記為Xt。

(15)

所以擴(kuò)展卡爾曼濾波中的狀態(tài)轉(zhuǎn)移過程為：

(16)

(17)

(18)

將其寫為統(tǒng)一的矩陣形式為：

Xt+1=FtXt+ωt

(19)

(20)

式中：ωt為狀態(tài)轉(zhuǎn)移過程中的噪聲，它的方差矩陣記為Qt：

(21)

至此，推導(dǎo)完成了擴(kuò)展卡爾曼濾波中的狀態(tài)轉(zhuǎn)移過程。

3.2 量測方程的建模

式(5)中，已經(jīng)給出了磁力計的量測模型，量測值為hm,t，但是由于狀態(tài)量設(shè)為了Xt，其中不僅包含了量測值hm,t，還包含了畸變矩陣D和偏移矢量o，所以需要求解其雅可比矩陣，對其進(jìn)行線性化。解得量測方程為：

Zt=HtXt+vt

(22)

(23)

3.3 擴(kuò)展卡爾曼濾波

根據(jù)以上內(nèi)容，結(jié)合擴(kuò)展卡爾曼濾波算法，推導(dǎo)出適用于磁場校準(zhǔn)的EKF公式為：

預(yù)測階段：

Xt|t-1=FXt-1

(24)

(25)

更新階段：

(26)

Xt=Xt|t-1+Kt(Zt-HXt|t-1)

(27)

Pt=(I-KtHt)Pt|t-1

(28)

式中：Xt|t-1為狀態(tài)量從t-1時刻到t時刻的一步預(yù)測值；Xt和Pt為通過EKF算法求得的t時刻的狀態(tài)量估計值和狀態(tài)的方差估計值；Kt為濾波增益。

4 實(shí)驗(yàn)

4.1 算法介紹

為了對比本文提出算法在磁力計動態(tài)校準(zhǔn)時的穩(wěn)定性，分別實(shí)現(xiàn)了校準(zhǔn)算法中較為經(jīng)典的基于最小二乘的橢球擬合算法和目前最為新穎的僅使用陀螺儀補(bǔ)償?shù)男?zhǔn)算法，并將本文算法與這兩種算法進(jìn)行對比試驗(yàn)。

4.1.1 基于最小二乘的橢球擬合算法

橢球擬合算法是一種僅使用磁力計就可以進(jìn)行校準(zhǔn)的方法，不需要其他外部設(shè)備。通過充分旋轉(zhuǎn)傳感器來獲取廣泛分布在橢球表面的磁力計數(shù)據(jù)，通過將橢球方程線性化后，使用最小二乘法求解橢球參數(shù)，從而獲得當(dāng)?shù)卮艌龅男?zhǔn)參數(shù)，并對磁力計數(shù)據(jù)進(jìn)行校準(zhǔn)。但是這種方法需要用戶旋轉(zhuǎn)傳感器來采集數(shù)據(jù)，使用并不是很方便，并且當(dāng)磁場環(huán)境改變時，需要重新采集數(shù)據(jù)進(jìn)行校準(zhǔn)，不能實(shí)時動態(tài)校準(zhǔn)。并且由于僅使用了磁力計數(shù)據(jù)，磁力計的噪聲也難以抑制，影響磁力計數(shù)據(jù)的質(zhì)量。

4.1.2 僅使用陀螺儀補(bǔ)償?shù)男?zhǔn)算法

僅使用陀螺儀補(bǔ)償?shù)男?zhǔn)算法是通過陀螺儀數(shù)據(jù)對磁力計數(shù)據(jù)進(jìn)行旋轉(zhuǎn)預(yù)測，然后使用擴(kuò)展卡爾曼濾波對預(yù)測值和量測值進(jìn)行融合。這種方法雖然實(shí)現(xiàn)了實(shí)時的動態(tài)校準(zhǔn)，且對數(shù)據(jù)分布的要求比較低，但是由于陀螺儀數(shù)據(jù)沒有預(yù)先進(jìn)行處理，存在較為嚴(yán)重的漂移現(xiàn)象，所以磁力計數(shù)據(jù)并不穩(wěn)定，同樣存在漂移現(xiàn)象。

4.2 實(shí)驗(yàn)對比

本文中使用九軸IMU模塊LMPS-B2(如圖1所示)來采集磁力計、陀螺儀和加速度計的原始數(shù)據(jù)，模塊的采樣頻率是100 Hz，并通過藍(lán)牙將數(shù)據(jù)實(shí)時傳輸?shù)诫娔X客戶端。

圖1 九軸IMU模塊LMPS-B2

為保證實(shí)驗(yàn)效果，數(shù)據(jù)采集時要求傳感器一直處于同一磁場環(huán)境下。

先充分旋轉(zhuǎn)傳感器，使得數(shù)據(jù)的分布滿足橢球擬合算法的需求，之后將傳感器靜止10 min，采集靜止數(shù)據(jù)。對于橢球擬合算法，由于其不能實(shí)時校準(zhǔn)數(shù)據(jù)，所以將旋轉(zhuǎn)時的數(shù)據(jù)輸入到算法中計算校準(zhǔn)參數(shù)，之后用這些校準(zhǔn)參數(shù)對靜止時數(shù)據(jù)進(jìn)行校準(zhǔn)。而僅使用陀螺儀補(bǔ)償?shù)男?zhǔn)算法和本文中的校準(zhǔn)算法可以實(shí)時輸出校準(zhǔn)后的數(shù)據(jù)。

圖2是3種算法的校準(zhǔn)效果，圖中實(shí)點(diǎn)數(shù)據(jù)點(diǎn)是原始數(shù)據(jù)，叉點(diǎn)數(shù)據(jù)點(diǎn)是算法校準(zhǔn)后的數(shù)據(jù)(每隔10個數(shù)據(jù)畫一個點(diǎn))，可以發(fā)現(xiàn)由于軟鐵干擾和硬鐵干擾的存在，原始數(shù)據(jù)所在的橢球并不在原點(diǎn)位置處，這也是磁力計數(shù)據(jù)校準(zhǔn)的意義所在。經(jīng)過3種算法校準(zhǔn)后，磁力計的數(shù)據(jù)可以很好的恢復(fù)到理想球體附近，說明3種算法都達(dá)到了不錯的校準(zhǔn)效果。

(a)橢球擬合算法 (b)僅使用陀螺儀補(bǔ)償?shù)乃惴?(c)本文算法圖2 3種算法的校準(zhǔn)效果

接下來比較各算法在磁力計校準(zhǔn)的穩(wěn)定性上的差異。

首先來對比基于最小二乘的橢球擬合算法和本文算法在磁場校準(zhǔn)時的穩(wěn)定性，圖3中畫出了10 min的靜止時間內(nèi)，磁力計傳感器x軸數(shù)據(jù)的情況，圖中直線是橢球擬合算法校準(zhǔn)后的磁力計數(shù)據(jù)，點(diǎn)是本文算法校準(zhǔn)后的數(shù)據(jù)?？梢园l(fā)現(xiàn)，由于磁力計本身量測時存在噪聲，所以數(shù)據(jù)的波動是很大的，而經(jīng)過橢球擬合算法校準(zhǔn)后的數(shù)據(jù)，雖然能夠剔除軟鐵干擾和硬鐵干擾，但是不能消除這一部分噪聲，導(dǎo)致校準(zhǔn)后的數(shù)據(jù)仍存在很大的波動，大概在2 μT左右，而本文算法融合了加速度計和陀螺儀的數(shù)據(jù)來對磁力計數(shù)據(jù)進(jìn)行校準(zhǔn)，可以很好的降低磁力計數(shù)據(jù)的噪聲波動，僅為0.5 μT左右，提高了數(shù)據(jù)的可信度和穩(wěn)定性。

圖3 x軸的磁力計數(shù)據(jù)

其他軸上的效果也是如此，圖4是磁力計3個軸上靜止數(shù)據(jù)的箱型圖，圖5是xz平面上的數(shù)據(jù)以及置信度為95%的置信橢圓，可以發(fā)現(xiàn)每個軸上，本文算法校準(zhǔn)后的噪聲波動都小于橢球擬合算法。

(a)磁力計x軸數(shù)據(jù) (b)磁力計y軸數(shù)據(jù) (c)磁力計z軸數(shù)據(jù)圖4 各軸數(shù)據(jù)的箱型圖

圖5 x軸和z軸的磁力計數(shù)據(jù)和置信橢圓

同時，還采集了多組數(shù)據(jù)進(jìn)行實(shí)驗(yàn)，并計算了靜止時三軸數(shù)據(jù)上的協(xié)方差矩陣，如表1所示，從表中可以發(fā)現(xiàn)本文算法校準(zhǔn)后數(shù)據(jù)的方差普遍小于橢球擬合算法。

接下來比較僅使用陀螺儀補(bǔ)償?shù)拇艌鲂?zhǔn)算法和本文算法，由于二者都可以實(shí)時的進(jìn)行磁場校準(zhǔn)，所以直接比較2個算法輸出的參數(shù)即可。為了更加直觀的比較兩種算法的穩(wěn)定性，本文計算了從靜止開始，每一個數(shù)據(jù)點(diǎn)到靜止起始數(shù)據(jù)點(diǎn)的距離，并以此來定量的表示磁力計數(shù)據(jù)的漂移情況，距離越大，漂移越嚴(yán)重，穩(wěn)定性越差。

由于兩種算法都用到了陀螺儀的數(shù)據(jù)，而陀螺儀本身就已經(jīng)存在了一定的漂移現(xiàn)象，為了定量的分析不同陀螺儀漂移下，兩種算法的穩(wěn)定性。首先計算了陀螺儀三軸的漂移值，并在陀螺儀數(shù)據(jù)中將其剔除，然后人工添加不同級別的漂移，并比較各個情況下兩種算法的漂移距離。

如圖6所示，分別在陀螺儀數(shù)據(jù)上添加了0.1～0.5(°)/s的不同級別的零漂，然后將其輸入兩種算法中進(jìn)行計算，圖中2條直線分別是本文算法計算得到的磁力計漂移距離和陀螺儀補(bǔ)償算法的漂移距離，可以明顯的發(fā)現(xiàn)，本文算法在減少磁力計數(shù)據(jù)漂移、提高校準(zhǔn)穩(wěn)定性上有著良好的效果。

給陀螺儀3個測量軸添加了0.3(°)/s的零漂，并驗(yàn)證本文算法對漂移的抑制效果。由于加速度計具有長期穩(wěn)定性，加入了加速度計的輔助后，本文算法的校準(zhǔn)數(shù)據(jù)在10 min內(nèi)僅漂移了1.67 μT，優(yōu)于陀螺儀補(bǔ)償算法的19.56 μT。

5 結(jié)束語

本文先通過互補(bǔ)濾波使用加速度修正陀螺儀數(shù)據(jù)，然后使用修正后的陀螺儀數(shù)據(jù)對磁力計進(jìn)行旋轉(zhuǎn)，以更準(zhǔn)確地預(yù)測磁力計數(shù)據(jù)，完成擴(kuò)展卡爾曼濾波中的狀態(tài)預(yù)測過程，接著使用擴(kuò)展卡爾曼濾波融合預(yù)測值和磁力計量測值，實(shí)現(xiàn)磁力計的動態(tài)校準(zhǔn)。實(shí)驗(yàn)表明，相比于傳統(tǒng)的橢球擬合算法，本文算法可以降低磁力計數(shù)據(jù)的噪聲波動，二者的噪聲波動分別為2 μT和0.5 μT；相較于最新的陀螺儀補(bǔ)償算法，由于加速度計具有長期穩(wěn)定性，可以抑制磁力計數(shù)據(jù)的漂移現(xiàn)象，磁力計數(shù)據(jù)的漂移距離由19.56 μT降低到了1.67 μT。實(shí)現(xiàn)了一種穩(wěn)定、高精度的實(shí)時磁力計校準(zhǔn)。

本文僅對磁力計數(shù)據(jù)進(jìn)行了校準(zhǔn)，并未繼續(xù)求解航向等姿態(tài)信息，后續(xù)可以將校準(zhǔn)后的磁力計數(shù)據(jù)用于姿態(tài)結(jié)算等多種需要磁場信息的算法當(dāng)中。