采用卡爾曼濾波算法的MEMS器件姿態測量

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

?

采用卡爾曼濾波算法的MEMS器件姿態測量

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

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

卡爾曼濾波； 姿態角解算； 微機電系統傳感器； 四元素法

0 引 言

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

1 相關坐標系引入及姿態角定義

1.1 相關坐標系引入

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

1.2 姿態角的定義

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

2 捷聯矩陣與四元素法

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

2.1 捷聯矩陣的確定

機體坐標系與地理坐標系間關系如圖1所示[7]。

圖1 兩坐標關系圖

捷聯矩陣[9-10]的確定思路為：

第①次變換為：

第②次變換為：

第③次變換為：

則由Oxtytzt坐標系轉到Oxbybzb坐標系的變換關系為：

其中，T為捷聯矩陣，根據矩陣中的元素可以求得飛機的姿態角。

2.2 捷聯矩陣的四元素表示

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

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

Q=q0+q1i+q2j+q3k

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

(1)

3 姿態角求解

根據四元素法表示的捷聯矩陣求解姿態角為[13]：

(2)

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

又根據捷聯慣導系統中四元素的相關理論，得到四元素的微分方程為：

?ω

(3)

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

(4)

即：

(5)

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

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

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

(6)

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

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

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

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

(7)

其中：

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

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

(8)

根據三軸磁阻傳感器測量信息與地球磁場存在的關系得：

(9)

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

(10)

式中，V為測量噪聲。

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

(11)

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

4 實驗測試

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

4.1 靜態測試

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

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

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

4.2 動態測試

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

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

(a) 航向角

(b) 俯仰角

(c) 橫梁角

5 結 語

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

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

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

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

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

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

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

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

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

[9] 關 勁,李仔冰. 旋轉式捷聯慣導系統精對準方法[J]. 中國慣性技術學報,2010,18(4): 396-400.

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

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

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

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

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

[15] 秦永元. 卡爾曼濾波與組合導航原理[M]. 西安: 西北工業大學出版社, 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-)，男，浙江寧波人，教授，現主要從事信號與信息處理研究。Tel.:025-80830101；E-mail:zhousd70131@sina.com

TP 274+.2

A

1006-7167(2015)02-0038-05