欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-04-17 22:08:07
...

ref:https://blog.csdn.net/zhaojun1204/article/details/52790697

结合加速度计和陀螺仪的平衡解决方案
首先通过ADC采样,读出陀螺仪和编码器的电压值,并将其转化为可用单位:
1.偏移(offset):让陀螺仪静止,加速度计保持水平并静止,这时候读到电压值,就是偏移值。如果读数不稳定,取平均值即可。
2.转换系数(scale):传感器输出电压值乘以系数,得到我们想要的单位,这个系数可以在传感器的数据手册(datasheet)中找到。
关于加速度计
根据加速度计Y轴和X轴的值,我们可以用反正切函数求出角度值,没有Y轴,我们可以通过sin或者cos以及X轴值,求得角度值(重力加速度是已知的)。

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

数字滤波器
实际单片机用的积分,其实就是个累加;
设采样间隔为dt,每次采到的角速度值是gyro,那么,
角度可以这样得到: angle += gyro * dt;
低通滤波器
低通滤波的目的,保留长期变化,滤掉短暂变化。让信号一点一点的起作用而不是一下子猛烈作用,即在时间范围上取平均。
angle = 0.98 * angle + 0.02 * x_acc;//x_acc是加速度计的实时角度值。
浅谈陀螺仪和加速度计的互补滤波
高通滤波器
数字高通滤波器,不那么容易解释,它跟低通滤波器是刚好相反的:除去长期变化,保留短暂变化。高通滤波器加于陀螺仪,可以滤掉温漂;
采样周期
程序每次大循环用的时间,下面标记为dt;
时间常数
时间常数用来定义滤波器对信号的作用;对于低通滤波器,信号长度比时间常数持久的,可以通过滤波器;对于高通滤波器,信号长度比时间常数短的,可以通过滤波器;对于一个数字滤波器:
y = a * y + (1-a) * x ;

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

所以,当你知道了要用的时间常数,就可以通过上述公式得到滤波器的系数a;
互补
互补意味着滤波器的两个部分的和总为1,以确保得到的结果是准确的,也使得线性估算变得可行;这里用到的角度互补滤波,并不是完全“互补”,不过得到的结果还能满足我们需求。
浅谈陀螺仪和加速度计的互补滤波
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