基于磁阻傳感器和加速計的三自由度姿態(tài)解算模塊設(shè)計

摘 要：本文設(shè)計了基于HMC5883L三軸磁阻傳感器和ADXL345三軸加速度計以及C8051F340的三自由度姿態(tài)解算模塊，它具備通過串口實時輸出模塊的航向角、俯仰角、橫滾角度信息的功能，并且提供了磁場補償校正功能和磁偏角手動修改、根據(jù)GPS信息自動修改功能。文中介紹了給出了硬件設(shè)計框圖、姿態(tài)解算方法和程序流程圖，提出了一種磁場補償校正算法。經(jīng)重復(fù)性和分布均勻性試驗，其航向角精度誤差不超過2°。

關(guān)鍵詞：三自由度；姿態(tài)結(jié)算；磁場補償校正；磁偏角

中圖分類號：TP274.4

1 三自由度姿態(tài)解算和校正原理

1.1 姿態(tài)解算原理

地球的磁場強度大約為0.5至0.6高斯，平行于地球表面分布，始終指向磁北[1]。模塊姿態(tài)信息包括航向角、俯仰角和橫滾角，三者可以準(zhǔn)確的標(biāo)記出模塊自身坐標(biāo)系與地理坐標(biāo)系的相對關(guān)系，進而表示出當(dāng)前的狀態(tài)[2]。

三軸加速度計可以通過測量重力加速度在三個軸向上的分量值，對測量結(jié)果進行計算可以得到俯仰角度、橫滾角度。俯仰角φ，橫滾角θ，重力加速度在三個軸上的投影分別為：Gx、Gy、Gz，則俯仰角和橫滾角可以通過公式1、2求出。

（1）

（2）

為求出航向角η要得到目標(biāo)坐標(biāo)X，Y軸上磁場分量在地理坐標(biāo)XY平面的投影Dx、Dy，再通過公式即可計算出航向角。

模塊坐標(biāo)和地理坐標(biāo)的轉(zhuǎn)換矩陣為包含俯仰角φ和橫滾角θ信息，其中Mdx、Mdy、Mdz為磁阻傳感器的值，Mhx、Mhy、Mhz為轉(zhuǎn)換到地理坐標(biāo)的值，公式為：

（3）

將由公式1和2得到的俯仰角、橫滾角與磁阻傳感器三個軸向上的值一并帶入公式3，即可將地磁場重新轉(zhuǎn)換到水平面上來，再將Mhx、Mhy賦值給Dx、Dy，由公式可求出航向角。

1.2 磁場補償校正原理

磁場的干擾來自周圍環(huán)境中的鐵質(zhì)金屬以及永磁性材料，如何有效的補償?shù)敉饨绺蓴_成為決定姿態(tài)解算模塊性能的關(guān)鍵。下文給出了為本模塊所設(shè)計的一種磁場補償校正算法的原理。

當(dāng)周圍沒有磁干擾時，模塊在水平面勻速旋轉(zhuǎn)會在以Mhx和Mhy為橫縱坐標(biāo)的平面上會得到一些點，整體上接近圓形。而當(dāng)周圍存在干擾時會得到左下圖1所示的偏離坐標(biāo)中心的近似橢圓形。

圖1

將橢圓整形為坐標(biāo)在原點的圓即可達(dá)到磁場補償?shù)哪康模虼?，需要完成坐?biāo)原點平移和橢圓映射為圓形兩步。σ為投影面與坐標(biāo)面得夾角，則當(dāng)cos（σ）=b/a，當(dāng)b

（4）

其中，b=（yMAX-yMIN）/2，a=（xMAX-xMIN）/2

橢圓的坐標(biāo)原點值為o′，則公式5給出了其坐標(biāo)值，公式6給出平移后結(jié)果。

（5）

（6）

將b和a的值帶入到公式6中即可求出新的值。

2 模塊的硬件設(shè)計

三自由度姿態(tài)解算模塊的硬件組成分為傳感器、信息處理、通信三部分。

2.1 傳感器

HMC5883L是霍尼韋爾公司出品的一種表貼型三軸磁阻傳感器模塊（尺寸僅為3.0×3.0mm），內(nèi)部集成了放大器、自動消磁驅(qū)動器、ADC轉(zhuǎn)換模塊，并帶有I2C通信接口。

ADXL345是一款超低功耗3軸加速度計，分辨率為13位，測量范圍達(dá)±16g。數(shù)字輸出數(shù)據(jù)為16位二進制補碼格式，可通過SPI（3線或4線）或I2C數(shù)字接口訪問。

2.2 信息處理

主控制芯片采用Siliocn Labs公司的C8051F340。

2.3 通信接口

模塊的通信接口包括數(shù)據(jù)傳輸接口和軟件升級接口，數(shù)據(jù)傳輸接口采用UART0硬件串口輸出姿態(tài)信息，提供設(shè)置磁偏角、波特率、安裝方向、上位機校正等功能，UART1提供軟件升級功能。

3 實驗結(jié)果及分析

將模塊置于碳素轉(zhuǎn)臺上，采用按鍵或串口方式對模塊進行磁場補償校正。校正結(jié)束后按如下方式驗證。每間隔45°采集一個輸出數(shù)據(jù)，來檢驗?zāi)K的分布均勻性，旋轉(zhuǎn)一周共8個數(shù)據(jù)。在重復(fù)10次，比較每個位置的變化情況，以測試其重復(fù)性，數(shù)據(jù)如下表1。

表1

從表1中的實驗數(shù)據(jù)可以看出模塊具有很好的分布均勻性和重復(fù)性，精度在2°以內(nèi)。

參考文獻：

[1]Grant George A.和Klinkert John 船用羅盤 2d ed，1970.

[2]劉越，王涌天，胡曉明.測量運動物體姿態(tài)的三自由度定位算法的研究[J].計算機測量與控制，2002.

[3]潘琢金，孫德龍，夏秀峰.C8051F單片機應(yīng)用解析[M].北航出版社，2002，10.

作者簡介：郭秉剛（1985-），男，遼寧沈陽人，助理工程師，碩士研究生，研究方向：信號與信息處理。

作者單位：華中光電技術(shù)研究所-武漢光電國家實驗室，武漢 430223