采用卡爾曼濾波算法的MEMS器件姿態(tài)測量

周樹道， 金永奇， 衛(wèi)克晶， 劉 星(解放軍理工大學 氣象海洋學院，江蘇 南京 211101)

?

采用卡爾曼濾波算法的MEMS器件姿態(tài)測量

周樹道， 金永奇， 衛(wèi)克晶， 劉 星
(解放軍理工大學 氣象海洋學院，江蘇 南京 211101)

針對小型無人機上使用的MEMS慣性傳感器在精度、噪聲上存在的問題，采用卡爾曼濾波算法結(jié)合角度傳感器、加速度傳感器、磁阻傳感器的傳感信息，來解算姿態(tài)角最優(yōu)值。采用四元素法確立了捷聯(lián)矩陣，利用三軸陀螺儀傳感器所得到的角速度信息建立系統(tǒng)狀態(tài)方程，利用三軸加速度傳感器和磁阻傳感器信息建立了系統(tǒng)測量方程，進而設(shè)計了一種卡爾曼濾波器來濾除MEMS傳感器存在的隨機噪聲，并解算出了小型無人機的姿態(tài)角。通過實驗室靜態(tài)測試和動態(tài)測試表明，無人機姿態(tài)角解算結(jié)果與實際值對比，誤差能夠控制在2°以內(nèi)，可滿足工程應(yīng)用要求。

卡爾曼濾波； 姿態(tài)角解算； 微機電系統(tǒng)傳感器； 四元素法

0 引 言

姿態(tài)角是飛機飛行過程中重要參數(shù)，無論在控制飛機飛行還是進行各種無人機探測時，都需要獲得機體的姿態(tài)信息，因此，對飛機姿態(tài)角的準確測量具有重要意義。通常在傳統(tǒng)的大型飛機上安裝有高精度的靜電陀螺儀、液浮陀螺儀等，零偏穩(wěn)定性優(yōu)于0.0015 °/s[1-2]，可長久地得到高精度的姿態(tài)信息，但這些器件價格昂貴，體積、重量也較大，無法在小型無人機上應(yīng)用。隨著MEMS技術(shù)發(fā)展，硅微機械電子陀螺儀得以應(yīng)用，其體積小、重量輕、功耗低、慣性小[3-4]，成為無人機姿態(tài)測量的很好選擇，但精度偏低[5]，無法單獨使用。本文在提出相關(guān)數(shù)學算法基礎(chǔ)上，融合多種傳感器信息，有效提高了姿態(tài)角的測量精度。

1 相關(guān)坐標系引入及姿態(tài)角定義

1.1 相關(guān)坐標系引入

機體坐標系：以飛機重心為原點，機體縱軸指向前、橫軸指向右、豎軸指向上分別為三軸建立三維直角坐標系，用Oxbybzb表示；地理坐標系：以無人機重心為原點，地理方向指向東、北和鉛直向上為三軸建立另一個直角坐標系，用Oxtytzt表示[6]。

1.2 姿態(tài)角的定義

飛機姿態(tài)角是機體坐標系與地理坐標系之間的方位關(guān)系。無人機繞橫軸xb轉(zhuǎn)動，縱軸yb與地理坐標系Oxtyt面之間的夾角叫做無人機的俯仰角，用θ表示，從Oxtyt面算起，向上為正，向下為負，定義域為[-90°，90°]；無人機繞橫縱yb轉(zhuǎn)動，橫軸xb與地理坐標系Oxtyt面之間的夾角叫做無人機的橫滾角，用γ表示，從Oxtyt算起，右傾為正，左傾為負，定義域為(-180°，180°]；無人機繞橫縱zb轉(zhuǎn)動，縱軸yb與地理坐標系Oytzt面之間的夾角叫做無人機的航向角，用ψ表示，從Oytzt面算起，0°、90°、180°和270°分別指向正北、正東、正南和正西，定義域為[0°，360°)[7]。

2 捷聯(lián)矩陣與四元素法

由于飛機姿態(tài)角蘊含于機體坐標系和地理坐標系之間的關(guān)系中，所以可將飛機姿態(tài)角解算問題轉(zhuǎn)化到機體坐標系與地理坐標系變換計算上[8]。而計算兩坐標系變換，關(guān)鍵是找出捷聯(lián)矩陣。

2.1 捷聯(lián)矩陣的確定

機體坐標系與地理坐標系間關(guān)系如圖1所示[7]。

圖1 兩坐標關(guān)系圖

捷聯(lián)矩陣[9-10]的確定思路為：

第①次變換為：

第②次變換為：

第③次變換為：

則由Oxtytzt坐標系轉(zhuǎn)到Oxbybzb坐標系的變換關(guān)系為：

其中，T為捷聯(lián)矩陣，根據(jù)矩陣中的元素可以求得飛機的姿態(tài)角。

2.2 捷聯(lián)矩陣的四元素表示

上述所得T是以三個姿態(tài)角為變量來表示的，雖然直觀易懂，但在求解姿態(tài)角，當θ=90°時，將出現(xiàn)奇點，因而該表示法的應(yīng)用有一定的局限性，不宜用在全姿態(tài)運動體的姿態(tài)解算中[11]。

三維空間中，一個向量在不同坐標系之間轉(zhuǎn)化時，可以采用類似二維空間中的乘以一個復數(shù)的方法來實現(xiàn)，只是其中涉及到的旋轉(zhuǎn)角度有3個，復數(shù)標記也需要推廣到空間[12]，這樣就引出了一個新的變量——四元素，其表示形式為：

Q=q0+q1i+q2j+q3k

設(shè)Q為由地理坐標系向機體坐標系變化的四元素，則用四元素表示的捷聯(lián)矩陣為：

(1)

3 姿態(tài)角求解

根據(jù)四元素法表示的捷聯(lián)矩陣求解姿態(tài)角為[13]：

(2)

其中，ψ和γ需要進行主值判斷[14]。

又根據(jù)捷聯(lián)慣導系統(tǒng)中四元素的相關(guān)理論，得到四元素的微分方程為：

?ω

(3)

式中，ω=0+wxi+wyj+wzk，wx、wy、wz分別為三軸角速度，矩陣形式為：

(4)

即：

(5)

這樣，求解微分方程(5)，得到四元素的4個值，代入式(1)得捷聯(lián)矩陣，再根據(jù)式(2)即可解得飛機的姿態(tài)角。

使用MEMS陀螺儀，其隨機漂移達到了10-2°/s，求解微分方程得到的姿態(tài)角會隨著時間推移誤差積累增大，解算值將不可信，因此需要進行濾波處理。陀螺儀漂移是一種隨機噪聲，沒有確定的頻譜，無法用常規(guī)濾波器去除，但有確定的功率譜，可以看做是白噪聲，可用卡爾曼濾波從量測量中去估計所需信號[15]，達到濾波的效果。

這里主要使用離散型卡爾曼濾波器?？柭鼮V波算法的主要工作是建立系統(tǒng)的狀態(tài)方程和測量方程：

(6)

圖2 卡爾曼濾波計算更新流程圖

兩個方程建立思路：根據(jù)陀螺儀測量信息與姿態(tài)角的關(guān)系建立狀態(tài)方程；根據(jù)加速度與磁阻傳感器測量信息與重力場和地磁場關(guān)系建立測量方程。

首先進行狀態(tài)方程的建立，將四元素的4個參數(shù)作為狀態(tài)量，式(4)作為系統(tǒng)狀態(tài)方程，利用比卡逼近法將其離散化，忽略高階量，得到：

Q(k+1)=Φ(k+1/k)Q(k)+T(k)W(k)

(7)

其中：

式中：ΔT為傳感器采樣周期；W(k)為陀螺儀噪聲。這里將陀螺儀漂移看作是一個隨機游走的模型，即存在一個隨機誤差，故將W(k)近似為均值為零的獨立高斯白噪聲，且其協(xié)方差矩陣是非負定常值對角陣。

然后進行測量方程的建立。根據(jù)三軸加速度估計值與重力加速度存在的關(guān)系得：

(8)

根據(jù)三軸磁阻傳感器測量信息與地球磁場存在的關(guān)系得：

(9)

綜合式(8)、(9)建立系統(tǒng)的測量方程：

(10)

式中，V為測量噪聲。

對式(10)離散化，得到離散測量方程為：

(11)

至此，建立了狀態(tài)方程和測量方程，完成了卡爾曼濾波器的設(shè)計。這樣按照圖2計算流程利用傳感器數(shù)據(jù)進行姿態(tài)角解算。

4 實驗測試

為驗證所設(shè)計的卡爾曼濾波器解算姿態(tài)角的實際效果，設(shè)計傳感器電路測試系統(tǒng)進行實驗測試。實驗主要分靜態(tài)和動態(tài)測試，靜態(tài)測試主要對比卡爾曼濾波算法和陀螺儀積分算法解算姿態(tài)角的效果。動態(tài)測試主要驗證卡爾曼濾波解算姿態(tài)角的準確性。

4.1 靜態(tài)測試

采集測試系統(tǒng)傳感器在靜止狀態(tài)下的測量值，分別使用卡爾曼濾波和角速度積分的算法，計算姿態(tài)角，結(jié)果如圖3所示。

由結(jié)果可見，由于MEMS陀螺儀存在的較大漂移，使得直接積分得到的角度呈現(xiàn)一種發(fā)散的狀況；而卡爾曼濾波算法，融合了無時間積累誤差的加速度和磁阻傳感器數(shù)據(jù)，有效濾除了陀螺儀漂移誤差，使計算的角度曲線非常穩(wěn)定，波動值在1°以內(nèi)。

圖3 卡爾曼濾波(上)和角速度積分(下)解算姿態(tài)角結(jié)果圖

4.2 動態(tài)測試

為便于姿態(tài)解算結(jié)果與實際值進行對比，測試系統(tǒng)分別繞各軸作勻速轉(zhuǎn)動，通過觀察姿態(tài)角變化曲線各段的線性程度和計算線性斜率值，得出姿態(tài)角解算的精確程度。

控制轉(zhuǎn)臺分別繞x、y、z軸做正、反勻速轉(zhuǎn)動，使用卡爾曼濾波算法融合各傳感器數(shù)據(jù)解算的姿態(tài)角如圖4所示。觀察曲線可知，各角度的變化曲線呈現(xiàn)出較好的線性性，并具有良好的周期性。對實際數(shù)據(jù)分析得到周期為3.34 s，直接測量轉(zhuǎn)臺旋轉(zhuǎn)周期為3.36 s，基本吻合。對幾個特定位置進行直接測量，與解算結(jié)果偏差在2°以內(nèi)。

(a) 航向角

(b) 俯仰角

(c) 橫梁角

5 結(jié) 語

本文使用四元素法表示捷聯(lián)矩陣，采用所設(shè)計的卡爾曼濾波器對實驗測試數(shù)據(jù)進行了姿態(tài)解算并對結(jié)果做了對比分析。實驗表明，按照本文所提供的方法，可以有效濾除MEMS陀螺儀漂移誤差，將姿態(tài)角誤差控制在2°以內(nèi)，基本滿足實際應(yīng)用要求。

[1] 王壽榮. 硅微型慣性器件理論及應(yīng)用[M]. 南京:東南大學出版社,2000:172-173.

[2] 劉 宇,李秋俊,劉 俊. 基于壓電陀螺的快速方位測井短接[J]. 電子測量與儀器學報,2007(2): 90-94.

[3] 姜巖峰. 微納電子器件[M]. 北京: 化學工業(yè)出版社, 2005:222-224.

[4] 錢華明,夏全喜,闕興濤,等.基于Kalman濾波的MENS陀螺儀濾波算法[J]. 哈爾冰工程大學學報,2010,31(9): 1217-1221.

[5] 程 承,潘 泉,王坤龍,等. 基于壓縮感知理論的MEMS陀螺儀信號降噪研究[J]. 儀器儀表學報，2012,33(4): 769-773.

[6] 王彥杰,周樹道,朱國濤,等. 基于轉(zhuǎn)動狀態(tài)的氣象無人機測風方法[J]. 氣象科學,2011,31(5):576-581.

[7] 袁 信，鄭 諤. 捷聯(lián)式慣性導航原理[M]. 北京: 航空專業(yè)教材編審組出版, 1985:5-7.

[8] 周樹道,王彥杰,王 敏,等. 基于氣象無人機飛行角度的改進型測風模型[J].飛行力學,2011,29(1): 26-29.

[9] 關(guān) 勁,李仔冰. 旋轉(zhuǎn)式捷聯(lián)慣導系統(tǒng)精對準方法[J]. 中國慣性技術(shù)學報,2010,18(4): 396-400.

[10] 張榮輝,賈宏光,陳 濤,等. 基于四元素法的捷聯(lián)式慣性導航系統(tǒng)的姿態(tài)解算[J].光學精密工程,2008,16(10): 1963-1970.

[11] 苑海靜. 船用MEMS航姿測量系統(tǒng)算法研究與實現(xiàn)[D]. 哈爾濱: 哈爾濱工程大學, 2010：5-70.

[12] 以光衢. 陀螺理論與應(yīng)用[M]. 北京: 北京航空航天大學出版社, 1990:39-40.

[13] 孫冬梅,田增山,韓令軍. 捷聯(lián)慣導系統(tǒng)中四元素法求解姿態(tài)角仿真模擬[J]. 彈箭與制導學報,2009,29(1): 51-53.

[14] 秦永元. 慣性導航[M]. 北京: 科學出版社, 2006:297-298.

[15] 秦永元. 卡爾曼濾波與組合導航原理[M]. 西安: 西北工業(yè)大學出版社, 1998:1-4.

Attitude Measurement of MEMS Devices by Using the Kalman Filter Algorithm

ZHOUShu-dao，JINYong-qi，WEIKe-jing，LIUXing
(Institute of Meteorology and Oceanography, PLA University of Science and Technology, Nanjing 211101, China)

For the problems of MEMS inertial sensors in accuracy and noise, Kalman filter algorithm is presented by fusing multi-sensor information obtained from angle sensor, acceleration sensor and magnetoresistive sensor to get optimal result of attitude angle. At first, a strapdown equation described by quaternion is established; an equation of state of the system is established by using tri-axis gyroscopes data, and a system measurement equation is established by using tri-axis accelerometer and MARG sensor data. Then a Kalman filter is designed to remove the random noise of the MEMS sensors and calculated attitude angles. Static test and dynamic test show that the error of attitude solver results can be controlled within 2°, and the design satisfies engineering requirement.

Kalman filter; solution of attitude angles; Micro-Electro-Mechanical Systems (MEMS )sensors; quaternion method

2013-12-12

周樹道(1964-)，男，浙江寧波人，教授，現(xiàn)主要從事信號與信息處理研究。Tel.:025-80830101；E-mail:zhousd70131@sina.com

TP 274+.2

A

1006-7167(2015)02-0038-05