智能车制作
标题: 卡尔曼滤波，自平衡车 [打印本页]
作者: 疯狂土豆儿 时间: 2016-8-9 17:40
标题: 卡尔曼滤波，自平衡车
我用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的变化正常吗？
2，Angle正常变化的时候是应该与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; //dt为kalman滤波器采样时间;
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); //卡尔曼滤波计算倾角
作者: 疯狂土豆儿 时间: 2016-8-9 18:19
现在情况就是，就算我是在减小倾角，只要我快速地改变，它显示的倾角都会先增大再减小，而如果我慢速改变的话，倾角就会缓慢减小而不会出现那些增大的角
作者: 黄施泓 时间: 2016-8-12 13:41
先增大后变小不是角速度 吗？
作者: 疯狂土豆儿 时间: 2016-8-13 15:06
角速度和倾角都是这样变化，这样是我的卡尔曼算法的问题还是参数的问题，还是说正常啊？
作者: 黄施泓 时间: 2016-8-13 16:56
这样倾角变化肯定不对啊，滤波是把倾角那部分的噪声滤掉。你角度不变，值是不可能上去又下来的。或者你角速度极性错了，加个负号试试。
补充内容 (2016-8-13 17:49):
Kalman_Filter(Angle_ax,Gyro_y); 而且为啥你用的是加速度是X轴，陀螺仪是Y轴，应该是同轴吧
作者: 个性十足 时间: 2016-8-30 10:05
平衡车总朝一个方向走，这是什么原因，求大神指导
| 欢迎光临 智能车制作 (http://dns.znczz.com/) |
Powered by Discuz! X3.2 |