极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 67535|回复: 129

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

    [复制链接]
发表于 2014-6-23 13:56:01 | 显示全部楼层 |阅读模式
本帖最后由 迷你强 于 2017-4-9 21:54 编辑

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

// 陀螺仪
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+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;//最优角速度
}


3 三种滤波比较源代码 //
mpu6050.png

#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;//最优角速度
}

4 数据绘图
123.png

5.附件  mp6050+时间轮换头文件+数据绘图
串口数据采集.rar (4.27 MB, 下载次数: 1898)
回复

使用道具 举报

发表于 2014-6-23 21:13:23 | 显示全部楼层
好贴,马克~~
回复 支持 0 反对 1

使用道具 举报

 楼主| 发表于 2014-9-12 12:05:02 | 显示全部楼层
本帖最后由 275891381 于 2014-9-12 12:06 编辑
慢、节奏 发表于 2014-9-11 22:13
看了楼主的程序,自己亲测是可以用而且很准,但是楼主测的是X轴的倾角,我现在想测Y轴的倾角,修改程序如下 ...


程序你改的没问题  滞后的话 不用滤波直接用 angleAy gyroGx 试试,我估计是滤波参数有关
回复 支持 1 反对 0

使用道具 举报

发表于 2014-6-23 19:06:25 | 显示全部楼层
作個記號,改天好好研究
感謝您
回复 支持 反对

使用道具 举报

发表于 2014-6-23 21:41:25 | 显示全部楼层
这个好啊!!!!
回复 支持 反对

使用道具 举报

发表于 2014-6-24 00:57:45 | 显示全部楼层
赞一个!!!!!
回复 支持 反对

使用道具 举报

发表于 2014-6-24 08:16:51 | 显示全部楼层
very good!!!!!!
回复 支持 反对

使用道具 举报

发表于 2014-6-24 09:18:45 | 显示全部楼层
非常好,谢谢了
回复 支持 反对

使用道具 举报

发表于 2014-6-24 10:29:50 | 显示全部楼层
VCC应该接5V的吧
回复 支持 反对

使用道具 举报

发表于 2014-6-24 10:53:05 | 显示全部楼层
好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟好东东哟
回复 支持 反对

使用道具 举报

发表于 2014-6-24 11:42:33 | 显示全部楼层
做的漂亮,卡尔曼在这里并没有显出明显的优势,我后来干脆就互补了
回复 支持 反对

使用道具 举报

高级模式  
您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2017-10-19 08:14 , Processed in 0.045074 second(s), 7 queries , File On.

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表