MPU6050數據輕鬆分析

Arduino代碼 在開始之前先介紹一下用到的組件和算法。Mpu6050是通過i2c接口和arduino連接，所以需要i2c庫，另外mpu6050也有現成的庫。由於陀螺儀本身就有偏差，多次積累之後偏差會越來越大越來越不準確，所以需要用一些濾波算法來校正，這裡用的是卡爾曼濾波算法。 卡爾曼濾波在網上講的很多，這裡就不多說了。網上有一個測量溫度的通俗版，另外維基百科裡的介紹比較簡單。有興趣的可以去研究一下。另外也可以用其它算法來濾波，這裡也只是做演示所以不多介紹。

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h

#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project

#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69

MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
double total_angle = 0;

/* 把mpu6050放在水平桌面上，分別讀取讀取2000次，然後求平均值 */
#define AX_ZERO (-1476) /* 加速度計的0偏修正值 */
#define GX_ZERO (-30.5) /* 陀螺儀的0偏修正值 */

void setup()
{
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();
    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(38400);
    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" :
                   "MPU6050 connection failed");
}

/* 通過卡爾曼濾波得到的最終角度 */
float Angle = 0.0;
/*由角速度計算的傾斜角度 */
float Angle_gy = 0.0;
float Q_angle = 0.9;
float Q_gyro = 0.001;
float R_angle = 0.5;
float dt = 0.01; /* dt為kalman濾波器採樣時間; */
char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0 = 0.0, PCt_1 = 0.0, E = 0.0;
float K_0 = 0.0, K_1 = 0.0, t_0 = 0.0, t_1 = 0.0;
float Pdot[4] = {0, 0, 0, 0};
float PP[2][2] = { { 1, 0 }, { 0, 1 } };

/* 卡爾曼濾波函數,具體實現可以參考網上資料,也可以使用其它濾波算法 */
void Kalman_Filter(float Accel, float Gyro)
{
    Angle += (Gyro - Q_bias) * dt;
    Pdot[0] = Q_angle - PP[0][1] - PP[1][0];
    Pdot[1] = - PP[1][1];
    Pdot[2] = - PP[1][1];
    Pdot[3] = Q_gyro;

    PP[0][0] += Pdot[0] * dt;
    PP[0][1] += Pdot[1] * dt;
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;
    Angle_err = Accel - Angle;
    PCt_0 = C_0 * PP[0][0];
    PCt_1 = C_0 * PP[1][0];
    E = R_angle + C_0 * PCt_0;

    if (E != 0) {
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
    }

    t_0 = PCt_0;
    t_1 = C_0 * PP[0][1];

    PP[0][0] -= K_0 * t_0;
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;
    Angle        += K_0 * Angle_err;
    Q_bias      += K_1 * Angle_err;
}

void loop()
{
    // read raw accel/gyro measurements from device
    double ax_angle = 0.0;
    double gx_angle = 0.0;
    unsigned long time = 0;
    unsigned long mictime = 0;
    static unsigned long pretime = 0;
    float gyro = 0.0;

    if (pretime == 0) {
        pretime = millis();
        return;
    }

    mictime = millis();
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    /*
    * 加速度量程範圍設置2g 16384 LSB/g
    * 計算公式：
    * 前邊已經推導過這裡再列出來一次
    * x是小車傾斜的角度,y是加速度計讀出的值
    * sinx = 0.92*3.14*x/180 = y/16384
    * x=180*y/(0.92*3.14*16384)=
    */
    ax -= AX_ZERO;
    ax_angle = ax / 262;
    /*
    * 陀螺儀量程範圍設置250 131 LSB//s
    * 陀螺儀角度計算公式:
    * 小車傾斜角度是gx_angle,陀螺儀讀數是y,時間是dt
    * gx_angle +=(y/(131*1000))*dt
    */
    gy -= GX_ZERO;
    time = mictime - pretime;
    gyro = gy / 131.0;
    gx_angle = gyro * time;
    gx_angle = gx_angle / 1000.0;
    total_angle -= gx_angle;
    dt = time / 1000.0;
    Kalman_Filter(ax_angle, gyro);
    Serial.print(ax_angle);
    Serial.print(",");
    Serial.print(total_angle);
    Serial.print(",");
    Serial.println(Angle);
    pretime = mictime;
}
Mpu6050取值範圍調整的方法, 修改MPU6050庫中的mpu6050.cpp,
        修改initialize函數中標成紅色的行。
        void MPU6050::initialize()
{
    setClockSource(MPU6050_CLOCK_PLL_XGYRO);
    setFullScaleGyroRange(MPU6050_GYRO_FS_250); /* 陀螺儀取值範圍 */
    setFullScaleAccelRange(MPU6050_ACCEL_FS_2); /* 加速度計取值範圍 */
    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
}