基于MEMS航姿參考系統的角度估計算法研究

王慶輝， 陳芳萍， 魏立峰

(沈陽化工大學 信息工程學院， 遼寧 沈陽 110142)

基于MEMS航姿參考系統的角度估計算法研究

王慶輝， 陳芳萍， 魏立峰

(沈陽化工大學 信息工程學院， 遼寧 沈陽 110142)

為解決行人導航系統中，由于采用的基于微機械加工技術(MEMS)的慣性傳感器精度較低，計算誤差會隨時間積累的問題，在基于微機電系統(MEMS)的航姿參考系統(AHRS)基礎上，提出了一種動態過程修正算法，通過利用靜止條件判斷，進行角度校準，減少角度累積計算誤差.在雙軸轉臺進行實際測試結果表明，角度計算精度得到了提高.

MEMS； AHRS； 角度

隨著時代的發展，智能終端的普及，人們在生活和工作中越來越多地應用到位置服務，而這一切的前提是定位，最原始的定位裝置主要依靠的是對自然界信息的測量，再后來，人類在生產實踐中，逐步掌握了運用慣性裝置(如陀螺儀)進行定位的技術.GPS的問世是定位行業里程碑式的事件，在大多數環境下能做到全天時較高精度的定位，但在諸如商場、室內、隧道等復雜環境中，GPS由于信號易受到遮擋，有時無法提供定位信息.近幾年，個人室內定位技術獲得了很大的市場需求，如商場內基于個人位置的精確營銷、需要跟蹤值班人員行動路線的巡更系統等，目前大部分智能手機都開始安裝基于MEMS的慣性器件，來提供個人位置輔助定位，但要想獲得相對精確的定位，準確的姿態信息是非常重要的.慣性器件包括軸陀螺儀，加速度計和磁強計等.姿態與航向參考系統是基于慣性測量器件的姿態解算方法，能夠提供航向角，橫滾角和俯仰角信息，應用于飛行器導航和衛星姿態控制[1].國內低成本航姿器的研究和開發也是基于MEMS慣性傳感器，其中，中航618所的朱少華等人提出了基于MEMS器件的大氣航姿系統的設計思路和算法，樣機進行了車載試驗的驗證[2].空軍工程大學工程學院的黃國榮等人提出了基于MEMS器件的軍用航姿系統的設計思路和算法，姿態誤差在1°之內[3].Sebastian O.H，Madgwick提出了基于四元素的梯度下降算法，利用慣性傳感器和三軸磁強計的測量值計算出姿態歐拉角，實現了對載體姿態的確定[4].由此可見，AHRS的發展趨勢主要體現在低成本化，高精度化，高度集成化，應用范圍不斷擴大：從目前的軍用市場將擴展到民用市場[5].

本文根據姿態與航向參考系統及互補濾波算法，提出一種AHRS過程修正算法，能夠在個人定位時動態修正角度的測量值.在獲取姿態角度時，通過滑動窗口濾波的方法加入判斷人靜止的條件，在人靜止時重新對AHRS進行初始化修正，提高了角度的計算精度.

1 系統的總體設計

系統硬件部分采用導航處理器模塊、慣性傳感器模塊和磁強計模塊組成系統的總體框架.導航處理器模塊主要是采集各個傳感器的信息，并對信息進行解算，最后再將解算出的數據傳輸給上位機.該系統選擇ST公司生產的基于ARM Cortex-M3內核的32位處理器STM32F103RBT6作為導航處理器[6].慣性傳感器的性能主要體現在精度、量程和穩定性等方面.該系統選擇QFN24封裝的MPU-6050型號傳感器，它整合了三軸陀螺儀和三軸加速度計[7]，在系統工作中，主要作用是感知系統水平方向的傾斜，并用于修正陀螺在俯仰和滾動方向的漂移.三軸磁阻型磁強計測量三維地磁強度，用于提供方向角的初始對準以及修正航向角漂移，利用磁強計的誤差不隨時間積累的特點，修正陀螺隨時間推移的累計誤差.該系統選擇廣泛應用于磁場檢測領域的低成本羅盤——Honeywell的HMC5883L表面貼裝高集成模塊[8].使用慣性傳感器和三軸磁強計輸出的數據，應用AHRS過程修正算法校正陀螺儀的姿態角得到載體的姿態，在TinyCapture上位機上可以很好地顯示橫滾角、俯仰角和偏航角.系統結構框圖如圖1所示.

圖1 系統的總體結構

2 AHRS互補濾波算法

該系統對姿態的求解應用的是基于四元數的AHRS互補濾波算法，使用的慣性傳感器包括陀螺儀、加速度計和磁強計.利用三軸加速度計、三軸陀螺儀分別測量載體坐標系3個軸上的線加速度及角速率，利用三軸磁強計測量地磁場在載體坐標系3個軸上的分量.通過對傳感器測量得到的數據應用互補濾波算法，即引入積分反饋，利用陀螺補償角計算出陀螺測量誤差的四元數微分形式，進而求出估計的四元數，根據公式轉化成姿態歐拉角，實現對載體姿態的確定.

2.1 航姿參考系統的初始對準

橫滾角和仰俯角由加速度計測量得到的重力在載體坐標系的分量求得，而偏航角由地磁場在載體坐標系的分量計算得到.系統啟動后，首先根據加速度計和磁強計的輸出值解算出載體的橫滾角、仰俯角和偏航角.

γ=arctan(ay/az)

(1)

θ=arcsin(ax/-g)

(2)

φ=arctan[(mz×sinγ-my×cosγ)/

(mx×cosγ+my×sinγ+

mz×sinθ×cosγ)]

(3)

其中:γ表示橫滾角，θ表示仰俯角，φ表示偏航角；g為重力加速度；ax，ay，az分別表示加速度計在載體坐標系的測量值；mx，my，mz分別表示磁強計在載體坐標系的測量值.

由(4)～(7)式可求出四元數的初始值：

(4)

(5)

(6)

(7)

將四元數轉換成姿態歐拉角：

Euler_γ=

(8)

Euler_θ=arcsin(-2(q0×q2-q1×q3));

(9)

Euler_φ=

(10)

至此，AHRS的初始對準結束.

2.2 航姿參考系統的更新算法

首先利用三軸加速度計和三軸磁強計分別測量載體坐標系3個軸上的線加速度和地磁場在載體坐標系3個軸上的分量，然后對其進行歸一化處理.公式(11)～(13)為加速度歸一化測量值；公式(14)～(16)為羅盤歸一化測量值.

(1) 歸一化加速度和羅盤的測量值

(11)

(12)

(13)

(14)

(15)

(16)

(2) 磁失真補償

令

(17)

(18)

磁強只在水平面有作用，從三軸變成兩軸就是消除傾斜影響之后測量磁場方向.

(3) 計算姿態誤差

(19)

(20)

(4) 引入積分反饋

(21)

gx，gy和gz為陀螺補償角，因為陀螺的零點偏移將隨著時間而變化，所以直接采用姿態誤差的積分反饋對陀螺零偏進行補償；twoKi為引入的反饋因子，該因子的引入使整個算法的精度得到很大提高；Δt是采樣周期.

(5) 由陀螺補償角計算出四元數的導數：

bg=[0 gxgygz]

(23)

(24)

(25)

2.3 過程修正算法

由于MEMS角速率陀螺儀存在嚴重的零點漂移和隨機誤差，在捷聯慣性導航解算中會產生積分誤差，難以達到應用精度.由此，系統在初始時刻所測角速率精度是比較高的，即為AHRS初始對準時所得的姿態歐拉角是比較準確的，所以在此提出AHRS的過程修正算法，即在獲取姿態角度的過程中采用滑動窗口濾波的方法，時刻判斷人是否處于靜止狀態，當人靜止時重新對AHRS進行初始對準，這樣就減少了陀螺隨時間的推移而累積的誤差，從而提高了角度的測量精度.由于加速度的采樣率是100 Hz，即10 ms采樣一次，所以2 s內加速度采樣200次，設定窗口長度為200，對這2 s內的采樣值取均值，并不斷更新滑動窗口，由下一時刻的采樣值代替最初時刻的采樣值，此時若均值在閾值之內則認為人處于靜止狀態.由于人在靜止時只有重力加速度，對三軸加速度計在載體坐標系3個軸上的線加速度求平方根，其值認為是采樣一次重力加速度的值.該系統是利用轉臺的轉動來模擬人的運動，當人佩帶傳感器時，傳感器是處于垂直狀態的，但由于靜止時重力加速度是有波動的，所以對程序進行在線調試，同時利用轉臺測控軟件設置傳感器處于垂直狀態且轉臺靜止，當所求均值g的值在8 182.035 LSB/g和8 238.521 LSB/g之間時，認為人處于靜止狀態.系統工作流程如圖2所示.

圖2 系統工作流程

3 測試及結果

角度的測量通過轉臺來完成，將傳感器連接到轉臺上，通過轉臺測控軟件可設定主軸和俯仰軸轉過的角度，在TinyCapture的上位機上來觀測傳感器橫滾和俯仰所轉過的實際角度.通過這樣的方式可檢測出所設定角度與傳感器變化角度的差值.系統使用的雙軸轉臺機械臺體采用UO形鋁合金框架結構，由內環橫滾軸框架和外環俯仰軸框架組成相互垂直的兩維旋轉坐標系，其角位置綜合測量精度為：±0.08°.

由于陀螺存在隨時間的積累誤差會累積的特點，所以設定轉臺在連續工作的情況下，通過測量多組數據，來得到傳感器實際轉過的俯仰角和橫滾角.以60°為例，將轉臺測控軟件的俯仰軸和主軸設置成60°，讓轉臺在連續工作1 h的情況下測出傳感器實際轉過的角度.表1和表2的左側是算法改進前通過觀測TinyCapture上位機測得的角度.右側是算法改進后測得的角度.

表1 算法改進前后傳感器俯仰軸角度

表2 算法改進前后傳感器橫滾軸角度

由以上數據分析可知:算法改進后俯仰軸測量角精度平均提高了1.77 %；橫滾軸測量角精度平均提高了1.07 %.在利用轉臺的轉動模擬人的運動時，在轉臺靜止也就是人靜止時重新對AHRS進行初始對準，減少了陀螺的測量誤差，即改進的算法可行.

4 結 論

本系統實現了對俯仰角與橫滾角的精度測量，可以應用到個人定位時對人在行走和靜止時的姿態檢測.系統由于在對AHRS進行初始化時用到磁強計的測量值，很容易受到來自環境和平臺的磁干擾，獲取準確的角度測量值頗具挑戰性.所以研究更加有效的動態修正算法，減小角度測量誤差，提高定位精度是很有必要的.

[1] 張麗杰，常佶.微小型航姿測量系統及其數據融合方法[J].中國慣性技術學報，2011，19(3):307-311.

[2] 朱少華，汪芳，郭志想，等.一種適用于高動態制導飛行的大氣航姿系統算法[J].彈箭與制導學報，2011，31(4):14-16.

[3] 杜繼永，黃國榮，張鳳鳴，等.基于低成本 MEMS 器件的捷聯航姿系統設計[J].傳感技術學報，2010，23(11):1662-1666.

[4] Madgwick S O H.An Efficient Orientation Filter for Inertial and Inertial/magnetic Sensor Arrays[EB/OL].(2010-04-30)[2013-10-10].http://sharenet-wii-motion-trac.googlecode.com/files/An_efficient_oriention_filter_for_inertial_and_inertialmagnetic_sensor_arrays.pdf.

[5] 劉智平，譚芳.國外航姿器的技術現狀分析[J].四川兵工學報，2012，33(2):11-13.

[6] ST公司.STM32參考手冊英文版[EB/OL].(2009-12-05)[2013-09-11].http://wenku.baidu.com/view/2901b526ed 630b1c59eeb5f3.html.

[7] InvenSense公司.MPU6050 產品說明書[EB/OL].(2011-11-16)[2013-09-11].http://wenku.baidu.com/view/d7f8dd080740be1e640e9a01.html.

[8] Honeywell公司.HMC5883L中文數據手冊[EB/OL].(2011-03-05)[2013-09-11].http://wenku.baidu.com/view/91bf2c1d964bcf84b9d57bfd.

Angle Estimation Algorithm Based on MEMS Attitude and Heading Reference System

WANG Qing-hui， CHEN Fang-ping， WEI Li-feng

(Shenyang University of Chemical Technology， Shenyang 110142， China)

In the Pedestrian Navigation System,the micro machining technology based on the inertial sensors(MEMS) is low precision,calculation error will be accumulated over time.Based on MEMS Attitude and Heading Reference System,a dynamic process correction algorithm has been proposed,which could carry through the angle calibration and reduce the calculation error of angle by using static condition.The test results,which was realised on two-axis turntable,show that the measurement precision of angle has been improved.

MEMS； AHRS； angle

2013-09-10

沈陽市科技攻關項目(F11-009-2-00)

王慶輝(1972-)，男，黑龍江齊齊哈爾人，副教授，博士，主要從事無線自組網技術，嵌入式系統等方面的研究.

2095-2198(2015)01-0054-05

10.3969/j.issn.2095-2198.2015.01.012

TP212.9

A