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

Mini AHRS硬件框圖


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

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

請注意，加速度計反應的加速向量與當前的受力方向是相反的.如上圖所示,受力方向向 左,但是加速度的向量方向為右.
如果我們把這個小盒子拿來放在地球上,那么小球會落在Z-壁面上，并會為1G的底壁施加一個力，在下面的圖片所示：

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

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

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


方向余弦
我們感興趣的是向量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) 當使用 arccos() 反余弦 :
Axr = arccos(RX / R)
Ayr = arccos(RY / R)
Azr = arccos(RZ / R)
已以通過很多公式解釋加速度計模型。我們也會很快解釋陀螺儀 以及如何用加速度計和陀 螺儀的數(shù)據(jù)進行整合，以得到更精確的角度估計。
在這之前 我們先來看看更有用的公式： cosX = cos(Axr) = Rx / R
cosY = cos(Ayr) = Ry / R
cosZ = cos(Azr) = Rz / R
這三個公式通常被稱為方向余弦。你可以輕松地驗證： SQRT（cosX ^ 2 + COSY ^ 2 + cosZ ^ 2）= 1
這個屬性可以避免監(jiān)視R矢量的模（長度）。很多時候，我們只對慣性矢量方向感興趣，對 矢量進行規(guī)范化對簡化程序運算很有意義。

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

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

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

回到加速度向量模型，將相關角度符號補上，如下圖


陀螺儀測量什么？
MPU6050帶有三個陀螺儀，每個陀螺儀各自負責檢測相應軸的轉動速度，也就是檢測圍繞各個軸轉動的速度。像三軸的陀螺儀將同時檢測 X Y Z軸的旋轉。 由上面這個模型圖，首先我們定義： 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 形成的直角三角我們不會在本文中使用這些公式，只是讓讀者認識到所有值之間的關系。 同時我們將定義Z軸和Rxz 、RyZ之間的夾角。 Axz  -  Rxz和Z軸間的夾角 Ayz  -  Ryz和Z軸間的夾角 現(xiàn)在看看,從這個模型中,陀螺儀測量什么?
上述的說明,已經(jīng)知道陀螺儀測量角度的變化率.為了解釋這一點，讓我們假設，我們已經(jīng) 測量圍繞Y軸的旋轉角（這將是Axz角）在時刻t0，我們將其定義為Axz0，接下來，我們測 量這個角度是在稍后的時間t1是Axz1。變化率將被計算如下：
RateAxz =（Axz1 - Axz0）/（t1 - t0）
如果Axz單位是度，并以秒為時間單位，那么RateAxz將以deg / s表示。
MPU6050并不會以 deg / s 單位輸出，我們需要在讀完后進行轉換。先來看看各個量程對 應的靈敏度。



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

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


從指南針開始
  指南針是用以測定方向基準的儀器。其主要組成部分是一根裝在軸上可以自由轉動的磁 針。指南針之所以能夠指示方向，是因為地球本身存在磁場，磁針在地磁場作用下能保持在 磁子午線的切線方向上。磁針的N極指向地磁的南極，利用這一性能可以辨別方向。常用于 航海、大地測量、旅行及軍事等方面。   指南針是用以測定方向基準的儀器。其主要組成部分是一根裝在軸上可以自由轉動的磁 針。指南針之所以能夠指示方向，是因為地球本身存在磁場，磁針在地磁場作用下能保持在 磁子午線的切線方向上。磁針的N極指向地磁的南極，利用這一性能可以辨別方向。常用于 航海、大地測量、旅行及軍事等方面。


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


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

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

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

完整的pdf格式文檔51黑下載地址（共11頁）：
MPU6050原始數(shù)據(jù)分析.pdf (1.93 MB, 下載次數(shù): 635)


作者: admin    時間: 2017-8-18 19:04
好資料，51黑有你更精彩!!!
作者: wzm4657    時間: 2018-1-26 15:16
為保存，而回復。找不到收藏的地方。
作者: KawhiL    時間: 2018-1-28 09:38
好帖！準備收藏學習
作者: bojue3509    時間: 2018-2-8 16:08
厲害
作者: 110691    時間: 2018-2-13 10:59
真的厲害，收藏了
作者: stiffzhang    時間: 2018-3-13 16:32
真的厲害，收藏了
作者: stiffzhang    時間: 2018-3-13 16:39
把姿態(tài)控制和四元數(shù)講的完全透徹。
作者: 錦瑟年花    時間: 2018-3-27 12:42
非常棒，學到了很多新東西
作者: 1103278135    時間: 2018-3-27 14:12
非常棒，學到了很多新東西
作者: yeziff001    時間: 2018-4-17 11:14
贊，學習一下
作者: 真實的    時間: 2018-5-5 11:05
可以的，正在學習
作者: meilitianshan    時間: 2018-6-15 17:56
好文，謝謝！
作者: meilitianshan    時間: 2018-6-15 18:00
非常好！謝謝！
作者: 若一心之念    時間: 2018-6-21 22:46
謝謝分享
作者: laoying_rc    時間: 2018-6-22 10:24
好東西，，講的比較透徹。。

謝謝樓主
作者: 蟪蛄望秋    時間: 2018-6-25 09:06
非常棒
作者: fjs2035    時間: 2018-7-1 19:44
好東西，值得收藏
作者: RORRE    時間: 2018-7-30 14:13
講的很詳細，學習了！
作者: youxiaohai    時間: 2018-8-5 13:18
感覺陀螺儀adc輸出講的有點問題
作者: dystm    時間: 2018-8-6 11:39
非常好的帖子，總結的不錯
作者: zenmvol    時間: 2018-11-2 17:04
正在做相關的學習�。。〗o力�。�！
作者: qq729595716    時間: 2018-11-26 20:53
真不錯，對我挺有幫助的
作者: rain2018    時間: 2018-11-30 10:05
好好學習樓主的詳細介紹
作者: 48648664    時間: 2019-3-16 14:27
太棒了，學到了很多東西
作者: 陳小曉    時間: 2019-4-14 22:56
陀螺儀，讀出數(shù)據(jù)后怎么處理，然后用數(shù)值對直流電機加速？？求教
作者: TCL000    時間: 2019-8-19 11:32
wonderful
作者: syytudou    時間: 2019-8-28 09:22
不錯，寫的比較簡單易懂。
作者: wanglukang    時間: 2019-8-31 14:44
好帖！準備收藏學習
作者: J2366079297    時間: 2019-9-7 17:17
多謝樓主分享，贊
作者: gxm0451    時間: 2019-9-11 07:23
學習到了，感謝分享
作者: eee123    時間: 2019-10-8 11:00
留點定位，日后好找
作者: eee123    時間: 2019-10-8 11:03
牛逼了，可以學習學習
作者: Sznnnn    時間: 2019-10-31 20:33
好膩害，可以學習學習
作者: tradepro    時間: 2019-11-2 01:46
講的很好， 正需要了解這方面的資料
作者: babylove    時間: 2019-11-19 16:25
想要這個好東西
作者: chenmcu    時間: 2019-11-29 08:45
講的非常好。。
作者: xlvecle    時間: 2019-12-19 16:01
支持一下~為了保存
作者: 吃了嗎    時間: 2019-12-25 11:07
感謝樓主
作者: TKDRIVER    時間: 2019-12-27 08:41
我也來保存一下，感謝樓主
作者: qianqianzige    時間: 2020-1-3 17:30
細致講解，學習一下
作者: joker丶    時間: 2020-1-13 09:21
感謝樓主，正好用的上
作者: duyi324    時間: 2020-1-25 14:53
好資料，51黑有你更精彩!!!
作者: duyi324    時間: 2020-1-25 14:54
我也來保存一下，感謝樓主
作者: yuancat    時間: 2020-1-30 14:11
非常感謝，學習了
作者: xinyue129    時間: 2020-2-14 11:28
學習用，感謝感謝
作者: kate110zhen    時間: 2020-3-9 01:08
這個是真的厲害，解決了我很多疑問
作者: weskyfly    時間: 2020-4-2 17:35
確實有一定幫助，但是磁力計只是均值處理會不會草率了點
作者: 76609789    時間: 2020-5-28 22:58
學習了，感謝樓主精彩的講解
作者: 庚申12345    時間: 2020-5-31 21:51
非常棒，感謝分享
作者: 庚申12345    時間: 2020-5-31 21:52
這個是真的厲害，解決了我很多疑問
作者: Harryhu123    時間: 2020-6-3 20:15
這個是真的厲害，解決了我很多疑問
作者: bnsong    時間: 2020-8-21 18:24
頂，寫的很詳細，通俗易懂
作者: bnsong    時間: 2020-8-21 18:25
頂！寫的很詳細，通俗易懂
作者: 林宇天順    時間: 2021-3-3 11:26
寫的較詳細，對MPU6050的開發(fā)有很大幫助。
作者: 林宇天順    時間: 2021-3-3 11:28
寫的很詳細，對MPU6050開發(fā)有很大幫助。
作者: 點擊小子    時間: 2021-3-4 17:02
大佬，有沒有仿真程序呀！
作者: BlueFox    時間: 2021-3-7 14:19
好貼，初學者正需要�。�
作者: 曾現(xiàn)路    時間: 2021-4-18 17:30
了解一下，感覺是好東西就是不知怎么用，超級感謝
作者: xulin269    時間: 2021-8-27 18:00
請教一下如何計算一段時間旋轉的度數(shù)了
作者: sky2006    時間: 2022-12-6 14:03
非常細致
作者: sky2006    時間: 2022-12-6 14:03
好文章，感謝樓主
作者: qwertyu78526    時間: 2023-11-1 17:34

非常棒，學到了很多新東西
作者: blueprince000    時間: 2023-11-15 10:59
很厲害，贊一個
作者: kanwoe    時間: 2024-3-4 14:20
這個講的是真的好，我收藏備用。



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1