mpu6050的陀螺仪的角速度换成角度算法

疯狂土豆儿 · 发表于 2016-8-8 21:00:37

楼主，我用卡尔曼滤波了烧完程序后发现，我快速改变板子的倾角时得到的角度数值很大，比如变化10度时，如果我快速的变化10度，它瞬间可能显示100度以上，而我慢速改变的时候，它是正常的值。这种变化是正常的吗？我用的是MPU6050测速的，然后用的卡尔曼滤波测得倾角。MPU6050的两个轴的角度显示的变化一直是正常的，就是融合后的倾角有这个问题

黄施泓 · 发表于 2016-8-9 11:20:47

疯狂土豆儿发表于 2016-8-8 21:00
楼主，我用卡尔曼滤波了烧完程序后发现，我快速改变板子的倾角时得到的角度数值很大，比如变化10度时，如 ...

上图才好说话。你滤完波

回来16位数据有归一化成90度吗？

疯狂土豆儿 · 发表于 2016-8-9 16:17:09

我是想做一个自平衡车，用卡尔曼滤波完了之后得到的就是角度啊，16位数据是什么啊？不太懂

*************读取数据********************
//定义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:13:12

疯狂土豆儿发表于 2016-8-9 16:17
我是想做一个自平衡车，用卡尔曼滤波完了之后得到的就是角度啊，16位数据是什么啊？不太懂

************ ...

现在情况就是，就算我是在减小倾角，只要我快速地改变，它显示的倾角都会先增大再减小，而如果我慢速改变的话，倾角就会缓慢减小而不会出现那些增大的角

疯狂土豆儿 · 发表于 2016-8-10 13:11:47

人呢？！！！求助啊，help

疯狂土豆儿 · 发表于 2016-8-15 18:39:36

我在做平衡小车，在卡尔曼滤波计算出倾角之后我再进行PID调节的时候，因为在PID调节函数里面加上了if(Angle>-3.0&&Angle<3.0){Angle=0.0;}，然后发现倾角在我缓慢变化至3度后，刚过3度时倾角会突然猛的增加，然后减小最后稳定在正常值上，如果我不加这句话，倾角变化一直正常，百思不得其解，希望有大神能帮帮我。。。。

另外，卡尔曼滤波和速度PID控制我都是写在中断里的，周期5ms，PID参数我暂时只加了一个P调节，其他均为零。

//******卡尔曼参数************

const float Q_angle=0.001;

const float Q_gyro=0.003;

const float R_angle=0.5;

const float dt=0.07; //dt为kalman滤波器采样时间;或0.005,0.07

const char C_0 = 1;

float Q_bias=0.0, Angle_err=0.0;

float PCt_0=0, PCt_1=0, E=0;

float K_0=0, K_1=0, t_0=0, t_1=0;

float Pdot[4] ={0,0,0,0};

float PP[2][2] = { { 1, 0 },{ 0, 1 } };

float position;

static const float Kp = 40.0; //PID参数 Angle

static const float Kd = 0.0; //PID参数 Gyro_y

static const float Ksp = 0.0; //PID参数 motor_speed

static const float Ksi = 0.0; //PID参数 position

/*****************卡尔曼滤波**************************************************/

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;

Angle_gy += (Gyro_y)*0.003;

/***************************卡尔曼融合*************************************/

Kalman_Filter(Angle_ax,Angle_gy); //卡尔曼滤波计算倾角

}

void Speed_Pid_Calculate(void)

{

if(Angle>-3.0&&Angle<3.0){Angle=0.0;}

Speed_control = (Kp*Angle + Kd*Gyro_y); //PID：角速度和角度调节

Speed_control +=(Ksp*motor_speed + Ksi*position);

//PID：车速度调节

speed_control_l= (int)Speed_control ; //左轮速度

speed_control_r= (int)- Speed_control; //右轮速度

if(speed_control_l> MAX_SPEED) speed_control_l = MAX_SPEED;

if(speed_control_l< -MAX_SPEED) speed_control_l = -MAX_SPEED;

if(speed_control_r> MAX_SPEED) speed_control_r = MAX_SPEED;

if(speed_control_r< -MAX_SPEED) speed_control_r = -MAX_SPEED;

}

疯狂土豆儿 · 发表于 2016-8-15 18:39:59

黄施泓发表于 2016-8-9 11:20
上图才好说话。你滤完波回来16位数据有归一化成90度吗？