感知定位篇之IMU:陀螺仪和加速度计及互补滤波

 欢迎关注微信公众号 “四足机器人研习社”，本公众号的文章和资料和四足机器人相关，包括行业的经典教材、行业资料手册，同时会涉及到职业知识学习及思考、行业发展、学习方法等一些方面的文章。

目录

|1.IMU介绍

2.陀螺仪

|2.1陀螺仪测量原理

|2.2陀螺仪零飘

|3.加速度计

|4.滤波融合

|4.1互补滤波

---

|1.IMU介绍

机器人姿态的获取常常依赖于IMU，IMU全称Inertial Measurement Unit，惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器。其原理是采用惯性定律实现的，这些传感器从超小型的的MEMS传感器，到测量精度非常高的激光陀螺，无论尺寸只有几个毫米的MEMS传感器，到直径几近半米的光纤器件采用的都是这一原理。

板载IMU示意图

IMU模块

    最基础的惯性传感器包括加速度计和角速度计（陀螺仪），他们是惯性系统的核心部件，是影响惯性系统性能的主要因素。尤其是陀螺仪其漂移对惯导系统的位置误差增长的影响是时间的三次方函数。而高精度的陀螺仪制造困难，成本高昂。因此提高陀螺仪的精度、同时降低其成本也是当前追求的目标。

    IMU 一般指6轴传感器，内部包含了3轴陀螺仪和3轴加速度计，3轴就是表示 XYZ 平面下的3个坐标轴。3轴陀螺仪测量的是每个轴上面的角速度，精度一般为 °/s ，也就是按照这个趋势旋转，每秒钟能走过的度数。3轴加速度计测量的是每个轴所受的重力加速度，比如传感器水平放置水平地面时，理论上只受到z轴负向的重力加速度，大小为9.8 m/s^2。

2.陀螺仪

|2.1陀螺仪测量原理

   

    假设整个传感器经过了一次旋转，可以看陀螺仪还是在测量每个轴上的角速度，它并不关心当前的姿态，它反馈的是瞬时的物理量，陀螺仪积分出来的角度只是我们人为根据这个物理量的特性推导而来的。而加速度计却有点差别，通过第一幅图可以看到，理论上，重力加速度在z轴的负半轴上，其在xy平面并没有分量，这时 xy轴 测得的重力加速度就是实际机器人在运动过程中产生的加速度，对它做二次积分，就可以得到理论上真实的位移。

    而第二幅图地球重力加速度仍然垂直向下，但此时3轴姿态已经发生变化（不处在水平姿态），那么重力加速度就会在三轴上投影出长短不一的重力加速度分量，这个分量在二次积分时就会带来污染。但同时通过这个分量与三轴之间的关系，我们就可以推导出当前传感器相对于地理坐标系的姿态。

    IMU的工作原理是通过其中包含的加速度计来测量X、Y、Z三个移动方向上的加速度，通过一重积分得到速度，通过二重积分得到位置，通过包含的陀螺仪测量Roll、Pitch、Yaw三个旋转方向上的角速度，通过一重积分得到姿态，包含磁力计的IMU还可以通过测量与地球磁场的夹角来确定当前的朝向，弥补加速度计在水平面上的测量缺失问题。

    在实际应用中，我们获得的只能是离散的信号，想要通过离散信号来还原真实连续时域信号的曲线是不现实的，我们只能在固定的时间间隔下采样得到当前的角速度，并假设在这一小段△t中是匀速运动。

    代码中的实现，最简单的办法就是累加当前角度 += 当前角速度 * 采样时间间隔这样也能得到基本的陀螺仪积分出来的角度。即

|2.2陀螺仪零飘

    通过上面的式子，我们大概能理解，从机器人上电就开始，直到关电后，这段时间都要不停的积分角度，那么如果传感器数据本身有问题的话，哪怕只是一点点问题，经过长时间的积分得到的累计误差都是非常惊人的。陀螺仪上有一个非常常见的误差项，叫零飘和温漂。零飘就是机器人静止状态下，采集出来的角速度不是0，而是在某个值上下乱跳，去除零飘也很简单，就是在机器人刚上电后，采集1000帧陀螺仪原始数据，然后看这1000帧数据的方差和标准差，方差小于阈值，代表在采零飘这段时间内，机器人基本保持静止。那么此时其标准差就是我们获取到的零飘。

        我们在积分之前，将获取的原始数据减去零飘，然后再进行积分，那么就能大大减缓积分漂移的现象（仍不可避免）

|3.加速度计

    加速度计测量的是三轴线加速度，普通的消费级IMU，往往加速度计的高频噪声特别大，单独使用加速度计一般效果都比较糟糕。常见误差来源：

a.轴间误差：上面我们看到，加速度计三轴间理论是严格互相垂直的，但实际的传感器是很难保证严格垂直安装，比如期望是90°，实际是88°，这种误差统称为轴间误差。

b.尺度误差：这个是传感器内部测量不准，比如机器人朝Y轴以1m/s2的加速度超前走，实际传感器测量的可能是 0.8m/s2，这两个误差都可以通过椭球拟合或6面法标定，扫地机项目中未使用标定算法。

|4.滤波融合

    我们知道，陀螺仪可以获取载体的角速度，由角速度积分，就能得到角度，也就得到了载体的姿态。但是，陀螺仪给出的角速度存在测量误差、噪声和漂移，经过积分运算之后，会形成累积误差，这个误差会随着时间延长越来越大，最终导致偏差太大而无法使用。

    另一方面，加速度计可以测量到地球的重力，当载体静止或者匀速运动时，重力的方向就是竖直向下的，通过测量重力加速度的方向，可以获取当前载体的俯仰角、滚转角。但是加速度计容易受到高频噪声的干扰，动态响应慢，只在长时间内数据比较有效。

    那么，既然陀螺仪可以积分得出姿态角度，加速度计可以通过3轴关系得到姿态角度，陀螺仪获得姿态角，短时间准确，长时间会漂移。加速度计获得姿态角，短时间震荡严重，长时间相对准确。因此，一般我们使用加速度计的数据来修正陀螺仪，以加速度计获取的实时姿态角来修正陀螺仪的累积误差，就能在短时间和长时间内都能获取比较满意的姿态信息。

|4.1互补滤波

        最简单的就是互补滤波。所谓滤波，可以简单粗暴理解为像一个筛子一样可以把东西给过滤掉，然后获得相对“纯净”的结果。互补滤波由一个高通滤波器和低通滤波器组成。高通即只有高频信号可以通过，过滤掉了低频的干扰信号，低通滤波器正好相反，过滤掉了高频信号。

        假设有两路信号，一个带有高频噪声，一个带有低频噪声，所以我把他们的噪声分别滤除，然后合并，就得到了没有噪声的原始信号。

即：

        当我们把滤波器简化为最简单的一个比例系数时，有如下形式：

姿态角 = k*陀螺仪姿态角 + (1-k)*加速度计姿态角

        其中0<k<1，k的选择，取决于我们更相信陀螺仪的数据，还是更相信加速度计的数据，这就类似于一个简单粗暴的加权平均，等式右边的两项，由于k小于1，所以陀螺仪姿态角的低频噪声被缩小了k倍；同样，1-k也小于1，所以加速度计姿态角的高频噪声被缩小了1-k倍；二者相加，姿态角的倍数仍为1，而高频和低频的噪声都被缩小了。虽然这个“滤波”效果有点弱，但是这样我们至少得到了一个高频、低频都比原始信号稍好一些的姿态信息，这就实现了高频和低频信号的互补。

        更进一步的互补滤波器是长这样的：即低通滤波器为LPF=C(s)/(C(s)+s)，高通滤波器HPF=s/(C(s)+s)。选用不同的C(s)可以形成不同滤波效果。  k的选择，取决于我们更相信陀螺仪的数据，还是更相信加速度计的数据) ，k的选择，取决于我们更相信陀螺仪的数据，还是更相信加速度计的数据)

        进一步化简就成了这样：

这样，通过互补滤波我们就得到了相对可信赖的姿态角了

        当然，除了互补滤波外还有其他滤波方法，常见的方法如卡尔曼滤波。卡尔曼滤波是一种基于状态估计的滤波方法，通过将加速度计和陀螺仪的测量数据与系统模型进行融合，可以得到更准确的姿态和运动状态估计。卡尔曼滤波可以根据系统的动态特性和传感器的噪声特性来自适应地调整权重，从而提高估计的准确性。后续的章节我们将继续介绍基于卡尔曼滤波的数据融合。

参考：

[1]什么是IMU？-CSDN博客

[2]说透互补滤波(1) - 线性互补滤波器从原理到实现 - 知乎 (zhihu.com)