NQ的图书馆 / 自平衡车 / 关于MPU6050 校准问题请教

分享

   

关于MPU6050 校准问题请教

2016-10-22  NQ的图书馆
     我的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);
}
}

    本站是提供个人知识管理的网络存储空间,所有内容均由用户发布,不代表本站观点。请注意甄别内容中的联系方式、诱导购买等信息,谨防诈骗。如发现有害或侵权内容,请点击一键举报。

    0条评论

    发表

    请遵守用户 评论公约

    类似文章 更多
    喜欢该文的人也喜欢 更多

    ×
    ×

    ¥.00

    微信或支付宝扫码支付:

    开通即同意《个图VIP服务协议》

    全部>>