諸昊罡 吳雨亮 張燕

（南京大學(xué)金陵學(xué)院，南京，210089）

九軸傳感器數(shù)據(jù)誤差糾正系統(tǒng)的設(shè)計(jì)

諸昊罡 吳雨亮 張燕

（南京大學(xué)金陵學(xué)院，南京，210089）

在已設(shè)計(jì)的三軸數(shù)字陀螺儀、三軸加速度計(jì)、三軸電子羅盤與ATmega128相結(jié)合的姿態(tài)測(cè)量系統(tǒng)基礎(chǔ)上，本文設(shè)計(jì)了一款可獲取機(jī)器人姿態(tài)信息的上位機(jī)軟件。由于電子羅盤的角度信息受周圍磁場(chǎng)干擾比較嚴(yán)重，因此本文使用橢圓假設(shè)法對(duì)電子羅盤的角度信息進(jìn)行糾正，并給出實(shí)驗(yàn)測(cè)試結(jié)果，驗(yàn)證了角度糾正的正確性。

機(jī)器人，姿態(tài)測(cè)量，傳感器，橢圓假設(shè)法，誤差補(bǔ)償

0 引言

近年，移動(dòng)機(jī)器人的研究及相關(guān)技術(shù)應(yīng)用得到了前所未有的發(fā)展，目前關(guān)鍵技術(shù)主要包括三種，分別是避障、導(dǎo)航和路徑規(guī)劃，其中，對(duì)未知環(huán)境中特別是一些動(dòng)態(tài)環(huán)境下的環(huán)境辨識(shí)和自身的定位是當(dāng)前亟待解決的一個(gè)問題。定位技術(shù)能夠獲取移動(dòng)機(jī)器人在任何時(shí)刻的相對(duì)位置，是實(shí)現(xiàn)導(dǎo)航和保證機(jī)器人自主導(dǎo)航的關(guān)鍵[1]。

在移動(dòng)機(jī)器人定位技術(shù)中，角度信息的獲取尤為重要。目前常用來獲取角度信息的傳感器是陀螺儀以及電子羅盤。陀螺儀通過對(duì)角速度積分獲取角度，存在的問題是易產(chǎn)生零漂；電子羅盤一般情況下獲取角度信息比較準(zhǔn)確，但是如果周圍有磁場(chǎng)干擾，便會(huì)出現(xiàn)大的角度偏移，產(chǎn)生誤差。本文針對(duì)電子羅盤的角度如何進(jì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組成。

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

2 數(shù)據(jù)獲取軟件

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

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

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

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

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

單片機(jī)傳輸?shù)缴衔粰C(jī)的數(shù)據(jù)設(shè)置格式是：

＄羅盤角度，陀螺儀x軸角速度，y軸角速度，z軸角速度，加速度計(jì)x軸加速度， y軸加速度， z軸加速度。

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

3 電子羅盤數(shù)據(jù)誤差糾正

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

3.1橢圓假設(shè)法

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

圖4 磁方向角的定義

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

ψ可以由公式ψ= - arctan()求出。

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

當(dāng)角度從0°-360°變化一周時(shí)，和合成向量的頂點(diǎn)在平面上的軌跡是個(gè)圓，由于誤差的影響，變成了

，變成了，而和合成向量的頂點(diǎn)在平面上的軌跡不再是圓。為了確定該軌跡，我們提出橢圓假設(shè)。假設(shè)和合成向量的頂點(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ù)和，其對(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,3,4,5)。根據(jù)（5）式可列出方程組

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

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

4 實(shí)驗(yàn)結(jié)果

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

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

4.1橢圓假設(shè)法實(shí)驗(yàn)結(jié)果

點(diǎn)擊上位機(jī)串口接收界面中的角度處理（橢圓羅差法）可以彈出如圖6所示界面。

圖6 橢圓假設(shè)法顯示界面

該界面可以選擇數(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)前界面回到主界面。

圖7中，補(bǔ)償前電子羅盤的數(shù)據(jù)為紅色曲線所示，補(bǔ)償后電子羅盤的數(shù)據(jù)如藍(lán)色曲線所示，從曲線可以看出：補(bǔ)償后電子羅盤的磁場(chǎng)信息趨向于圓，比補(bǔ)償前數(shù)據(jù)有所改進(jìn)。

圖7 橢圓羅差法糾正數(shù)據(jù)對(duì)比圖

5 結(jié)束語

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

[1]張燕,夏慶鋒,謝鵬飛. 基于多傳感器的姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)[J].機(jī)器人技術(shù)與應(yīng)用，2003（3）：30-33.

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

[3] Smith R, Frost A, Probert P. A Sensor System for the Navigation of an Underwater Vehicle [J]. The International Journal of Robot iics Research , 1999 , 18（7） : 697- 710.

[4]張愛軍, 王昌明, 趙輝. 三軸電子羅盤的磁航向角誤差補(bǔ)償研究 [J] . 傳感器與微系統(tǒng), 2008 , 27（ 8） : 33-35.

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

[6]劉詩斌,馮曉毅,李宏. 基于橢圓假設(shè)的電子羅盤誤差補(bǔ)償方法 [J].傳感器技術(shù), 2002,21（10）:28-30.

本項(xiàng)目是江蘇省高等學(xué)校大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃項(xiàng)目，項(xiàng)目編號(hào)：201313646006Y。