六軸IMU補(bǔ)償?shù)拇帕τ?jì)動(dòng)態(tài)穩(wěn)定校準(zhǔn)

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

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

0 引言

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

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

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

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

1 磁力計(jì)量測(cè)模型

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

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

(1)

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

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

(2)

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

(3)

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

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

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

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

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

(4)

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

(5)

D=CscCnoCsiRbmb

(6)

o=CscCnoohi+ozb

(7)

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

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

(8)

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

e=v×a

(9)

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

(10)

3 磁力計(jì)校準(zhǔn)

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

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

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

(11)

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

(12)

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

(13)

(14)

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

(15)

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

(16)

(17)

(18)

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

Xt+1=FtXt+ωt

(19)

(20)

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

(21)

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

3.2 量測(cè)方程的建模

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

Zt=HtXt+vt

(22)

(23)

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

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

預(yù)測(cè)階段：

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時(shí)刻到t時(shí)刻的一步預(yù)測(cè)值；Xt和Pt為通過(guò)EKF算法求得的t時(shí)刻的狀態(tài)量估計(jì)值和狀態(tài)的方差估計(jì)值；Kt為濾波增益。

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

4.1 算法介紹

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

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

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

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

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

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

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

圖1 九軸IMU模塊LMPS-B2

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

先充分旋轉(zhuǎn)傳感器，使得數(shù)據(jù)的分布滿足橢球擬合算法的需求，之后將傳感器靜止10 min，采集靜止數(shù)據(jù)。對(duì)于橢球擬合算法，由于其不能實(shí)時(shí)校準(zhǔn)數(shù)據(jù)，所以將旋轉(zhuǎn)時(shí)的數(shù)據(jù)輸入到算法中計(jì)算校準(zhǔn)參數(shù)，之后用這些校準(zhǔn)參數(shù)對(duì)靜止時(shí)數(shù)據(jù)進(jìn)行校準(zhǔn)。而僅使用陀螺儀補(bǔ)償?shù)男?zhǔn)算法和本文中的校準(zhǔn)算法可以實(shí)時(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個(gè)數(shù)據(jù)畫(huà)一個(gè)點(diǎn))，可以發(fā)現(xiàn)由于軟鐵干擾和硬鐵干擾的存在，原始數(shù)據(jù)所在的橢球并不在原點(diǎn)位置處，這也是磁力計(jì)數(shù)據(jù)校準(zhǔn)的意義所在。經(jīng)過(guò)3種算法校準(zhǔn)后，磁力計(jì)的數(shù)據(jù)可以很好的恢復(fù)到理想球體附近，說(shuō)明3種算法都達(dá)到了不錯(cuò)的校準(zhǔn)效果。

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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

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