关于MPU6050 校准问题请教

镖叔 · 发表于 2013-12-20 23:29:25

我的MPU6050用了互补滤波法得到x， y，z轴的角度但是很不准，平放模块时不为x轴角度不为0，程序如下
//*********************************************************
// 倾角计算（卡尔曼融合）
//*********************************************************

void Angle_Calcu(unsigned char A,unsigned char G)
{
//------加速度--------------------------

//范围为2g时，换算关系：16384 LSB/g
//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14
//因为x>=sinx,故乘以1.3适当放大

Accel_x  = GetData(A); //读取X轴加速度
Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14;    //弧度转换为度,

//-------角速度-------------------------

//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)

Gyro_y = GetData(G);       //静止时角速度Y轴输出为-30左右
Gyro_y = -(Gyro_y + 30)/16.4;       //去除零点偏移，计算角速度值,负号为方向处理
Angle_gy = Angle_gy + Gyro_y*0.01;  //角速度积分得到倾斜角度.

if(A==ACCEL_XOUT_H)
ComplementFilterX(Angle_ax,Gyro_y);
if(A==ACCEL_YOUT_H)
ComplementFilterY(Angle_ax,Gyro_y);
if(A==ACCEL_ZOUT_H)
ComplementFilterZ(Angle_ax,Gyro_y);
}
/*//-------卡尔曼滤波融合-----------------------

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


//-------互补滤波-----------------------

//补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与
//陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度
//0.5为放大倍数，可调节补偿度；0.01为系统周期10ms

Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);
                       */
/**
* 互补滤波参数
*/
float tau = 0.065, p = 0.0;       // 用于计算比例
float dtc = 0.01;                // 抽样时间10ms
static float bias_gx = 0.0,bias_gy = 0.0,bias_gz = 0.0,gy_temp; // 零点漂移误差消除
static float fix_angle,fiy_angle,fiz_angle;
/**
* 互补滤波算法
* angle 为重力感应获得的角度值
* gy_speed 为角速度
*/
float ComplementFilterX(float angle, float gy_sped)
{
bias_gx = bias_gx*0.998+gy_sped*0.002;  // 消除零点误差
gy_temp = gy_sped - bias_gx;          // 减去零点误差
p = tau/(tau+dtc);

// 对角速度gy_temp进行高通滤波,对重力感应得到的角度angle进行低通滤波
fix_angle = p*(fix_angle+gy_temp*dtc)+(1-p)*angle;
return fix_angle;
}
float ComplementFilterY(float angle, float gy_sped)
{
bias_gz = bias_gz*0.998+gy_sped*0.002;  // 消除零点误差
gy_temp = gy_sped - bias_gz;          // 减去零点误差
p = tau/(tau+dtc);

// 对角速度gy_temp进行高通滤波,对重力感应得到的角度angle进行低通滤波
fiy_angle = p*(fiy_angle+gy_temp*dtc)+(1-p)*angle;
return fiy_angle;
}
float ComplementFilterZ(float angle, float gy_sped)
{
bias_gx = bias_gx*0.998+gy_sped*0.002;  // 消除零点误差
gy_temp = gy_sped - bias_gx;          // 减去零点误差
p = tau/(tau+dtc);

// 对角速度gy_temp进行高通滤波,对重力感应得到的角度angle进行低通滤波
fiz_angle = p*(fiz_angle+gy_temp*dtc)+(1-p)*angle;
return fiz_angle;
}
int main(void)
{
Stm32_Clock_Init(9);//系统时钟设置
/*IIC接口初始化*/
I2C_MPU6050_Init();
/*陀螺仪传感器初始化*/
InitMPU6050();
//hw_tick_start(); // 定时器周期性中断，用于提供系统脉搏
while(1)
{


   Angle_Calcu(ACCEL_XOUT_H,GYRO_YOUT_H);
      //Display10BitData(fix_angle);

   Angle_Calcu(ACCEL_YOUT_H,GYRO_ZOUT_H);
      //Display10BitData(fiy_angle);

      Angle_Calcu(ACCEL_ZOUT_H,GYRO_XOUT_H);
      //Display10BitData(fiz_angle);
}
}