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

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

1 引 言

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

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

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

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

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

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

2 算法原理概述

2.1 算法原理概述

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

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

2.2 EKF動(dòng)態(tài)磁場(chǎng)校準(zhǔn)

(1)

(2)

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

(3)

(4)

(5)

(6)

綜上易得：

(7)

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

2.3 互補(bǔ)濾波

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

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

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

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

3 信賴參數(shù)

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

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

首先考慮3種情況：

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

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

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

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

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

(8)

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

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

(9)

其中λa是一系數(shù)，如圖4所示，當(dāng)λa=2時(shí)ξa的圖形?？梢灾庇^地看出，指數(shù)函數(shù)的形狀符合對(duì)加速度計(jì)信賴程度動(dòng)態(tài)調(diào)整的過程，加速度測(cè)量值越接近G，加速度越可信賴；測(cè)量值越偏離G，加速度的信賴參數(shù)越小，且當(dāng)偏差過大時(shí)趨近于0。

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

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

Kp=Kp，0ξ.

(10)

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

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

(11)

4 實(shí)驗(yàn)與結(jié)果

4.1 實(shí)驗(yàn)設(shè)備

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

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

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

4.2 EKF磁場(chǎng)校準(zhǔn)實(shí)驗(yàn)

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

圖6 存在軟磁干擾、硬磁干擾的磁場(chǎng)校準(zhǔn)實(shí)驗(yàn)Fig.6 Calibration experiment of magnetic field with soft and hard magnetic interference

從圖6可以直觀地得出結(jié)論，橢球擬合和EKF都能對(duì)硬磁干擾起到校準(zhǔn)效果，但是橢球擬合對(duì)軟磁干擾的校準(zhǔn)效果非常有限，而EKF可以較好地處理軟磁干擾。取圖6中每個(gè)橢球上的點(diǎn)與坐標(biāo)原點(diǎn)的距離與理想球體的差，得到關(guān)于校準(zhǔn)效果的誤差曲線，如圖7所示?？梢钥闯?，EKF和橢球擬合的誤差曲線與零線較近，這是因?yàn)閮煞N算法對(duì)硬磁干擾都起到了校準(zhǔn)作用，而EKF誤差曲線更平滑，說明EKF對(duì)軟磁干擾的校準(zhǔn)效果更好。

圖7 三個(gè)橢球與標(biāo)準(zhǔn)球體表面的距離曲線Fig.7 Curves of the distance between ellipsoids and standard sphere

(12)

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

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

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

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

由圖9看出，在短時(shí)間內(nèi)，兩種算法計(jì)算出的歐拉角并無太大差異。

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

4.4 與LPMS-B2內(nèi)置濾波器的航向角對(duì)比實(shí)驗(yàn)

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

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

(2)將模塊拿起隨意轉(zhuǎn)動(dòng)5 s作為校準(zhǔn)階段，然后放回抵住參考線，將此時(shí)作為t0時(shí)刻。

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

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

(5)關(guān)閉記錄數(shù)據(jù)按鈕，結(jié)束實(shí)驗(yàn)。

實(shí)驗(yàn)場(chǎng)景如圖10所示。

圖10 實(shí)驗(yàn)場(chǎng)景Fig.10 Scene of the experiment

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

選擇gyr+acc(Kalman)濾波器，記錄其歐拉角輸出和九軸傳感器的原始數(shù)據(jù)，得到的數(shù)據(jù)集為L(zhǎng)PMSB2_3.csv，總時(shí)長(zhǎng)615 s，約為10 min，共順時(shí)針旋轉(zhuǎn)了105圈。取數(shù)據(jù)集的第2 640～3 470行作為初始段，比較之后的停靠過程與此段航向角的偏差，偏差曲線由圖11給出。為了便于讀者閱讀，圖11對(duì)縱軸做了截?cái)啵焕L出了-10°～30°的部分。藍(lán)色線為本文九軸算法的結(jié)果偏差，可讀出每次?？亢蠛较蚪桥c初始的偏差都很??；紅色線為L(zhǎng)PMS模塊的六軸計(jì)算結(jié)果偏差，偏差呈增大趨勢(shì)(彩圖見期刊電子版)。具體數(shù)值見表2。

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

表2 航向角偏差對(duì)比1

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

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

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

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

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

表3 航向角偏差對(duì)比2

4.5 運(yùn)算量分析

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

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

5 討 論

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

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

6 結(jié) 論

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

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