在互补滤波算法详解中的 高频噪声和低频噪声的定义是？

来源：蜘蛛抓取(WebSpider) 时间：2018-01-28 11:53 标签： 互补滤波算法

基于一阶互补滤波算法的MPU6050数据融合

我们已经知道，MPU6050可以获得加速度和角速度，但是由于传感器的噪声信号比较大，无法直接通过两个数据来获得该轴上的角度。因此，需要进行互补滤波来近似得到一个比较准确的角度。

传感器原始数据特点分析：

加速度数据，不是很准确，但是趋势是对的，通过受力分能够显示角度变化趋势，在长期变化来看是可以利用的。
角速度数据，加速度积分得到角度，但是由于传感器误差，积分作用会造成累计误差，所以角速度数据在短期是可以使用的，长期来看误差会很大是不可使用的

两种特点的数据使得我们对得到精确地角度看到了希望，以下几个内容需要明白

可以让长期变化通过，滤波消除短期变化，用于对加速度计的滤波

可以让短时信号通过，过滤出变化缓慢的信号，消除漂移

dt是采样时间越短积分值越准确

这个简单，比如1ms获取一次时间，则采样率是1000HZ，采样时间为0.001s

τ是常用时间常数符号，一般表示过渡反应的时间过程的常数。指该物理量从最大值衰减到最大值的1/e所需要的时间，对于滤波器而言是信号会作用的相对时间。
对于低通滤波器周期比时间常数大的多的信号可以平稳通过，周期比时间常数短的信号被滤出，高通则相反。
设互补滤波系数为a，则

即，高频信号与低频信号都与我们将求得的量相关，我们将其进行滤波运算，给两部分一个权重得到一个相对准确的值，或者得到一个很好的近似的过程

我们可以根据采样周期来算出滤波器的时间常数，最终求得它的响应速度。同样的，我们也可以根据自己的需要，确定一个时间常数，进而获得合适的滤波系数，为了获取更加准确的值需要对系数进行一定的调整。


* 描述 ：滤波器相关函数。结论：一般阶次越高，传递函数越复杂。 

IIR滤波器是无限冲击响应滤波器，
1. 采用模拟原型滤波的标准设计，容易理解。
2. 可以用低阶设计实现，并且可以高速运行
3. 对于相同公差设计方案，其阶数比FIR短。
4. 可以采用闭环设计
2. 可能会出现极限环
3. 多频道设计困难，只能设计低通、高通和带通
4. 反馈会引入不稳定
5. 非常难得到高速流水线设计 

//deltaT：时间差 F-CUT：超高速切割,低通滤波器截止频率,单位：Hz


想要达到更好的滤波效果，FIR或者IIR滤波器是更好的选择。
窗口平均滑动差了一点（但是比LPF要好），但是数据实时性
性能指标上比前者响应速度提高了近一倍。因此在制作四轴的
进阶阶段，可以考虑将窗口平均滑动换成Butterworth滤波器。
式中a1,a2,b0,b1,b2是二阶滤波器IIR系数，其决定滤波器的频响应曲线以及增益。
对于一个二阶IIR滤波器，标准的技术指标如下：

中心频率: 通常定义为带通滤波器（或带阻滤波器）的两个3 dB点之间的中点，一般用两个3 dB点的算术平均来表示 。其实低通和高通滤波器也有中心频率,只不过它的定义和带通就不一样了，它就等于我们通常说的截止频率.但我们在说低通高通时，都是用截止频率，而几乎不用其中心频率。不过在做归一化时就会有这个概念了。那时可以看到，低通高通的归一化,截止频率=截止频率/中心频率=1.

通常，对一个滤波器的要求，我们主要给出以下技术规格：中心频率frequency，采样频率sampleRate，增益dBgain，品质因数Q。

根据上面的技术指标，可以确定以下几个通用计算量：

所以二阶IIR高通滤波器系数的计算：

/ / 众所周知， 加速度计的高频噪声较严重，尤其是廉价的加速度传感器，针对加速度滤波，

//更新数值，留待下次运算 


对mpu6050来说，加速度计对四轴或小车的加速度比较敏感，取瞬时值计算倾角误差比较大；
而陀螺仪积分得到的角度不受小车加速度的影响，但是随着时间的增加积分漂移和温度漂移带来
的误差比较大。所以这两个传感器正好可以弥补相互的缺点。不过要怎么弥补呢？经过上面的介绍
是否感觉到可以用滤波器做文章呢？ 
这里讲的互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度
进行取平均值来校正陀螺仪的得到的角度。就是，短时间内用陀螺仪比较准确，以它为主；长时间用
加速度计比较准确，这时候加大它的比重，这就是互补了，不过滤波在哪里加速度计要滤掉高频信号，
陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），
然后再相加得到整个频带的信号，例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，
所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。
通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好
的信号，互补滤波需要选择切换的频率点，即高通和低通的频率。 
由于陀螺零点漂移和离散采样产生的累积误差， 由陀螺得到的四元数只能保证短期的精度， 需要
使用加速度计和磁力计对其进行矫正
由于四旋翼飞行器所使用的加速度计具有长期可信、短期噪声较大的特点；陀螺仪
具有短期可信，长期不稳定的特点，因此可以使用互补滤波对其进行融合。简而言之，
互补滤波相当于将低通滤波器与高通滤波器结合在一起，对加速度计进行低通滤波，而
对陀螺仪进行高通滤波，通过整定合适的参数来得到一个较好的滤波器特性

上接【互补滤波器】，继续学习互补滤波。。。。

本文中 mahony 的应用场景为 多旋翼无人机的姿态估计。

陀螺仪、加速度计、MPU6050 详述，请参考：

陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，最终导致陀螺仪的低频干扰和漂移。

输出当前加速度（包含重力加速度 g ）的方向【也叫重力感应器】，在悬停时，输出为 g 。由其测量原理导致的高频信号敏感，使得加速度计在振动环境中高频干扰较大。

输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好，易受周围磁场干扰。
磁力计的工作原理参考：

导航坐标系：在多旋翼中，又叫地球坐标系、地理坐标系。通常，采用北东地（NED）构成坐标系的 X,Y,Z  轴。

机体坐标系 ：固联在多旋翼飞行器上，即，坐标系原点定位于飞行器中心点（假设中心点与重心点重合）。

关于航空导航用到的坐标系，请参考 AIAA 系列丛书。在多旋翼中，因为只在低空飞行，且时间较短，只需要以上两个。

欧拉角 ：较直观，描述刚体在三维欧式空间中的姿态。此时，坐标系为机体坐标系，随着刚体的旋转而旋转。缺点：万向节锁。

四元数：由一组四维向量，表示刚体的三维旋转。适合用于计算机计算。

方向余弦矩阵DCM：用欧拉角余弦值或四元数，表示的坐标系的旋转。

互补滤波要求两个信号的干扰噪声处在不同的频率，通过设置两个滤波器的截止频率，确保融合后的信号能够覆盖需求频率。
在 IMU 的姿态估计中，互补滤波器对陀螺仪（低频噪声）使用高通滤波；对加速度/磁力计（高频噪声）使用低频滤波。
（此处尚未对传感器数据实测，噪声和有用频率未知。。。。待后期补足）

互补滤波中，两个滤波器的截止频率一致，此时就需要根据有用频率的值对其进行取舍。

机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角。磁力计也有同样问题，无法测得要磁轴的旋转量。故，需要加速度计和磁力计同时对陀螺仪进行校正。

PI 调节器中：[更新]

主要是公式，不包含推导过程。。。。

欧拉角与机体角速度的关系：


旋转矩阵与机体角速度的关系：

四元数与机体角速度的关系：

参考：北航全权老师 第五章内容、惯性导航（秦永元）第九章内容。

与卡尔曼滤波相似，mahony 滤波也分为预测-校正。
在预测环节，由三轴陀螺仪测得的角速度，通过式（1）计算出四元数姿态预测。q b e   表示从地球坐标系到机体坐标系，或机体坐标系姿态在地球坐标系下的表示。

在预测环节得到的四元数 ，通过加速度计和磁力计的值进行校正。该环节通常分为两部分：

    ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。

加速度计信号首先经过低通滤波器（消除高频噪声）：

然后，对得到的结果进行归一化（normalized）

式中， v  表示重力向量在机体坐标系的向量。

与加速度计向量垂直分量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】

此处，extern “C” 表示以 C 格式编译； __EXPORT 表示 将函数名输出到链接器（Linker）； using 关键字 表示引入名称到 using 说明出现的声明区域。。

  1. 导航系中，D正方向朝下;

  2. //因 向量 k 已归一化，故分母等于1;

  3. ，根据设定校正磁偏，归一化;

  1. 由加速度计和磁力计初始化旋转矩阵和四元数;

  2. 不使用外部航向，或外部航向异常时，采用磁力计校准;
    将磁力计读数从机体坐标系转换到导航坐标系;

  3. 将磁场偏差转换到机体坐标系，并乘以其对应权重;

  4. 此处修正值为 mahony 滤波的 pi 控制器的 积分部分;

  5. 更新四元数，derivative为求导函数，使用一阶龙格库塔求导。

我要回帖

更多关于 互补滤波算法 的文章

 

随机推荐