mpu6050角度滤波后问题

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

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

魂萦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[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;
}
}

魂萦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

楼主这个卡尔曼滤波是不是不能用？
你的小车好了吗？

		自动登录	找回密码
密码			立即注册