Attitude Estimation Algorithm for Low Cost MEMSBased on Quaternion EKF*

JIARuicai

(1.The54th Research Institute of CETC，Shijiazhuang 050081，China; 2.Satellite Navigation Technology and Equipment Engineering Technology Research Center of Hebei province，Shijiazhuang 050081，China)

Attitude Estimation Algorithm for Low Cost MEMSBased on Quaternion EKF*

JIARuicai1，2*

(1.The54th Research Institute of CETC，Shijiazhuang 050081，China; 2.Satellite Navigation Technology and Equipment Engineering Technology Research Center of Hebei province，Shijiazhuang 050081，China)

Since low－costMEMShas the feature of low precision and high noise，and these error accumulated over time quickly，it can’tmeet requirements of long time attitudemeasurement.For this，an attitude algorithm based on quaternion extended Kalman filter(EKF)is designed.The algorithm treats quaternion as EKF filter state，and update state with measured angular rate.Then，update quaternion state by gravitational and geomagnetic data for inhibiting the error divergence.In order to test the feasibility of proposed algorithm，the precision and long time work testwere carried out based on turntable.The results showed that pitch and roll angle accuracy were better than 0.1°，and heading angle accuracy was better than 0.2°.The standard deviation of attitude errorwas below 0.07 in two hours test，with no error divergence.The results verify the feasibility of proposed algorithm for long time use fully.

quaternion;low－cost MEMS;attitude estimation;EKF

低成本MEMS慣性器件精度差噪聲高［1－2］，無法滿足載體位置、速度導航要求，而常常用于載體姿態測量［3－5］。其具有體積小、成本低、重量輕，便于集成、攜帶等優點［6］，既可應用于小型航空飛行器［7］，車輛自主駕駛［8］，人體關節姿態測量等領域［9］，又可應用于軍用領域，如低成本制導彈藥、小型無人機、雷達系統［10］等，具有廣闊的應用前景。

MEMS慣性器件誤差隨時間不斷累加，短時間內姿態精度迅速惡化［11－12］，甚至無法應用，針對該問題，將地球重力場與地磁場數據應用到四元數EKF姿態算法中，應用相對穩定的重力與磁場強度抑制MEMS姿態隨時間不斷惡化問題。本文在推導四元數EKF濾波器實現角速率、加速度，磁場強度數據融合算法的基礎上，通過轉臺精度實驗與靜態試驗驗證算法有效性。

1 四元數EKF姿態估計算法

圖1給出了四元數EKF姿態估計的組成，主要包含了三軸MEMS陀螺、三軸加速度計、三軸磁強計以及四元數EKF濾波器算法部分。圖中θ、γ、ψ分別表示俯仰角、橫滾角、航向角。

圖1 姿態估計算法框圖

本文推導的四元數EKF姿態估計濾波器算法包含了四部分，詳細推導如下。

1.1 確定濾波器狀態與觀測量

通常情況下，將四元數誤差與陀螺誤差作為EKF濾波器狀態量。

若qnb=［q0q1q2q3］T為姿態四元數，Δx =［ΔxgxΔxgyΔxgz］T為陀螺輸出誤差。則EKF濾波器7維狀態變量^x可表示為

將加速度計、磁強測量值與當地重力場、磁場強度在載體坐標系下投影的差值作為6維觀測量。

載體系中，加速度計測量值ya可表示為:

導航系中重力場為:G=［0 0－g］T，載體系可表示為為導航系到載體系坐標轉換矩陣。

載體系中，磁強計量測值為ym。地理系中磁場強度為M=［0 mnmu］T，載體系可表示為M。

綜合以上分析，EKF觀測量Z:

1.2 初始化

應用加速度計、磁強計測量姿態、航向初始化EKF狀態。

平穩狀態下采集加速度計數據ya，磁強計數據ym，并計算加速度、磁強平均值。

根據式(1)～式(2)分別計算平穩狀態下俯仰角θ、橫滾角角γ:

根據式(3)計算航向角ψ:

Mag_y=mx cosγ+mz sinγ

由姿態、航向角，計算姿態矩陣:

由姿態矩陣計算四元數:

由式(4)四元數表達式可知，它是歐拉角的函數，首先根據式(6)計算歐拉角到四元數的雅克比矩陣，然后根據式(5)計算出協方差初值P(0)，其維數為7×7。

式中，各參數定義如下:

1.3 狀態更新

由于低成本MEMS器件的低精度與高噪聲特性，其狀態方程存在較強的非線性，根據陀螺輸出建立的狀態更新公式如下:

將式(7)上式分別對7維狀態向量求偏導，得到7×7維矩陣J(f，X):

將式(7)分別對陀螺過程噪聲求偏導，得到7× 6維矩陣J(f，U):

1.4 量測更新

將加速度計、磁強計測量值與各自的真值(即當地重力場與磁力場)的差作為觀測量，對狀態、狀態協方差量進行量測更新。量測更新過程模型如下:

Step 1:量測增益Kk計算公式如下:

式中，Rk為量測方差矩陣;Hk為量測方程;P－k+1為狀態更新后的狀態方差;量測矩陣Hk及其分量定義如下:

mn、md分別為北向、地向磁場強度;g為重力加速度;Cbn為導航系到載體系轉換矩陣。將ha與hm分別對狀態量求偏導數，結果如下:

Step 2:狀態量測更新過程如下式:

Step 3:狀態方差更新方程:

2 實驗結果與分析

2.1 轉臺精度實驗

為驗證四元數EKF姿態估計算法精度，將實驗板卡安裝至雙軸轉臺開展實驗。實驗板卡集成了L3G4200型三軸陀螺，ADXL345型三軸加速度計，HMC5883型三軸磁強計，實驗板卡具有低成本特點，數據采樣率均為100 Hz。實驗安排如下:

(1)俯仰角實驗:實驗板卡XYZ軸分別表示右前上，實驗板卡Y軸與轉臺俯仰軸安裝至同一直線上，俯仰軸分別位于+90°、0°、－90°、0°4個位置，俯仰角范圍±90°，共計實驗時間長度534 s，實驗結果見圖3。

圖2 轉臺精度實驗照片

圖3 俯仰角

(2)橫滾角實驗:在俯仰角實驗基礎上，轉臺主軸旋轉90°，此時X軸與轉臺俯仰軸同一直線，俯仰軸分別位于0°、90°、0°、－90°4個位置，俯仰角范圍±90°，共計實驗時間521 s，實驗結果見圖4。

圖4 橫滾角

(3)航向角實驗:在橫滾角實驗基礎上，轉臺俯仰軸旋轉至0°位置，轉臺主軸分別處于0°、90°、－180°、－90°、0°5個位置，航向角范圍±180°，實驗結果見圖5。

圖5航向角

圖6 給出了姿態與航向角誤差收斂曲線，按照誤差計算公式計算的姿態與航向誤差統計結果如表1所示。誤差計算公式:

式中:Xˉ為均值，σ為方差。由實驗結果可知，本文提出的四元數EKF姿態估計算法具備了較高的測量精度以及較快的誤差收斂速度。

圖6 姿態與航向角誤差

表1 姿態與航向角誤差統計

2.2 靜態實驗

為驗證四元數EKF姿態估計算法長時間工作能力，將實驗板卡水平放置轉臺上，運行時間超過2 h，姿態與航向結果如圖7～圖9所示。每幅圖分為上下子圖，上子圖為前100 s誤差角收斂情況，下子圖為100 s～7 200 s誤差角。

圖7 俯仰角誤差

圖8 橫滾角誤差

圖9 航向角誤差

由實驗結果可直觀地看出，2 h運行期間，姿態與航向誤差始終處于可穩定的范圍內，無發散，具備長時間工作的能力。

3 總結

針對低精度高噪聲MEMS器件在長時間姿態測量應用中存在的問題，本文給出了基于四元數EKF的姿態估計算法，詳細推導了算法應用流程，并開展了轉臺精度實驗與2 h靜態實驗，實驗結果充分驗證了本文算法的可行性。

應用算法時應注意:選擇較寬闊應用環境，避免應用在復雜磁場環境中(如，鐵質物體較多)，應用前應消除硬磁與軟磁干擾。

［1］Markely F L.Attitude Error Representations for Kalman Filtering［J］.Journal of Guidance，Control and Dynamics，2003，63(2): 311－317.

［2］喬相偉，周衛東，吉宇人.用四元數狀態切換無跡卡爾曼濾波器估計飛行器姿態［J］.控制理論與應用，2012，29(1):97－103.

［3］Psiaki ML.The Super－Iterated Extended Kalman Filter［C］// AIAA Guidance，Navigation，and Control Conference，2004(8): AIAA－04－5418.

［4］Dah J J，Ta S，Critical Remarks on the Linearised and Extended Kalman Filters with Geodetic Navigation Examples［J］.Measurement，2010.43(9):1077－1089.

［5］Groves P.Principles of GNSS，Inertial，and Multisensor Integrated Navigation Systems［M］.Artech House Publishers，2008.

［6］Klein I，Filin S，Toledo T.Pseudo－Measurements as Aiding to INS during GPSOutages［J］.Navigation，2010，57(1):25－34.

［7］Shuster MD.Kalman Filtering of Spacecraft Attitude and the QUEST Model［J］.Journal of Guidance，Control，and Dynamics，1990，13(3): 506－514.

［8］何昆鵬.MEMS慣性器件參數辨識及系統誤差補償技術［D］.哈爾濱:哈爾濱工程大學，2009.

［9］程龍，王壽榮，葉甫.硅微機械震動陀螺零偏溫度補償研究［J］.傳感器技術學報，2008，21(3):483－485.

［10］Pahadia，A.GPS/INS Integration Aided with Gyroscope－Free IMU for Pedestrian Applications［D］Departmentof Geomatics Engineering，University of Calgary，Canada，2010.

［11］吳峰，王向軍，湯其劍.基于數字調節方法的MEMS陀螺零位補償技術研究［J］.傳感器技術學報，2012，25(12):1717－1721.

［12］Egziabher D G.Design ofMulti－Sensor Attitude Determination Systems［J］.IEEE Transactionson Aerospace and Electronic Systems，2004，40(2):627－649.

賈瑞才(1986－)，男，山東臨沂人，2012年獲得哈爾濱工程大學博士學位，現為中國電子科技集團公司第五十四研究所博士后工作人員，主要從事組合導航、慣性導航、衛星導航技術研究。

基于四元數EKF的低成本MEMS姿態估計算法*

賈瑞才1，2*
(1.中國電子科技集團公司第五十四研究所，石家莊050081;2.河北省衛星導航技術與裝備工程技術研發中心，石家莊050081)

低成本MEMS器件具有低精度高噪聲特點，其誤差隨時間不斷累加，無法滿足長時間載體姿態測量要求，針對此問題，設計了基于四元數擴展卡爾曼濾波器EKF(Extended Kalman Filter)的姿態估計算法。該算法將姿態四元數作為EKF濾波器狀態，應用測量的角速率完成濾波器時間更新;應用載體所處位置的重力場與地磁場數據，完成濾波器量測更新，抑制了姿態誤差發散，解決了長時間姿態測量問題。為檢驗算法可行性，分別開展了轉臺精度實驗與靜態實驗，實驗結果表明:俯仰、橫滾角精度優于0.1°，航向角精度優于0.2°，2 h內靜態試驗期間姿態角誤差標準差為0.07°，無發散問題，具備長時間姿態測量要求。

四元數;低成本MEMS;姿態估計;EKF

V241.5

A

1004－1699(2014)01－0090－06

2013－09－26修改日期:2013－12－10

C:2575

10.3969/j.issn.1004－1699.2014.01.017

項目來源:國家高技術發展計劃(863計劃)項目(2012AA12A206);中國博士后基金項目(2013M541202)