浅谈陀螺仪和加速度计的互补滤波

结合加速度计和陀螺仪的平衡解决方案
首先通过ADC采样,读出陀螺仪和编码器的电压值,并将其转化为可用单位:
1.偏移(offset):让陀螺仪静止,加速度计保持水平并静止,这时候读到电压值,就是偏移值。如果读数不稳定,取平均值即可。
2.转换系数(scale):传感器输出电压值乘以系数,得到我们想要的单位,这个系数可以在传感器的数据手册(datasheet)中找到。
关于加速度计
根据加速度计Y轴和X轴的值,我们可以用反正切函数求出角度值,没有Y轴,我们可以通过sin或者cos以及X轴值,求得角度值(重力加速度是已知的)。
浅谈陀螺仪和加速度计的互补滤波_第1张图片
数字滤波器
实际单片机用的积分,其实就是个累加;
设采样间隔为dt,每次采到的角速度值是gyro,那么,
角度可以这样得到: angle += gyro * dt;
低通滤波器
低通滤波的目的,保留长期变化,滤掉短暂变化。让信号一点一点的起作用而不是一下子猛烈作用,即在时间范围上取平均。
angle = 0.98 * angle + 0.02 * x_acc;//x_acc是加速度计的实时角度值。
浅谈陀螺仪和加速度计的互补滤波_第2张图片
高通滤波器
数字高通滤波器,不那么容易解释,它跟低通滤波器是刚好相反的:除去长期变化,保留短暂变化。高通滤波器加于陀螺仪,可以滤掉温漂;
采样周期
程序每次大循环用的时间,下面标记为dt;
时间常数
时间常数用来定义滤波器对信号的作用;对于低通滤波器,信号长度比时间常数持久的,可以通过滤波器;对于高通滤波器,信号长度比时间常数短的,可以通过滤波器;对于一个数字滤波器:
y = a * y + (1-a) * x ;

时间常数tau可以这样计算:
tau = (a*dt) / (1-a) <—> a = tau/(tau + dt);

所以,当你知道了要用的时间常数,就可以通过上述公式得到滤波器的系数a;
互补
互补意味着滤波器的两个部分的和总为1,以确保得到的结果是准确的,也使得线性估算变得可行;这里用到的角度互补滤波,并不是完全“互补”,不过得到的结果还能满足我们需求。
浅谈陀螺仪和加速度计的互补滤波_第3张图片
angle = (0.98) * (angle + gyro*dt) + (0.02) * (x_acc);
如果这里滤波器的循环频率时每秒100次(亦即循环周期 = 0.01s),那么上述公式的时间常数就是:
tau = (a * dt)/(1-a) = ( 0.98*0.01s )/0.02s = 0.49s

这个时间常数确定了陀螺仪和加速度计的信任边界。信号时间周期低于半秒时,陀螺仪占主导地位,加速度计的噪声除掉;信号时间周期大于半秒时,加速度计的角度平均值就占据主导地位,有温漂的陀螺仪可以站一边去。

实验,是检验时间常数的标准之一。首先你要挑一个时间常数,然后算出滤波器的系数a,弄到代码中去。检查角度拟合曲线,如果拟合角度与实际角度总相差那么2°,就说明陀螺仪的温漂影响较大,你可能需要把时间常数设置到1s以下,来保证加速度计能尽快校正陀螺仪的温漂值。不过,时间常数越短,就意味着加速度计的噪声更容易通过。

  1. MPU6050陀螺仪

// 陀螺仪
float angleAx,gyroGy;
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//原始数据采集
angleAx=atan2(ax,az)*180/PI;//加速度计算角度
gyroGy=-gy/131.00;//陀螺仪角速度,注意正负号与放置有关

2.滤波参数及函数
//一阶互补滤波
float K1 =0.05; // 对加速度计取值的权重
float dt=20*0.001;//注意:dt的取值为滤波器采样时间
float angle1;

void Yijielvbo(float angle_m, float gyro_m)//采集后计算的角度和角加速度
{
angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}

//二阶互补滤波
float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;
float dt=20*0.001;//注意:dt的取值为滤波器采样时间
float angle2;

void Erjielvbo(float angle_m,float gyro_m)//采集后计算的角度和角加速度
{
x1=(angle_m-angle2)(1-K2)(1-K2);
y1=y1+x1*dt;
x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
angle2=angle2+ x2*dt;
}

转载链接:http://www.geek-workshop.com/thread-10172-1-1.html

你可能感兴趣的:(algorithm)