林亞坤，錢正洪，白　茹，冀　亮

(杭州電子科技大學(xué)磁電子中心，浙江杭州　310018)

0　引言

載體姿態(tài)的準(zhǔn)確測量在航天、機(jī)器人、導(dǎo)航、人體運(yùn)動分析以及機(jī)器交互等方面有著至關(guān)重要的作用[1-2]。姿態(tài)檢測系統(tǒng)能夠?qū)崟r準(zhǔn)確提供載體的航向角、俯仰角和橫滾角等姿態(tài)信息，在航空航天技術(shù)領(lǐng)域，是飛行器中姿態(tài)穩(wěn)定平臺的重要組成部分；在機(jī)器人技術(shù)領(lǐng)域或醫(yī)療設(shè)備領(lǐng)域，可作為動作識別的關(guān)鍵組成部分；在智能手機(jī)等終端設(shè)備，是設(shè)備空間姿態(tài)判斷識別、防抖動技術(shù)、智能應(yīng)用的重要硬件支撐[3]。

目前，MEMS傳感器在消費(fèi)類電子產(chǎn)品中得到廣泛應(yīng)用。GMR傳感器作為新型磁阻傳感器，與AMR傳感器相比，具有體積小、靈敏度高、線性度好，磁場分辨率高、接口電路簡單等優(yōu)點(diǎn)，更適合在姿態(tài)檢測系統(tǒng)中應(yīng)用[4]。

文中設(shè)計(jì)了基于三軸GMR傳感器、三軸陀螺儀和三軸加速度計(jì)的姿態(tài)檢測系統(tǒng)，該系統(tǒng)利用陀螺儀測量載體的角速度信息解算姿態(tài)，通過GMR傳感器和加速度計(jì)測量地磁場方向和重力場方向來修正陀螺儀的漂移誤差，能夠有效解決傳統(tǒng)電子羅盤在運(yùn)動過程中姿態(tài)測量誤差較大的問題。

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

姿態(tài)檢測系統(tǒng)整體設(shè)計(jì)結(jié)構(gòu)如圖1所示，主要包括傳感器及信號調(diào)理電路部分和微控制器及外圍電路部分。系統(tǒng)采集三軸GMR傳感器、三軸陀螺儀和三軸加速度計(jì)的輸出信號，在微控制器內(nèi)進(jìn)行數(shù)據(jù)融合和姿態(tài)解算，通過RS232接口電路將載體的航向角、俯仰角和橫滾角信息輸入電腦，通過3D姿態(tài)顯示上位機(jī)軟件，直觀顯示當(dāng)前載體的姿態(tài)信息。

圖1　姿態(tài)檢測系統(tǒng)整體設(shè)計(jì)框圖

2　基本原理

2．1四元素和歐拉角的關(guān)系

姿態(tài)是表征載體坐標(biāo)系相對地理坐標(biāo)系的角位置信息，歐拉角法和四元素法是兩種常用的描述載體姿態(tài)轉(zhuǎn)動變化關(guān)系的方法。地理坐標(biāo)系的矢量u到載體坐標(biāo)系的矢量v之間的轉(zhuǎn)動變化關(guān)系可以表示為v=Tu[5]，其中T為方向余弦矩陣。

(1)

式中：Ψ、θ、γ分別為航向角、俯仰角和橫滾角。

兩坐標(biāo)系之間的轉(zhuǎn)動變換關(guān)系也可以用四元素表示如下：

(2)

由式(1)和式(2)可得，歐拉角與轉(zhuǎn)動四元素的相互關(guān)系：

(3)

θ=arcsin(2(q1q3-q0q2))

(4)

(5)

2．2電子羅盤姿態(tài)確定方法

當(dāng)載體在地理坐標(biāo)系下處于水平靜止時，加速度計(jì)各軸測量輸出為A=[00g]。當(dāng)載體處于任意姿態(tài)靜止時，假設(shè)加速度計(jì)測量值為B=[axayaz]，則由B=TA可得：

θ=arcsinax

(6)

(7)

在計(jì)算得到θ和γ的情況下，可由GMR的三軸輸出計(jì)算磁航向角[6]：

(8)

3　數(shù)據(jù)融合算法

3．1誤差函數(shù)的推導(dǎo)

在文中介紹的濾波算法中，假設(shè)加速度計(jì)只測重力場矢量，GMR傳感器只測地磁場矢量。為了方便推導(dǎo)，文中將三軸傳感器測量到的三維矢量擴(kuò)展成四維矢量[7-9]。一般的，將地理坐標(biāo)系下的場矢量用Ed表示，該場矢量在載體坐標(biāo)系下三軸傳感器輸出值用Ss表示，q表示轉(zhuǎn)動四元素，則將誤差目標(biāo)函數(shù)的一般形式表示為：

f(q)=q*?Ed?q-Ss

(9)

q=[q0q1q2q3]

(10)

Ed=[0dxdydz]

(11)

Ss=[0sxsysz]

(12)

地理坐標(biāo)系下地磁場矢量為b=[bx0bz]，重力場矢量為g=[001]。載體坐標(biāo)系下GMR傳感器的輸出量m=[mxmymz]，加速度計(jì)的輸出量a=[axayaz]，根據(jù)式(9)～式(12)，可得具體的誤差目標(biāo)函數(shù)為

(13)

(14)

誤差平方和函數(shù)為

(15)

3．2姿態(tài)四元素更新算法

根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)的四元素理論可知：

(16)

(17)

式(17)是姿態(tài)四元素?cái)?shù)據(jù)融合更新方程[10]。其中▽f為誤差平方和函數(shù)的梯度，Δt為兩次采樣的時間間隔。λk為k時刻最優(yōu)收斂步長。

(18)

式中Hk是4×4的對稱矩陣，是誤差目標(biāo)函數(shù)梯度對四元素的偏微分。

姿態(tài)融合算法框圖如圖2所示，算法框圖中上標(biāo)S和E分別表示載體坐標(biāo)系和地理坐標(biāo)系[7]。

圖2　姿態(tài)融合算法框圖

4　測試結(jié)果及分析

圖3是為了驗(yàn)證文中算法而設(shè)計(jì)開發(fā)的姿態(tài)檢測模塊，該模塊主要包含三軸GMR傳感器、三軸陀螺儀、三軸加速度計(jì)和stm32控制器。

圖3　姿態(tài)檢測模塊

圖4是在靜止條件下單獨(dú)使用陀螺儀數(shù)據(jù)解算的姿態(tài)角信息。可以看出，隨著采樣點(diǎn)數(shù)的增加，姿態(tài)角曲線快速漂移，不具有收斂性，和陀螺儀的固有缺陷即存在漂移現(xiàn)象一致。

圖4　靜止時陀螺解算姿態(tài)角

圖5采用文中提及的數(shù)據(jù)融合算法，利用三軸GMR傳感器和三軸加速度計(jì)來修正陀螺儀的漂移?？梢钥闯?，在靜止條件下，采集點(diǎn)數(shù)多，姿態(tài)角曲線依然呈直線，陀螺漂移誤差得到較好的修正。

圖5　靜止時數(shù)據(jù)融合姿態(tài)角

航向角抖動測試方法為保持模塊水平放置，使俯仰角和橫滾角為0°，載體在當(dāng)前位置-120°繞z軸極小范圍快速來回變加速運(yùn)動。

從圖6可以看出，電子羅盤解算出的航向角劇烈波動，最大波動范圍超過±10°，而數(shù)據(jù)融合后的航向角最大波動范圍小于±1°。該系統(tǒng)較傳統(tǒng)電子羅盤在航向角測量方面具有較好的抗抖動干擾的作用。

圖6　航向角抖動測試

圖7和圖8的測試方法為先使載體水平靜止，待到數(shù)據(jù)收斂穩(wěn)定后，圖7為繞y軸小范圍快速來回變加速運(yùn)動，圖8為繞x軸小范圍快速來回變加速運(yùn)動。

圖7　俯仰角抖動測試

圖8　橫滾角抖動測試

測試結(jié)果表明，融合后的姿態(tài)角振幅較小，與實(shí)際的運(yùn)動測試姿態(tài)角的變化情況貼近，說明該系統(tǒng)對外界抖動的抗干擾能力更強(qiáng)，適合船舶在海上航行時指向和測量姿態(tài)時使用，也適合車輛在道路崎嶇顛簸運(yùn)動過程中使用。圖9為姿態(tài)檢測系統(tǒng)上位機(jī)軟件。

圖9　姿態(tài)檢測系統(tǒng)上位機(jī)

5　結(jié)束語

傳統(tǒng)電子羅盤在靜止或勻速運(yùn)動情況下具有較高的使用精度，但是在有外界加速度或是抖動干擾情況下，姿態(tài)測量誤差較大。陀螺儀在測量姿態(tài)方面具有較好的動態(tài)性能，但單獨(dú)使用無法克服漂移現(xiàn)象。

文中的數(shù)據(jù)融合方法在靜態(tài)情況下，可以很好地修正陀螺的漂移誤差，該姿態(tài)檢測系統(tǒng)較傳統(tǒng)電子羅盤在抗抖動干擾方面有較強(qiáng)的優(yōu)勢。

設(shè)計(jì)的姿態(tài)檢測系統(tǒng)上位機(jī)軟件，可以接受載體的姿態(tài)角信息，以3D形式直觀展現(xiàn)載體的當(dāng)前姿態(tài)。該系統(tǒng)可以應(yīng)用于消費(fèi)電子與一般工業(yè)的姿態(tài)檢測與物體穩(wěn)定控制的應(yīng)用中。

參考文獻(xiàn)：

[1]李榮冰，劉建業(yè)，曾慶化．基于 MEMS 技術(shù)的微型慣性導(dǎo)航系統(tǒng)的發(fā)展現(xiàn)狀．中國慣性技術(shù)學(xué)報(bào)，2004，12(6)：88-94．

[2]徐玉．微小型無人直升機(jī)飛控平臺與姿態(tài)融合算法研究：[學(xué)位論文]．杭州：浙江大學(xué)，2008．

[3]張謙，裴海龍，羅沛．基于MEMS器件的姿態(tài)航向參考系統(tǒng)設(shè)計(jì)及應(yīng)用．計(jì)算機(jī)工程與設(shè)計(jì)，2007，28(3)：631-634．

[4]錢正洪，白茹，黃春奎，等．先進(jìn)磁電子材料和器件．儀表技術(shù)與傳感器，2009(增刊1)：96-101．

[5]高宗余，李德勝．多MEMS傳感器姿態(tài)測量系統(tǒng)的研究．電光與控制，2010，17(3)：68-70．

[6]林乾浩，錢正洪，龔天平，等．基于GMR傳感器的三軸電子羅盤．機(jī)電工程，2013，30(1)：56-59．

[7]MADGWICK S O H ，HARRISON A J L，VAIDYANATHAN R．Estimation of IMU and MARG orientation using a gradient descent algorithm．Rehabilitation．Robotics IEEE International Conference，Switzerland，2011．

[8]CASTELLANOS J F G,LESECQ S,MARCHAND N，et al．A low-cost air data attitude heading reference system for the tourism airplane applications．IEEE Sensors，2005:1388-1391．

[9]XIA L L,WANG J G,ZHANG L H，et al．Nonlinear Gaussian filter algorithm enhancements for low-cost integrated navigation system．Mechatronics and Automation，ICMA 2009,International confereme on．IEEE,2009．

[10]劉海穎，王惠南．低成本姿態(tài)測量系統(tǒng)研究．南京理工大學(xué)學(xué)報(bào)(自然科學(xué)版)，2007(2)：233-237．