|
給51黑論壇的朋友們分享關(guān)于MPU6050的超好文章,介紹姿態(tài)解算的入門知識(shí),非常通俗易懂. AHRS俗稱航姿參考系統(tǒng)，AHRS由加速度計(jì),，磁場(chǎng)計(jì),，陀螺儀構(gòu)成，AHRS的真正參考來(lái)自于地球的重力場(chǎng)和地球的磁場(chǎng)～～他的靜態(tài)終精度取決于對(duì)磁場(chǎng)的測(cè)量精度和對(duì)重力的測(cè)量精度 ,而則陀螺決定了他的動(dòng)態(tài)性能,。 這就是AHRS～在這種前提下,。說(shuō)明AHRS離開(kāi)了地球這種有重力和磁場(chǎng)環(huán)境的時(shí)候是沒(méi)法正常工作的～～ 本章旨在講解以下內(nèi)容 1.加速度 2.陀螺儀 3.磁力計(jì) 一直想寫篇文章關(guān)于姿態(tài)解算原理的，使用盡量通俗的語(yǔ)句說(shuō)明如何從加速度計(jì)和陀螺 儀的數(shù)據(jù),，融合得到載體的姿態(tài)角,。無(wú)奈自己的水平有限，一直擱置,。 淡泊以明志,寧?kù)o以致遠(yuǎn).人總是要逼自己做些事,，才過(guò)得心安理得。那就拿點(diǎn)時(shí)間把這 方面的資料整合一下吧,。這篇文章的大部分內(nèi)容都不是本人原創(chuàng)的,感謝網(wǎng)絡(luò)上無(wú)私奉獻(xiàn)的 人. 在此介紹一下實(shí)驗(yàn)的姿態(tài)板 ,新一代的mini AHRS,采用STM32F103單片機(jī)進(jìn)行姿態(tài)解算,，板子上集成有 1.MPU6050，三軸的加速度和陀螺儀 2.HMC5883 三軸的磁力計(jì) 3.BMP180 高精度氣壓高度計(jì) 這些傳感器都通過(guò)I2C接口連接到主控制器STM32.不需要額外的ADC電路,直接通過(guò)數(shù)字接口就可以讀取傳感器的當(dāng)前輸出. Mini AHRS硬件框圖  1 加速度計(jì) 加速度計(jì)顧名思義,就是測(cè)量加速度的.那么,我們?nèi)绾握J(rèn)識(shí)這個(gè)加速度呢?在此用一個(gè)盒子形 狀的立方體來(lái)做模型,，認(rèn)識(shí)加速度,如下,盒子內(nèi)的圖像,。  如果我們把盒子形狀的立方體 放在一個(gè)沒(méi)引力場(chǎng)的地方,球會(huì)保持在盒子的中間.你可以想 象，這個(gè)盒子是在外太空,，遠(yuǎn)離任何天體,，很難找到這樣的地方,就想象飛船軌道圍繞地球飛，一切都是在失重狀態(tài)下,。那么六個(gè)壁面感受到的壓力都是0. 如果我們突然將立方體向左側(cè)移動(dòng)（我們加快加速,，1G =9.8米/ S ^ 2），皮球打在了墻上X-,。然后,，我們測(cè)量球適用于在X軸上的壁和輸出-1g值的壓力。如下圖  請(qǐng)注意,，加速度計(jì)反應(yīng)的加速向量與當(dāng)前的受力方向是相反的.如上圖所示,受力方向向 左,但是加速度的向量方向?yàn)橛? 如果我們把這個(gè)小盒子拿來(lái)放在地球上,那么小球會(huì)落在Z-壁面上,，并會(huì)為1G的底壁施加一個(gè)力,，在下面的圖片所示：  在這種情況下，框不動(dòng),，但我們?nèi)匀豢梢缘玫絑軸的讀數(shù)-1G,。球在墻壁上的壓力造成的引力場(chǎng)。 到目前為止,，我們已經(jīng)分析了單個(gè)軸加速度計(jì)的輸出,，這是你會(huì)得到一個(gè)單軸加速度計(jì)。 三軸加速度計(jì)的真正價(jià)值,，即是他們可以同時(shí)檢測(cè)到所有三個(gè)軸的慣性力,。讓我們回到我 們的盒模型，并讓旋轉(zhuǎn)45度在右邊的框中,。球會(huì)觸及兩面墻：Z和X-在下面的圖片所示：  x 和 z 軸受到值的0.71是不是任意的? 它們實(shí)際上是一個(gè)近似SQRT（1/2）.要知道當(dāng)盒子只受重力場(chǎng)時(shí),，x^2+Y^2+z^2 =1g 這將變得更加清晰，為大家介紹一下我們的下一個(gè)樣子的加速度計(jì),。 在之前的盒子模型中,，我們有固定的引力場(chǎng)及旋轉(zhuǎn),。在剛剛說(shuō)明的兩個(gè)例子中,，我們分析 了2個(gè)不同的輸出框位置，而力矢量保持不變,。這更有利于了解加速度計(jì)如何與外部交互,， 并顯示當(dāng)前讀數(shù)。  請(qǐng)看一下上面的模型,，這是一個(gè)新的模型代替剛剛的盒子立方體,。試想一下，在新模型中 的每個(gè)軸是垂直于盒子的壁面,。向量R是加速度計(jì)測(cè)量（從上面的例子或這兩者的組合,，它 可以是引力場(chǎng)或慣性力）的力矢量。接收時(shí),，Ry,，R Z與上的X，Y,，Z軸的R矢量投影,。請(qǐng)注 意下面的關(guān)系式： R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2 （公式1） 請(qǐng)記住，早一點(diǎn)我告訴你,，SQRT（1/2）的值不是隨機(jī)的?0.71,。如果您將上述公式中，回 顧我們的引力場(chǎng)為1 g后,，我們可以驗(yàn)證： 1 ^ 2 =（-SQRT（1/2））^ 2 + 0 ^ 2 +（-SQRT（1/2））^ 2 簡(jiǎn)單地通過(guò)代以R = 1時(shí),，接收= SQRT（1/2）時(shí),，Ry = 0，RZ = SQRT（1/2）在方程 經(jīng)過(guò)以上的理論分析我們?cè)絹?lái)越接近現(xiàn)實(shí)生活中的加速度計(jì),。 值RX,，RY，RZ實(shí)際上是呈線性關(guān)系對(duì)應(yīng)到真正的現(xiàn)實(shí)加速度計(jì)的x軸 y軸 z軸. 在此提出一個(gè)問(wèn)題,， 加速度計(jì)如何將這些信息告訴我們,？ 目前市面上的加速度計(jì)從輸出上區(qū)分為兩種，一種是數(shù)字的,，另一種是模擬 的.miniAHRS 使用的是MPU6050三軸加速度計(jì),，是I2C接口的數(shù)字傳感器.通過(guò)特定的命令可以配置加速度的量程,并將內(nèi)部ADC的轉(zhuǎn)換結(jié)果讀出來(lái). 現(xiàn)在，我們有我們的加速度計(jì)的讀數(shù),，以LSB為單位的，它仍然不是g（9.8米/秒^ 2）,，需要最后的轉(zhuǎn)換,，我們要知道加速度計(jì)靈敏度，通常表示為L(zhǎng)SB /g,。比方說(shuō)當(dāng)我們選擇2g的量程時(shí),，對(duì)應(yīng)的靈敏度= 16384 LSB/ G 。為了得到最終的力值,，單位為g,，我們用下面的公式： RX = ADCRx /靈敏度 也就是說(shuō) 當(dāng)x軸的計(jì)數(shù)為ADCRx 時(shí)，那么對(duì)應(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) 已以通過(guò)很多公式解釋加速度計(jì)模型。我們也會(huì)很快解釋陀螺儀 以及如何用加速度計(jì)和陀 螺儀的數(shù)據(jù)進(jìn)行整合,，以得到更精確的角度估計(jì),。 在這之前 我們先來(lái)看看更有用的公式： 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矢量的模（長(zhǎng)度）,。很多時(shí)候,，我們只對(duì)慣性矢量方向感興趣，對(duì) 矢量進(jìn)行規(guī)范化對(duì)簡(jiǎn)化程序運(yùn)算很有意義,。 重力向量 我們暫且從理論分析回到現(xiàn)實(shí)的傳感器輸出中,當(dāng)水平放置MPU6050,只有Z軸感受到重力向量,，它將輸出1g。對(duì)應(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ù)手冊(cè)上的都是理論的值,，真正的芯片在水平時(shí)Z軸可能并不是16384.我們需要找到當(dāng)各個(gè)軸在0g重力時(shí)的計(jì)數(shù)， 1g時(shí)的讀數(shù),，以及-1g時(shí)的讀數(shù),，得到一個(gè)補(bǔ)償值，在每次讀取ADC結(jié)果后都進(jìn)行補(bǔ)償,。這個(gè)過(guò)程我們稱之為標(biāo)定,。用數(shù)學(xué)公式表示為: ADCx = K*Gx + Offset ADCx 傳感器輸出 Gx 真實(shí)的加速值 Offset 加速度為0g時(shí)傳感器的輸出 K 標(biāo)度因數(shù) 陀螺儀 陀螺儀是測(cè)試角速度的傳感器，也有人把角速度說(shuō)成角速率,，說(shuō)的是一樣的物理量,。拿電機(jī)做例子，當(dāng)我們說(shuō)一個(gè)電機(jī)10轉(zhuǎn)每秒,。一轉(zhuǎn)是360度,那么它的主軸在一秒內(nèi)轉(zhuǎn)過(guò)3600度,。也就是說(shuō)這個(gè)電機(jī)在轉(zhuǎn)動(dòng)時(shí)的角速度是3600dps.dps 就是dergee per second 度每秒(或者寫成 deg/s). MPU6050 集成了三軸的陀螺儀.角速度全格感測(cè)范圍為±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ǔ)上,，如下圖  陀螺儀測(cè)量什么？ MPU6050帶有三個(gè)陀螺儀,，每個(gè)陀螺儀各自負(fù)責(zé)檢測(cè)相應(yīng)軸的轉(zhuǎn)動(dòng)速度,，也就是檢測(cè)圍繞各個(gè)軸轉(zhuǎn)動(dòng)的速度。像三軸的陀螺儀將同時(shí)檢測(cè) 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，這可以來(lái)自從公式1和上面的等式,，或它可以是來(lái)自于由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è)模型中,陀螺儀測(cè)量什么? 上述的說(shuō)明,已經(jīng)知道陀螺儀測(cè)量角度的變化率.為了解釋這一點(diǎn)，讓我們假設(shè)，我們已經(jīng) 測(cè)量圍繞Y軸的旋轉(zhuǎn)角（這將是Axz角）在時(shí)刻t0,，我們將其定義為Axz0,，接下來(lái)，我們測(cè) 量這個(gè)角度是在稍后的時(shí)間t1是Axz1,。變化率將被計(jì)算如下： RateAxz =（Axz1 - Axz0）/（t1 - t0） 如果Axz單位是度,，并以秒為時(shí)間單位，那么RateAxz將以deg / s表示,。 MPU6050并不會(huì)以 deg / s 單位輸出,，我們需要在讀完后進(jìn)行轉(zhuǎn)換。先來(lái)看看各個(gè)量程對(duì) 應(yīng)的靈敏度,。  從ADC值到 dps 通過(guò)I2C接口讀出來(lái)的轉(zhuǎn)換結(jié)果ADC值,，并不是以度每秒為單位。一般按以下公式進(jìn)行轉(zhuǎn)換： Anglerate = ADCrate /靈敏度 以量程為±1000o/s為例,，說(shuō)明如何轉(zhuǎn)換,。假設(shè)讀取x軸的ADC值為200,從上表中得知在±1000o/s下的靈敏度為32.8LSB/(o/s) 。根據(jù)上面的公式： Anglerate = 200/32.8 = 6.09756o/s 這就是說(shuō),MPU6050檢測(cè)到模塊正在以約6度每秒的速度繞X軸(或者叫在YZ平面上)旋轉(zhuǎn). ADC值并不都是正的,請(qǐng)注意,當(dāng)出現(xiàn)負(fù)數(shù)時(shí),意味著該設(shè)備從現(xiàn)有的正方向相反的方向旋轉(zhuǎn). 磁力計(jì) 通過(guò)上面的介紹,，讀者對(duì)加速度和陀螺儀已經(jīng)有一個(gè)認(rèn)識(shí)了,。 1.陀螺儀的強(qiáng)項(xiàng)在于測(cè)量設(shè)備自身的旋轉(zhuǎn)運(yùn)動(dòng)。對(duì)設(shè)備自身運(yùn)動(dòng)更擅長(zhǎng),。但不能確定設(shè)備的方位。 2.加速計(jì)的強(qiáng)項(xiàng)在于測(cè)量設(shè)備的受力情況,。對(duì)設(shè)備相對(duì)外部參考物(比如,，地面)的運(yùn)動(dòng)更擅長(zhǎng)。 那么 為什么所有的AHRS模塊都帶有一個(gè)三軸的磁力計(jì)呢,？磁力計(jì)是做什么用的,？ 首先，AHRS 全稱為姿態(tài)航向參考系統(tǒng),，加速度和陀螺只能提供姿態(tài)參考,，并不能解算出正確的航向。而磁力計(jì)是用于感受地磁向量以解算出模塊與北的夾角,。磁力計(jì)的這個(gè)功能類似 于指南針,，所以也叫電子指南針,或者稱為電子磁羅盤。 為了認(rèn)識(shí)這個(gè)傳感器,，重新認(rèn)識(shí)一下指南針,。  從指南針開(kāi)始 指南針是用以測(cè)定方向基準(zhǔn)的儀器。其主要組成部分是一根裝在軸上可以自由轉(zhuǎn)動(dòng)的磁 針,。指南針之所以能夠指示方向,，是因?yàn)榈厍虮旧泶嬖诖艌?chǎng)，磁針在地磁場(chǎng)作用下能保持在 磁子午線的切線方向上。磁針的N極指向地磁的南極,，利用這一性能可以辨別方向,。常用于 航海、大地測(cè)量,、旅行及軍事等方面,。 指南針是用以測(cè)定方向基準(zhǔn)的儀器。其主要組成部分是一根裝在軸上可以自由轉(zhuǎn)動(dòng)的磁 針,。指南針之所以能夠指示方向,，是因?yàn)榈厍虮旧泶嬖诖艌?chǎng)，磁針在地磁場(chǎng)作用下能保持在 磁子午線的切線方向上,。磁針的N極指向地磁的南極,，利用這一性能可以辨別方向。常用于 航海,、大地測(cè)量,、旅行及軍事等方面。 霍爾傳感器 霍爾傳感器是根據(jù)霍爾效應(yīng)制作的一種磁場(chǎ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ǎng)，則在垂直于電流和磁場(chǎng)的方向上,，將產(chǎn)生電勢(shì)差為VH的霍爾電壓. 霍爾效應(yīng)的典型應(yīng)用就是霍爾開(kāi)關(guān),它由穩(wěn)壓器,、霍爾元件、差分放大器,，斯密特觸發(fā)器和輸出級(jí)組成,，它輸出數(shù)字量。常常用于檢測(cè)發(fā)動(dòng)機(jī)轉(zhuǎn)速和產(chǎn)生點(diǎn)火信號(hào),等等 在這里說(shuō)霍爾效應(yīng) 是證明磁場(chǎng)是存在的,也是可以被檢測(cè)到的. 磁力計(jì)與霍爾傳感器是有差別的,前者是用于檢測(cè)微量的磁場(chǎng),大地磁場(chǎng)約在0.3-0.6高斯,如此微弱的磁力,我們需要借助惠斯通電橋來(lái)檢測(cè), 磁力計(jì)的惠斯通電橋 如上圖所示,。R1/R2/R3/R4是初始狀態(tài)相同的AMR電阻,，但是R1/R2和R3/R4具有相反的磁化特性。當(dāng)檢測(cè)到外界正交偏置磁場(chǎng)的時(shí)候,，R1/R2阻值增加?R而R3/R4減少?R,。這樣在沒(méi)有外界正交偏置磁場(chǎng)的情況下，電橋的輸出為零,；而在有外界磁場(chǎng)時(shí)電橋的輸出為一個(gè)微小的電壓?V,。磁力計(jì)就是利用惠斯通電橋檢測(cè)AMR阻值的變化,來(lái)感覺(jué)外部的磁力.當(dāng)然這里的?V很小，需要進(jìn)入放大電路處理,。 在些解釋一下電橋的關(guān)鍵部件AMR電阻,也叫各向異性磁電阻效應(yīng),簡(jiǎn)稱磁控電阻.當(dāng)外部的 磁力線垂直于電阻時(shí)與外部磁力線平行于電阻時(shí)呈現(xiàn)不一樣的電阻率. HMC5883是三軸的磁力計(jì),當(dāng)然它內(nèi)部有三個(gè)電橋.將三維磁阻傳感器按照載體三維坐標(biāo)系安裝,通過(guò)測(cè)量載體空間磁場(chǎng)的三維磁感應(yīng)強(qiáng)度,，按照一定的算法就可以計(jì)算出載體在空間 的姿態(tài)信息,。 這就是電子指南針. 磁干擾 這個(gè)世界上不是只有地球才能產(chǎn)生磁場(chǎng),我們身邊很多的物體都是可以產(chǎn)生磁場(chǎng)的,比如磁 鐵,電機(jī),鋼筋構(gòu)建的樓房,通電流的直導(dǎo)線也會(huì)產(chǎn)生磁場(chǎng). 我們可以做這樣的實(shí)驗(yàn),找個(gè)指南針,當(dāng)用磁鐵靠近指南針時(shí),它指示的方向會(huì)發(fā)生變化,此 時(shí)它再不能用做指南針給我們導(dǎo)航.因?yàn)樗艿酵饨绱帕Ω蓴_,指示的方向已經(jīng)不能保證正確 了.這樣的現(xiàn)象,我們稱之為磁干擾. 電子指南針主要是通過(guò)感知地球磁場(chǎng)的存在來(lái)計(jì)算磁北極的方向。然而由于地球磁場(chǎng)在一 般情況下只有微弱的0.5高斯左右,，而一個(gè)普通的手機(jī)喇叭當(dāng)相距2厘米時(shí)仍會(huì)有大約4高斯 的磁場(chǎng),，一個(gè)手機(jī)馬達(dá)在相距2厘米時(shí)會(huì)有大約6高斯的磁場(chǎng)，這一特點(diǎn)使得針對(duì)電子設(shè)備表 面地球磁場(chǎng)的測(cè)量很容易受到電子設(shè)備本身的干擾,。 磁場(chǎng)干擾是指由于具有磁性物質(zhì)或者可以影響局部磁場(chǎng)強(qiáng)度的物質(zhì)存在,，使得磁傳感器所 放置位置上的地球磁場(chǎng)發(fā)生了偏差。 磁干擾又分成兩種,一種是硬磁干擾,另一個(gè)是軟磁干擾. 硬鐵磁場(chǎng)由磁力計(jì)平臺(tái)(可認(rèn)為是載體)上的永久性磁鐵和被磁化的鋼鐵物質(zhì)組成,，其特點(diǎn) 是當(dāng)載體位于某一固定位置時(shí),，其強(qiáng)度為一定值，不隨航向的變化而變化.軟鐵磁場(chǎng)可認(rèn)為 由地球磁場(chǎng)與磁力計(jì)周圍的磁化物質(zhì)相互作用而產(chǎn)生,。與硬鐵磁場(chǎng)不同的是,，軟鐵磁場(chǎng)強(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)下的磁場(chǎng)強(qiáng)度測(cè)量值．通過(guò)對(duì)測(cè)量值 分析,，進(jìn)行磁羅盤的校準(zhǔn) 平面校準(zhǔn)方法 針對(duì)XY軸的校準(zhǔn),，將配備有磁傳感器的設(shè)備分別在XY平面內(nèi)自轉(zhuǎn)，相當(dāng)于將地球磁場(chǎng)矢量 繞著過(guò)點(diǎn)O(γx,γy)垂直于XY平面的法線旋轉(zhuǎn),， 而紅色的圓為磁場(chǎng)矢量在旋轉(zhuǎn)過(guò)程中在XY平 面內(nèi)投影的軌跡,。這可以找到圓心的位置為((Xmax + Xmin)/2, (Ymax + Ymin)/2). |
|
|
