An Introduction for IMU 2 - IMU数据融合与姿态解算

在上一篇博客中，我们已经介绍了IMU的内部工作原理，以及如何通过Arduino读取MPU6050的数据。虽然可以从DMP直接读取姿态角，但其数据返回的频率相对较低，同时由于DMP库不是开源的，其内部的工作原理、输出姿态角的准确性都不清楚；而除了MPU以外的IMU传感器基本不带有DMP功能，因此大部分时候还是需要我们自己在单片机里实现IMU姿态角的计算。本片博客将介绍如何使用采集到的加速度和角速度数据进行数据融合，也就是IMU的姿态解算。

IMU姿态解算

MPU6050得到的原始数据仅有加速度与角速度，单独由加速度计或陀螺仪都可以解算出三轴的角度。

根据IMU的测量原理可知，加速度计测量的是底座对于质量块作用的力所产生的加速度。当模块处于静止时，质量块会受到向下的重力加速度，而加速度计的底座向质量块提供相反的加速度使质量块保持静止，因此在静止条件下加速度计会测量到1g竖直向上的加速度。当模块倾斜且保持静止时，可以通过重力加速度在另外一个/两个轴线上的分量计算得到模块的姿态角度：
$${AngleY}_{acc}=\arctan \frac{accY}{accZ}$$
另一方面，由于角速度是角度的导数，通过对角速度积分也可以得到姿态角：
$$Angle{Y}_{gyro}={AngleY}_0+{\int }_0^{T}gyroYdt$$
虽然可以从加速度和角速度分别解算出角度，但这两种方式都存在很大的问题。一方面由于加速度计容易受到振动的影响，噪声很大，所以解算角度的噪声也很大，同时加速度解算的角度仅适用于静态过程；另一方面虽然陀螺仪测量角速度的噪声不是很大，经过积分环节后噪声进一步被变小，但由于初始角度并不能准确得到，而且角速度存在零漂问题，经过积分后误差会被累积。因此，两种方式解算出来的角度都无法直接使用，但我们可以采用数据融合的方法，把两种角度融合在一起，得到一个既没有累计误差、噪声又小的角度数据。

---

互补滤波

一阶互补滤波是最简单但却非常实用的数据融合算法，它把由加速度解算的角度和由角速度积分的角度按照一定比例加到一起，公式如下：
$$Angle=K\cdot Angl{e}_{acc}+(1-K)\cdot (Angle+\omega \cdot dt)$$