智能车制作

 找回密码
 注册

扫一扫,访问微社区

查看: 12201|回复: 5

卡尔曼滤波,自平衡车

[复制链接]

2

主题

15

帖子

0

精华

注册会员

Rank: 2

积分
124
威望
72
贡献
34
兑换币
33
注册时间
2016-8-8
在线时间
9 小时
毕业学校
北方工业大学
发表于 2016-8-9 17:40:34 | 显示全部楼层 |阅读模式
我用printf("%0.2f   %0.2f   %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度,角速度和倾角,我发现当我快速的改变板子的倾角的时候,比如快速变化10度,Angle(卡尔曼滤波后的倾角)瞬时变化非常快,可能会瞬间变化几十度然后回到正常角度,而当我缓慢变化10度的时候,Angle变化是正常线性变化到10度,在这两种变化中,Angle_ax(从MPU6050读取的值经过处理后的陀螺仪的Y轴数据)的变化一直都是线性正常的,并且Angle的值特别接近Angle_ax的值
问题:1,我快速改变板子倾角时Angle的变化正常吗?
     2Angle正常变化的时候是应该与Angle_ax的值相近吗?
*************读取数据********************
//定义MPU6050内部地址
#define     SMPLRT_DIV             0x19          //陀螺仪采样率典型值 0X07 125Hz
#define     CONFIG                        0x1A     //低通滤波频率 典型值 0x00
#define     GYRO_CONFIG                  0x1B         //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s
#define     ACCEL_CONFIG        0x1C         //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG     0x37
#define INT_ENABLE      0x38
#define INT_STATUS      0x3A   //只读
#define     ACCEL_XOUT_H       0x3B
#define     ACCEL_XOUT_L        0x3C
#define     ACCEL_YOUT_H       0x3D
#define     ACCEL_YOUT_L        0x3E
#define     ACCEL_ZOUT_H       0x3F
#define     ACCEL_ZOUT_L        0x40
#define     TEMP_OUT_H          0x41
#define     TEMP_OUT_L           0x42
#define     GYRO_XOUT_H    0x43
#define     GYRO_XOUT_L                  0x44         
#define     GYRO_YOUT_H        0x45
#define     GYRO_YOUT_L                  0x46
#define     GYRO_ZOUT_H        0x47
#define     GYRO_ZOUT_L                  0x48
//读取寄存器原生数据
         
       MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);
       MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);
       MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
       MPU6050_Raw_Data.Temp =   (buf[6]<<8 | buf[7]);  
       MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
       MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
       MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
      
      
       //将原生数据转换为实际值,计算公式跟寄存器的配置有关
       MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
       MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
       MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
             MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X- gyroADC_X_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y- gyroADC_Y_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z- gyroADC_Z_offset)/65.5;   
    }
   
//******卡尔曼参数************
                  
const float Q_angle=0.001;  
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01;                           //dtkalman滤波器采样时间;
const char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
/*****************卡尔曼滤波**************************************************/
void Kalman_Filter(float Accel,float Gyro)               
{
         Angle+=(Gyro- Q_bias) * dt; //先验估计
         
         Pdot[0]=Q_angle- PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
         Pdot[1]=-PP[1][1];
         Pdot[2]=-PP[1][1];
         Pdot[3]=Q_gyro;
         
         PP[0][0]+= Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
         PP[0][1]+= Pdot[1] * dt;   // =先验估计误差协方差
         PP[1][0]+= Pdot[2] * dt;
         PP[1][1]+= Pdot[3] * dt;
                  
         Angle_err= Accel - Angle;        //zk-先验估计
         
         PCt_0= C_0 * PP[0][0];
         PCt_1= C_0 * PP[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 * PP[0][1];
         PP[0][0]-= K_0 * t_0;                  //后验估计误差协方差
         PP[0][1]-= K_0 * t_1;
         PP[1][0]-= K_1 * t_0;
         PP[1][1]-= K_1 * t_1;
                  
         Angle        += K_0 * Angle_err;           //后验估计
         Q_bias      += K_1 * Angle_err;           //后验估计
         Gyro_y   = Gyro - Q_bias;       //输出值(后验估计)的微分=角速度
}
******************倾角计算*****************
void Angle_Calculate(void)
{
/****************************加速度****************************************/
         
         Accel_x  = MPU6050_Real_Data.Accel_X;           //读取X轴加速度
         Angle_ax= Accel_x*1.2*180/3.14;     //弧度转换为度
/****************************角速度****************************************/
         
          Gyro_y = MPU6050_Real_Data.Gyro_Y;           
时间dt,所以此处不用积分
/***************************卡尔曼融合*************************************/
         Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
         

回复

使用道具 举报

0

主题

36

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1328
威望
596
贡献
384
兑换币
393
注册时间
2015-12-22
在线时间
174 小时
发表于 2016-8-13 16:56:38 | 显示全部楼层
这样倾角变化肯定不对啊,滤波是把倾角那部分的噪声滤掉。你角度不变,值是不可能上去又下来的。或者你角速度极性错了,加个负号试试。

补充内容 (2016-8-13 17:49):
Kalman_Filter(Angle_ax,Gyro_y);  而且为啥你用的是加速度是X轴,陀螺仪是Y轴,应该是同轴吧
回复 支持 1 反对 0

使用道具 举报

2

主题

15

帖子

0

精华

注册会员

Rank: 2

积分
124
威望
72
贡献
34
兑换币
33
注册时间
2016-8-8
在线时间
9 小时
毕业学校
北方工业大学
 楼主| 发表于 2016-8-9 18:19:02 | 显示全部楼层
现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现那些增大的角
回复 支持 反对

使用道具 举报

0

主题

36

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1328
威望
596
贡献
384
兑换币
393
注册时间
2015-12-22
在线时间
174 小时
发表于 2016-8-12 13:41:30 | 显示全部楼层
先增大后变小不是角速度 吗?
回复 支持 反对

使用道具 举报

2

主题

15

帖子

0

精华

注册会员

Rank: 2

积分
124
威望
72
贡献
34
兑换币
33
注册时间
2016-8-8
在线时间
9 小时
毕业学校
北方工业大学
 楼主| 发表于 2016-8-13 15:06:26 | 显示全部楼层
黄施泓 发表于 2016-8-12 13:41
先增大后变小不是角速度 吗?

角速度和倾角都是这样变化,这样是我的卡尔曼算法的问题还是参数的问题,还是说正常啊?
回复 支持 反对

使用道具 举报

0

主题

1

帖子

0

精华

注册会员

Rank: 2

积分
26
威望
24
贡献
2
兑换币
3
注册时间
2016-8-30
在线时间
0 小时
毕业学校
清华大学
发表于 2016-8-30 10:05:02 | 显示全部楼层
平衡车总朝一个方向走,这是什么原因,求大神指导
回复 支持 反对

使用道具 举报

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

本版积分规则

关于我们|联系我们|小黑屋|智能车制作 ( 黑ICP备2022002344号

GMT+8, 2024-7-19 20:57 , Processed in 0.065448 second(s), 38 queries , Gzip On.

Powered by Discuz! X3.2

© 2001-2013 Comsenz Inc.

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