MPU6050数据采集及其意义和滤波（一阶互补滤波、二阶互补滤波、卡尔曼滤波）

275891381

1. MPU6050陀螺仪
[kenrobot_code]
// 陀螺仪
float angleAx,gyroGy;
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;

    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//原始数据采集
    angleAx=atan2(ax,az)*180/PI;//加速度计算角度
    gyroGy=-gy/131.00;//陀螺仪角速度，注意正负号与放置有关

// 陀螺仪
float angleAx,gyroGy;
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;

    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//原始数据采集
    angleAx=atan2(ax,az)*180/PI;//加速度计算角度
    gyroGy=-gy/131.00;//陀螺仪角速度，注意正负号与放置有关
[/kenrobot_code]

2.滤波参数及函数
[kenrobot_code]
//一阶互补滤波
float K1 =0.05; // 对加速度计取值的权重
float dt=20*0.001;//注意：dt的取值为滤波器采样时间
float angle1;

void Yijielvbo(float angle_m, float gyro_m)//采集后计算的角度和角加速度
{
     angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}

//二阶互补滤波

float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;
float dt=20*0.001;//注意：dt的取值为滤波器采样时间
float angle2;

void Erjielvbo(float angle_m,float gyro_m)//采集后计算的角度和角加速度
{
    x1=(angle_m-angle2)*(1-K2)*(1-K2);
    y1=y1+x1*dt;
    x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
    angle2=angle2+ x2*dt;
}


//卡尔曼滤波参数与函数
float dt=0.005;//注意：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;


//卡尔曼滤波
void 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;//最优角速度
}
[/kenrobot_code]

3 三种滤波比较源代码 //

[kenrobot_code]
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Timer.h"//时间操作系统头文件  本程序用作timeChange时间采集并处理一次数据

Timer t;//时间类

float timeChange=20;//滤波法采样时间间隔毫秒
float dt=timeChange*0.001;//注意：dt的取值为滤波器采样时间
// 陀螺仪
float angleAx,gyroGy;//计算后的角度（与x轴夹角）和角速度
MPU6050 accelgyro;//陀螺仪类
int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度

//一阶滤波
float K1 =0.05; // 对加速度计取值的权重
//float dt=20*0.001;//注意：dt的取值为滤波器采样时间
float angle1;//一阶滤波角度输出
//二阶滤波
float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;//运算中间变量
//float dt=20*0.001;//注意：dt的取值为滤波器采样时间
float angle2;//er阶滤波角度输出

//卡尔曼滤波参数与函数
float angle, angle_dot;//角度和角速度
float angle_0, angle_dot_0;//采集来的角度和角速度
//float dt=20*0.001;//注意：dt的取值为kalman滤波器采样时间
//一下为运算中间变量
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;

void setup() {
    Wire.begin();//初始化
    Serial.begin(9600);//初始化
    accelgyro.initialize();//初始化

    int tickEvent1=t.every(timeChange, getangle);//本语句执行以后timeChange毫秒执行回调函数getangle

    int tickEvent2=t.every(50, printout) ;//本语句执行以后50毫秒执行回调函数printout，串口输出
}
void loop() {

    t.update();//时间操作系统运行

}
void printout()
{
     Serial.print(angleAx);Serial.print(',');
    Serial.print(angle1);Serial.print(',');
    Serial.print(angle2);Serial.print(',');
    // Serial.print(gx/131.00);Serial.print(',');
    Serial.println(angle);//Serial.print(',');

//   Serial.println(Output);
}


void getangle()
{
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据
    angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角
    gyroGy=-gy/131.00;//计算角速度
    Yijielvbo(angleAx,gyroGy);//一阶滤波
    Erjielvbo(angleAx,gyroGy);//二阶滤波
    Kalman_Filter(angleAx,gyroGy);   //卡尔曼滤波
}



void Yijielvbo(float angle_m, float gyro_m)
{
    angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}

void Erjielvbo(float angle_m,float gyro_m)
{
    x1=(angle_m-angle2)*(1-K2)*(1-K2);
    y1=y1+x1*dt;
    x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
    angle2=angle2+ x2*dt;
}

void Kalman_Filter(double angle_m,double gyro_m)
{
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;//最优角速度
}
[/kenrobot_code]
4 数据绘图


5.附件  mp6050+时间轮换头文件+数据绘图


6.部分使用帮助

mpu6050关于16384.0 131.0 两个参数来历
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据
使劲晃陀螺仪看看最大数据时多大
就知道该除以多少换算成g和度/秒
我的为 +—32768  +—250所以判定 头文件初始化的为最大小量程  +—2g +—250
ax/16384.0=x方向的加速度 单位g ，加速度计算角度用上面的方法
gx/131.0=x轴向角速度  单位 度/秒
来历：

截图出来


6.定时器pid//非全代码
[kenrobot_code]
#include "Timer.h"
//pid参数
float Setpoint;
float Input,Output,errSum,lastErr;
float kp, ki, kd;
float timeChange=10;//pid采样时间间隔毫秒

Timer t;//定时器时间

void setup(void)
{
t.every(timeChange, PIDCalc) ;//每10毫秒pid一次

}
void PIDCalc ()
{
   float error = Setpoint - Input;
   errSum += (error * timeChange);
   float dErr = (error - lastErr) / timeChange;
   Output = kp * error +ki * errSum + kd * dErr;
   lastErr = error;
}


void loop(void)
{
    t.update();
}

[/kenrobot_code]

7.MPU6050_DMP，国外dmp库

使用的时候需要删除#include "MPU6050.h"这个库文件，有冲突；这个库文件#include "MPU6050.h"的功能#include "MPU6050_6Axis_MotionApps20.h"都能实现
[kenrobot_code]
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu(0x68);
float alpha,omiga;


volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
bool dmpReady = false;  // set true if DMP init was successful


uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector


void setup() {
  Serial.begin(115200);
  init_mpu6050();
}


double now,last;

void loop()
{  
     clc_mpu6050();   
}



void init_mpu6050(void)
{
Wire.begin();
  mpu.initialize();
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(2);
  devStatus = mpu.dmpInitialize();
  if (devStatus == 0) {
    Serial.println("Enabling DMP...");
    mpu.setDMPEnabled(true);
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    Serial.println("DMP ready! Waiting for first interrupt...");
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }
  else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print("DMP Initialization failed (code ");
    Serial.print(devStatus);
    Serial.println(")");
  }
}

void clc_mpu6050(void)//9-11ms 更新一次数据  pid 取20ms
{
  if (!dmpReady) return;
  if (!mpuInterrupt && fifoCount < packetSize) return;
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    mpu.resetFIFO();
    Serial.println("FIFO overflow!");
  }

  else if (mpuIntStatus & 0x02) {
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  
   
    alpha=-ypr[1] * 180/M_PI;//从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。单位：弧度
    omiga=mpu.getRotationY()/16.4;//配置是16位表示正负2000°/s, 65536/4000
   // Serial.println(millis()-last);
   // last=millis();
    Serial.print(alpha);
    Serial.print(",");
    Serial.println(omiga);
  }
}

void dmpDataReady() {

  mpuInterrupt = true;

}
[/kenrobot_code]

8，卡尔曼库
https://github.com/TKJElectronics/KalmanFilter里面的例子有绘图部分，附件太大给个链接
[kenrobot_code]
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Kalman.h>

Kalman kalmanY;
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;
double accX, accY, accZ, gyroX, gyroY, gyroZ;
uint32_t timer;
double pitch, Angle, Yrate;
void setup() {
  Wire.begin();
  Serial.begin(115200);
  accelgyro.initialize();
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  accX = ax; accY = ay; accZ = az; gyroX = gx; gyroY = gy; gyroZ = gz;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  kalmanY.setAngle(pitch);


}

void loop() {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    accX = ax; accY = ay; accZ = az; gyroX = gx; gyroY = gy; gyroZ = gz;
    double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
    timer = micros();
    pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
    Yrate = gyroY / 131.0; // Convert to deg/s
    Angle = kalmanY.getAngle(pitch, Yrate, dt);
  
  Serial.print(Angle); Serial.print(',');
  Serial.println(Yrate);
}
[/kenrobot_code]

hsr18299

作個記號,改天好好研究
感謝您

wo123

好贴，马克~~

275891381

慢、节奏 发表于 2014-9-11 22:13
看了楼主的程序，自己亲测是可以用而且很准，但是楼主测的是X轴的倾角，我现在想测Y轴的倾角，修改程序如下 ...

程序你改的没问题  滞后的话 不用滤波直接用 angleAy gyroGx 试试，我估计是滤波参数有关

赤色强袭

这个好啊！！！！

咯小3虽

赞一个！！！！！

数字

very good!!!!!!

firewise

非常好，谢谢了

diyreon

VCC应该接5V的吧

4463424

好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟

ranqingfa

做的漂亮，卡尔曼在这里并没有显出明显的优势，我后来干脆就互补了

wetnt

非常棒的帖子，果断收藏！

275891381

ranqingfa 发表于 2014-6-24 11:42
做的漂亮，卡尔曼在这里并没有显出明显的优势，我后来干脆就互补了

卡尔曼参数没调好应该，调好了理论上应该漂亮一点

hrrstone

非常好，谢谢了

shenhaiyu

做得好，这才是技术贴，非常感谢分享~~~~~！！！！

wing

有介绍算法原理么?