陀螺仪与加速度计的姿态融合——互补滤波

本篇文章我们来讲讲如何将陀螺仪和加速度计的数据结合起来，获取更准确的姿态数据，使用的是互补滤波的方法。

阅读本文需有一定的知识基础，可以参见作者以前MPU6050的两篇文章：《MPU6050陀螺仪和加速度计数据的获取和校准》、《MPU6050官方DMP的移植和使用》，以及了解四元数的一些基本概念。

1）为什么要进行姿态融合

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

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

因此，一般我们使用加速度计的数据来修正陀螺仪，以加速度计获取的实时姿态角来修正陀螺仪的累积误差，就能在短时间和长时间内都能获取比较满意的姿态信息。

我们以下图为例，Z轴向上，约定绕X轴旋转的角度为滚转角；绕Y轴旋转的角度为俯仰角，绕Z轴旋转的角度为偏航角。

那么，滚转角（绕X轴转）和俯仰角（绕Y轴转）发生变化时，MPU6050测出来的重力加速度g的方向相对于载体也会发生变化。而加速度计测出来的重力加速度g的方向是实时的，不会受长时间计算累加的影响。所以，我们可以用加速度计测到的g的方向，来修正陀螺仪积分得到的滚转角和俯仰角。

而偏航角（绕Z轴转）变化时，MPU6050测到的重力加速度g方向是不会变化的，所以g的方向不能用来修正偏航角。要想修正偏航角，需要测量地磁偏角的传感器信息。

这个用加速度计或磁力计来修正姿态角的过程，一般称为数据融合，有多种方法实现。在之前的文章中，我们使用MPU6050芯片官方提供的DMP库直接获取了融合后的数据；但是这只适用于特定厂家的芯片，而更通用的方法是使用互补滤波或者卡尔曼滤波来实现，本节要讲的就是使用互补滤波的方法，来进行数据融合。

(注意，由于没有引入磁力计的数据，本文使用互补滤波的方法只是修正了俯仰角和滚转角的信息，无法修正偏航角，后续有机会再讲解如何用磁力计修正偏航角)