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］，無(wú)法滿(mǎn)足載體位置、速度導(dǎo)航要求，而常常用于載體姿態(tài)測(cè)量［3－5］。其具有體積小、成本低、重量輕，便于集成、攜帶等優(yōu)點(diǎn)［6］，既可應(yīng)用于小型航空飛行器［7］，車(chē)輛自主駕駛［8］，人體關(guān)節(jié)姿態(tài)測(cè)量等領(lǐng)域［9］，又可應(yīng)用于軍用領(lǐng)域，如低成本制導(dǎo)彈藥、小型無(wú)人機(jī)、雷達(dá)系統(tǒng)［10］等，具有廣闊的應(yīng)用前景。

MEMS慣性器件誤差隨時(shí)間不斷累加，短時(shí)間內(nèi)姿態(tài)精度迅速惡化［11－12］，甚至無(wú)法應(yīng)用，針對(duì)該問(wèn)題，將地球重力場(chǎng)與地磁場(chǎng)數(shù)據(jù)應(yīng)用到四元數(shù)EKF姿態(tài)算法中，應(yīng)用相對(duì)穩(wěn)定的重力與磁場(chǎng)強(qiáng)度抑制MEMS姿態(tài)隨時(shí)間不斷惡化問(wèn)題。本文在推導(dǎo)四元數(shù)EKF濾波器實(shí)現(xiàn)角速率、加速度，磁場(chǎng)強(qiáng)度數(shù)據(jù)融合算法的基礎(chǔ)上，通過(guò)轉(zhuǎn)臺(tái)精度實(shí)驗(yàn)與靜態(tài)試驗(yàn)驗(yàn)證算法有效性。

1 四元數(shù)EKF姿態(tài)估計(jì)算法

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

圖1 姿態(tài)估計(jì)算法框圖

本文推導(dǎo)的四元數(shù)EKF姿態(tài)估計(jì)濾波器算法包含了四部分，詳細(xì)推導(dǎo)如下。

1.1 確定濾波器狀態(tài)與觀(guān)測(cè)量

通常情況下，將四元數(shù)誤差與陀螺誤差作為EKF濾波器狀態(tài)量。

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

將加速度計(jì)、磁強(qiáng)測(cè)量值與當(dāng)?shù)刂亓?chǎng)、磁場(chǎng)強(qiáng)度在載體坐標(biāo)系下投影的差值作為6維觀(guān)測(cè)量。

載體系中，加速度計(jì)測(cè)量值ya可表示為:

導(dǎo)航系中重力場(chǎng)為:G=［0 0－g］T，載體系可表示為為導(dǎo)航系到載體系坐標(biāo)轉(zhuǎn)換矩陣。

載體系中，磁強(qiáng)計(jì)量測(cè)值為ym。地理系中磁場(chǎng)強(qiáng)度為M=［0 mnmu］T，載體系可表示為M。

綜合以上分析，EKF觀(guān)測(cè)量Z:

1.2 初始化

應(yīng)用加速度計(jì)、磁強(qiáng)計(jì)測(cè)量姿態(tài)、航向初始化EKF狀態(tài)。

平穩(wěn)狀態(tài)下采集加速度計(jì)數(shù)據(jù)ya，磁強(qiáng)計(jì)數(shù)據(jù)ym，并計(jì)算加速度、磁強(qiáng)平均值。

根據(jù)式(1)～式(2)分別計(jì)算平穩(wěn)狀態(tài)下俯仰角θ、橫滾角角γ:

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

Mag_y=mx cosγ+mz sinγ

由姿態(tài)、航向角，計(jì)算姿態(tài)矩陣:

由姿態(tài)矩陣計(jì)算四元數(shù):

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

式中，各參數(shù)定義如下:

1.3 狀態(tài)更新

由于低成本MEMS器件的低精度與高噪聲特性，其狀態(tài)方程存在較強(qiáng)的非線(xiàn)性，根據(jù)陀螺輸出建立的狀態(tài)更新公式如下:

將式(7)上式分別對(duì)7維狀態(tài)向量求偏導(dǎo)，得到7×7維矩陣J(f，X):

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

1.4 量測(cè)更新

將加速度計(jì)、磁強(qiáng)計(jì)測(cè)量值與各自的真值(即當(dāng)?shù)刂亓?chǎng)與磁力場(chǎng))的差作為觀(guān)測(cè)量，對(duì)狀態(tài)、狀態(tài)協(xié)方差量進(jìn)行量測(cè)更新。量測(cè)更新過(guò)程模型如下:

Step 1:量測(cè)增益Kk計(jì)算公式如下:

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

mn、md分別為北向、地向磁場(chǎng)強(qiáng)度;g為重力加速度;Cbn為導(dǎo)航系到載體系轉(zhuǎn)換矩陣。將ha與hm分別對(duì)狀態(tài)量求偏導(dǎo)數(shù)，結(jié)果如下:

Step 2:狀態(tài)量測(cè)更新過(guò)程如下式:

Step 3:狀態(tài)方差更新方程:

2 實(shí)驗(yàn)結(jié)果與分析

2.1 轉(zhuǎn)臺(tái)精度實(shí)驗(yàn)

為驗(yàn)證四元數(shù)EKF姿態(tài)估計(jì)算法精度，將實(shí)驗(yàn)板卡安裝至雙軸轉(zhuǎn)臺(tái)開(kāi)展實(shí)驗(yàn)。實(shí)驗(yàn)板卡集成了L3G4200型三軸陀螺，ADXL345型三軸加速度計(jì)，HMC5883型三軸磁強(qiáng)計(jì)，實(shí)驗(yàn)板卡具有低成本特點(diǎn)，數(shù)據(jù)采樣率均為100 Hz。實(shí)驗(yàn)安排如下:

(1)俯仰角實(shí)驗(yàn):實(shí)驗(yàn)板卡XYZ軸分別表示右前上，實(shí)驗(yàn)板卡Y軸與轉(zhuǎn)臺(tái)俯仰軸安裝至同一直線(xiàn)上，俯仰軸分別位于+90°、0°、－90°、0°4個(gè)位置，俯仰角范圍±90°，共計(jì)實(shí)驗(yàn)時(shí)間長(zhǎng)度534 s，實(shí)驗(yàn)結(jié)果見(jiàn)圖3。

圖2 轉(zhuǎn)臺(tái)精度實(shí)驗(yàn)照片

圖3 俯仰角

(2)橫滾角實(shí)驗(yàn):在俯仰角實(shí)驗(yàn)基礎(chǔ)上，轉(zhuǎn)臺(tái)主軸旋轉(zhuǎn)90°，此時(shí)X軸與轉(zhuǎn)臺(tái)俯仰軸同一直線(xiàn)，俯仰軸分別位于0°、90°、0°、－90°4個(gè)位置，俯仰角范圍±90°，共計(jì)實(shí)驗(yàn)時(shí)間521 s，實(shí)驗(yàn)結(jié)果見(jiàn)圖4。

圖4 橫滾角

(3)航向角實(shí)驗(yàn):在橫滾角實(shí)驗(yàn)基礎(chǔ)上，轉(zhuǎn)臺(tái)俯仰軸旋轉(zhuǎn)至0°位置，轉(zhuǎn)臺(tái)主軸分別處于0°、90°、－180°、－90°、0°5個(gè)位置，航向角范圍±180°，實(shí)驗(yàn)結(jié)果見(jiàn)圖5。

圖5航向角

圖6 給出了姿態(tài)與航向角誤差收斂曲線(xiàn)，按照誤差計(jì)算公式計(jì)算的姿態(tài)與航向誤差統(tǒng)計(jì)結(jié)果如表1所示。誤差計(jì)算公式:

式中:Xˉ為均值，σ為方差。由實(shí)驗(yàn)結(jié)果可知，本文提出的四元數(shù)EKF姿態(tài)估計(jì)算法具備了較高的測(cè)量精度以及較快的誤差收斂速度。

圖6 姿態(tài)與航向角誤差

表1 姿態(tài)與航向角誤差統(tǒng)計(jì)

2.2 靜態(tài)實(shí)驗(yàn)

為驗(yàn)證四元數(shù)EKF姿態(tài)估計(jì)算法長(zhǎng)時(shí)間工作能力，將實(shí)驗(yàn)板卡水平放置轉(zhuǎn)臺(tái)上，運(yùn)行時(shí)間超過(guò)2 h，姿態(tài)與航向結(jié)果如圖7～圖9所示。每幅圖分為上下子圖，上子圖為前100 s誤差角收斂情況，下子圖為100 s～7 200 s誤差角。

圖7 俯仰角誤差

圖8 橫滾角誤差

圖9 航向角誤差

由實(shí)驗(yàn)結(jié)果可直觀(guān)地看出，2 h運(yùn)行期間，姿態(tài)與航向誤差始終處于可穩(wěn)定的范圍內(nèi)，無(wú)發(fā)散，具備長(zhǎng)時(shí)間工作的能力。

3 總結(jié)

針對(duì)低精度高噪聲MEMS器件在長(zhǎng)時(shí)間姿態(tài)測(cè)量應(yīng)用中存在的問(wèn)題，本文給出了基于四元數(shù)EKF的姿態(tài)估計(jì)算法，詳細(xì)推導(dǎo)了算法應(yīng)用流程，并開(kāi)展了轉(zhuǎn)臺(tái)精度實(shí)驗(yàn)與2 h靜態(tài)實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果充分驗(yàn)證了本文算法的可行性。

應(yīng)用算法時(shí)應(yīng)注意:選擇較寬闊應(yīng)用環(huán)境，避免應(yīng)用在復(fù)雜磁場(chǎng)環(huán)境中(如，鐵質(zhì)物體較多)，應(yīng)用前應(yīng)消除硬磁與軟磁干擾。

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

［2］喬相偉，周衛(wèi)東，吉宇人.用四元數(shù)狀態(tài)切換無(wú)跡卡爾曼濾波器估計(jì)飛行器姿態(tài)［J］.控制理論與應(yīng)用，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，F(xiàn)ilin 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慣性器件參數(shù)辨識(shí)及系統(tǒng)誤差補(bǔ)償技術(shù)［D］.哈爾濱:哈爾濱工程大學(xué)，2009.

［9］程龍，王壽榮，葉甫.硅微機(jī)械震動(dòng)陀螺零偏溫度補(bǔ)償研究［J］.傳感器技術(shù)學(xué)報(bào)，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］吳峰，王向軍，湯其劍.基于數(shù)字調(diào)節(jié)方法的MEMS陀螺零位補(bǔ)償技術(shù)研究［J］.傳感器技術(shù)學(xué)報(bào)，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年獲得哈爾濱工程大學(xué)博士學(xué)位，現(xiàn)為中國(guó)電子科技集團(tuán)公司第五十四研究所博士后工作人員，主要從事組合導(dǎo)航、慣性導(dǎo)航、衛(wèi)星導(dǎo)航技術(shù)研究。

基于四元數(shù)EKF的低成本MEMS姿態(tài)估計(jì)算法*

賈瑞才1，2*
(1.中國(guó)電子科技集團(tuán)公司第五十四研究所，石家莊050081;2.河北省衛(wèi)星導(dǎo)航技術(shù)與裝備工程技術(shù)研發(fā)中心，石家莊050081)

低成本MEMS器件具有低精度高噪聲特點(diǎn)，其誤差隨時(shí)間不斷累加，無(wú)法滿(mǎn)足長(zhǎng)時(shí)間載體姿態(tài)測(cè)量要求，針對(duì)此問(wèn)題，設(shè)計(jì)了基于四元數(shù)擴(kuò)展卡爾曼濾波器EKF(Extended Kalman Filter)的姿態(tài)估計(jì)算法。該算法將姿態(tài)四元數(shù)作為EKF濾波器狀態(tài)，應(yīng)用測(cè)量的角速率完成濾波器時(shí)間更新;應(yīng)用載體所處位置的重力場(chǎng)與地磁場(chǎng)數(shù)據(jù)，完成濾波器量測(cè)更新，抑制了姿態(tài)誤差發(fā)散，解決了長(zhǎng)時(shí)間姿態(tài)測(cè)量問(wèn)題。為檢驗(yàn)算法可行性，分別開(kāi)展了轉(zhuǎn)臺(tái)精度實(shí)驗(yàn)與靜態(tài)實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明:俯仰、橫滾角精度優(yōu)于0.1°，航向角精度優(yōu)于0.2°，2 h內(nèi)靜態(tài)試驗(yàn)期間姿態(tài)角誤差標(biāo)準(zhǔn)差為0.07°，無(wú)發(fā)散問(wèn)題，具備長(zhǎng)時(shí)間姿態(tài)測(cè)量要求。

四元數(shù);低成本MEMS;姿態(tài)估計(jì);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

項(xiàng)目來(lái)源:國(guó)家高技術(shù)發(fā)展計(jì)劃(863計(jì)劃)項(xiàng)目(2012AA12A206);中國(guó)博士后基金項(xiàng)目(2013M541202)