一種微慣性與磁組合測量單元的姿態(tài)解算方法

嚴丹，鄧志紅，張雁鵬

(1.北京理工大學 自動化學院,北京 100081；2.中國廣核集團 蘇州熱工研究院,江蘇 蘇州 215004)

0 引言

基于微慣性測量單元(MIMU)或微慣性與磁組合測量單元(MIMMU)的姿態(tài)解算系統(tǒng)，具有自主性強、受外界環(huán)境影響小、體積小、質(zhì)量輕、性價比高的優(yōu)點，因此被廣泛應用于無人機、機器人、人體動作捕捉等領(lǐng)域。

MIMU由三軸陀螺儀和三軸加速度計組成，可用于測量載體的角速率和加速度。MIMMU相比于MIMU增加了三軸磁力計，可用于測量外界磁感應強度。利用角速率積分可以獲得載體姿態(tài)，但由于微陀螺儀隨機漂移較嚴重，在積分過程中解算誤差會隨時間累積，長時間工作很可能造成發(fā)散。當載體處于靜止或勻速運動時，加速度計只測得載體坐標系下重力加速度分量，可用于校正橫滾角和俯仰角，但當載體加速運動時會影響校正效果。當載體附近無鐵磁干擾時，磁力計只測得載體坐標系下地球磁場分量，可用于校正偏航角，但當磁力計受外界鐵磁干擾較嚴重時會影響校正效果。由于利用后二者解算姿態(tài)角只基于一個解算周期內(nèi)的加速度計數(shù)據(jù)和磁數(shù)據(jù)，解算誤差不隨時間積累，因此使用MIMU或MIMMU數(shù)據(jù)進行信息融合，共同解算載體姿態(tài)，一方面可以有效地提高解算精度，另一方面可以使系統(tǒng)長時間工作而誤差不發(fā)散。

基于MIMU或MIMMU解算載體姿態(tài)的信息融合算法可分為三大類：Kalman濾波(KF)法[1-9]、互補濾波法[10-12]和最優(yōu)化補償方法[13-17]。KF法通過建立狀態(tài)方程和觀測方程，實現(xiàn)多傳感器數(shù)據(jù)融合。Luinge等[1]提出了一種基于IMU數(shù)據(jù)解算人體骨骼姿態(tài)的線性KF算法。該算法以姿態(tài)角估計誤差和陀螺儀補償誤差作為狀態(tài)量，通過建立誤差模型推導出Kalman濾波器的狀態(tài)方程和觀測方程。Roetenberg等在文獻[1]算法基礎(chǔ)上增加了磁擾動誤差作為狀態(tài)量，實現(xiàn)了基于IMMU數(shù)據(jù)的信息融合[2]，相對于文獻[1]可以有效預防航向漂移和外界磁干擾。但上述算法存在計算量較大的問題，Sabatelli等[3]提出一種兩級擴展Kalman濾波(EKF)算法，相比于使用Kalman濾波器同時融合加速度計和磁力計數(shù)據(jù)，該算法的計算量更小。針對載體運動加速度或外界鐵磁干擾過大、影響系統(tǒng)姿態(tài)解算問題，近年來一部分文獻使用自適應Kalman濾波器來增加系統(tǒng)的魯棒性。例如：Sabatini[4]設(shè)計了一種門限式自適應因子，當運動加速度或者磁擾動大于某個閾值時，不使用該時刻的相應數(shù)據(jù)修正狀態(tài)量。Ren等[5]提出一種改進的自適應因子，隨運動加速度或磁擾動的增大線性加大觀測噪聲方差陣中對應元素的值，從而實現(xiàn)逐漸減小Kalman濾波器對加速度計或磁力計數(shù)據(jù)的置信度。Tong等[6]基于傳感器誤差模型建立乘性擴展Kalman濾波器(MEKF)，并提出了使用隱馬爾可夫模型對當前加速度以及磁干擾狀態(tài)進行判斷，以作為自適應調(diào)整MEKF中觀測噪聲方差陣的依據(jù)。另一部分文獻使用較新的KF算法，例如無跡Kalman濾波器[7-8]、協(xié)方差膨脹的乘法擴展Kalman濾波器(CI-MEKF)[9]。該類方法雖然解算效果較好，但計算量相比于后兩類算法較大。

根據(jù)文獻[10]的理論推導，互補濾波法可通過比例積分(PI)控制器對陀螺儀所估計姿態(tài)進行高通濾波，對加速度計或磁力計所估計姿態(tài)進行低通濾波。該類算法計算量小，但解算效果受參數(shù)影響較大。

最優(yōu)化補償算法是通過構(gòu)建并優(yōu)化損失函數(shù)進行信息融合的方法，其計算量介于前兩類之間，且可以對非高斯噪聲所引入的誤差進行校正。Bachmann等[14]提出一種使用加速度計和磁力計數(shù)據(jù)，對基于角速率更新的先驗四元數(shù)進行補償?shù)乃惴?。它通過上一時刻后驗四元數(shù)獲得當前加速度計和磁力計數(shù)據(jù)的估計值，將測量值與估計值的均方誤差看作代價函數(shù)，以姿態(tài)四元數(shù)作為自變量，利用高斯牛頓法對該代價函數(shù)進行最優(yōu)化，以獲得后驗四元數(shù)。Madgwick等在文獻[15-16]中提出了一種基于梯度下降的濾波方法。相比于文獻[14]，在代價函數(shù)不變的前提下，該算法改變了最優(yōu)化方法，有效降低了計算復雜度。此外，它利用誤差積分反饋進行陀螺儀漂移補償，利用慣性坐標系下磁感應強度在y軸沒有分量的特點進行磁補償。該算法解算精度與KF算法類似且計算量更小。

綜上所述，基于最優(yōu)化補償?shù)奈T性與磁組合姿態(tài)測量方法，相比于KF法計算量更小，相比于互補濾波法魯棒性更強。但目前的最優(yōu)化補償算法未考慮外界鐵磁物質(zhì)對于磁力計的干擾，當鐵磁干擾較大時該算法無法正常工作。另外，該算法使用同一個濾波器對MIMMU數(shù)據(jù)進行融合，加速度計數(shù)據(jù)會對偏航角的解算造成不利影響，磁力計數(shù)據(jù)會對橫滾角和俯仰角的解算造成不利影響。因此，本文提出一種基于梯度下降法的兩級姿態(tài)更新算法：在第1級濾波中通過陀螺儀和加速度計數(shù)據(jù)更新俯仰角和橫滾角，在第2級濾波中通過陀螺儀和磁力計數(shù)據(jù)更新偏航角，以提高系統(tǒng)解算精度。針對磁力計易受外界鐵磁干擾問題，該算法在第2級濾波中對鐵磁干擾進行實時估計并補償，使算法具有一定抗鐵磁干擾能力。最后對所提方法進行了仿真和實測實驗驗證。

1 傳感器建模

1.1 坐標系建立

取地心慣性坐標系Oixiyizi(簡稱i系)為慣性坐標系，其原點Oi位于地球地心，xi軸指向春分點，zi軸沿地球自轉(zhuǎn)軸，yi軸與xi、zi軸構(gòu)成右手直角坐標系。取載體坐標系Obxbybzb(簡稱b系)與傳感器模塊固連，其原點Ob位于傳感器模塊重心，xb軸沿傳感器模塊橫軸指向右側(cè)，zb軸沿傳感器模塊豎軸指向上方，yb軸與xb、zb軸構(gòu)成右手直角坐標系。設(shè)初始時刻的載體坐標系與導航坐標系Onxnynzn(簡稱n系)重合。

1.2 姿態(tài)表示與測定

目前，常用的姿態(tài)更新方法有四元數(shù)法[1-10,12-17]、歐拉角法[11]、旋轉(zhuǎn)矢量法[18-19]等。其中，歐拉角法存在萬向節(jié)死鎖問題，無法適用于全姿態(tài)解算。旋轉(zhuǎn)矢量法采用多子樣算法對不可交換誤差進行有效補償，計算量相比于四元數(shù)法較大，適用于角機動頻繁或存在嚴重角振動的運載體的姿態(tài)更新。由于機器人、無人機、人體動作捕捉等一般不存在嚴重角振動，因此本文選用四元數(shù)法進行載體姿態(tài)更新。

1.2.1 四元數(shù)姿態(tài)更新

四元數(shù)可用于表示剛體旋轉(zhuǎn)。設(shè)剛體相對于n系做定點運動，采用四元數(shù)表示剛體的定點運動：

(1)

式中：Q為從n系到b系的旋轉(zhuǎn)四元數(shù)；un為旋轉(zhuǎn)軸和旋轉(zhuǎn)方向；α為轉(zhuǎn)過的角度。通過四元數(shù)表示可以將b系看作由n系經(jīng)過無中間過程的一次性等效旋轉(zhuǎn)形成的。

利用四元數(shù)表示旋轉(zhuǎn)矩陣：

(2)

四元數(shù)微分方程：

(3)

由于陀螺儀測量需要一定時間，所測得角速率是離散數(shù)據(jù)，需要對(3)式進行基于1階近似的離散化處理，可得

(4)

式中：I4為4階單位矩陣；T為陀螺儀采樣時間；ωk為k時刻陀螺儀量測值；Qk為k時刻n系到b系的旋轉(zhuǎn)四元數(shù)；Z(·)為ωk的反對稱矩陣，定義為

(5)

ωk,x、ωk,y、ωk,z分別為向量ωk的3個元素。

1.2.2 歐拉角

載體的空間姿態(tài)可以看作是導航坐標系Onxnynzn依次繞航向軸、俯仰軸、橫滾軸旋轉(zhuǎn)ψ、θ、γ后的復合結(jié)果。

當旋轉(zhuǎn)順序設(shè)定為z-x-y時，從n系到b系的旋轉(zhuǎn)矩陣為

(6)

1.3 傳感器模型

本文所使用的傳感器MIMMU由微陀螺儀、微加速度計及微磁力計組成。設(shè)t時刻微陀螺儀所測數(shù)據(jù)為yg,t，其中包含b系相對于i系的角速度信息、陀螺零偏、測量噪聲；設(shè)t時刻微加速度計所測數(shù)據(jù)為ya,t，其中包含載體運動加速度、b系下所測得的重力加速度、系統(tǒng)固有偏置、測量噪聲；設(shè)t時刻微磁力計所測數(shù)據(jù)為ym,t，其中包含b系下所測得的地球磁場、外界鐵磁干擾、系統(tǒng)固有偏置、測量噪聲。在不考慮傳感器非正交誤差、靈敏度誤差前提下，其具體量測模型如下：

(7)

為了簡化計算，本文將ds,t和dh,t合并為t時刻磁力計所受鐵磁干擾矢量dt，原磁力計量測模型可簡化為

(8)

2 數(shù)據(jù)融合算法設(shè)計

圖1 本文提出的姿態(tài)解算算法框圖Fig.1 Block diagram of the proposed attitude determination method

由于Kg、Ka、Km、bg、ba、bm屬于傳感器系統(tǒng)誤差，可通過數(shù)據(jù)預處理校正。假設(shè)當前傳感器已經(jīng)過校正，設(shè)Kg、Ka、Km均為3階單位矩陣以及bg、ba、bm均為零向量，可得

yg,t=ωt+vg,t,

(9)

(10)

(11)

若初始時刻傳感器水平放置，即初始姿態(tài)的z軸(即zn軸)與重力加速度方向重合，則加速度計所測x軸與y軸數(shù)據(jù)均為0，即g=[0 0gz]T。隨后，當傳感器旋轉(zhuǎn)到某一姿態(tài)并保持靜止或勻速運動時，可通過(6)式、(10)式求得加速度計量測值ya：

(12)

此時傳感器的俯仰角θ和橫滾角γ為

θ=arcsinya,y,

(13)

(14)

但無法通過加速度計數(shù)據(jù)算出偏航角ψ.

一方面，由于磁力計相比于加速度計更容易受到外界環(huán)境干擾(如電子設(shè)備等)，其數(shù)據(jù)可信度相對較小。另一方面，無論傳感器初始姿態(tài)如何，磁力計所測數(shù)據(jù)都與偏航角ψ有關(guān)，可以利用磁力計數(shù)據(jù)修正載體偏航角。因此本文采用兩級姿態(tài)更新法，在第1級更新中將陀螺儀與加速度計數(shù)據(jù)融合，對載體的俯仰角和橫滾角進行更新；在第2級更新中將陀螺儀與磁力計數(shù)據(jù)融合，對載體的偏航角進行更新。

2.1 第1級姿態(tài)更新

由于本文所提算法主要應用于人體運動捕捉、小型機器人、無人機等設(shè)備姿態(tài)解算，載體運動加速度相對于重力加速度較小，因此忽略運動加速度部分，加速度計量測模型可簡化為

(15)

t時刻加速度的實際測量值與估計值之間誤差可以表示為

(16)

由此可知，當估計四元數(shù)q越接近t時刻由n系到b系旋轉(zhuǎn)四元數(shù)的真實值qt時，ε1(q)越接近[0 0 0]T。利用ε1(·)的歐幾里得范數(shù)作為代價函數(shù)：

F1(q)=ε1(q)Tε1(q),

(17)

將t-1時刻由n系到b系的后驗估計四元數(shù)作為估計四元數(shù)的初始值，利用梯度下降法對F1(q)進行最優(yōu)化，沿損失函數(shù)的負梯度方向更新變量，有助于減小損失函數(shù)：

(18)

設(shè)q=[q0q1q2q3]T、g=[gxgygz]T，由(2)式、(16)式、(17)式可得

(19)

(20)

利用陀螺儀數(shù)據(jù)更新旋轉(zhuǎn)四元數(shù)的1階離散公式為

(21)

(22)

式中：μ1為置信于a,t的權(quán)重。

(23)

式中：ρ1=μ1β1，為可調(diào)參數(shù)。

2.2 第2級姿態(tài)更新

由于地球地表磁場強度在25～65 μT范圍內(nèi)，比較微弱，鐵磁干擾數(shù)值常大于地球地表磁場，因此無法忽略鐵磁干擾部分，本文選擇對鐵磁干擾dt進行估計。

由(11)式得到t時刻磁力計的實際測量值與估計值之間誤差為

(24)

由(11)式和(24)式可知，當估計四元數(shù)q越接近t時刻旋轉(zhuǎn)四元數(shù)的真實值qt，且估計鐵磁干擾d越接近t時刻鐵磁干擾的真實值dt時，ε2(q,d)越接近[0 0 0]T。

令x=[qd]T，利用ε2(·)的歐幾里得范數(shù)作為代價函數(shù):

F2(x)=ε2(x)Tε2(x)，

(25)

利用梯度下降法對F2(x)進行最優(yōu)化：

(26)

(27)

式中：Λ(x)為第2級代價函數(shù)F2(x)關(guān)于估計四元數(shù)q求偏導后轉(zhuǎn)置的結(jié)果，可用于優(yōu)化估計四元數(shù)。由于該公式太過復雜，在此省略展示。

(28)

式中：μ2為置信于m,t的權(quán)重，其數(shù)值較?。沪?=μ2β2，為可調(diào)參數(shù)。

2.3 整體更新

根據(jù)磁力計量測模型(11)式，可得t時刻后驗估計鐵磁干擾為

(29)

3 實驗驗證

3.1 解算誤差表示

令真實姿態(tài)四元數(shù)為qtrue，通過算法解算出的姿態(tài)四元數(shù)為q，q相對于qtrue的誤差可通過Δq表示為

(30)

根據(jù)四元數(shù)的輻角公式(1)式，可得解算誤差角Δθ為

(31)

式中：scalar(·)為提取四元數(shù)內(nèi)標量的函數(shù)。本文在后續(xù)的實驗中通過Δθ衡量解算誤差。

3.2 仿真實驗

通過軌跡發(fā)生器可自定義載體運動情況，并產(chǎn)生模擬陀螺儀、加速度計、磁力計數(shù)據(jù)以及真實姿態(tài)數(shù)據(jù)。

設(shè)計載體運動方式和外部環(huán)境的步驟如下：

1)載體靜止20 s；

2)載體繞xn軸以3π rad/s角速率旋轉(zhuǎn)40 s，沿xn、yn、zn軸分別施加10 μT的鐵磁干擾；

3)載體靜止20 s，外界鐵磁干擾保持不變；

4)載體繞xn軸以3π rad/s角速率旋轉(zhuǎn)40 s，沿xn、yn、zn軸分別施加30 μT的鐵磁干擾；

5)載體靜止20 s，外界鐵磁干擾保持不變；

6)載體繞xn軸以3π rad/s角速率旋轉(zhuǎn)40 s，沿xn、yn、zn軸分別施加50 μT的鐵磁干擾；

7)載體靜止20 s，外界鐵磁干擾保持不變。

采樣周期設(shè)為100 Hz，陀螺儀漂移設(shè)置為0.001°/s，加速度計漂移設(shè)置為8×10-6g/s，磁力計漂移設(shè)置為0.05 μT.地球局部參數(shù)均根據(jù)北京進行設(shè)置，重力加速度設(shè)為[0 m/s2，0 m/s2，-9.801 47 m/s2]，地球表面磁場設(shè)為[27.77 μT，-3.35 μT，46.97 μT].地球自轉(zhuǎn)角速率設(shè)置為0.000 072 921 rad/s，地球近似半徑設(shè)置為6 378 137 m.

利用上述數(shù)據(jù)對本文提出算法進行仿真測試。將本文提出算法的實驗結(jié)果與角速率積分法、互補濾波法、傳統(tǒng)優(yōu)化法以及兩級姿態(tài)更新法的運行結(jié)果進行對比。其中，角速率積分法僅利用微陀螺儀數(shù)據(jù)，通過離散化四元數(shù)微分方程更新姿態(tài)四元數(shù)。本文用作對比的互補濾波法由左國玉等[12]提出，同時對MIMMU數(shù)據(jù)進行融合。其余3種算法都是基于梯度下降法優(yōu)化姿態(tài)四元數(shù)，具體來說：傳統(tǒng)優(yōu)化法由Madgwick等[16]提出，它使用一個濾波器同時對MIMMU數(shù)據(jù)進行融合，且在建模過程中未考慮外界鐵磁干擾；兩級姿態(tài)更新法是根據(jù)本文提出算法去除鐵磁補償部分獲得的，它與前者區(qū)別在于采用了兩級濾波器，先對陀螺儀與加速度計數(shù)據(jù)進行融合，更新載體的俯仰角和橫滾角，再對陀螺儀與磁力計數(shù)據(jù)進行融合，更新載體的偏航角；本文提出算法相比于兩級姿態(tài)更新法，在第2級濾波建模中考慮了鐵磁干擾，增加了鐵磁干擾補償。

在本文提出算法中令ρ1=0.000 026 7，ρ2=0.000 030 7.各算法解算誤差角Δθ的階段均值與總體均值如表1所示，表中MeanΔθ為Δθ均值。各算法針對該數(shù)據(jù)的解算誤差角Δθ隨時間變化情況如圖2所示。

表1 各算法MeanΔθ統(tǒng)計表Tab.1 MeanΔθ of each algorithm (°)

圖2 各算法解算誤差對比圖(針對仿真數(shù)據(jù))Fig.2 Comparison among the attitude determination errors of various algorithms (for simulated data)

根據(jù)仿真設(shè)定，實驗載體在20～60 s、60～120 s、120～200 s階段所受外界鐵磁干擾逐漸增加，通過該仿真數(shù)據(jù)可對各個算法抗鐵磁干擾能力和姿態(tài)解算精度進行測試。由表1可以看出，互補濾波法、傳統(tǒng)優(yōu)化法、兩級姿態(tài)更新法以及本文提出算法的各階段MeanΔθ始終低于角速率積分法，表明使用MIMMU數(shù)據(jù)解算載體姿態(tài)有利于提高解算精度。另外，本文所提算法各階段的MeanΔθ相近，沒有呈現(xiàn)遞增狀況，且都小于3°；而其余3種融合解算算法各階段的MeanΔθ都呈現(xiàn)出明顯遞增，且均大于3°，表明本文提出算法相比于其他對比算法可以有效估計并補償外界鐵磁干擾，使系統(tǒng)具有一定的抗鐵磁干擾能力且解算精度更高。

根據(jù)仿真設(shè)定，載體在60～80 s、120～140 s、180～200 s處于靜止狀態(tài)，且所受鐵磁干擾遞增。由圖2可以看出，在這些時間段，角速率積分法解算誤差不變，傳統(tǒng)優(yōu)化法、兩級姿態(tài)更新法以及本文提出算法的解算誤差隨時間減少，而互補濾波法解算誤差在60～80 s區(qū)間隨時間減少、在120～140 s和180～200 s區(qū)間隨時間增加。表明基于最優(yōu)化的補償算法相比于互補濾波法魯棒性更強。通過對比傳統(tǒng)優(yōu)化法和兩級姿態(tài)更新法的解算結(jié)果可以看出，兩級姿態(tài)更新相比于單濾波器更有利于提高解算精度，當載體處于靜止時其補償解算誤差的速度也更快。通過對比兩級姿態(tài)更新法和本文提出算法的解算結(jié)果可以看出，本文所設(shè)計的鐵磁干擾補償算法效果明顯，可以有效提高系統(tǒng)的魯棒性，使之適用于各種鐵磁干擾環(huán)境。

3.3 實測實驗

使用荷蘭Xsens公司生產(chǎn)的MTi-G-710作為傳感器，其內(nèi)部包含三軸微陀螺儀、三軸微加速度計、三軸微磁力計，相關(guān)參數(shù)如表2所示。MTi-G-710所配套的軟件內(nèi)置一套基于KF的姿態(tài)解算算法，當傳感器處于均勻磁場中時，該算法解算結(jié)果的誤差均方根(RMS)如表3所示。本文將該算法輸出的姿態(tài)四元數(shù)看作真實姿態(tài)四元數(shù)，進行后續(xù)實驗。

通過手持MIMMU模塊，分別在鐵磁干擾不同的3個地點進行自由晃動、獲得3組數(shù)據(jù)，陀螺儀、加速度計、磁力計的采樣周期都設(shè)置為100 Hz.由于MTi-G-710中磁力計的輸出數(shù)據(jù)是單位化后的結(jié)果，即當外界無鐵磁干擾時，每一時刻磁力計數(shù)據(jù)的范數(shù)都為1，因此外界鐵磁干擾越大，每一時刻磁力計數(shù)據(jù)的范數(shù)越偏離1.根據(jù)(32)式量化該組數(shù)據(jù)所受鐵磁干擾的大?。?/p>

表2 MTi-G-710內(nèi)置傳感器相關(guān)參數(shù)表Tab.2 Specifications of MTi-G-710

表3 MTi-G-710配套算法解算精度表Tab.3 Attitude determination performance (°)

(32)

式中：n為該組數(shù)據(jù)總時刻數(shù)。

利用(32)式對各數(shù)據(jù)受鐵磁干擾大小進行計算，結(jié)果如表4所示。由表4可知，數(shù)據(jù)1所受鐵磁干擾整體最小，數(shù)據(jù)2次之，數(shù)據(jù)3所受鐵磁干擾最大。

利用上述3組數(shù)據(jù)進行算法檢驗，將本文提出算法與角速率積分法、互補濾波法、傳統(tǒng)優(yōu)化法以及兩級姿態(tài)更新法進行對比。

由于上述實驗算法都需要進行參數(shù)設(shè)置，利用數(shù)據(jù)1對各算法進行調(diào)參，其中在本文提出算法中令ρ1=0.000 026 7，ρ2=0.000 030 7.各算法解算誤差角Δθ的均值如表5所示。將互補濾波法與角速率積分法的MeanΔθ進行對比，通過表5可以看出：隨著外界鐵磁干擾的增大，該算法校正陀螺儀數(shù)據(jù)的效果逐漸變差，甚至在數(shù)據(jù)3時其解算誤差超過單純角速率積分法。由此表明互補濾波法的解算效果受參數(shù)影響大、魯棒性較弱。而傳統(tǒng)優(yōu)化法、兩級姿態(tài)更新法以及本文提出算法對于上述3組數(shù)據(jù)，它們的解算誤差始終低于角速率積分法以及互補濾波法，表明最優(yōu)化補償法相對于互補濾波法解算效果更好，且魯棒性較強，在不同鐵磁干擾環(huán)境下都能對陀螺儀數(shù)據(jù)進行一定程度的有效校正。根據(jù)表5，對比傳統(tǒng)優(yōu)化法和兩級姿態(tài)更新法的MeanΔθ可以看出，通過兩級姿態(tài)更新可以有效提高姿態(tài)解算精度；對比兩級姿態(tài)更新法與本文提出算法的MeanΔθ可以看出，增加鐵磁干擾補償有利于進一步提高算法解算精度，并使其可以很好地適應不同鐵磁干擾環(huán)境。

表5 各算法MeanΔθ統(tǒng)計表Tab.5 MeanΔθ of each algorithm (°)

以數(shù)據(jù)1為例進行具體分析。數(shù)據(jù)1的角速率、加速度、磁力計數(shù)據(jù)范數(shù)隨時間變化情況如圖3所示，各算法針對數(shù)據(jù)1的解算誤差角Δθ隨時間變化情況如圖4所示。通過對比圖3、圖4可以看出：0～25 s，傳感器處于靜止狀態(tài)，此時角速率積分法、互補濾波法、傳統(tǒng)優(yōu)化法的解算誤差都隨時間累積，而兩級姿態(tài)更新法和本文提出算法的解算誤差先增大、后減小，表明兩級姿態(tài)更新相比于單濾波器融合法可以更有效地補償傳感器靜置時陀螺儀漂移帶來的解算誤差；40～50 s，陀螺儀發(fā)生較大漂移，各算法解算誤差都出現(xiàn)不同程度的陡增；50～70 s，根據(jù)角速率積分法可以看出在這一時段陀螺儀漂移情況較平穩(wěn)，本文提出算法相比于其他對比算法能更快地通過加速度計和磁力計數(shù)據(jù)降低解算誤差；70～85 s，傳感器恢復靜止狀態(tài)，前3種方法解算誤差都隨時間增加，兩級姿態(tài)更新法雖能在一定程度上補償此前解算誤差，但補償速率較慢，本文提出算法由于加入了鐵磁補償，可以有效估計外界鐵磁干擾，以較快速率將解算誤差降低到較低狀態(tài)，這一特點使本算法更適用于長時間正常工作。

圖3 陀螺儀、加速度計、磁力計數(shù)據(jù)范數(shù)隨時間變化(針對數(shù)據(jù)1)Fig.3 Magnitudes of gyroscope data,accelerometer data and magnetometer data (for Data 1)

圖4 各算法解算誤差對比(針對數(shù)據(jù)1)Fig.4 Comparison among the attitude determination errors of various algorithms (for Data 1)

各算法針對數(shù)據(jù)2、數(shù)據(jù)3的解算誤差角Δθ隨時間變化情況分別如圖5、圖6所示。對比圖4、圖5、圖6可以看出，外界鐵磁干擾的增大會對互補濾波法、傳統(tǒng)優(yōu)化法、兩級姿態(tài)更新法的解算精度有較大影響，此時本文所提鐵磁補償法發(fā)揮作用，使系統(tǒng)在較強鐵磁干擾環(huán)境下仍能正常工作。

圖5 各算法解算誤差對比(針對數(shù)據(jù)2)Fig.5 Comparison among the attitude determination errors of various algorithms (for Data 2)

圖6 各算法解算誤差對比(針對數(shù)據(jù)3)Fig.6 Comparison among the attitude determination errors of various algorithms (for Data 3)

4 結(jié)論

本文使用MIMMU作為傳感器，提出了一種抗鐵磁干擾的兩級姿態(tài)更新姿態(tài)解算方法：它基于姿態(tài)四元數(shù)更新，可用于全姿態(tài)解算；第1級通過陀螺儀和加速度數(shù)據(jù)更新俯仰角和偏航角，第2級在考慮鐵磁干擾的情況下通過磁力計數(shù)據(jù)修正偏航角。

仿真和實測實驗結(jié)果表明：兩級姿態(tài)更新相比于單濾波器方法可以有效提高解算精度，而本文提出的利用梯度下降法估計并補償鐵磁干擾方法，可以使算法具有一定抗鐵磁干擾能力，解決了在外界存在鐵磁干擾的環(huán)境下低精度器件中高精度姿態(tài)獲取的難題。本文方法可廣泛應用于人體運動捕捉、無人機及機器人等領(lǐng)域，具有廣闊的應用前景。