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

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

中圖分類號(hào)：TP274.4

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

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

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

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

（1）

（2）

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

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

（3）

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

1.2 磁場(chǎng)補(bǔ)償校正原理

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

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

圖1

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

（4）

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

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

（5）

（6）

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

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

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

2.1 傳感器

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

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

2.2 信息處理

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

2.3 通信接口

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

3 實(shí)驗(yàn)結(jié)果及分析

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

表1

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

參考文獻(xiàn)：

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

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

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

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

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