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

魏振楠， 伊國興， 徐澤遠(yuǎn)

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

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

魏振楠， 伊國興， 徐澤遠(yuǎn)

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

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

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

0 引 言

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

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

1 微機(jī)械慣性導(dǎo)航系統(tǒng)模型建立

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

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

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

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

(1)

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

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

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

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

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

(2)

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

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

(3)

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

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

圖1 陀螺儀仿真模型

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

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

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

(4)

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

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

(5)

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

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

圖2 加速度計(jì)仿真模型

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

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

2.1 坐標(biāo)系定義與變換關(guān)系

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

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

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

(6)

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

(7)

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

(8)

2.2 擴(kuò)展卡爾曼濾波算法

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

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

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

(9)

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

A(k)=

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

(10)

其中

(11)

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

(12)

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

(13)

(14)

3 微機(jī)械姿態(tài)估計(jì)系統(tǒng)仿真實(shí)驗(yàn)

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

3.1 微機(jī)械系統(tǒng)仿真標(biāo)定

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

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

表2 陀螺儀、加速度計(jì)安裝角度

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

表4 陀螺儀、加速度計(jì)安裝角度標(biāo)定結(jié)果

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

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

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

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

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

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

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

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

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

4 結(jié) 論

本文詳細(xì)分析了INS中陀螺儀與加速度計(jì)輸出模型，在考慮到系統(tǒng)溫度模型、傳感器標(biāo)度系數(shù)溫度模型與零偏模型、傳感器飽和特性等客觀條件前提下，實(shí)現(xiàn)了微機(jī)械姿態(tài)估計(jì)硬件仿真系統(tǒng)，配合適當(dāng)載體角運(yùn)動與線運(yùn)動模型，可輸出指定參數(shù)下的陀螺儀與加速度計(jì)數(shù)據(jù)，為設(shè)計(jì)和驗(yàn)證導(dǎo)航算法提供仿真平臺。此外，本文提出了一種應(yīng)用EKF算法進(jìn)行加速度計(jì)與陀螺儀數(shù)據(jù)融合估計(jì)載體姿態(tài)角的方法，通過仿真驗(yàn)證，該算法俯仰角估計(jì)誤差小于0.04°，橫滾角估計(jì)誤差小于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的無人機(jī)姿態(tài)測量系統(tǒng)設(shè)計(jì)[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] 黃 旭,王常虹.基于微機(jī)械慣性器件和磁強(qiáng)計(jì)的姿態(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] 秦永元,張洪鉞,汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社，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-)，男，碩士研究生，主要研究方向?yàn)榻M合導(dǎo)航。