互补滤波算法（双轮平衡车）

1万 · 2020-10-26 09:10

本人菜鸟，最近在调融合互补滤波算法。我对算法的理解大概如下。陀螺仪的短期测得的值比较准，但是由于存在温漂，进过积分，长期就不能用了。所以要通过加速度计进行修正角度，但是加速度计的短期不行，噪声比较大，所以要将陀螺仪高通，加速度计低通后进行一定比列的融合。才能比较准确的反应角度的变化。
我是这样做的：陀螺仪和加速度计分别进行了递推和一阶滤波，得到的滤波结果再按一定的比例进行拟合，采样周期为0.3ms。但是出来的效果不理想。最终直立控制量=P*（融合滤波后的角度）+D*(角速度（即陀螺仪的值）)
angle=(int)(0.99*(angle+(gyro-2940)*0.008)+0.01*(y_acc-3150))
int PIDCalc( unsigned int NextPoint )
{
  int PID;
  Error = standard - NextPoint;    // 偏差E(t)
  iError+=Error;                   //E(t)+E(t-1)+...+E(1)
  dError=Error-LastError;       //E(t)-E(t-1)
  PID=(int)(Proportion * Error       //比例
      +Integral*iError       //积分
         + Derivative * dError);       // 微分项
  PrevError = LastError;
  LastError = Error;
  return (
         PID
                     );
}
void PIDBEGIN()
{
  Proportion = 6; // Set PID Coefficients 10
  Integral=0.002;
  Derivative = 120;       // 4
}
我用的传感器是mma-7361,enc-03
小车现象是抖动，站立不了多久，总是往一边倒去，不知道是不是陀螺仪温漂
然后传感器采的是AD值，不知道和角度的具体关系，所以不知道怎么调