陀螺仪与加速度计的姿态融合

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

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

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

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

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

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

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

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

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

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

假如我们有两种途径测量某个信号，测得的结果一个带有高频噪声，一个带有低频噪声。我们为了获取更准确的信号，可以把它们的噪声分别滤掉，再合并，就得到了没有噪声的原始信号。具体操作如下图所示：

这就是互补滤波最基础的理论。

为了使得最后相加的重构信号与我们想要的信号尽量相等，一般要求F1(s)+F2(s) = 1。

具体到我们的实际问题，姿态信息可以通过陀螺仪角速度积分得到，也可以通过加速度计实时测量得到；而且，陀螺仪积分得到的姿态主要包含低频噪声（零偏、累积误差），加速度计得到的姿态主要包含高频噪声。使用互补滤波正好可以融合二者的姿态信息。

依据上面这个框图，我们可以想到以下的姿态融合方法：

图中ω是角速度，n1和n2为噪声，θ’为融合后的角度。

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

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

(其中0