加速度计与陀螺仪融合计算倾角：结合FishBot代码讲解

大家好，我是爱学习的小鱼。在之前的文章中，我们已经探讨了如何通过加速度计单独计算X和Y轴的倾角。虽然这种方法可以提供基础的角度信息，但我们发现在处理高频动态时，它的响应并不理想。为了改进这一点，今天我们将尝试将陀螺仪数据融合进来，以期达到更精确和快速的倾角估计。

互补滤波介绍

互补滤波是一种简单高效的方法，它结合了加速度计和陀螺仪的优势，利用加速度计的长期稳定性和陀螺仪的快速动态响应。基本的互补滤波公式如下：

$$\text{角度}=\alpha (\text{陀螺仪测得的角度})+(1-\alpha )\text{加速度计测得的角度}$$

其中，$\alpha$ 是滤波参数

通过加速度计算倾角

加速度计测量重力加速度的分量，可以通过以下公式从加速度计数据中计算倾角：

$$\text{angleAccX}=\arctan \left(\frac{accY}{\sqrt{acc{X}^2+acc{Z}^2}}\right)\times \frac{180}{\pi }$$

$$\text{angleAccY}=-\arctan \left(\frac{accX}{\sqrt{acc{Y}^2+acc{Z}^2}}\right)\times \frac{180}{\pi }$$

通过陀螺仪积分计算倾角

陀螺仪测量的是角速度，通过对这个速度积分可以得到角度变化，公式如下：

$$\text{angleGyroX}=\text{angleGyroX}+\text{gyroX}\cdot dt$$

$$\text{angleGyroY}=\text{angleGyroY}+\text{gyroY}\cdot dt$$

实现代码

通过加速度计计算倾斜角

float sgZ = accZ < 0 ? -1 : 1;
angleAccX = atan2(accY, sgZ * sqrt(accZ * accZ + accX * accX)) * RAD_TO_DEG;
angleAccY = -atan2(accX, sqrt(accZ * accZ + accY * accY)) * RAD_TO_DEG;

计算dt,用于积分

unsigned long Tnew = millis();
float dt = (Tnew - preInterval) * 1e-3;
preInterval = Tnew;

互补滤波，得到最终的角度

angleX = wrap(filterGyroCoef * (angleAccX + wrap(angleX + gyroX * dt - angleAccX, 180)) + (1.0 - filterGyroCoef) * angleAccX, 180);
angleY = wrap(filterGyroCoef * (angleAccY + wrap(angleY + gyroY * dt - angleAccY, 90)) + (1.0 - filterGyroCoef) * angleAccY, 90);

注意这里的互补滤波和公式有些不太一样，主要原因是角度相加不能像普通数字相加求平均值，比如数字+179和 −179 之间的平均值为 0，但角度+179° 和 −179°之间的平均值应计算为 ±180°。所以这里用了很多个wrap防止出现此类问题，当然这也是因为欧拉角不像四元数可以平滑的变换。

看完后发现，是不是没有angleZ的计算，因为z没办法用加速度计的倾角来互补滤波计算，只能最简单的积分来计算，下一篇文章介绍。