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

  1. MPU6050陀螺仪



    // 陀螺仪

    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;//陀螺仪角速度，注意正负号与放置有关





    2.滤波参数及函数

    //一阶互补滤波

    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+x1dt;

    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[

mpu6050.png

2014-6-23 14:09 上传
(432.1 KB)



#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+x1dt;

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[]=Q_angle - P[

123.png

2014-6-23 13:50 上传
(49.73 KB)





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

串口数据采集.rar

(4.27 MB, 下载次数: 1110)

2014-6-23 13:55 上传
点击文件名下载附件


头文件.rar

(53.17 KB, 下载次数: 949)

2014-6-23 13:55 上传
点击文件名下载附件



6.部分使用帮助

定时器常用函数.rar

(10.68 KB, 下载次数: 738)

2014-6-23 14:00 上传
点击文件名下载附件



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轴向角速度单位 度/秒

来历：

MPU-6050.pdf

(1.6 MB, 下载次数: 415)

2014-6-23 14:16 上传
点击文件名下载附件



截图出来

1.png

2014-6-23 14:16 上传
(64.17 KB)


2.png

2014-6-23 14:16 上传
(62.04 KB)



6.定时器pid//非全代码



#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();

}











via - 极客工坊

标签: Arduino教程