9軸電子羅盤工作原理，校準(zhǔn)與應(yīng)用

電子羅盤，也叫數(shù)字指南針，是利用地磁場(chǎng)來(lái)定北極的一種方法，作為導(dǎo)航儀器或姿態(tài)傳感器已被廣泛應(yīng)用。古代稱為羅經(jīng)，現(xiàn)代利用先進(jìn)加工工藝生產(chǎn)的磁阻傳感器為羅盤的數(shù)字化提供了有力的幫助。現(xiàn)在一般由用磁阻傳感器或磁通門等芯片加工而成的電子羅盤。

可應(yīng)用在水平孔和垂直孔測(cè)量、水下勘探、飛行器導(dǎo)航、科學(xué)研究、教育培訓(xùn)、建筑物定位、設(shè)備維護(hù)、導(dǎo)航系統(tǒng)等領(lǐng)域。

地球的磁場(chǎng)像一個(gè)條形磁體一樣由磁南極指向磁北極。在磁極點(diǎn)處磁場(chǎng)和當(dāng)?shù)氐乃矫娲怪?，在赤道磁?chǎng)和當(dāng)?shù)氐乃矫嫫叫校栽诒卑肭虼艌?chǎng)方向傾斜指向地面。用來(lái)衡量磁感應(yīng)強(qiáng)度大小的單位是Tesla或者Gauss（1Tesla=10000Gauss）。

隨著地理位置的不同，通常地磁場(chǎng)的強(qiáng)度是0.4-0.6 Gauss。需要注意的是，磁北極和地理上的北極并不重合，通常他們之間有11度左右的夾角。

因?yàn)榈卮艌?chǎng)是一個(gè)矢量，對(duì)于一個(gè)固定的地點(diǎn)來(lái)說(shuō)，這個(gè)矢量可以被分解為兩個(gè)與當(dāng)?shù)厮矫嫫叫械姆至亢鸵粋€(gè)與當(dāng)?shù)厮矫娲怪钡姆至?。如果保持電子羅盤和當(dāng)?shù)氐乃矫嫫叫校敲戳_盤中磁力計(jì)的三個(gè)軸就和這三個(gè)分量對(duì)應(yīng)起來(lái)。

實(shí)際上對(duì)水平方向的兩個(gè)分量來(lái)說(shuō)，他們的矢量和總是指向磁北的。羅盤中的航向角（Azimuth）就是當(dāng)前方向和磁北的夾角。由于羅盤保持水平，如果加入地球磁偏角，只需要用磁力計(jì)水平方向兩軸（通常為X軸和Y軸）的檢測(cè)數(shù)據(jù)就可以計(jì)算出航向角。當(dāng)羅盤水平旋轉(zhuǎn)的時(shí)候，航向角在0°- 360°之間變化。

農(nóng)業(yè)機(jī)械，車載裝備，航天航空， 水下勘探，工程機(jī)械，地質(zhì)監(jiān)測(cè)等等需要測(cè)量方向方位的應(yīng)用。

羅盤硬件的主要構(gòu)成分為五部分，一是磁力計(jì)模塊（測(cè)量羅盤周圍的磁場(chǎng)），二是加速度計(jì)模塊（測(cè)量羅盤的加速度），三是陀螺儀模塊（測(cè)量羅盤的角速度），四是MCU模塊（接收信號(hào)后進(jìn)行角度計(jì)算，坐標(biāo)轉(zhuǎn)換，系統(tǒng)誤差補(bǔ)償?shù)龋瑥亩贸隽_盤的姿態(tài)參數(shù)，并將數(shù)據(jù)輸出到上位機(jī)），五是串口轉(zhuǎn)換模塊（可將信號(hào)轉(zhuǎn)換成RS-232接口模式與電腦通訊）。以下對(duì)部分硬件及原理進(jìn)行介紹。

1）磁力計(jì)

由于地磁場(chǎng)是矢量，在某一地點(diǎn)時(shí)，這個(gè)矢量可以被分解為兩個(gè)與當(dāng)?shù)厮矫嫫叫械姆至亢鸵粋€(gè)與當(dāng)?shù)厮矫娲怪钡姆至?。那么如果保持羅盤模塊和當(dāng)?shù)氐乃矫嫫叫衅渲械拇帕τ?jì)的三個(gè)軸就可以與這三個(gè)分量相對(duì)應(yīng)。

目前是通過(guò)傾角補(bǔ)償來(lái)實(shí)現(xiàn)對(duì)模塊與水平面平行，進(jìn)而通過(guò)補(bǔ)償后數(shù)據(jù)進(jìn)行航向角計(jì)算。

2）加速度計(jì)和陀螺儀

加速度： 加速度可以通過(guò)三軸數(shù)據(jù)求出姿態(tài)角，雖然靜態(tài)穩(wěn)定性有優(yōu)勢(shì)，但是動(dòng)態(tài)效果就比較差；

陀螺儀：陀螺儀可以通過(guò)角速度積分求出姿態(tài)角，雖然動(dòng)態(tài)響應(yīng)有優(yōu)勢(shì)，但是靜態(tài)穩(wěn)定性差。

所以基于卡爾曼濾波對(duì)加速度、陀螺儀進(jìn)行融合計(jì)算，得出最優(yōu)估計(jì)姿態(tài)角對(duì)傾角進(jìn)行補(bǔ)償。同時(shí)因?yàn)椴捎猛勇輧x和加速度進(jìn)行融合，所以可實(shí)現(xiàn)動(dòng)態(tài)和靜態(tài)角度的測(cè)量，從而實(shí)現(xiàn)羅盤可以滿足動(dòng)態(tài)和靜態(tài)情況下的使用。

3）卡爾曼濾波

卡爾曼濾波（Kalman filtering）是一種利用線性系統(tǒng)狀態(tài)方程，通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。由于觀測(cè)數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計(jì)也可看作是濾波過(guò)程。

在卡爾曼濾波中假設(shè)：

a 其狀態(tài)轉(zhuǎn)移是線性的，因此我們可以直接用矩陣F表示其線性特征。

b 其狀態(tài)和觀測(cè)都是高斯分布（實(shí)際生活中一大部分都是高斯分布的，并且高斯分布計(jì)算相對(duì)簡(jiǎn)單）。

由于假設(shè)b，在多維高斯分布概率密度函數(shù)中，最為重要的兩個(gè)量分別為均值和協(xié)方差矩陣 。

因此卡爾曼濾波本質(zhì)就是將預(yù)測(cè)方程和觀測(cè)方程的高斯分布的概率密度融合得到新的高斯分布的概率密度函數(shù)作為最優(yōu)估計(jì)，并不斷迭代。

總結(jié)出以下幾點(diǎn)：

① 卡爾曼濾波是一個(gè)算法，它適用于線性、離散和有限維系統(tǒng)。每一個(gè)有外部變量的自回歸移動(dòng)平均系統(tǒng)(ARMAX)或可用有理傳遞函數(shù)表示的系統(tǒng)都可以轉(zhuǎn)換成用狀態(tài)空間表示的系統(tǒng)，從而能用卡爾曼濾波進(jìn)行計(jì)算。

② 任何一組觀測(cè)數(shù)據(jù)都無(wú)助于消除x(t)的確定性。增益K(t)也同樣地與觀測(cè)數(shù)據(jù)無(wú)關(guān)。

③ 當(dāng)觀測(cè)數(shù)據(jù)和狀態(tài)聯(lián)合服從高斯分布時(shí)用卡爾曼遞歸公式計(jì)算得到的是高斯隨機(jī)變量的條件均值和條件方差，從而卡爾曼濾波公式給出了計(jì)算狀態(tài)的條件概率密度的更新過(guò)程線性最小方差估計(jì)，也就是最小方差估計(jì)。

卡爾曼濾波的一個(gè)典型實(shí)例是從一組有限的，對(duì)物體位置的，包含噪聲的觀察序列中預(yù)測(cè)出物體的坐標(biāo)位置及速度。在很多工程應(yīng)用(雷達(dá)、計(jì)算機(jī)視覺(jué))中都可以找到它的身影。同時(shí)，卡爾曼濾波也是控制理論以及控制系統(tǒng)工程中的一個(gè)重要話題。

比如，在雷達(dá)中，人們感興趣的是跟蹤目標(biāo)，但目標(biāo)的位置、速度、加速度的測(cè)量值往往在任何時(shí)候都有噪聲。卡爾曼濾波利用目標(biāo)的動(dòng)態(tài)信息,設(shè)法去掉噪聲的影響，得到一個(gè)關(guān)于目標(biāo)位置的好的估計(jì)。這個(gè)估計(jì)可以是對(duì)當(dāng)前目標(biāo)位置的估計(jì)(濾波)，也可以是對(duì)于將來(lái)位置的估計(jì)(預(yù)測(cè))，也可以是對(duì)過(guò)去位置的估計(jì)(插值或平滑)。

4）傾斜補(bǔ)償及航偏角計(jì)算

電子羅盤一般通電后在水平面上就可以正常使用。但是更多的時(shí)候并不是保持水平的，通常它和水平面都有一個(gè)夾角。這個(gè)夾角會(huì)影響航向角的精度，需要通過(guò)加速度傳感器進(jìn)行傾斜補(bǔ)償。具體算法講解我們放到校準(zhǔn)方法部分。

功能邏輯描述

1）電子羅盤通上電，內(nèi)部就開(kāi)始運(yùn)行。先是內(nèi)部的磁力計(jì),加速度計(jì)，陀螺儀等芯片開(kāi)始采集數(shù)據(jù)，同時(shí)讀取校準(zhǔn)保存在掉電保存區(qū)的補(bǔ)償值，將補(bǔ)償值添加入采集的原始數(shù)據(jù)中實(shí)現(xiàn)補(bǔ)償修正功能。

2）補(bǔ)償后的數(shù)據(jù)使用頻域或時(shí)域?yàn)V波，濾除雜質(zhì)數(shù)據(jù)。

3）通過(guò)卡爾曼濾波融合算法得roll,pitch,yaw數(shù)值。

4）子羅盤開(kāi)始360°平面校準(zhǔn)，把校準(zhǔn)后的值再串口通訊給上位機(jī)。

羅盤硬件框架圖：

羅盤軟件框架圖：

電子羅盤主要是通過(guò)感知地球磁場(chǎng)的存在來(lái)計(jì)算磁北極的方向。然而由于地球磁場(chǎng)在一般情況下只有微弱的0.5高斯，而一個(gè)普通的手機(jī)喇叭當(dāng)相距2厘米時(shí)仍會(huì)有大約4高斯的磁場(chǎng)，一個(gè)手機(jī)在相距2厘米時(shí)會(huì)大約6高斯的磁場(chǎng)，這一特點(diǎn)使得電子羅盤測(cè)量表面地球磁場(chǎng)時(shí)很容易受到電子設(shè)備本身的干擾。磁場(chǎng)干擾是指由于具有磁性物質(zhì)或者可以影響局部磁場(chǎng)強(qiáng)度的物質(zhì)存在，使得磁傳感器所放置位置上地球磁場(chǎng)發(fā)生了偏差。

如圖所示，在磁傳感器的XYZ坐標(biāo)系中，綠色的圓表示地球磁場(chǎng)矢量繞Z軸圓周轉(zhuǎn)到過(guò)程中在XY平面內(nèi)的投影軌跡，再?zèng)]有外界任何磁場(chǎng)干擾的情況下，此軌跡將會(huì)是一個(gè)標(biāo)準(zhǔn)的以O(shè)(0,0)為中心的圓。當(dāng)存在外界磁干擾的情況時(shí)，測(cè)量得到的磁場(chǎng)強(qiáng)度矢量α將為該點(diǎn)地球磁場(chǎng)β與干擾磁場(chǎng)γ的矢量和。記作：

校準(zhǔn)方法類型

1）平面校準(zhǔn)法

針對(duì)XY軸的校準(zhǔn)，將電子羅盤在XY平面內(nèi)自轉(zhuǎn)，等價(jià)于將地球磁場(chǎng)矢量繞著過(guò)點(diǎn)O(γx,γy)垂直與XY平面的法線旋轉(zhuǎn)，而紅色的圓為磁場(chǎng)矢量在旋轉(zhuǎn)過(guò)程中在XY平面內(nèi)投影的軌跡。這可以找到圓心的位置為（（Xman+Xmin）/2,(Ymax+Ymin)/2）同樣將設(shè)備在XY平面內(nèi)旋轉(zhuǎn)可以得到地球磁場(chǎng)在XY平面上的軌跡圓，這可以求出三維空間中的磁場(chǎng)干擾矢量γ(γx,γy,γz).

這也是最常用的一種方法，快捷簡(jiǎn)便。

2）橢球擬合校準(zhǔn)方法

對(duì)于給定平面上的一組樣本點(diǎn)，尋找一個(gè)橢圓，使其盡可能靠近這些樣本點(diǎn)。也就是說(shuō)到，將圖像中的一組數(shù)據(jù)以橢圓方程為模型進(jìn)行擬合，使某一橢圓方程盡量滿足這些數(shù)據(jù)，并求出該橢圓方程的各個(gè)參數(shù)。最后確定的最佳橢圓的中心即是我們要確定的靶心。

這是另一種可選校準(zhǔn)方法，除此之外其實(shí)還有好多種校準(zhǔn)方法，但是因?yàn)椴┲髦橇τ邢蓿ㄆ鋵?shí)就是太懶了哈哈）這里就不多列舉了，有興趣的朋友可以去自行搜索學(xué)習(xí)。

以下詳細(xì)介紹平面校準(zhǔn)法是如何校準(zhǔn)補(bǔ)償?shù)摹?/p>

平面校準(zhǔn)法

如果磁力計(jì)在含有附加的局部磁場(chǎng)的環(huán)境中進(jìn)行操作，磁力計(jì)的輸出做附加的修正將是必要的。 在沒(méi)有任何本地磁場(chǎng)的影響下，可以通過(guò)旋轉(zhuǎn)設(shè)備360°產(chǎn)生的平面 。

使用方法：修正的輸出可以根據(jù)下面的方法來(lái)計(jì)算：

1） 在磁場(chǎng)干擾的條件下進(jìn)行， 數(shù)據(jù)收集設(shè)備被旋轉(zhuǎn)360°。

2）數(shù)據(jù)進(jìn)行分析，以產(chǎn)生偏差的偏移和靈敏度的比例因子，以補(bǔ)償所述干擾。

舉個(gè)例子：

從數(shù)據(jù)中發(fā)現(xiàn)的X和Y磁強(qiáng)計(jì)的最大輸出：

X min = -0.284gauss X max = +0.402gauss

Ymin = -0.322gauss Ymax = +0.246gauss

從中可以看出X軸的數(shù)據(jù)，X具有更大的反應(yīng)，我們?cè)O(shè)置其比例系數(shù)為1

X s = 1

再計(jì)算其他比例系數(shù)：
（ X max - X min ）
Y s = ————————
（ Y max - Y min ）

對(duì)于偏置補(bǔ)償：

X b = X s[1/2( X max - X min ) - X max ]

Y b = Y s[1/2( Y max - Y min ) - Y max ]

正確的輸出： X out = X in*X s + X b Y out = Y in*Y s + Y b

好了，電子羅盤的介紹就到這里了，博主能力有限，有不夠詳細(xì)或者說(shuō)的不對(duì)的地方請(qǐng)多多包涵。