微機電系統(tǒng)姿態(tài)估計系統(tǒng)算法與仿真

魏振楠， 伊國興， 徐澤遠

(哈爾濱工業(yè)大學 航天學院，黑龍江 哈爾濱 150001)

微機電系統(tǒng)姿態(tài)估計系統(tǒng)算法與仿真

魏振楠， 伊國興， 徐澤遠

(哈爾濱工業(yè)大學 航天學院，黑龍江 哈爾濱 150001)

結合陀螺儀、加速度計誤差模型，實現(xiàn)了以微機電系統(tǒng)(MEMS)陀螺儀與MEMS加速度計為基礎的姿態(tài)估計硬件仿真系統(tǒng)，可用于模擬任意噪聲強度和安裝偏差下三軸捷聯(lián)慣導系統(tǒng)(INS)，即按照給定運動曲線仿真輸出陀螺儀與加速度計數(shù)據(jù)，為設計姿態(tài)估計算法提供仿真驗證平臺。同時，以姿態(tài)四元數(shù)為狀態(tài)變量，載體俯仰角與橫滾角為觀測值設計了基于擴展卡爾曼濾波器(EKF)的姿態(tài)估計算法，俯仰角估計誤差小于0.04°，橫滾角估計誤差小于0.05°，偏航角漂移速度0.01(°)/s。

姿態(tài)估計； 微機電系統(tǒng)(MEMS)慣導系統(tǒng)； 誤差模型； 卡爾曼濾波

0 引 言

微機電系統(tǒng)(MEMS)姿態(tài)估計系統(tǒng)是指以微機械陀螺儀和加速度計等低成本慣性器件為敏感元件，利用捷聯(lián)慣導算法并結合適當?shù)臄?shù)據(jù)融合手段，估計并輸出載體三軸姿態(tài)信息的導航系統(tǒng)。由于微機械慣性器件成本低、體積小，由其構成的姿態(tài)估計系統(tǒng)在機器人姿態(tài)控制[1]、飛行器控制[2～4]、海洋衛(wèi)星跟蹤天線[5]、姿態(tài)測量[6]等領域越來越多的領域得到了應用。但微機械慣性器件噪聲大、溫漂劇烈等[7]問題也嚴重影響了姿態(tài)估計系統(tǒng)的精度，因而，在實現(xiàn)微機械導航系統(tǒng)時，設計穩(wěn)定且快速收斂的濾波算法尤為重要。

本文以微機械陀螺儀與微機械加速度計輸出模型為基礎，實現(xiàn)了根據(jù)給定的運動曲線，設定參數(shù)輸出陀螺儀與加速度計數(shù)據(jù)的微機械姿態(tài)估計硬件仿真系統(tǒng)，該系統(tǒng)為設計和驗證導航算法提供仿真平臺。同時，也提供了利用擴展卡爾曼濾波算法實現(xiàn)的姿態(tài)估計方法，用于驗證仿真系統(tǒng)性能。

1 微機械慣性導航系統(tǒng)模型建立

本文以捷聯(lián)形式建立微機械慣性導航系統(tǒng)(INS)仿真模型。

1.1 系統(tǒng)溫度模型

系統(tǒng)溫度模型包含理論溫度變化模型和溫度采樣模型。理論溫度變化模型描述系統(tǒng)核心器件溫度隨時間變化的規(guī)律，該模型利用數(shù)據(jù)表形式實現(xiàn)，給定系統(tǒng)特定時刻的溫度，任意時刻的溫度值利用牛頓插值獲得。溫度采樣模型描述了包含采樣噪聲與溫度傳感器參數(shù)漂移所引起誤差的系統(tǒng)核心器件溫度測量值隨時間變化的規(guī)律，設tk時刻系統(tǒng)理論溫度值為T(tk)，參數(shù)漂移導致的隨機游走誤差為Twn(tk)，采樣噪聲誤差為Trn(tk)，則系統(tǒng)溫度采樣模型輸出為

Ts(tk)=T(tk)+Twn(tk)+Trn(tk)

(1)

1.2 系統(tǒng)運動模型

系統(tǒng)運動模型包含角運動模型與線運動模型。角運動模型描述了姿態(tài)角、姿態(tài)角速度以及姿態(tài)角加速度隨時間變化的規(guī)律。線運動模型描述了載體位移、速度及加速度隨時間變化的規(guī)律。系統(tǒng)運動模型利用數(shù)據(jù)表實現(xiàn)，根據(jù)需求產生指定采樣周期的離散角運動與線運動軌跡后，任意時刻運動參數(shù)通過牛頓插值獲得。

1.3 微機械陀螺儀系統(tǒng)模型

捷聯(lián)慣導系統(tǒng)中，陀螺儀實際輸出與載體運動角速度不完全一致，其受陀螺安裝偏差、漂移、刻度系數(shù)誤差等因素影響。

陀螺安裝偏差指陀螺儀敏感軸與相應載體坐標軸不重合，使得該方向陀螺儀不僅僅敏感自身方向角速度，還受其它軸角速度的耦合影響，由陀螺安裝偏差導致的角速度測量誤差可通過對陀螺儀安裝角進行標定補償。設i方向陀螺與j方向載體坐標軸夾角為ψij，則捷聯(lián)慣導系統(tǒng)中三軸陀螺儀安裝矩陣可表示為

(2)

陀螺儀漂移及刻度系數(shù)誤差為隨機誤差，刻度系數(shù)誤差Sg一般使用隨機常數(shù)描述，漂移則可分為逐次啟動漂移(常值零偏)、慢變漂移(隨機游走)、及快變漂移(白噪聲)等[8]，則捷聯(lián)慣導系統(tǒng)中三軸陀螺儀輸出模型可表示為

ωgo=diag(1+sgx,1+sgy,1+sgz)Mgωgv+ωgb+ωgr+ωgn

(3)

式中ωgo為陀螺儀輸出;sgi為i軸方向陀螺儀標度系數(shù)誤差;ωgv為載體角速度輸入;ωgb為陀螺儀逐次啟動漂移;ωgr為慢變漂移，ωgn為快變漂移。

考慮到陀螺儀飽和特性及溫度特性，最終陀螺儀仿真模型如圖1所示。

圖1 陀螺儀仿真模型

1.4 微機械加速度計系統(tǒng)模型

同陀螺儀一樣，捷聯(lián)慣導系統(tǒng)中加速度計的輸出亦受安裝偏差，漂移及標度系數(shù)誤差等影響；與陀螺儀不同的是，在導航系統(tǒng)中使用加速度計進行姿態(tài)角測量時，慢變漂移對于角度測量精度影響不大，故為了降低濾波器維度，一般僅考慮逐次啟動漂移fab及快變漂移fan。

設i方向加速度計與j方向載體坐標軸夾角為Γij，則捷聯(lián)慣導系統(tǒng)中三軸陀螺儀安裝矩陣可表示為

(4)

則系統(tǒng)中三軸加速度計輸出模型可表示為

fao=diag(1+sax,1+say,1+saz)Mafav+fab+fan

(5)

式中fao為加速度計輸出;fav為載體加速度輸入;fab為加速度計逐次啟動漂移;fan為加速度計快變漂移。

考慮到加速度計飽和特性及溫度特性，最終加速度計仿真模型如圖2所示。

圖2 加速度計仿真模型

2 姿態(tài)估計系統(tǒng)算法

姿態(tài)用于表征坐標系與坐標系間的相對旋轉關系，載體坐標系與導航坐標系間姿態(tài)角常用方位角β、俯仰角α以及滾動角γ表示。

2.1 坐標系定義與變換關系

系統(tǒng)中主要應用載體坐標系O-xyz與導航坐標系O-ENU，姿態(tài)角定義如圖3所示。O-xyz坐標軸選取遵循“右前上”原則，與載體固聯(lián)；O-ENU坐標軸選取遵循“東北天”原則，隨載體運動在地球表面平移和旋轉。

圖3 姿態(tài)角定義

載體坐標系到導航坐標系姿態(tài)變換矩陣為

(6)

采用四元數(shù)Q=[q1q2q3q4]T表示為[9]

(7)

則采用載體四元數(shù)可計算得到載體姿態(tài)角

(8)

2.2 擴展卡爾曼濾波算法

在此使用擴展卡爾曼(EKF)，解決由于利用加速度計解算姿態(tài)角時存在三角函數(shù)運算而導致的觀測方程非線性問題。

選取四元數(shù)Q作為系統(tǒng)狀態(tài)變量，則系統(tǒng)狀態(tài)預測方程為

Q(k)=A(k)Q(k-1)

(9)

其中狀態(tài)轉移矩陣

A(k)=

選取俯仰角及橫滾角為觀測變量，即z=[αγ]T。由于式(8)具有非線性，現(xiàn)對其進行線性化處理。記F(q1,q2,q3,q4)=[α(q1,q2,q3,q4)γ(q1,q2,q3,q4)]T，其雅各比矩陣為

(10)

其中

(11)

設重力加速度在導航坐標系O-ENU中投影為G=[0 0g]T，其在載體坐標系O-xyz中投影為Xg=[xgygzg]T，則可得俯仰角α與橫滾角γ計算公式

(12)

綜上,得到以Q為狀態(tài)變量，以α,γ為觀測值的擴展卡爾曼姿態(tài)角估計算法，時間更新方程如式(13)所示，狀態(tài)更新方程如式(14)所示

(13)

(14)

3 微機械姿態(tài)估計系統(tǒng)仿真實驗

現(xiàn)以俯仰軸為例分析仿真結果。

3.1 微機械系統(tǒng)仿真標定

對比表1、表3，可以看出：對于標度系數(shù)，陀螺儀與加速度計的標定分辨率為0.000 1，由于考慮了陀螺儀的隨機游走噪聲，其零偏標定分辨率僅為0.1，而未考慮隨機游走噪聲的加速度計，其零偏標定分辨率可達到0.000 1。對比表2、表4，可知系統(tǒng)中陀螺儀與加速度計的敏感軸安裝角標定精度均能達到0.01°。在給定角運動曲線情況下，陀螺儀原始輸出、經(jīng)補償輸出以及角速度測量誤差如圖4所示;加速度計原始輸出、經(jīng)補償輸出及加速度測量誤差如圖5所示?？梢钥闯觯河捎跇硕认禂?shù)誤差、零漂、安裝偏差角等存在，陀螺儀與加速度計測量結果與真實值存在偏差，經(jīng)過標定補償后，偏差明顯減小，剩余偏差統(tǒng)計特性與設定的陀螺儀與加速度計模型中測量白噪聲統(tǒng)計特性相當。

表1 陀螺儀、加速度計標定系數(shù)及零偏溫度模型

表2 陀螺儀、加速度計安裝角度

表3 陀螺儀、加速度計標定系數(shù)及零偏溫度標定模型

表4 陀螺儀、加速度計安裝角度標定結果

圖4 陀螺儀原始輸出、其經(jīng)過補償后的輸出以及角速度測量誤差

圖5 加速度計原始輸出、其經(jīng)過補償后的輸出及加速度測量誤差

3.2 姿態(tài)估計系統(tǒng)仿真測試

由圖6可知，利用加速度估計載體姿態(tài)角其均值與理論值吻合，但單次測量偏差較大，其角度測量標準差約為0.1°；由圖7可知，利用陀螺儀積分估計載體姿態(tài)角，其估計值變化平穩(wěn)，但由于陀螺儀存在隨機漂移，其零偏無法完全補償，姿態(tài)角估計結果會隨時間發(fā)散；由圖8可知，利用EKF算法，融合加速度計與陀螺儀測量結果，既減小了單次角度估計誤差，又很好地抑制了角度估值的漂移，俯仰角估計值誤差小于0.05°。

圖6 載體俯仰角理論值、加速度計估計值以及估計誤差

圖7 載體俯仰角理論值、陀螺儀積分估計值以及估計誤差

圖8 載體俯仰角理論值、EKF估計值及估計誤差

實際上，由于采用加速度計僅能觀測載體的俯仰角與橫滾角，故而該濾波器對于載體方位角發(fā)散無任何抑制作用，如圖9所示。

圖9 載體方位角EKF估值與理論值

4 結 論

本文詳細分析了INS中陀螺儀與加速度計輸出模型，在考慮到系統(tǒng)溫度模型、傳感器標度系數(shù)溫度模型與零偏模型、傳感器飽和特性等客觀條件前提下，實現(xiàn)了微機械姿態(tài)估計硬件仿真系統(tǒng)，配合適當載體角運動與線運動模型，可輸出指定參數(shù)下的陀螺儀與加速度計數(shù)據(jù)，為設計和驗證導航算法提供仿真平臺。此外，本文提出了一種應用EKF算法進行加速度計與陀螺儀數(shù)據(jù)融合估計載體姿態(tài)角的方法，通過仿真驗證，該算法俯仰角估計誤差小于0.04°，橫滾角估計誤差小于0.05°，偏航角漂移速度約0.01(°)/s。

[1] Hu Y,Yan Y,Liang J,et al.A miniature,low-cost MEMS AHRS with application to posture control of robotic fish[C]∥2013 IEEE International Instrumentation and Measurement Technology Conference(I2MTC),IEEE,2013:1392-1395.

[2] Safaee A,Moussavi S Z,Mehrabani M S,et al.Construction and control of monocopter using MEMS AHRS[C]∥11th IEEE International Conference on Control & Automation(ICCA),IEEE,2014:219-224.

[3] 劉輝邦,褚金奎,支 煒,等.基于STM32的無人機姿態(tài)測量系統(tǒng)設計[J].傳感器與微系統(tǒng),2013,32(8):108-110.

[4] 胡少興,劉東昌,張愛武,等.一種基于MEMS傳感器的無人飛艇航姿測量系統(tǒng)[J].傳感器與微系統(tǒng),2014,33(3):58-61.

[5]WangY,HussainA,SoltaniM.AMEMS-basedadaptiveAHRSformarinesatellitetrackingantenna[J].IFAC-PapersOnline,2015,48(16):121-126.

[6] 黃 旭,王常虹.基于微機械慣性器件和磁強計的姿態(tài)航向系統(tǒng)[J].傳感器與微系統(tǒng),2005,24(8):54-56.

[7]DuJ,GerdtmanC,LindenM.SignalprocessingalgorithmsfortemperauturedriftinaMEMS-gyro-basedheadmouse[C]∥2014InternationalConferenceonSystems,SignalsandImageProces-sing,IEEE,2014:123-126.

[8] 秦永元,張洪鉞,汪叔華.卡爾曼濾波與組合導航原理[M].西安:西北工業(yè)大學出版社，2015:332-335.

Algorithm and simulation of MEMS attitude estimation system

WEI Zhen-nan, YI Guo-xing, XU Ze-yuan

(School of Astronautics,Harbin Institute of Technology,Harbin 150001,China)

An attitude estimation hardware simulation system is realized by using MEMS gyroscopes and MEMS accelerometers,which is based on error model of gyroscope and accelerometer.The system can be used to simulate three-axis strapdown inertial navigation system(INS)under the situation of any noise intensity and installation error,that is to output data of gyroscopes and accelerometers according to given motion curve,in order to supply simulation verification platform for designing attitude estimation algorithm.Meanwhile,an attitude estimation algorithm which is based on extended Kalman filter is designed.In the algorithm,the attitude quaternion is chosen as state variable,the pitching angle and roll angle are chosen as observed variables.As a result,estimation error of pitching angle is less than 0.04°,estimation error of roll angle is less than 0.05°,and the drift rate of yaw angle is 0.01(°)/s.

attitude estimation; MEMS INS; error model; Kalman filtering

10.13873/J.1000—9787(2017)05—0149—04

2016—05—22

V 249

A

1000—9787(2017)05—0149—04

魏振楠(1992-)，男，碩士研究生，主要研究方向為組合導航。