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

嚴丹，鄧志紅，張雁鵬

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

0 引言

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

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

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

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

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

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

1 傳感器建模

1.1 坐標系建立

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

1.2 姿態表示與測定

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

1.2.1 四元數姿態更新

四元數可用于表示剛體旋轉。設剛體相對于n系做定點運動，采用四元數表示剛體的定點運動：

(1)

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

利用四元數表示旋轉矩陣：

(2)

四元數微分方程：

(3)

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

(4)

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

(5)

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

1.2.2 歐拉角

載體的空間姿態可以看作是導航坐標系Onxnynzn依次繞航向軸、俯仰軸、橫滾軸旋轉ψ、θ、γ后的復合結果。

當旋轉順序設定為z-x-y時，從n系到b系的旋轉矩陣為

(6)

1.3 傳感器模型

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

(7)

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

(8)

2 數據融合算法設計

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

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

yg,t=ωt+vg,t,

(9)

(10)

(11)

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

(12)

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

θ=arcsinya,y,

(13)

(14)

但無法通過加速度計數據算出偏航角ψ.

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

2.1 第1級姿態更新

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

(15)

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

(16)

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

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

(17)

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

(18)

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

(19)

(20)

利用陀螺儀數據更新旋轉四元數的1階離散公式為

(21)

(22)

式中：μ1為置信于a,t的權重。

(23)

式中：ρ1=μ1β1，為可調參數。

2.2 第2級姿態更新

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

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

(24)

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

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

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

(25)

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

(26)

(27)

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

(28)

式中：μ2為置信于m,t的權重，其數值較??；ρ2=μ2β2，為可調參數。

2.3 整體更新

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

(29)

3 實驗驗證

3.1 解算誤差表示

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

(30)

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

(31)

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

3.2 仿真實驗

通過軌跡發生器可自定義載體運動情況，并產生模擬陀螺儀、加速度計、磁力計數據以及真實姿態數據。

設計載體運動方式和外部環境的步驟如下：

1)載體靜止20 s；

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

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

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

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

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

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

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

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

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

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

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

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

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

3.3 實測實驗

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

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

表2 MTi-G-710內置傳感器相關參數表Tab.2 Specifications of MTi-G-710

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

(32)

式中：n為該組數據總時刻數。

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

利用上述3組數據進行算法檢驗，將本文提出算法與角速率積分法、互補濾波法、傳統優化法以及兩級姿態更新法進行對比。

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

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

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

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

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

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

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

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

4 結論

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

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