AHRS姿態(tài)解算說明(加速度++磁力計(jì)原理及原始數(shù)據(jù)分析)

AHRS俗稱參考系統(tǒng)，AHRS由加速度計(jì)，磁場計(jì)，陀螺儀構(gòu)成，AHRS的真正參考來自于地球的重力場和地球的磁場～～他的靜態(tài)終精度取決于對磁場的測量精度和對重力的測量精度 ,而則陀螺決定了他的動(dòng)態(tài)性能。
　　這就是AHRS～在這種前提下。說明AHRS離開了地球這種有重力和磁場環(huán)境的時(shí)候是沒法正常工作的～～
本章旨在講解以下內(nèi)容
1.加速度 
2.陀螺儀
3.磁力計(jì)
  一直想寫篇文章關(guān)于姿態(tài)解算原理的，使用盡量通俗的語句說明如何從加速度計(jì)和陀螺 儀的數(shù)據(jù)，融合得到載體的姿態(tài)角。無奈自己的水平有限，一直擱置。   淡泊以明志,寧靜以致遠(yuǎn).人總是要逼自己做些事，才過得心安理得。那就拿點(diǎn)時(shí)間把這 方面的資料整合一下吧。這篇文章的大部分內(nèi)容都不是本人原創(chuàng)的,感謝網(wǎng)絡(luò)上無私奉獻(xiàn)的 人.
  在此介紹一下實(shí)驗(yàn)的姿態(tài)板 ,新一代的mini AHRS,采用STM32F103單片機(jī)進(jìn)行姿態(tài)解算，板子上集成有
1.MPU6050，三軸的加速度和陀螺儀
2.HMC5883  三軸的磁力計(jì)
3.BMP180   高精度氣壓高度計(jì)
這些傳感器都通過I2C接口連接到主控制器STM32.不需要額外的ADC電路,直接通過數(shù)字接口就可以讀取傳感器的當(dāng)前輸出.

Mini AHRS硬件框圖

 

1 加速度計(jì)
加速度計(jì)顧名思義,就是測量加速度的.那么,我們?nèi)绾握J(rèn)識(shí)這個(gè)加速度呢?在此用一個(gè)盒子形 狀的立方體來做模型，認(rèn)識(shí)加速度,如下,盒子內(nèi)的圖像。

 
如果我們把盒子形狀的立方體 放在一個(gè)沒引力場的地方,球會(huì)保持在盒子的中間.你可以想 象，這個(gè)盒子是在外太空，遠(yuǎn)離任何天體，很難找到這樣的地方,就想象飛船軌道圍繞地球飛，一切都是在失重狀態(tài)下。那么六個(gè)壁面感受到的壓力都是0.
如果我們突然將立方體向左側(cè)移動(dòng)（我們加快加速，1G =9.8米/ S ^ 2），皮球打在了墻上X-。然后，我們測量球適用于在X軸上的壁和輸出-1g值的壓力。如下圖

 
請注意，加速度計(jì)反應(yīng)的加速向量與當(dāng)前的受力方向是相反的.如上圖所示,受力方向向 左,但是加速度的向量方向?yàn)橛?/span>.
如果我們把這個(gè)小盒子拿來放在地球上,那么小球會(huì)落在Z-壁面上，并會(huì)為1G的底壁施加一個(gè)力，在下面的圖片所示：

 
在這種情況下，框不動(dòng)，但我們?nèi)匀豢梢缘玫?/span>Z軸的讀數(shù)-1G。球在墻壁上的壓力造成的引力場。
到目前為止，我們已經(jīng)分析了單個(gè)軸加速度計(jì)的輸出，這是你會(huì)得到一個(gè)單軸加速度計(jì)。 三軸加速度計(jì)的真正價(jià)值，即是他們可以同時(shí)檢測到所有三個(gè)軸的慣性力。讓我們回到我 們的盒模型，并讓旋轉(zhuǎn)45度在右邊的框中。球會(huì)觸及兩面墻：Z和X-在下面的圖片所示：

 
x 和 z 軸受到值的0.71是不是任意的? 它們實(shí)際上是一個(gè)近似SQRT（1/2）.要知道當(dāng)盒子只受重力場時(shí)，x^2+Y^2+z^2 =1g  這將變得更加清晰，為大家介紹一下我們的下一個(gè)樣子的加速度計(jì)。
在之前的盒子模型中，我們有固定的引力場及旋轉(zhuǎn)。在剛剛說明的兩個(gè)例子中，我們分析 了2個(gè)不同的輸出框位置，而力矢量保持不變。這更有利于了解加速度計(jì)如何與外部交互， 并顯示當(dāng)前讀數(shù)。

 
請看一下上面的模型，這是一個(gè)新的模型代替剛剛的盒子立方體。試想一下，在新模型中 的每個(gè)軸是垂直于盒子的壁面。向量R是加速度計(jì)測量（從上面的例子或這兩者的組合，它 可以是引力場或慣性力）的力矢量。接收時(shí)，Ry，R Z與上的X，Y，Z軸的R矢量投影。請注 意下面的關(guān)系式：
R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2     （公式1）
請記住，早一點(diǎn)我告訴你，SQRT（1/2）的值不是隨機(jī)的?0.71。如果您將上述公式中，回 顧我們的引力場為1 g后，我們可以驗(yàn)證：
1 ^ 2 =（-SQRT（1/2））^ 2 + 0 ^ 2 +（-SQRT（1/2））^ 2
簡單地通過代以R = 1時(shí)，接收= SQRT（1/2）時(shí)，Ry = 0，RZ = SQRT（1/2）在方程
經(jīng)過以上的理論分析我們越來越接近現(xiàn)實(shí)生活中的加速度計(jì)。 值RX，RY，RZ實(shí)際上是呈線性關(guān)系對應(yīng)到真正的現(xiàn)實(shí)加速度計(jì)的x軸 y軸 z軸.
在此提出一個(gè)問題， 加速度計(jì)如何將這些信息告訴我們？ 目前市面上的加速度計(jì)從輸出上區(qū)分為兩種，一種是數(shù)字的，另一種是模擬
的.miniAHRS 使用的是MPU6050三軸加速度計(jì)，是I2C接口的數(shù)字傳感器.通過特定的命令可以配置加速度的量程,并將內(nèi)部ADC的轉(zhuǎn)換結(jié)果讀出來.
現(xiàn)在，我們有我們的加速度計(jì)的讀數(shù)，以LSB為單位的，它仍然不是g（9.8米/秒^ 2），需要最后的轉(zhuǎn)換，我們要知道加速度計(jì)靈敏度，通常表示為LSB /g。比方說當(dāng)我們選擇2g的量程時(shí)，對應(yīng)的靈敏度= 16384 LSB/ G 。為了得到最終的力值，單位為g，我們用下面的公式：
RX = ADCRx /靈敏度
也就是說 當(dāng)x軸的計(jì)數(shù)為ADCRx 時(shí)，那么對應(yīng)的加速度值就是 (ADCRx/16384)g.回到加速度向量模型，將相關(guān)角度符號(hào)補(bǔ)上，如下圖

 

方向余弦
我們感興趣的是向量R 和 X、Y、Z軸之間的角度，將它們定義為 Axr  Ayr  Azr. 可以看到
由R 和Rx 組成的直角三角形：
COS（Axr）= RX / R  依此類推：
COS（Ayr）= RY / R
COS（Azr）= RZ / R
可以得到 R = SQRT（RX ^ 2 + RY ^ 2 + RZ ^ 2） 我們發(fā)現(xiàn) 當(dāng)使用 arccos() 反余弦 :
Axr = arccos(RX / R)
Ayr = arccos(RY / R)
Azr = arccos(RZ / R)
已以通過很多公式解釋加速度計(jì)模型。我們也會(huì)很快解釋陀螺儀 以及如何用加速度計(jì)和陀 螺儀的數(shù)據(jù)進(jìn)行整合，以得到更精確的角度估計(jì)。
在這之前 我們先來看看更有用的公式： cosX = cos(Axr) = Rx / R 
cosY = cos(Ayr) = Ry / R 
cosZ = cos(Azr) = Rz / R
這三個(gè)公式通常被稱為方向余弦。你可以輕松地驗(yàn)證： SQRT（cosX ^ 2 + COSY ^ 2 + cosZ ^ 2）= 1
這個(gè)屬性可以避免監(jiān)視R矢量的模（長度）。很多時(shí)候，我們只對慣性矢量方向感興趣，對 矢量進(jìn)行規(guī)范化對簡化程序運(yùn)算很有意義。

重力向量
我們暫且從理論分析回到現(xiàn)實(shí)的傳感器輸出中,當(dāng)水平放置MPU6050,只有Z軸感受到重力向量，它將輸出1g。對應(yīng)的ADC值就是16384 (2g的量程)。此時(shí),R就是重力向量,Rx=0.  Ry=0.  Rz = R =1g.滿足 R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2   得到重力向量與各個(gè)軸的夾角
Axr = arccos(RX / R) = 90度
Ayr = arccos(RY / R) = 90度
Azr = arccos(RZ / R) = 0 度

加速度計(jì)的標(biāo)定
當(dāng)MPU6050水平放置時(shí)，理論上Z軸感受到重力 將讀出16384。同時(shí)X 軸和Y軸的讀數(shù)將是0.可實(shí)際并不是這樣的。這是由于每個(gè)芯片在制作時(shí)都不一樣，數(shù)據(jù)手冊上的都是理論的值，真正的芯片在水平時(shí)Z軸可能并不是16384.我們需要找到當(dāng)各個(gè)軸在0g重力時(shí)的計(jì)數(shù)，
1g時(shí)的讀數(shù)，以及-1g時(shí)的讀數(shù)，得到一個(gè)補(bǔ)償值，在每次讀取ADC結(jié)果后都進(jìn)行補(bǔ)償。這個(gè)過程我們稱之為標(biāo)定。用數(shù)學(xué)公式表示為:
ADCx = K*Gx + Offset
ADCx  傳感器輸出
Gx     真實(shí)的加速值
Offset  加速度為0g時(shí)傳感器的輸出
K      標(biāo)度因數(shù)

陀螺儀
陀螺儀是測試角速度的傳感器，也有人把角速度說成角速率，說的是一樣的物理量。拿電機(jī)做例子，當(dāng)我們說一個(gè)電機(jī)10轉(zhuǎn)每秒。一轉(zhuǎn)是360度,那么它的主軸在一秒內(nèi)轉(zhuǎn)過3600度。也就是說這個(gè)電機(jī)在轉(zhuǎn)動(dòng)時(shí)的角速度是3600dps.dps 就是dergee per second 度每秒(或者寫成 deg/s).
MPU6050 集成了三軸的陀螺儀.角速度全格感測范圍為±250、±500、±1000與±2000°/sec (dps).當(dāng)選擇量程為±250dps的時(shí)候，將會(huì)得到分辯率為131LSB/(o/s).也就是當(dāng)載體在X+軸轉(zhuǎn)動(dòng)1dps時(shí)，ADC將輸出131.

 
回到加速度向量模型，將相關(guān)角度符號(hào)補(bǔ)上，如下圖

 

陀螺儀測量什么？
MPU6050帶有三個(gè)陀螺儀，每個(gè)陀螺儀各自負(fù)責(zé)檢測相應(yīng)軸的轉(zhuǎn)動(dòng)速度，也就是檢測圍繞各個(gè)軸轉(zhuǎn)動(dòng)的速度。像三軸的陀螺儀將同時(shí)檢測 X Y Z軸的旋轉(zhuǎn)。 由上面這個(gè)模型圖，首先我們定義： Rxz - 是R向量在XZ平面上的投影 Ryz - 是R向量在XY平面上的投影 Rxz和Rz所形成的直角三角形，利用勾股定理，我們得到： Rxz ^ 2 = RX ^ 2 + RZ ^ 2，和同樣： Ryz ^ 2 = RY ^ 2 + RZ ^ 2
還要注意的是：
R ^ 2 = RXZ ^ 2 + Ry^ 2，這可以來自從公式1和上面的等式，或它可以是來自于由R和Ryz 
R ^ 2 = Ryz ^ 2 + Rx^ 2 形成的直角三角我們不會(huì)在本文中使用這些公式，只是讓讀者認(rèn)識(shí)到所有值之間的關(guān)系。 同時(shí)我們將定義Z軸和Rxz 、RyZ之間的夾角。 Axz  -  Rxz和Z軸間的夾角 Ayz  -  Ryz和Z軸間的夾角 現(xiàn)在看看,從這個(gè)模型中,陀螺儀測量什么?
上述的說明,已經(jīng)知道陀螺儀測量角度的變化率.為了解釋這一點(diǎn)，讓我們假設(shè)，我們已經(jīng) 測量圍繞Y軸的旋轉(zhuǎn)角（這將是Axz角）在時(shí)刻t0，我們將其定義為Axz0，接下來，我們測 量這個(gè)角度是在稍后的時(shí)間t1是Axz1。變化率將被計(jì)算如下：
RateAxz =（Axz1 - Axz0）/（t1 - t0）
如果Axz單位是度，并以秒為時(shí)間單位，那么RateAxz將以deg / s表示。
MPU6050并不會(huì)以 deg / s 單位輸出，我們需要在讀完后進(jìn)行轉(zhuǎn)換。先來看看各個(gè)量程對 應(yīng)的靈敏度。

 


從ADC值到 dps
  通過I2C接口讀出來的轉(zhuǎn)換結(jié)果ADC值，并不是以度每秒為單位。一般按以下公式進(jìn)行轉(zhuǎn)換：
Anglerate = ADCrate /靈敏度
以量程為±1000o/s為例，說明如何轉(zhuǎn)換。假設(shè)讀取x軸的ADC值為200,從上表中得知在±1000o/s下的靈敏度為32.8LSB/(o/s) 。根據(jù)上面的公式：
Anglerate = 200/32.8 = 6.09756o/s
這就是說,MPU6050檢測到模塊正在以約6度每秒的速度繞X軸(或者叫在YZ平面上)旋轉(zhuǎn). 
ADC值并不都是正的,請注意,當(dāng)出現(xiàn)負(fù)數(shù)時(shí),意味著該設(shè)備從現(xiàn)有的正方向相反的方向旋轉(zhuǎn).

磁力計(jì)
  通過上面的介紹，讀者對加速度和陀螺儀已經(jīng)有一個(gè)認(rèn)識(shí)了。
1.陀螺儀的強(qiáng)項(xiàng)在于測量設(shè)備自身的旋轉(zhuǎn)運(yùn)動(dòng)。對設(shè)備自身運(yùn)動(dòng)更擅長。但不能確定設(shè)備的方位。
2.加速計(jì)的強(qiáng)項(xiàng)在于測量設(shè)備的受力情況。對設(shè)備相對外部參考物(比如，地面)的運(yùn)動(dòng)更擅長。
那么 為什么所有的AHRS模塊都帶有一個(gè)三軸的磁力計(jì)呢？磁力計(jì)是做什么用的？
首先，AHRS 全稱為姿態(tài)航向參考系統(tǒng)，加速度和陀螺只能提供姿態(tài)參考，并不能解算出正確的航向。而磁力計(jì)是用于感受地磁向量以解算出模塊與北的夾角。磁力計(jì)的這個(gè)功能類似 于指南針，所以也叫電子指南針,或者稱為電子磁羅盤。 為了認(rèn)識(shí)這個(gè)傳感器，重新認(rèn)識(shí)一下指南針。

 

從指南針開始
  指南針是用以測定方向基準(zhǔn)的儀器。其主要組成部分是一根裝在軸上可以自由轉(zhuǎn)動(dòng)的磁 針。指南針之所以能夠指示方向，是因?yàn)榈厍虮旧泶嬖诖艌觯裴樤诘卮艌鲎饔孟履鼙３衷?/span> 磁子午線的切線方向上。磁針的N極指向地磁的南極，利用這一性能可以辨別方向。常用于 航海、大地測量、旅行及軍事等方面。   指南針是用以測定方向基準(zhǔn)的儀器。其主要組成部分是一根裝在軸上可以自由轉(zhuǎn)動(dòng)的磁 針。指南針之所以能夠指示方向，是因?yàn)榈厍虮旧泶嬖诖艌觯裴樤诘卮艌鲎饔孟履鼙３衷?/span> 磁子午線的切線方向上。磁針的N極指向地磁的南極，利用這一性能可以辨別方向。常用于 航海、大地測量、旅行及軍事等方面。

 

霍爾傳感器 
  霍爾傳感器是根據(jù)霍爾效應(yīng)制作的一種磁場傳感器。霍爾效應(yīng)是磁電效應(yīng)的一種，這一現(xiàn) 象是霍爾（A.H.Hall，1855—1938）于1879年在研究金屬的導(dǎo)電機(jī)構(gòu)時(shí)發(fā)現(xiàn)的.
  在半導(dǎo)體薄片兩端通以控制電流I，并在薄片的垂直方向施加磁感應(yīng)強(qiáng)度為B的勻強(qiáng)磁場，則在垂直于電流和磁場的方向上，將產(chǎn)生電勢差為VH的霍爾電壓.
  霍爾效應(yīng)的典型應(yīng)用就是霍爾開關(guān),它由穩(wěn)壓器、霍爾元件、差分放大器，斯密特觸發(fā)器和輸出級(jí)組成，它輸出數(shù)字量。常常用于檢測發(fā)動(dòng)機(jī)轉(zhuǎn)速和產(chǎn)生點(diǎn)火信號(hào),等等   在這里說霍爾效應(yīng) 是證明磁場是存在的,也是可以被檢測到的.
  磁力計(jì)與霍爾傳感器是有差別的,前者是用于檢測微量的磁場,大地磁場約在0.3-0.6高斯,如此微弱的磁力,我們需要借助惠斯通電橋來檢測,

 

磁力計(jì)的惠斯通電橋
  如上圖所示。R1/R2/R3/R4是初始狀態(tài)相同的AMR電阻，但是R1/R2和R3/R4具有相反的磁化特性。當(dāng)檢測到外界正交偏置磁場的時(shí)候，R1/R2阻值增加?R而R3/R4減少?R。這樣在沒有外界正交偏置磁場的情況下，電橋的輸出為零；而在有外界磁場時(shí)電橋的輸出為一個(gè)微小的電壓?V。磁力計(jì)就是利用惠斯通電橋檢測AMR阻值的變化,來感覺外部的磁力.當(dāng)然這里的?V很小，需要進(jìn)入放大電路處理。
  在些解釋一下電橋的關(guān)鍵部件AMR電阻,也叫各向異性磁電阻效應(yīng),簡稱磁控電阻.當(dāng)外部的 磁力線垂直于電阻時(shí)與外部磁力線平行于電阻時(shí)呈現(xiàn)不一樣的電阻率.
  HMC5883是三軸的磁力計(jì),當(dāng)然它內(nèi)部有三個(gè)電橋.將三維磁阻傳感器按照載體三維坐標(biāo)系安裝,通過測量載體空間磁場的三維磁感應(yīng)強(qiáng)度，按照一定的算法就可以計(jì)算出載體在空間 的姿態(tài)信息。 這就是電子指南針.

磁干擾
  這個(gè)世界上不是只有地球才能產(chǎn)生磁場,我們身邊很多的物體都是可以產(chǎn)生磁場的,比如磁 鐵,電機(jī),鋼筋構(gòu)建的樓房,通電流的直導(dǎo)線也會(huì)產(chǎn)生磁場.
  我們可以做這樣的實(shí)驗(yàn),找個(gè)指南針,當(dāng)用磁鐵靠近指南針時(shí),它指示的方向會(huì)發(fā)生變化,此 時(shí)它再不能用做指南針給我們導(dǎo)航.因?yàn)樗艿酵饨绱帕Ω蓴_,指示的方向已經(jīng)不能保證正確 了.這樣的現(xiàn)象,我們稱之為磁干擾.
  電子指南針主要是通過感知地球磁場的存在來計(jì)算磁北極的方向。然而由于地球磁場在一 般情況下只有微弱的0.5高斯左右，而一個(gè)普通的手機(jī)喇叭當(dāng)相距2厘米時(shí)仍會(huì)有大約4高斯 的磁場，一個(gè)手機(jī)馬達(dá)在相距2厘米時(shí)會(huì)有大約6高斯的磁場，這一特點(diǎn)使得針對電子設(shè)備表 面地球磁場的測量很容易受到電子設(shè)備本身的干擾。   磁場干擾是指由于具有磁性物質(zhì)或者可以影響局部磁場強(qiáng)度的物質(zhì)存在，使得磁傳感器所 放置位置上的地球磁場發(fā)生了偏差。   磁干擾又分成兩種,一種是硬磁干擾,另一個(gè)是軟磁干擾.
  硬鐵磁場由磁力計(jì)平臺(tái)(可認(rèn)為是載體)上的永久性磁鐵和被磁化的鋼鐵物質(zhì)組成，其特點(diǎn) 是當(dāng)載體位于某一固定位置時(shí)，其強(qiáng)度為一定值，不隨航向的變化而變化.軟鐵磁場可認(rèn)為 由地球磁場與磁力計(jì)周圍的磁化物質(zhì)相互作用而產(chǎn)生。與硬鐵磁場不同的是，軟鐵磁場強(qiáng)度 的大小與方向與磁力計(jì)的方位有關(guān).

磁校準(zhǔn) 
  受環(huán)境因素和電子磁羅盤自身因素的影響，電子磁羅盤常存在較大的航向角誤差，因此經(jīng) 常需要在使用前校準(zhǔn)．磁羅盤校準(zhǔn)的一般方法是使安裝有磁羅盤的載體做特定的運(yùn)動(dòng)，或者 將載體轉(zhuǎn)動(dòng)到某些特定的角度，得到磁羅盤在不同姿態(tài)下的磁場強(qiáng)度測量值．通過對測量值 分析，進(jìn)行磁羅盤的校準(zhǔn)
平面校準(zhǔn)方法   針對XY軸的校準(zhǔn)，將配備有磁傳感器的設(shè)備分別在XY平面內(nèi)自轉(zhuǎn)，相當(dāng)于將地球磁場矢量 繞著過點(diǎn)O(γx,γy)垂直于XY平面的法線旋轉(zhuǎn)， 而紅色的圓為磁場矢量在旋轉(zhuǎn)過程中在XY平 面內(nèi)投影的軌跡。這可以找到圓心的位置為((Xmax + Xmin)/2,  (Ymax + Ymin)/2).