平衡车互补滤波后的图象，求鉴定是否可用

hanlinson

以下图象是加速度计和陀螺仪进入互补滤波器后的结果，
红色为加速度计的角度输入，黄色为陀螺仪的角速度输入，绿色为互补滤波后的角度输出
第一张为加速度计和陀螺仪直接输入互补滤波器的图象

第二张为MPU6050装在小车上面，未启动电机，手动模拟小车摆动时

最后请懂互补滤波的大哥，看看这样的滤波器能行吗？、

仪124

hanlinson 发表于 2013-8-10 21:02
给你核心程序，你慢慢琢磨把！
float Read_Acc_x()
{

你好，我对“X-Y平面上所对应的(x,y)坐标的角度”不太理解，为什么要这样算，加速度计输出值减平衡值乘以比例， 陀螺仪输出值乘以比例 ，按互补滤波公式写好，保持互补滤波曲线与陀螺仪曲线跟随性一致，这样基础上调平衡车可以不？

仪124

hanlinson 发表于 2013-8-10 21:02
给你核心程序，你慢慢琢磨把！
float Read_Acc_x()
{

强

1249652816

hanlinson 发表于 2013-8-10 21:02
给你核心程序，你慢慢琢磨把！
float Read_Acc_x()
{

其实我想要的是IO口模拟IIC通信程序

hanlinson

1249652816 发表于 2013-8-10 20:51
MPU6050求教学

给你核心程序，你慢慢琢磨把！
float Read_Acc_x()
{
    float Accel_x,Accel_z, Angle_accel;
    //------加速度--------------------------

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

    Accel_x  = GetData(ACCEL_XOUT_H);      //从mpu6050读取X轴加速度
    Accel_z  = GetData(ACCEL_ZOUT_H);      //从mpu6050读取z轴加速度
            if(Accel_x>0)
            {
                Angle_accel = atan2((float)Accel_x,(float)Accel_z)*(180/3.14159265);//反正切计算rad
/*    atan2(y,x)是表示X-Y平面上所对应的(x,y)坐标的角度，
                  它的值域范围是(-Pi,Pi)   
      用数学表示就是：atan2(y,x)=arg(y/x)-Pi   
      当y<0时，其值为负，   
      当y>0时，其值为正. atan2*180/Pi可以计算出角度值         */
               
            }
                else
                {
                   Angle_accel = atan2((float)Accel_z,(float)Accel_x)*(180/3.14159265)-90;//反正切计算
                        if(Accel_z<0)
                            Angle_accel=Angle_accel+360;
                    Angle_accel = -Angle_accel;
                }
                                    /*    atan2(y,x)是表示X-Y平面上所对应的(x,y)坐标的角度，
                                              它的值域范围是(-Pi,Pi)   
                                  用数学表示就是：atan2(y,x)=arg(y/x)-Pi   
                                  当y<0时，其值为负，   
                                  当y>0时，其值为正. atan2*180/Pi可以计算出角度值         */
                return Angle_accel;
}

1249652816

hanlinson 发表于 2013-8-4 22:30
你又没有办法办法确定主控，因为你不知道到底是陀螺仪准，还是加速度计准，我这儿选的陀螺仪准，但是事实 ...

MPU6050求教学

hanlinson

yxw_bob 发表于 2013-8-4 18:20
对了。你的波形差不多了，比较不错啦。       如果采用互补滤波的话，在表达式中angle_dot=k1*angle_dot+k2 ...

你又没有办法办法确定主控，因为你不知道到底是陀螺仪准，还是加速度计准，我这儿选的陀螺仪准，但是事实上我的陀螺仪也不准，在PMU6050静止时，输出为三分之一是15，三分之二是0，然后我前面先加了一个求平均然后再搞了一个低通滤波，呵呵！我这儿取得是，0.95和0.05，效果还是不错，

yxw_bob

对了。你的波形差不多了，比较不错啦。       如果采用互补滤波的话，在表达式中angle_dot=k1*angle_dot+k2*gyro_m_cf;
    主参数所占的比重必须在0.95以上，最好在0.98-0.999之间。我是说转换为相同的单位的情况下的主参数的比重值。如果单位不同则没法给出限定。

yxw_bob

再说一下，陀螺仪的原始输出信号为角加速度w=d(theita) /dt。  你的系数为应该与你的采样率相关的。我上面给你的公式是在直立车偏离垂直方向比较小的角度(theita)下得到的。
真正的过程如下：
  （1） 计算 小车的等效运动加速度 a= g*cos(theita)-w*L ，[ g*cos(theita)为三轴加速度的小车运动方向的分量] ；；；[应该在这里进行卡尔曼滤波，或者互补滤波；但是有的为了利用非线性，对角度进行卡尔曼滤波，或者互补滤波]。
  {在这里a= g*cos(theita)-w*L，中 g*cos(theita)这项的值可能有误，请参照卓老师给出的第2版的计算}
   ( 2)  利用上面求出的  a来反求theita，[即小车偏离垂直方向的角度(theita)]，其实这个角度theita，表明你的小车是出于加速状态，还是减速状态，还是匀速速度状态。根据这里得出的状态和你期望的状态来对小车进行直立、方向、加/减速状态进行控制。[请注意为了进行得到更大的加速度，必须先进行减速/加速得到相应的偏离角度后再进行加速/减速操作。为了保证加速度是持续的，必须保证小车偏离垂直方向的角度在合理的区域内]  。
         在这里，小车偏离垂直方向是指小车偏离重心的垂直方向。而不是车体作为垂直方向。

hanlinson

奋斗小青年、 发表于 2013-8-4 08:29
碉堡了 不能用

void complement_filter(float angle_m_cf,float gyro_m_cf)//分别进入加速度计角度和陀螺仪角速度
{ //采样率为0.01
//    bias_cf*=0.0001;            //陀螺仪零飘低通滤波；500次均值；0.998
//    bias_cf+=gyro_m_cf*0.009;           //0.002
//    angle_dot=0.6*angle_dot+0.4*gyro_m_cf;
    Gyro_angle=angle+gyro_m_cf*0.01;//把陀螺仪的原始数据进行积分得到陀螺仪角度           
    angle=Gyro_angle*0.95+angle_m_cf*0.05;//使用互补滤波得到最终角度
    Send_data((int)angle_m_cf);    //发送原始加速度计角度到串口上位机
    Send_data((int)Gyro_angle);//发送陀螺仪角度到上位机
    Send_data((int)angle);    // 发送最终角度
}
上位机图象如下：红色的为加速度计的角度输入，黄色的为陀螺仪积分后的角度，绿色的为互补滤波器最后输出的角度
下面在上传一张；
下图为把加速度计角度和最角度融合在一起的图片，，红线为加速度计的角度，绿线为互补滤波最后输出的角度，黄线为陀螺仪的角速度