我的第六个单片机开源项目——PIC单片机模型直升机陀螺仪

joyrus

大家好，有一段时间没有更新了，其实这段时间我正在做一些小项目，当然同时也在进行学习。这个项目虽说是一个相对独立的东西，但是现在3轴系统都已经很成熟了，所以拿来参考一些也是非常有益的，我就顺手把九鹰318A/180D的3轴接收机打开来简单研究一下。具体情况请看这里：九鹰318A/180D无副翼3D直升机-三轴陀螺仪飞控系统的讨论！

joyrus

呵呵～～，经过这些个日子的深入研究，终于初步理解了Kalman滤波器的工作原理，并且用C语言完成了基础卡尔曼滤波器的代码，实际代码是很简单的，稍后发上来。经过实际的数据采用对比后可以发现，卡尔曼Kalman滤波后可以有效的降低随机误差！根据系统灵敏度的选择，可以到达50%以上的误差过滤。但是Kalman滤波器的使用需要事先对传感器进行大量的数据采集并分析的过程这中间需要结合误差理论来计算出Kanlman滤波器的参数。这就造成了不同的传感器要使用不同的参数了。当然作为DIY的目的可以使用一些日常事物常用的参数来代替特定事物的参数，这样就会简单很多，当然效果会差一些。

joyrus

这里是一个国外的朋友的Kalman滤波程序，我也是利用这个程序：

这是使用卡尔曼滤波器来完成对陀螺仪和加速度计滤波处理！非常实用和有效！

我自己简单分析一下，有错误之处请大家指出。

// --- Kalman filter module  ----------------------------------------------------------------------------

    float Q_angle  =  0.001; //0.001    这个是加速度计得到的角度误差(方差)
    float Q_gyro   =  0.003;  //0.003   这个是陀螺仪得到的角度误差(方差)
    float R_angle  =  0.03;  //0.03       这个是激励误差(方差)

    float x_angle = 0;                        这个是X的角度值，也就是滤波后得到的角度值
    float x_bias = 0;                          这个是白噪声
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;    这些是卡拉曼递推运算的保存值
    float dt, y, S;                              
    float K_0, K_1;                                                 这些是卡拉曼递推运算的保存值


newAngle是加速度计得到的角度，newRate是陀螺仪测得的角速度，looptime是程序循环的时间

  float kalmanCalculate(float newAngle, float newRate,int looptime)
  {
    dt = float(looptime)/1000;                               通过对循环时间的计算得到陀螺仪的采用时间
    x_angle += dt * (newRate - x_bias);                计算陀螺仪的角度

    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;    以下4行是对递推过程值的计算
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;

    y = newAngle - x_angle;                                  计算陀螺仪和加速度计的差值                                
    S = P_00 + R_angle;                                       计算激励误差

    K_0 = P_00 / S;                                             以下2行是对递推过程值的计算
    K_1 = P_10 / S;

    x_angle +=  K_0 * y;                                      用Y的值修正陀螺仪的角度值，得到最终的滤波值
    x_bias  +=  K_1 * y;                                       计算新的白噪声

    P_00 -= K_0 * P_00;                                      以下4行是对递推过程值的计算
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;

    return x_angle;                                             返回滤波后的角度
  }

cyq451213

牛人 版主的精神可嘉 值得学习  继续关注

joyrus

很久没有更新了，今天来聊聊这段时间累积的经验。

之前使用了Gy401的传感器，和九鹰3轴陀螺仪系统中的陀螺仪。先说说Gy401的传感器，这是一个很高级的传感器，模拟量输出，为了得到数字数据使用了一片带有12位有效数的ADC的MCU首先进行ADC转换并且对转换后的数据进行滤波处理，使用了kalman滤波器，滤波效果非常显著。

再说说九鹰的陀螺仪，3轴陀螺仪由一个单轴Z陀螺仪和一个XY 2轴陀螺仪，需要对3个陀螺仪进行ADC转换，这样就需要非常快的转换速度，否则再转换时间上可能会有损失，条件有限没有使用专用的多通道ADC采样IC，这类IC可以在ADI的官网上找到，性能非常优秀！比MCU集成的ADC好的多，而且基本都是高速ADC采样，还有些是多通道同时采样！

        在得到滤波后的数据就可以用积分的方式得到旋转的角度。使用的计算方法为：

        Angle = AdcValue * dt

        Angle就是计算后的角度，AdcValue是陀螺仪采样滤波后的数据，而dt则是系统每次采样的时间。

        得到了角度接下来就是将角度数据作为误差数据输入到PID算法，计算出执行机构所需要的数据了。

       为了得到较好的性能，需要选择运算速度更快的MCU，而其绝大多数运算都是使用浮点数，这就进一步提高了对MCU的负担。