请选择 进入手机版 | 继续访问电脑版

 找回密码
 立即注册
搜索
查看: 13933|回复: 6

mpu6050角度滤波后问题

[复制链接]

1

主题

11

帖子

20

积分

新手上路

Rank: 1

积分
20
发表于 2017-5-8 20:02:23 | 显示全部楼层 |阅读模式
互补滤波或卡尔曼滤波后的倾角比直接读取的角度慢半拍,所以在直立控制时总是慢半拍并且加速过头,以至于迅速反方向倒下。请问这是什么原因啊?谢谢!白线是滤波前的数据。
回复

使用道具 举报

1

主题

11

帖子

20

积分

新手上路

Rank: 1

积分
20
 楼主| 发表于 2017-5-8 20:05:30 | 显示全部楼层
图片发不上去啊 DataScope-OutPut[17-5-8 18_29]-1.jpg
回复

使用道具 举报

5

主题

231

帖子

1192

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
1192
发表于 2017-5-8 23:32:05 | 显示全部楼层
魂萦xy 发表于 2017-5-8 20:05
图片发不上去啊

代码是在中断里面执行吗?dt值是否和代码执行周期一致,是否足够短?比如5ms
回复

使用道具 举报

1

主题

11

帖子

20

积分

新手上路

Rank: 1

积分
20
 楼主| 发表于 2017-5-8 23:40:01 | 显示全部楼层
admin 发表于 2017-5-8 23:32
代码是在中断里面执行吗?dt值是否和代码执行周期一致,是否足够短?比如5ms

是在中断中执行的5ms一次,我去掉滤波后能够在平衡位置一直高频抖动,是滤波算法有问题吗?代码如下:
                Gyro_X=Gyro_X/16.4;                //陀螺仪量程转换为+-2000C/S       
                Angle_X=atan2(Accel_Y,Accel_Z)*180/PI;  //计算X轴倾角       
//                if(way==2)                //卡尔曼滤波
                       
                if(way==3)
                        angle1= 0.02 * Angle_X+ (1-0.02) * (angle1-Gyro_X * 0.005);//一阶滤波
//                        angle2 =Kalman_Filter(Angle_X,-Gyro_X);                //卡尔曼滤波
                *Angle_Bal=angle1;       
回复

使用道具 举报

1

主题

11

帖子

20

积分

新手上路

Rank: 1

积分
20
 楼主| 发表于 2017-5-8 23:51:34 | 显示全部楼层
这是完整的角度获取程序,帮忙看看,调了好久感觉就是这个滤波的问题,谢谢!

void Get_Angle(float *Angle_Bal,float *Gyro_Bal,u8 way)
{
        float Accel_X,Accel_Y,Accel_Z,Gyro_Y,Gyro_Z;
//        float Angle_X;
        if(way==1)                        //DMP
        {
                Read_DMP();      //===读取加速度、角速度、倾角
                *Angle_Bal=Pitch;    //===更新平衡倾角
                *Gyro_Bal=gyro[1];   //===更新平衡角速度
        }
        else
        {
                Gyro_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L);    //读取X轴陀螺仪
                Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);    //读取Y轴陀螺仪
                Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);    //读取Z轴陀螺仪
                Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度计
                Accel_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //读取Y轴加速度计
                Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度计
                if(Gyro_X>32768)  
                        Gyro_X-=65536;   
                if(Gyro_Y>32768)  
                        Gyro_Y-=65536;                //数据类型转换,也可通过short强制类型转换
                if(Gyro_Z>32768)  
                        Gyro_Z-=65536;                //数据类型转换
                if(Accel_X>32768)
                        Accel_X-=65536;                //数据类型转换
                if(Accel_Y>32768)
                        Accel_Y-=65536;                //数据类型转换
                if(Accel_Z>32768)
                        Accel_Z-=65536;                //数据类型转换
                *Gyro_Bal=Gyro_X;        //得到x轴角速度
                Gyro_X=Gyro_X/16.4;                //陀螺仪量程转换为+-2000C/S       
                Angle_X=atan2(Accel_Y,Accel_Z)*180/PI;  //计算X轴倾角       
//                if(way==2)                //卡尔曼滤波
                       
                if(way==3)
                        angle1= 0.02 * Angle_X+ (1-0.02) * (angle1-Gyro_X * 0.005);//一阶滤波
//                        angle2 =Kalman_Filter(Angle_X,-Gyro_X);                //卡尔曼滤波
                *Angle_Bal=angle1;       
        }
}
回复

使用道具 举报

1

主题

11

帖子

20

积分

新手上路

Rank: 1

积分
20
 楼主| 发表于 2017-5-9 11:12:39 | 显示全部楼层
已经解决了,是滤波算法的问题,k1太小,迭代次数太多了,stm32速度跟不上估计。把K1调大一点,滞后会减小一些,我还是理论知识不够强。
angle1= K1 * Angle_X+ (1-K1) * (angle1-Gyro_X * 0.005);//一阶滤波
回复

使用道具 举报

0

主题

2

帖子

8

积分

新手上路

Rank: 1

积分
8
发表于 2017-12-19 16:31:24 | 显示全部楼层
楼主这个卡尔曼滤波是不是不能用?
你的小车好了吗?
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

粤ICP备20017043号|小黑屋|手机版|Archiver|轮趣科技(东莞)有限公司  

GMT+8, 2024-3-29 19:26 , Processed in 0.061097 second(s), 24 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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