本人菜鸟，最近在调融合互补滤波算法。我对算法的理解大概如下。陀螺仪的短期测得的值比较准，但是由于存在温漂，进过积分，长期就不能用了。所以要通过加速度计进行修正角度，但是加速度计的短期不行，噪声比较大，所以要将陀螺仪高通，加速度计低通后进行一定比列的融合。才能比较准确的反应角度的变化。
我是这样做的：陀螺仪和加速度计分别进行了递推和一阶滤波，得到的滤波结果再按一定的比例进行拟合，采样周期为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值，不知道和角度的具体关系，所以不知道怎么调 |
|
楼主
