動態(tài)磁場校準的九軸慣性融合

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

(1.中國科學院 空天信息創(chuàng)新研究院 傳感技術國家重點實驗室，北京 100190；2.中國科學院大學，北京 100049；3.深圳前海維晟智能技術有限公司，廣東 深圳 518101)

1 引 言

隨著低成本、輕重量的微電子機械系統(tǒng)(Micro-Electro-Mechanical System，MEMS)的發(fā)展，更小、更便宜的慣性傳感器被越來越廣泛地應用，尤其是移動設備、游戲機等消費類電子產品。MEMS小尺寸、低載荷的特點還能夠滿足小型機器人和微型飛行器的要求，使自主導航和控制成為可能。但是，低成本MEMS輸出的數(shù)據(jù)受到高水平噪聲和時變偏差的影響，必須使用傳感器融合算法對數(shù)據(jù)進行處理，以獲得平滑且無偏差的方位估計[1]。

對于俯仰角和翻滾角，通過三軸陀螺儀和三軸加速度計的融合就可以獲得準確且不隨時間漂移的結果，但是由這種六軸融合算法解算的航向角會由于陀螺儀的積分誤差無法補償而隨時間漂移。所以，確定準確的航向角需要用磁力計測量地球磁場作為絕對參考[2-5]，而磁力計的實際使用極易受到環(huán)境磁場的干擾，包括硬磁干擾和軟磁干擾[6]。所以在低成本MEMS慣導領域，如何減小航向角隨時間的漂移是最困難的問題，其關鍵就是如何實時地、便捷地校準磁力計。

對于磁場的校準，一種傳統(tǒng)的方法是建立磁場模型，根據(jù)磁場數(shù)據(jù)的約束條件，僅利用磁力計本身的數(shù)據(jù)計算校準參數(shù)，最常用的是橢球擬合法[7-9]。橢球擬合在一般環(huán)境下能取得很好的校準效果，但對磁力計數(shù)據(jù)的要求很高，需要磁場數(shù)據(jù)點均勻地分布在橢球表面，實際使用中需要執(zhí)行數(shù)據(jù)采集動作，比如使設備在空間中繞“8”字運動，這對用戶非常不友好，而且在某些場景(如無人機、機器人)中很難實現(xiàn)。此外，橢球擬合法在磁力計使用環(huán)境發(fā)生變化后需要重新校準，所以它既不具備便捷性，也不具備實時性。

使用慣性器件輔助磁場傳感器的校準是一個新的思路。Kok[10]將磁強計測量和慣性測量結合起來進行方位估計，將標定算法歸結為最大似然問題，綜合使用了橢球擬合和擴展卡爾曼濾波(Extended Kalman Filter，EKF)等算法，但它不能實時工作。Han Ke等[11]在忽略傳感器軸定位誤差和假定陀螺儀偏差為常值的條件下，使用EKF融合了陀螺儀和磁力計數(shù)據(jù)，實時計算了包括磁場向量、硬磁干擾和軟磁干擾在內的磁場信息，但是由于EKF自身的特點，算法缺乏對突變狀態(tài)的快速響應能力[12]。Zhu Maoran等[13]通過求解齊次最小二乘問題，提出了一種陀螺輔助磁力計的校準算法，該算法雖能夠在一步內完成傳感器的內稟標定和交叉標定，但其只能單獨作為一種粗略的標定方法，或作為其他精細算法的良好初值，并不能實現(xiàn)實時校準。

姿態(tài)估計最常用的方法是擴展卡爾曼濾波(EKF)和互補濾波(Complementary Filter，CF)，而且大多數(shù)融合算法都是以四元數(shù)的形式進行方向估計的[1]。對于EKF，狀態(tài)量數(shù)目的增加也會增加濾波方程的復雜程度，同時增加矩陣求逆的運算復雜度，從而對計算造成負擔。當模型的線性化假設不成立時，EKF的線性化近似還會導致濾波器極度不穩(wěn)定。而且在實際應用中，外界噪聲也往往不符合高斯白噪聲的假設[14]。因此，互補濾波常常成為EKF的替代方案，因為其無需任何統(tǒng)計描述，簡單有效，收斂速度相比于EKF也更快[1]?；パa濾波的代表性研究中[15-17]，Euston使用PI控制器調控向量積形式的誤差[16]，Madgwick使用梯度下降法調控線性形式的誤差[17]。這兩種方法在低成本MEMS慣性傳感器中應用十分廣泛。

本文綜合考慮各種磁場校準與姿態(tài)估計的方法，認為可以從算法上減小航向角的漂移而不必增加傳感器的成本。為了實現(xiàn)動態(tài)的、更便捷的磁場校準，本文使用EKF融合慣性傳感器(陀螺儀)和磁力計，同時考慮EKF失效的問題，對其校準的效果做出量化評價，以調整其在姿態(tài)估計中的作用程度。作用程度通過互補濾波中PI控制器的參數(shù)來體現(xiàn)，它是一個動態(tài)的函數(shù)，可以實時調整磁力計在融合算法中的比例。此外，為了進一步減小慣性測量單元(Inertial Measurement Unit，IMU)的誤差，還要考慮自由加速度的影響，Hytti在卡爾曼濾波中通過干預測量矩陣協(xié)方差來減小有害加速度的影響[18]，本文使用更簡單并且與磁力計統(tǒng)一的調整方法，同樣定義其信賴函數(shù)，動態(tài)控制加速度在融合算法中的比例。

2 算法原理概述

2.1 算法原理概述

圖1為本文算法的原理圖。 首先在獲取傳感器數(shù)據(jù)階段對陀螺儀進行校準，本文將陀螺儀的漂移誤差認定為一個隨機變量，通過監(jiān)測陀螺儀在靜止或勻速狀態(tài)時的輸出實時更新陀螺儀的漂移值，并在以后的測量值中減去。加速度計，容易受到高頻噪聲的干擾，因此在這個階段還要對其進行低通濾波處理。EKF磁場校準單獨作為一個階段，使用陀螺儀輔助磁力計進行校準。在互補濾波中，本文分別對磁場信息和加速度信息定義信賴參數(shù)，并作用到PI控制器中，進一步減小由于實際運動環(huán)境引起的誤差。

圖1 算法原理圖Fig.1 Block diagram of the algorithm

2.2 EKF動態(tài)磁場校準

(1)

(2)

bg，k=(1-β)bg，k+βbg，k-1.

(3)

(4)

(5)

(6)

綜上易得：

(7)

式(7)表示，通過上一時刻的磁場準確值和陀螺儀準確值，可以先驗地預測出當前時刻的磁場準確值，這也是EKF中狀態(tài)預測的關鍵步驟。把磁場校準值、硬磁干擾矩陣、軟磁干擾矩陣組合為狀態(tài)量，就可以推導出全部的濾波方程。其中，由于測量方程是公式(4)，它對于狀態(tài)量來說是非線性的，所以要對其泰勒展開保留一階項，使用EKF完成校準。

2.3 互補濾波

本文的互補濾波系統(tǒng)框圖如圖2和圖3所示。

圖2 六軸融合原理圖Fig.2 Block diagram of six-axis fusion

圖3 九軸融合原理圖Fig.3 Block diagram of nine-axis fusion

圖2的六軸融合算法使用IMU，把加速度測量值和理論值做外積得到控制誤差，圖3的九軸融合算法加入了磁力計，把磁力計測量值和理論值的外積作為另一項控制誤差。關于互補濾波的原理，文獻[15-17]已經(jīng)做了詳細描述，此處不再贅述。圖3中，為了分別定義加速度計和磁力計的信賴參數(shù)，使用了兩個不同參數(shù)的PI控制器，這是對Euston[16]方法的改進。

3 信賴參數(shù)

3.1 加速度的信賴參數(shù)

無論是互補濾波還是卡爾曼濾波，都是把豎直向下的重力加速度作為一個絕對參考，所以非重力自由加速度是影響結果準確性的關鍵誤差來源。對于這個問題，Hytti修改了EKF的測量噪聲協(xié)方差矩陣，形式上把加速度計的測量作為了加速度計噪聲方差的權[18]。這樣，當加速度測量激增時，測量噪聲也會隨之激增，從而可以調整卡爾曼增益，達到自適應的目的，但可能面臨參數(shù)調節(jié)的問題。本文參考這樣的思路，在更簡單的互補濾波器中修改Pl控制器的比例環(huán)節(jié)，給其賦予自適應性。

首先考慮3種情況：

(1)靜止或勻速狀態(tài)，加速度計只有重力加速度作用，測量信息可以完全信賴。

(2)低加速度狀態(tài)，此時出現(xiàn)了非重力加速度的干擾，但影響并不大，應該使算法適當減少對加速度計測量的依賴。

(3)高加速狀態(tài)，這是最大的誤差來源，應該使對加速度計的依賴無限減小，當非重力加速度極大時甚至可以使其趨近于0。

對于這些情況，陳亮[14]給出的方法是對這3種狀態(tài)使用分段函數(shù)，對每一段進行實驗測試確定最佳參數(shù)。但是這種方法不僅需要分3段測試，還需要確定區(qū)分3種狀態(tài)的閾值，實際操作起來非常困難。本文直接使用一個指數(shù)函數(shù)來模擬上述過程。設加速度計的信賴參數(shù)為ξa，加速度向量為a，當?shù)氐闹亓铀俣却笮镚，k時刻加速度的大小與G的差為δk，即：

δk=abs(‖ak‖-G).

(8)

那么，信賴參數(shù)可以定義為：

ξa，k=exp(-λaδk)，

(9)

其中λa是一系數(shù)，如圖4所示，當λa=2時ξa的圖形。可以直觀地看出，指數(shù)函數(shù)的形狀符合對加速度計信賴程度動態(tài)調整的過程，加速度測量值越接近G，加速度越可信賴；測量值越偏離G，加速度的信賴參數(shù)越小，且當偏差過大時趨近于0。

圖4 信賴參數(shù)ξ隨δ變化曲線Fig.4 Curve of ξ varies with δ

設Kp，0為比例常數(shù)初始值，則修正后的比例環(huán)節(jié)為：

Kp=Kp，0ξ.

(10)

3.2 磁場向量的信賴參數(shù)

對于磁場校準的過程有兩點需要考慮，第一是算法開啟時的收斂過程，第二是校準完成后濾波器的抗突變能力。EKF的校準過程雖然不像橢球擬合一樣嚴格，但是也要求算法能足夠地適應磁場環(huán)境，那么收斂速度就會與使用狀態(tài)有關。在算法開啟時一般不直接使用EKF輸出的信息，而是監(jiān)測校準后的磁場強度，使用一個滑動窗口向后取固定長度的數(shù)據(jù)段，計算其均方根誤差(Root Mean Squared Error，RMSE)，當RMSE少于一個閾值并穩(wěn)定時，就認為磁場已校準完畢，可以開始使用磁場數(shù)據(jù)進行九軸融合。

(11)

4 實驗與結果

4.1 實驗設備

本文使用的采集設備是廣州阿路比電子科技有限公司的LPMS-B2，如圖5所示，它是一種微型無線慣性測量單元(IMU)/姿態(tài)航向參考系統(tǒng)(Attitude and Heading Reference System，AHRS)，表1為LPMS-B2與本文相關的技術參數(shù)。在本文的實驗中，傳感器的采樣率均為100 Hz。

圖5 LPMS-B2九軸模塊Fig.5 Photograph of LPMS-B2

表1 LPMS-B2技術參數(shù)

4.2 EKF磁場校準實驗

考察EKF校準與橢球擬合校準在軟磁、硬磁干擾下的校準效果。用來對比的橢球擬合算法是十分量橢球擬合算法，這種算法效果較好，但運算量較大。LPMS-B2模塊輸出的原始磁力計數(shù)據(jù)已經(jīng)包含了較大的硬磁干擾，還需要在模塊上綁一把鑰匙來作為軟磁干擾。將模塊在空中晃動約5 s，把EKF最終迭代得到的磁場向量作為真實值，則其模值即為理想磁場球體的半徑長度。把原始磁場數(shù)據(jù)、橢球擬合校準的磁場數(shù)據(jù)、EKF校準的磁場數(shù)據(jù)分別擬合成球體，如圖6所示，可以直觀地觀察3種數(shù)據(jù)的質量，擬合的球體越圓、球體半徑越接近真實地磁場強度、球體球心越接近原點，數(shù)據(jù)的質量越好。

圖6 存在軟磁干擾、硬磁干擾的磁場校準實驗Fig.6 Calibration experiment of magnetic field with soft and hard magnetic interference

從圖6可以直觀地得出結論，橢球擬合和EKF都能對硬磁干擾起到校準效果，但是橢球擬合對軟磁干擾的校準效果非常有限，而EKF可以較好地處理軟磁干擾。取圖6中每個橢球上的點與坐標原點的距離與理想球體的差，得到關于校準效果的誤差曲線，如圖7所示?？梢钥闯?，EKF和橢球擬合的誤差曲線與零線較近，這是因為兩種算法對硬磁干擾都起到了校準作用，而EKF誤差曲線更平滑，說明EKF對軟磁干擾的校準效果更好。

圖7 三個橢球與標準球體表面的距離曲線Fig.7 Curves of the distance between ellipsoids and standard sphere

(12)

另需要指出，橢球擬合算法對采樣有要求，在數(shù)據(jù)不夠時無法完成校準，文獻[19]對此進行了演示。因此，在實際應用的意義上橢球擬合的校準速度是不確定的，如果采樣滿足要求，則橢球擬合算法收斂部分的曲線斜率與EKF相仿。

圖8 磁場濾波值與真值隨時間變化曲線Fig.8 Time varying curves of filtered magnetic field value and true value

4.3 九軸融合姿態(tài)解算實驗

本節(jié)考察本文的融合算法與LPMS-B2的九軸融合算法在短時間內的計算結果有何差異。將模塊拿起在空中搖晃片刻，然后將記錄的傳感器原始數(shù)據(jù)輸入MATLAB計算，畫出MATLAB計算的姿態(tài)角和LPMS-B2輸出的姿態(tài)角，如圖9所示，尾標1表示本文算法，2表示LPMS-B2。

由圖9看出，在短時間內，兩種算法計算出的歐拉角并無太大差異。

圖9 本文的融合算法與LPMS-B2融合算法的姿態(tài)角結果

4.4 與LPMS-B2內置濾波器的航向角對比實驗

LPMS-B2內置的姿態(tài)角解算算法是卡爾曼濾波，包括六軸、九軸的卡爾曼濾波和磁場校準。本節(jié)考察本文算法的航向角漂移在相同的設備上是不是更小，具體實驗步驟為：

(1)將模塊按抵在桌面上的參考線邊緣，開啟模塊，與電腦藍牙連接，選擇一種濾波器(六軸或九軸卡爾曼濾波)和輸出的數(shù)據(jù)類型，點擊按鈕開始記錄。

(2)將模塊拿起隨意轉動5 s作為校準階段，然后放回抵住參考線，將此時作為t0時刻。

(3)滑出模塊，貼住桌面順時針旋轉3圈放回，停頓一段時間。

(4)連續(xù)重復步驟3，持續(xù)10 min左右。

(5)關閉記錄數(shù)據(jù)按鈕，結束實驗。

實驗場景如圖10所示。

圖10 實驗場景Fig.10 Scene of the experiment

4.4.1 與LPMS-B2的六軸算法作比較

選擇gyr+acc(Kalman)濾波器，記錄其歐拉角輸出和九軸傳感器的原始數(shù)據(jù)，得到的數(shù)據(jù)集為LPMSB2_3.csv，總時長615 s，約為10 min，共順時針旋轉了105圈。取數(shù)據(jù)集的第2 640～3 470行作為初始段，比較之后的?？窟^程與此段航向角的偏差，偏差曲線由圖11給出。為了便于讀者閱讀，圖11對縱軸做了截斷，只繪出了-10°～30°的部分。藍色線為本文九軸算法的結果偏差，可讀出每次停靠后航向角與初始的偏差都很?。患t色線為LPMS模塊的六軸計算結果偏差，偏差呈增大趨勢(彩圖見期刊電子版)。具體數(shù)值見表2。

圖11 本文九軸算法與LPMS-B2六軸算法的航向角偏差

表2 航向角偏差對比1

由表2，LPMS-B2模塊的六軸算法在連續(xù)的旋轉運動中也會出現(xiàn)比較大的誤差，偏差率為-1.8 (°)/min，而本九軸算法在此次實驗中的偏差率為-0.008 (°)/min，幾乎做到了不偏。

4.4.2 與LPMS-B2的九軸算法作比較

選擇gyr+acc+mag(Kalman)濾波器，記錄其歐拉角輸出和九軸傳感器的原始數(shù)據(jù)，得到數(shù)據(jù)集LPMSB2_5.csv，總時長670 s，約11 min，共順時針旋轉了117圈。取數(shù)據(jù)集的第3 500～4 500行作為初始段，比較之后的停靠過程與此段航向角的偏差，偏差曲線由圖12給出(彩圖見期刊電子版)，縱軸同樣做了截斷處理。藍色線為本文九軸算法的結果偏差，性質與上一個實驗相同；紅色線為LPMS模塊的九軸計算結果偏差，同樣呈增大趨勢。具體數(shù)值見表3。

圖12 本文九軸算法與LPMS-B2九軸算法的航向角偏差

由表3，在連續(xù)的旋轉運動中，LPMS-B2模塊的九軸算法航向角的偏差率為-1.39 (°)/min，與六軸的-1.8 (°)/min相比有所減小但不顯著；本文的九軸算法在此次實驗中的偏差率為-0.038 (°)/min，仍然遠小于模塊算法的偏差率。

表3 航向角偏差對比2

4.5 運算量分析

從磁場校準和姿態(tài)解算兩個部分分析算法運算量。對于橢球擬合算法，計算最復雜的部分的是矩陣求逆運算，矩陣求逆的算法復雜度是O(n3)，即復雜度隨矩陣階數(shù)n呈指數(shù)增長。十分量橢球擬合需要計算10階方陣的逆，朱建良[7]的橢球擬合算法需要計算6階方陣的逆，EKF磁場校準中需要求逆的矩陣只是3階的方陣，再加上其他的矩陣乘法運算需要的乘法數(shù)，與橢球擬合算法總體的乘法數(shù)相差不大。在姿態(tài)解算的方法中，互補濾波法除矢量外積外不涉及矩陣運算，比卡爾曼濾波有公認的計算速度和收斂速度上的優(yōu)勢[1]。

算法實際測試使用的MCU為主頻為180 MHz的STM32F427AG(1 024 K Flash，256 kB SRAM)，實際程序占用空間為90 kB(FLASH)，運行占用空間為25 kB(RAM)，每次迭代用時為16.4 ms，約每秒60幀運算。因此，根據(jù)4.2節(jié)對EKF磁場校準收斂速度的分析，算法在1 s內就可以完成校準，即時參與航向角的校正。

5 討 論

本文的EKF磁場校準，可以做到實時和動態(tài)，并且當磁場數(shù)據(jù)點不佳的情況下也能起到校準效果，與橢球擬合相比，還很好地補償了軟磁干擾。但是本文磁場校準的缺點是難以快速響應突變的磁場環(huán)境，這是因為穩(wěn)定狀態(tài)下濾波器的最佳增益矩陣不會隨著殘差的增大而增大。本文的處理方式是，當發(fā)生這種干擾過大的情況而導致校準后的磁場強度偏離正常值(55±10) mG太多時，就減小對磁場信息的信任程度，使其趨近于0，當磁場強度恢復到正常范圍內后再重新增加對磁場信息的信任程度，但在此過程中還是會累積航向角誤差。

本文的航向角輸出非常穩(wěn)定，在10 min的連續(xù)旋轉運動中，漂移僅為-0.08°和-0.42°，與商用的慣導模塊LPMS-B2相比幾乎是無偏的。但存在一個問題，由于EKF磁場校準的收斂速度相比于互補濾波器慢，所以在算法開啟的一小段時間內航向角并沒有迅速達到穩(wěn)定。所以，本文下一步的研究是如何加快EKF的收斂速度與跟蹤能力。

6 結 論

本文研究了兩個問題，第一是在使用MEMS傳感器解算姿態(tài)時，航向角隨時間偏移逐漸增大的問題，第二是傳統(tǒng)的橢球擬合磁場校準方法缺乏校準的實時性和便利性，以及難以應對軟磁干擾的問題。第二個問題的解決是第一個問題解決的關鍵。因此，本文使用了一種基于EKF的動態(tài)磁場校準方法，通過旋轉矩陣建立陀螺儀與磁力計的關系，把測量方程的元素同樣作為狀態(tài)量進行預測更新，實現(xiàn)了兼具實時性和便利性的動態(tài)磁場校準。實驗結果表明，本文的EKF磁場校準方法能有效改善橢球擬合算法對校準數(shù)據(jù)要求高的缺點，并且能很好地消除軟磁干擾的影響。在這個基礎上，本文在互補濾波中著重考慮了自由加速度和磁場環(huán)境突變的情況，定義了各自的信賴函數(shù)，對PI控制器做了修改，使其對加速度和磁場的突變具有自適應能力，基本消除了自由加速度和畸變磁場對姿態(tài)角解算的影響。實驗結果表明，當傳感器采樣率為100 Hz、運行時長約為11 min、旋轉圈數(shù)約為117圈時，本文算法航向角的漂移為0.42°，與對比的商用慣導模塊算法相比減小了14.9°，性能實現(xiàn)了數(shù)量級的提升。

本文提出的方法在控制減小航向角漂移上有較大的優(yōu)勢，并且同時滿足校準便捷、適用場景多樣等要求，在低成本MEMS慣性導航領域有廣闊的應用前景和極大的研究潛力。如何加強抗強磁場干擾的能力，以及如何改進濾波器、加快恢復收斂的速度是下一步研究的重點。