魂萦xy 发表于 2017-5-8 20:02:23

mpu6050角度滤波后问题

互补滤波或卡尔曼滤波后的倾角比直接读取的角度慢半拍，所以在直立控制时总是慢半拍并且加速过头，以至于迅速反方向倒下。请问这是什么原因啊？谢谢！白线是滤波前的数据。

魂萦xy 发表于 2017-5-8 20:05:30

图片发不上去啊

admin 发表于 2017-5-8 23:32:05

魂萦xy 发表于 2017-5-8 20:05
图片发不上去啊

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

魂萦xy 发表于 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;       

魂萦xy 发表于 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;   //===更新平衡角速度
        }
        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;       
        }
}

魂萦xy 发表于 2017-5-9 11:12:39

已经解决了，是滤波算法的问题，k1太小，迭代次数太多了，stm32速度跟不上估计。把K1调大一点，滞后会减小一些，我还是理论知识不够强。
angle1= K1 * Angle_X+ (1-K1) * (angle1-Gyro_X * 0.005);//一阶滤波

夜问夜问 发表于 2017-12-19 16:31:24

楼主这个卡尔曼滤波是不是不能用？
你的小车好了吗？
页: [1]
查看完整版本: mpu6050角度滤波后问题