MPU6050原理詳解及實例應用

一、MPU6050的原理分析。

1.組成：

要想知道MPU6050工作原理，得先了解下面倆個傳感器：

①陀螺儀傳感器：

陀螺儀的原理就是，一個旋轉物體的旋轉軸所指的方向在不受外力影響時，是不會改變的。人們根據這個道理，用它來保持方向。然後用多種方法讀取軸所指示的方向，並自動將數據信號傳給控制系統。我們騎自行車其實也是利用了這個原理。輪子轉得越快越不容易倒，因爲車軸有一股保持水平的力量。現代陀螺儀可以精確地確定運動物體的方位的儀器，它在現代航空，航海，航天和國防工業中廣泛使用的一種慣性導航儀器。傳統的慣性陀螺儀主要部分有機械式的陀螺儀，而機械式的陀螺儀對工藝結構的要求很高。70年代提出了現代光纖陀螺儀的基本設想，到八十年代以後，光纖陀螺儀就得到了非常迅速的發展，激光諧振陀螺儀也有了很大的發展。光纖陀螺儀具有結構緊湊，靈敏度高，工作可靠。光纖陀螺儀在很多的領域已經完全取代了機械式的傳統的陀螺儀，成爲現代導航儀器中的關鍵部件。光纖陀螺儀同時發展的除了環式激光陀螺儀外。

②加速度傳感器：

加速度傳感器是一種能夠測量加速度的傳感器。通常由質量塊、阻尼器、彈性元件、敏感元件和適調電路等部分組成。傳感器在加速過程中，通過對質量塊所受慣性力的測量，利用牛頓第二定律獲得加速度值。根據傳感器敏感元件的不同，常見的加速度傳感器包括電容式、電感式、應變式、壓阻式、壓電式等。

其實說簡單點，在mpu6050中我們用陀螺儀傳感器測角度，用加速度傳感器測加速度。

  

   MPU-60X0 :

            MPU-60X0是全球首例9軸運動處理傳感器。它集成了3軸MEMS陀螺儀， 3軸MEMS 加速度計，以及一個可擴展的數字運動處理器 DMP（DigitalMotion Processor），可用 I2C 接口連接一個第三方的數字傳感器，比如磁力計。擴展之後就可以通過其 I2C 或 SPI 接口 輸出一個 9 軸的信號（SPI 接口僅在 MPU-6000 可用）。MPU-60X0 也可以通過其 I2C 接口 連接非慣性的數字傳感器，比如壓力傳感器。 

                 

          MPU-60X0 對陀螺儀和加速度計分別用了三個 16 位的 ADC，將其測量的模擬量轉化 爲可輸出的數字量。爲了精確跟蹤快速和慢速的運動，傳感器的測量範圍都是用戶可控的， 陀螺儀可測範圍爲±250，±500，±1000，±2000°/秒（dps），加速度計可測範圍爲±2，±4， ±8，±16g。 一個片上 1024 字節的 FIFO，有助於降低系統功耗。 和所有設備寄存器之間的通信採用 400kHz 的 I2C 接口或 1MHz 的 SPI 接口（SPI 僅 MPU-6000 可用）。對於需要高速傳輸的應用，對寄存器的讀取和中斷可用 20MHz 的 SPI。 另外，片上還內嵌了一個溫度傳感器和在工作環境下僅有±1%變動的振盪器。 芯片尺寸 4×4×0.9mm，採用 QFN 封裝（無引線方形封裝），可承受最大 10000g 的衝 擊，並有可編程的低通濾波器。 關於電源，MPU-60X0 可支持 VDD 範圍 2.5V±5%，3.0V±5%，或 3.3V±5%。另外 MPU-6050 還有一個 VLOGIC 引腳，用來爲 I2C 輸出提供邏輯電平。VLOGIC 電壓可取 1.8±5%或者 VDD。

          數字運動處理器（DMP）:

 DMP 從陀螺儀、加速度計以及外接的傳感器接收並處理數據，處理結果可以從 DMP 寄存器讀出，或通過 FIFO 緩衝。DMP 有權使用 MPU 的一個外部引腳產生中斷。

2、數據傳輸:

        如果要寫 MPU-60X0 寄存器，主設備除了發出開始標誌（S）和地址位，還要加一個 R/W 位，0 爲寫，1 爲讀。在第 9 個時鐘週期（高電平時），MPU-60X0 產生應答信號。然 後主設備開始傳送寄存器地址（RA），接到應答後，開始傳送寄存器數據，然後仍然要有應 答信號，依次類推。

單字節寫時序：

多字節寫時序：

如果要讀取 MPU-60X0 寄存器的值，首先由主設備產生開始信號（S），然後發送從設 備地址位和一個寫數據位，然後發送寄存器地址，才能開始讀寄存器。緊接着，收到應答信 號後，主設備再發一個開始信號，然後發送從設備地址位和一個讀數據位。然後，作爲從設 備的 MPU-60X0 產生應答信號並開始發送寄存器數據。通信以主設備產生的拒絕應答信號 （NACK）和結束標誌（P）結束。拒絕應答信號（NACK）產生定義爲 SDA 數據在第 9 個 時鐘週期一直爲高。 

二、MPU6050姿態融合

姿態角（Euler角）pitch yaw roll
飛行器的姿態角並不是指哪個角度，是三個角度的統稱。
它們是：俯仰、滾轉、偏航。你可以想象是飛機圍繞XYZ三個軸分別轉動形成的夾角。

地面座標系（earth-surface inertial reference frame）Sg--------OXgYgZg
<ignore_js_op> 
①在地面上選一點Og
②使Xg軸在水平面內並指向某一方向
③Zg軸垂直於地面並指向地心(重力方向)
④Yg軸在水平面內垂直於Xg軸，其指向按右手定則確定

機體座標系(Aircraft-body coordinate frame)Sb-------OXYZ
<ignore_js_op> 

①原點O取在飛機質心處，座標系與飛機固連
②x軸在飛機對稱平面內並平行於飛機的設計軸線指向機頭
③y軸垂直於飛機對稱平面指向機身右方
④z軸在飛機對稱平面內，與x軸垂直並指向機身下方

歐拉角/姿態角（Euler Angle）
<ignore_js_op> 
<ignore_js_op> 

機體座標系與地面座標系的關係是三個Euler角，反應了飛機相對地面的姿態。
俯仰角θ（pitch）：機體座標系X軸與水平面的夾角。當X軸的正半軸位於過座標原點的水平面之上（擡頭）時，俯仰角爲正，否則爲負。
<ignore_js_op> 

偏航角ψ（yaw）：
機體座標系xb軸在水平面上投影與地面座標系xg軸（在水平面上，指向目標爲正）之間的夾角，由xg軸逆時針轉至機體xb的投影線時，偏航角爲正，即機頭右偏航爲正，反之爲負。
<ignore_js_op> 

滾轉角Φ（roll）：機體座標系zb軸與通過機體xb軸的鉛垂面間的夾角，機體向右滾爲正，反之爲負。
<ignore_js_op>

 

 

首先要明確，MPU6050 是一款姿態傳感器，使用它就是爲了得到待測物體（如四軸、平衡小車） x、y、z 軸的傾角（俯仰角 Pitch、滾轉角 Roll、偏航角 Yaw） 。我們通過 I2C 讀取到 MPU6050 的六個數據（三軸加速度 AD 值、三軸角速度 AD 值）經過姿態融合後就可以得到 Pitch、Roll、Yaw 角。

本帖主要介紹三種姿態融合算法：四元數法 、一階互補算法和卡爾曼濾波算法。

一、四元數法

關於四元數的一些概念和計算就不寫上來了，我也不懂。我能告訴你的是：通過下面的算法，可以把六個數據轉化成四元數（q0、q1、q2、q3），然後四元數轉化成歐拉角（P、R、Y 角）。

        雖然 MPU6050 自帶的 DMP庫可以直接輸出四元數，減輕 STM32 的運算負擔， 這裏在此沒有使用，因爲我是用 STM32 的硬件 I2C 讀取 MPU6050 數據的（http://bbs.elecfans.com/forum.ph ... 4&page=1#pid3625735），DMP庫需要對 I2C 函數進行修改，如 DMP 庫中的 I2C 寫：i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]))；有4個輸入變量，而 STM32 硬件 I2C 的 I2C 寫爲：void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 個輸入量（這之間的差異好像是由於 MPU6050 的 DMP 庫是針對 MSP430 單片機寫的），所以必須進行修改，但是改固件庫是一件很痛苦的事，你們應該都懂。當然，如果你用模擬 I2C 的話，是容易實現的，網上的 DMP 移植幾乎都是基於模擬 I2C 的。

 

複製代碼

#include<math.h>

#include "stm32f10x.h"

//---------------------------------------------------------------------------------------------------

// 變量定義

 

#define Kp 100.0f                        // 比例增益支配率收斂到加速度計/磁強計

#define Ki 0.002f                // 積分增益支配率的陀螺儀偏見的銜接

#define halfT 0.001f                // 採樣週期的一半

 

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元數的元素，代表估計方向

float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例縮小積分誤差

 

float Yaw,Pitch,Roll;  //偏航角，俯仰角，翻滾角

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

        float norm;

        float vx, vy, vz;

        float ex, ey, ez;  

 

        // 測量正常化

        norm = sqrt(ax*ax + ay*ay + az*az);      

        ax = ax / norm;                   //單位化

        ay = ay / norm;

        az = az / norm;      

 

        // 估計方向的重力

        vx = 2*(q1*q3 - q0*q2);

        vy = 2*(q0*q1 + q2*q3);

        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

 

        // 錯誤的領域和方向傳感器測量參考方向之間的交叉乘積的總和

        ex = (ay*vz - az*vy);

        ey = (az*vx - ax*vz);

        ez = (ax*vy - ay*vx);

 

        // 積分誤差比例積分增益

        exInt = exInt + ex*Ki;

        eyInt = eyInt + ey*Ki;

        ezInt = ezInt + ez*Ki;

 

        // 調整後的陀螺儀測量

        gx = gx + Kp*ex + exInt;

        gy = gy + Kp*ey + eyInt;

        gz = gz + Kp*ez + ezInt;

 

        // 整合四元數率和正常化

        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

 

        // 正常化四元

        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

        q0 = q0 / norm;

        q1 = q1 / norm;

        q2 = q2 / norm;

        q3 = q3 / norm;

 

        Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,轉換爲度數

        Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv

        //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此處沒有價值，注掉

}

      要注意的的是，四元數算法輸出的是三個量 Pitch、Roll 和 Yaw，運算量很大。而像平衡小車這樣的例子只需要一個角（Pitch 或 Roll ）就可以滿足工作要求，個人覺得做平衡小車最好不用四元數法。

二、一階互補算法

       MPU6050 可以輸出三軸的加速度和角速度。通過加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是說有兩組 Pitch、Roll 角，到底應該選哪組呢？別急，先分析一下。MPU6050 的加速度計和陀螺儀各有優缺點，三軸的加速度值沒有累積誤差，且通過算 tan()  可以得到傾角，但是它包含的噪聲太多（因爲待測物運動時會產生加速度，電機運行時振動會產生加速度等），不能直接使用；陀螺儀對外界振動影響小，精度高，通過對角速度積分可以得到傾角，但是會產生累積誤差。所以，不能單獨使用 MPU6050 的加速度計或陀螺儀來得到傾角，需要互補。一階互補算法的思想就是給加速度和陀螺儀不同的權值，把它們結合到一起，進行修正。得到 Pitch 角的程序如下：

 

複製代碼

//一階互補濾波

float K1 =0.1; // 對加速度計取值的權重

float dt=0.001;//注意：dt的取值爲濾波器採樣時間

float angle;

 

angle_ax=atan(ax/az)*57.3;     //加速度得到的角度

gy=(float)gyo[1]/7510.0;       //陀螺儀得到的角速度

Pitch = yijiehubu(angle_ax,gy);

 

float yijiehubu(float angle_m, float gyro_m)//採集後計算的角度和角加速度

{

     angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);

     return angle;

}

    互補算法只能得到一個傾角，這在平衡車項目中夠用了，而在四軸飛行器設計中還需要 Roll 和 Yaw，就需要兩個 互補算法，我是這樣寫的，注意變量不要搞混：

 

複製代碼

//一階互補濾波

float K1 =0.1; // 對加速度計取值的權重

float dt=0.001;//注意：dt的取值爲濾波器採樣時間

float angle_P,angle_R;

float yijiehubu_P(float angle_m, float gyro_m)//採集後計算的角度和角加速度

{

     angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);

         return angle_P;

}

 

float yijiehubu_R(float angle_m, float gyro_m)//採集後計算的角度和角加速度

{

     angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);

         return angle_R;

}

單靠 MPU6050 無法準確得到 Yaw 角，需要和地磁傳感器結合使用。

三、卡爾曼濾波

      其實卡爾曼濾波和一階互補有些相似，輸入也是一樣的。卡爾曼原理以及什麼5個公式等等的，我也不太懂，就不寫了，感興趣的話可以上網查。在此給出具體程序，和一階互補算法一樣，每次卡爾曼濾波只能得到一個方向的角度。

 

複製代碼

#include<math.h>

#include "stm32f10x.h"

#include "Kalman_Filter.h"

//卡爾曼濾波參數與函數

float dt=0.001;//注意：dt的取值爲kalman濾波器採樣時間

float angle, angle_dot;//角度和角速度

float P[2][2] = {{ 1, 0 },

                 { 0, 1 }};

float Pdot[4] ={ 0,0,0,0};

float Q_angle=0.001, Q_gyro=0.005; //角度數據置信度,角速度數據置信度

float R_angle=0.5 ,C_0 = 1;

float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

 

//卡爾曼濾波

float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy

{

        angle+=(gyro_m-q_bias) * dt;

        angle_err = angle_m - angle;

        Pdot[0]=Q_angle - P[0][1] - P[1][0];

        Pdot[1]=- P[1][1];

        Pdot[2]=- P[1][1];

        Pdot[3]=Q_gyro;

        P[0][0] += Pdot[0] * dt;

        P[0][1] += Pdot[1] * dt;

        P[1][0] += Pdot[2] * dt;

        P[1][1] += Pdot[3] * dt;

        PCt_0 = C_0 * P[0][0];

        PCt_1 = C_0 * P[1][0];

        E = R_angle + C_0 * PCt_0;

        K_0 = PCt_0 / E;

        K_1 = PCt_1 / E;

        t_0 = PCt_0;

        t_1 = C_0 * P[0][1];

        P[0][0] -= K_0 * t_0;

        P[0][1] -= K_0 * t_1;

        P[1][0] -= K_1 * t_0;

        P[1][1] -= K_1 * t_1;

        angle += K_0 * angle_err; //最優角度

        q_bias += K_1 * angle_err;

        angle_dot = gyro_m-q_bias;//最優角速度

 

        return angle；

}

      作個總結：三種融合算法都能夠輸出姿態角（Pitch 和 Roll ），一次四元數法可以輸出 P、R、Y 三個傾角，計算量比較大。一階互補和卡爾曼濾波每次只能輸出一個軸的姿態角。