HMC5883L電子羅盤的誤差補(bǔ)償系統(tǒng)設(shè)計(jì)※

張燕，陳華

（1.南京大學(xué)金陵學(xué)院，南京210089；2.南京五十中）

引 言

近年來(lái)，移動(dòng)機(jī)器人的研究及相關(guān)技術(shù)應(yīng)用得到了前所未有的發(fā)展。其中移動(dòng)機(jī)器人研究的關(guān)鍵技術(shù)主要有三種，分別是避障、導(dǎo)航以及路徑規(guī)劃。

對(duì)未知環(huán)境中特別是一些動(dòng)態(tài)環(huán)境下的環(huán)境辨識(shí)和自身定位是當(dāng)前亟待解決的一個(gè)問(wèn)題。定位技術(shù)能夠獲得在任何時(shí)刻移動(dòng)機(jī)器人的相對(duì)位置，因此是實(shí)現(xiàn)導(dǎo)航和保證機(jī)器人自主導(dǎo)航的關(guān)鍵［1］，而移動(dòng)機(jī)器人定位技術(shù)中角度信息的獲取就非常重要。目前，常用來(lái)獲取角度信息的傳感器是電子羅盤以及陀螺儀，陀螺儀通過(guò)對(duì)角速度積分獲取角度，存在的問(wèn)題是容易產(chǎn)生零漂。電子羅盤一般情況下獲取角度信息比較準(zhǔn)確，但是遇到周圍有磁場(chǎng)干擾的情況就會(huì)出現(xiàn)大的角度偏移產(chǎn)生誤差。本文針對(duì)電子羅盤的角度如何糾正的問(wèn)題進(jìn)行了研究。

論文研究的平臺(tái)是由陀螺儀L3G4200D、三軸MEMS加速度計(jì)ADX345、三軸電子羅盤HMC5883以及單片機(jī)ATmega128組成一個(gè)嵌入式姿態(tài)測(cè)量系統(tǒng)。在此系統(tǒng)獲取的數(shù)據(jù)基礎(chǔ)上編寫了上位機(jī)軟件，實(shí)現(xiàn)了各個(gè)傳感器數(shù)據(jù)的分離，并對(duì)電子羅盤獲取的角度信息如何糾正著重進(jìn)行了研究。

1 系統(tǒng)結(jié)構(gòu)

本系統(tǒng)主要由三軸數(shù)字陀螺儀L3G4200D、三軸MEMS加速度計(jì)ADX345、三軸電子羅盤HMC5883以及單片機(jī)ATmega128組成。嵌入式角度測(cè)量系統(tǒng)結(jié)構(gòu)框圖如圖1所示。

圖1 嵌入式角度測(cè)量系統(tǒng)結(jié)構(gòu)框圖

1 數(shù)據(jù)獲取軟件的設(shè)計(jì)

1.1 串口通信軟件設(shè)計(jì)

姿態(tài)測(cè)量系統(tǒng)的陀螺儀、加速度計(jì)、電子羅盤數(shù)據(jù)通過(guò)I2C總線與ATmega128進(jìn)行通信，下位機(jī)ATmega128通過(guò)串口將數(shù)據(jù)上傳到上位機(jī)上。上位機(jī)采用LabWindows CVI軟件編寫代碼實(shí)時(shí)讀取串口數(shù)據(jù)且顯示在界面上。顯示界面如圖2所示。

圖2 上位機(jī)顯示傳感器數(shù)據(jù)

圖2顯示的數(shù)據(jù)是字符格式，下一步的工作便是編寫代碼將各個(gè)部分的數(shù)據(jù)分離出來(lái)。

1.2 數(shù)據(jù)分離代碼的編寫

對(duì)于單片機(jī)傳輸?shù)缴衔粰C(jī)的數(shù)據(jù)設(shè)置的格式是：S羅盤角度、陀螺儀x軸角速度、y軸角度、z軸角速度、加速度計(jì)x軸加速度、y軸加速度、z軸加速度。

數(shù)據(jù)分離的流程圖見(jiàn)圖3。打開(kāi)文件后，首先判斷獲取的字符是否是＄，如果不是則繼續(xù)獲取下一個(gè)字符，直到獲取＄符號(hào)，接下來(lái)讀取＄和“；”之間的數(shù)據(jù)，存儲(chǔ)到字符數(shù)組中。接著讀取各個(gè)“；”之間的數(shù)據(jù)存儲(chǔ)到存儲(chǔ)傳感器各個(gè)數(shù)據(jù)的字符數(shù)組，直到讀取的字符是回車符，重新讀取第二輪數(shù)據(jù)存儲(chǔ)到各個(gè)字符數(shù)組中，當(dāng)讀取的字符為文件結(jié)束符時(shí)結(jié)束文本讀取。

圖3 傳感器數(shù)據(jù)分離流程圖

2 電子羅盤數(shù)據(jù)誤差修正

由于電子羅盤讀取的數(shù)據(jù)受到周圍磁場(chǎng)的干擾非常嚴(yán)重，需要對(duì)電子羅盤的數(shù)據(jù)進(jìn)行修正，目前常用的電子羅盤糾正方式有橢圓假設(shè)法和八方向二乘法。

2.1 橢圓假設(shè)法

電子羅盤的誤差主要包括硬鐵干擾和軟鐵干擾。由于地磁場(chǎng)是弱磁場(chǎng)，并且地磁場(chǎng)可以認(rèn)為在一定范圍內(nèi)是均勻的。硬鐵干擾產(chǎn)生于永久磁鐵或被磁化的金屬，這種干擾與羅盤的相對(duì)位置固定，相當(dāng)于在羅盤的輸出值上增加了一個(gè)定值，使羅盤輸出曲線圖的圓心偏移了。軟鐵本身沒(méi)有磁性，它被地磁場(chǎng)磁化后獲得磁性，軟鐵干擾產(chǎn)生于地球磁場(chǎng)和羅盤附近的任何磁性材料之間的相互作用，其干擾程度與羅盤的方向有關(guān)，使羅盤的輸出曲線畸變成一橢圓。［2－5］

4 磁方向角的定義

設(shè)電子羅盤有兩個(gè)相互垂直的軸，分別稱為OX 軸和OY 軸，如圖4所示。Ψ 表示電子羅盤測(cè)得的磁方向角，定義為從ON 到OX 順時(shí)針轉(zhuǎn)過(guò)的角度。設(shè)地磁場(chǎng)的水平分量為H0，則磁場(chǎng)分量為Hx＝H0cosψ （1）Hy＝－H0sinψ （2）

ψ可以由ψ＝－arctan（C＝（Hy／Hx）求出。

當(dāng)電子羅盤有誤差時(shí)，就會(huì)反映在Hx和Hy的變化上。設(shè)有誤差時(shí)Hx、Hy變?yōu)镠x1、Hy1，它們的關(guān)系可以寫成：

當(dāng)角度從0°～360°變化一周時(shí)，Hx和Hy合成向量的頂點(diǎn)在平面上的軌跡是個(gè)圓，由于誤差的影響，Hx變成了Hx1，Hy變成了Hy1，而Hx1和Hy1合成向量的頂點(diǎn)在平面上的軌跡不再是圓。為了確定該軌跡，我們提出橢圓假設(shè)，假設(shè)Hx1和Hy1合成向量的頂點(diǎn)在平面上的軌跡是個(gè)橢圓，則式（3）改寫為：

即假設(shè)對(duì)角陣K、正交陣φs和對(duì)稱陣φp的乘積等于M，首先正交陣將圓轉(zhuǎn)了一個(gè)角度φs，對(duì)稱陣使圓變成一個(gè)橢圓，對(duì)角陣將橢圓轉(zhuǎn)了一個(gè)角度φs，N 的作用是改變橢圓的中心位置。我們根據(jù)橢圓假設(shè)在某方向?qū)﹄娮恿_盤采樣一組數(shù)據(jù)Hx1和Hy1，其對(duì)應(yīng)橢圓上的一點(diǎn)，則橢圓方程可以表示成：

式（5）中有5個(gè)未知數(shù)，讓電子羅盤在5個(gè)方向上采樣數(shù)據(jù)，我們要求5個(gè)方向?yàn)榫鶆蚍较颍?°～360°采集0°、72°、144°、216°和288°這5個(gè)方向的數(shù)據(jù)。設(shè)采樣數(shù)據(jù)為Hx1i和Hy1i（i＝1，2，…，5），根據(jù)式（5）可列出方程組

其中：

只要采樣方向在0°～360°內(nèi)大致均勻分布，方程組（6）就有唯一解A、B、C、D、E。它們與式（4）中各系數(shù)之間的關(guān)系為［6］：

其中，Δ由采樣數(shù)據(jù)求出，且

只要求出φp、Kx、Ky、bx、by，就可以把橢圓還原成圓。再根據(jù)式（4）求得補(bǔ)償校正之后X 軸和Y 軸真實(shí)的磁場(chǎng)強(qiáng)度，從而求得準(zhǔn)確的航向角。

2.2 八方向二乘法

最小二乘法是一種數(shù)據(jù)擬合建模法［7］，通常是用最小二乘法原理［8］及用最小化誤差的平方和找到一組數(shù)據(jù)的最佳函數(shù)匹配，求出未知函數(shù)的最可信賴值作為測(cè)量結(jié)果。

基于八方向二乘法的電子羅盤誤差補(bǔ)償原理和實(shí)現(xiàn)：羅差是地磁場(chǎng)測(cè)量值時(shí)羅盤所固有的誤差，其對(duì)測(cè)量精度的影響最大，同時(shí)在不同的測(cè)試環(huán)境下其值是變化的。羅差是由外界干擾而產(chǎn)生的航向誤差，可以分為兩大類：硬磁材料引起的羅差和軟磁材料引起的羅差。

硬磁干擾可以認(rèn)為是一種永久磁鐵干擾，認(rèn)為硬磁材料所引起的磁場(chǎng)是均勻分布的，其產(chǎn)生的誤差在航向角0°～360°之間為近似正弦曲線，硬磁羅差△1表示：

式中：φ為補(bǔ)償前羅盤的輸出航向值，B、C 為羅差補(bǔ)償系數(shù)。

軟磁材料的本身是不產(chǎn)生磁場(chǎng)的，但是它會(huì)被周圍的磁場(chǎng)環(huán)境給磁化，從而影響周圍的磁場(chǎng)，它所引起的誤差大小可以分解為兩部分：一是圓周誤差，二是象限羅差，用△2表示：

式中：A、D、E為羅差補(bǔ)償系數(shù)。

綜上所述，△φ表示總羅差，所以

由以上分析可以得出，我們的主要工作是求出誤差補(bǔ)償系數(shù)，即A、B、C、D、E。具體方法如下：因?yàn)橐还残枰?個(gè)值，為了計(jì)算方便，在0°～360°之間取5個(gè)實(shí)驗(yàn)點(diǎn)進(jìn)行誤差測(cè)量即可求出補(bǔ)償系數(shù)，把后面測(cè)得的數(shù)據(jù)帶入公式就可以得到精確的航向角。

記誤差方程：

UC＝H

其中：

根據(jù)矩陣的性質(zhì)，可以求出羅差的補(bǔ)償系數(shù)：

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

本文設(shè)計(jì)了一個(gè)完整的數(shù)據(jù)獲取、存儲(chǔ)以及電子羅盤數(shù)據(jù)糾正軟件系統(tǒng)。運(yùn)行系統(tǒng)首先顯示的界面如圖5所示，接收字符界面可以實(shí)時(shí)顯示接收的數(shù)據(jù)，點(diǎn)擊保存可以將數(shù)據(jù)存儲(chǔ)到文本文件中。下面有兩個(gè)按鍵，分別可以彈出電子羅盤角度進(jìn)行糾正算法處理的兩個(gè)界面。

圖5 上位機(jī)串口接收界面

3.1 橢圓羅差法實(shí)驗(yàn)結(jié)果

點(diǎn)擊圖5中的“角度處理（橢圓羅差法）”按鈕可以彈出如圖6所示的界面。

圖6 橢圓羅差法顯示界面

該界面可以選擇數(shù)據(jù)采集環(huán)境，點(diǎn)擊“補(bǔ)償前”按鈕可以從文本文件中讀取電子羅盤采集的磁場(chǎng)信息，同時(shí)能夠顯示在某一環(huán)境下采集的磁場(chǎng)數(shù)據(jù)繪制的電子羅盤的磁場(chǎng)信息；點(diǎn)擊”補(bǔ)償后”按鈕則可以繪制補(bǔ)償后的磁場(chǎng)信息；點(diǎn)擊“退出”按鈕可以退出當(dāng)前界面，回到主界面。

橢圓羅差法糾正數(shù)據(jù)對(duì)比圖略——編者注。

3.2 八方向二乘法補(bǔ)償數(shù)據(jù)實(shí)驗(yàn)結(jié)果

八方向二乘法數(shù)據(jù)顯示界面略——編者注。點(diǎn)擊“誤差曲線”按鈕可以顯示數(shù)據(jù)處理前和處理后與實(shí)際數(shù)據(jù)的區(qū)別。點(diǎn)擊“退出”按鈕，可以退出當(dāng)前界面、回到主界面。

圖7中黑色粗線是糾正前讀取的數(shù)據(jù)，黑色細(xì)線為電子羅盤糾正后的數(shù)據(jù)。從圖中可以看出，在干擾較小的情況下，電子羅盤讀出來(lái)的數(shù)據(jù)基本與實(shí)際數(shù)據(jù)吻合。

圖7 八方向二乘法數(shù)據(jù)處理結(jié)果

結(jié) 語(yǔ)

本文設(shè)計(jì)了基于多傳感器進(jìn)行姿態(tài)測(cè)量的定位系統(tǒng)的上位機(jī)軟件，可以實(shí)現(xiàn)下位機(jī)通過(guò)串口將傳感器的數(shù)據(jù)傳輸?shù)缴衔粰C(jī)功能，編寫上位機(jī)顯示界面顯示下位機(jī)獲取的數(shù)據(jù)。在此基礎(chǔ)上研究了電子羅盤誤差糾正的兩種方式，并且編寫代碼實(shí)現(xiàn)了兩種糾正算法的代碼，在此基礎(chǔ)上編寫界面顯示糾正算法的處理結(jié)果。實(shí)驗(yàn)結(jié)果顯示，經(jīng)過(guò)電子羅盤算法處理之后的數(shù)據(jù)能夠比較準(zhǔn)確地顯示當(dāng)前的數(shù)據(jù)信息。

編者注：本文為期刊縮略版，全文見(jiàn)本刊網(wǎng)站www.mesnet.com.cn。

［1］張燕，夏慶鋒，謝鵬飛.基于多傳感器的姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)［J］.機(jī)器人技術(shù)，2013（3）.

［2］蔣賢志.數(shù)字電子羅盤誤差分析及較正技術(shù)研究［J］.現(xiàn)代雷達(dá)，2005，27（6）：39－40.

［3］Robert Smith，Andy Frost，Penny Probert.ASensor Sy stem for the Naviga tion of an Under water Vehicle［J］.The Interna tional Journal of Robotiics Research，1999，18（7）：697－710.

［4］張愛(ài)軍，王昌明，趙輝.三軸電子羅盤的磁航向角誤差補(bǔ)償研究［J］.傳感器與微系統(tǒng)，2008，27（8）：3－35.

［5］張靜，金志華，田蔚風(fēng).無(wú)航向基準(zhǔn)時(shí)數(shù)字式磁羅盤的自差校正［J］.上海交通大學(xué)學(xué)報(bào)2004，38（10）：1757－1860.

［6］劉詩(shī)斌、馮曉毅、李宏.基于橢圓假設(shè)的電子羅盤誤差補(bǔ)償方法［J］.傳感器技術(shù)，2002（10）.

［7］馬文.磁羅盤誤差補(bǔ)償及數(shù)據(jù)處理方法研究［D］.上海：上海交通大學(xué)，2006.

［8］張愛(ài)軍，王昌明.趙輝.三軸電子羅盤的磁航向角誤差補(bǔ)償研究［J］.傳感器與微系統(tǒng)，2008（8）.