王慶輝， 陳芳萍， 魏立峰

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

基于MEMS航姿參考系統(tǒng)的角度估計(jì)算法研究

王慶輝， 陳芳萍， 魏立峰

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

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

MEMS； AHRS； 角度

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

本文根據(jù)姿態(tài)與航向參考系統(tǒng)及互補(bǔ)濾波算法，提出一種AHRS過程修正算法，能夠在個(gè)人定位時(shí)動(dòng)態(tài)修正角度的測量值.在獲取姿態(tài)角度時(shí)，通過滑動(dòng)窗口濾波的方法加入判斷人靜止的條件，在人靜止時(shí)重新對(duì)AHRS進(jìn)行初始化修正，提高了角度的計(jì)算精度.

1 系統(tǒng)的總體設(shè)計(jì)

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

圖1 系統(tǒng)的總體結(jié)構(gòu)

2 AHRS互補(bǔ)濾波算法

該系統(tǒng)對(duì)姿態(tài)的求解應(yīng)用的是基于四元數(shù)的AHRS互補(bǔ)濾波算法，使用的慣性傳感器包括陀螺儀、加速度計(jì)和磁強(qiáng)計(jì).利用三軸加速度計(jì)、三軸陀螺儀分別測量載體坐標(biāo)系3個(gè)軸上的線加速度及角速率，利用三軸磁強(qiáng)計(jì)測量地磁場在載體坐標(biāo)系3個(gè)軸上的分量.通過對(duì)傳感器測量得到的數(shù)據(jù)應(yīng)用互補(bǔ)濾波算法，即引入積分反饋，利用陀螺補(bǔ)償角計(jì)算出陀螺測量誤差的四元數(shù)微分形式，進(jìn)而求出估計(jì)的四元數(shù)，根據(jù)公式轉(zhuǎn)化成姿態(tài)歐拉角，實(shí)現(xiàn)對(duì)載體姿態(tài)的確定.

2.1 航姿參考系統(tǒng)的初始對(duì)準(zhǔn)

橫滾角和仰俯角由加速度計(jì)測量得到的重力在載體坐標(biāo)系的分量求得，而偏航角由地磁場在載體坐標(biāo)系的分量計(jì)算得到.系統(tǒng)啟動(dòng)后，首先根據(jù)加速度計(jì)和磁強(qiáng)計(jì)的輸出值解算出載體的橫滾角、仰俯角和偏航角.

γ=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分別表示加速度計(jì)在載體坐標(biāo)系的測量值；mx，my，mz分別表示磁強(qiáng)計(jì)在載體坐標(biāo)系的測量值.

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

(4)

(5)

(6)

(7)

將四元數(shù)轉(zhuǎn)換成姿態(tài)歐拉角：

Euler_γ=

(8)

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

(9)

Euler_φ=

(10)

至此，AHRS的初始對(duì)準(zhǔn)結(jié)束.

2.2 航姿參考系統(tǒng)的更新算法

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

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

(11)

(12)

(13)

(14)

(15)

(16)

(2) 磁失真補(bǔ)償

令

(17)

(18)

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

(3) 計(jì)算姿態(tài)誤差

(19)

(20)

(4) 引入積分反饋

(21)

gx，gy和gz為陀螺補(bǔ)償角，因?yàn)橥勇莸牧泓c(diǎn)偏移將隨著時(shí)間而變化，所以直接采用姿態(tài)誤差的積分反饋對(duì)陀螺零偏進(jìn)行補(bǔ)償；twoKi為引入的反饋因子，該因子的引入使整個(gè)算法的精度得到很大提高；Δt是采樣周期.

(5) 由陀螺補(bǔ)償角計(jì)算出四元數(shù)的導(dǎo)數(shù)：

bg=[0 gxgygz]

(23)

(24)

(25)

2.3 過程修正算法

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

圖2 系統(tǒng)工作流程

3 測試及結(jié)果

角度的測量通過轉(zhuǎn)臺(tái)來完成，將傳感器連接到轉(zhuǎn)臺(tái)上，通過轉(zhuǎn)臺(tái)測控軟件可設(shè)定主軸和俯仰軸轉(zhuǎn)過的角度，在TinyCapture的上位機(jī)上來觀測傳感器橫滾和俯仰所轉(zhuǎn)過的實(shí)際角度.通過這樣的方式可檢測出所設(shè)定角度與傳感器變化角度的差值.系統(tǒng)使用的雙軸轉(zhuǎn)臺(tái)機(jī)械臺(tái)體采用UO形鋁合金框架結(jié)構(gòu)，由內(nèi)環(huán)橫滾軸框架和外環(huán)俯仰軸框架組成相互垂直的兩維旋轉(zhuǎn)坐標(biāo)系，其角位置綜合測量精度為：±0.08°.

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

表1 算法改進(jìn)前后傳感器俯仰軸角度

表2 算法改進(jìn)前后傳感器橫滾軸角度

由以上數(shù)據(jù)分析可知:算法改進(jìn)后俯仰軸測量角精度平均提高了1.77 %；橫滾軸測量角精度平均提高了1.07 %.在利用轉(zhuǎn)臺(tái)的轉(zhuǎn)動(dòng)模擬人的運(yùn)動(dòng)時(shí)，在轉(zhuǎn)臺(tái)靜止也就是人靜止時(shí)重新對(duì)AHRS進(jìn)行初始對(duì)準(zhǔn)，減少了陀螺的測量誤差，即改進(jìn)的算法可行.

4 結(jié) 論

本系統(tǒng)實(shí)現(xiàn)了對(duì)俯仰角與橫滾角的精度測量，可以應(yīng)用到個(gè)人定位時(shí)對(duì)人在行走和靜止時(shí)的姿態(tài)檢測.系統(tǒng)由于在對(duì)AHRS進(jìn)行初始化時(shí)用到磁強(qiáng)計(jì)的測量值，很容易受到來自環(huán)境和平臺(tái)的磁干擾，獲取準(zhǔn)確的角度測量值頗具挑戰(zhàn)性.所以研究更加有效的動(dòng)態(tài)修正算法，減小角度測量誤差，提高定位精度是很有必要的.

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

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

[3] 杜繼永，黃國榮，張鳳鳴，等.基于低成本 MEMS 器件的捷聯(lián)航姿系統(tǒng)設(shè)計(jì)[J].傳感技術(shù)學(xué)報(bào)，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] 劉智平，譚芳.國外航姿器的技術(shù)現(xiàn)狀分析[J].四川兵工學(xué)報(bào)，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 產(chǎn)品說明書[EB/OL].(2011-11-16)[2013-09-11].http://wenku.baidu.com/view/d7f8dd080740be1e640e9a01.html.

[8] Honeywell公司.HMC5883L中文數(shù)據(jù)手冊[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

沈陽市科技攻關(guān)項(xiàng)目(F11-009-2-00)

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

2095-2198(2015)01-0054-05

10.3969/j.issn.2095-2198.2015.01.012

TP212.9

A