STM32F103互补滤波器溢出

Question

我正在为我的四方飞行控制器编程。到目前为止，它在速率模式下效果很好，所以我决定添加角度模式。这需要融合我的加速度计和陀螺仪数据，以获取四边形在空间中的真实位置。因此，我能够将陀螺仪的度/秒积分到一个角度，并通过加速度获得角度，如下所示：

微控制器：STM32F103

Ps：所有变量均为struct {double x，y，z; }，dt是uint64_t

//getting angle from gyro rate
gyroAngle.x += gyroRate.x * dt / 1000;
gyroAngle.y += gyroRate.y * dt / 1000; 
gyroAngle.z += gyroRate.z * dt / 1000;

//getting angle from accel
accelAngle.x =  atan2(accel.y, sqrt(pow(accel.x,2) + pow(accel.z,2))) * (180/M_PI);
accelAngle.y = -atan2(accel.x, sqrt(pow(accel.y,2) + pow(accel.z,2))) * (180/M_PI);

//combines the two into true rotational position
angle.x = .85 * (angle.x + gyroAngle.x * (dt/1000)) + (.15 * accelAngle.x);
angle.y = .85 * (angle.y + gyroAngle.y * (dt/1000)) + (.15 * accelAngle.y);

问题：我在250hz的循环上遇到了这个问题。不幸的是，在一两分钟后gyroAngle开始返回乱码（我认为它溢出了），因为使用了gyroAngle，结果角度也返回了垃圾。 accelAngle没有给我带来任何麻烦。

这是串行监视器的一些输出：

angle: -1        3        /*
angle: -1        3        */normal until here
-----------------------------------------------------------
angle: -98       406      /*
angle: -338      582       *
angle: -710      646       *
angle: -1113     716       *  starts giving me garbage
angle: -1616     849       *
angle: -2327     1016      *
angle: -3047     1377      */

我完全不知道为什么会这样。非常感谢您的帮助。

Answer 1

[您很有可能陷入集成溢出。这是很正常的现象，在许多类似情况下都是“漂移”的原因。

发生的情况是您的测量是有限的，而不是无限精确的（传感器分辨率的极限）。随着时间的流逝，您正在读取的10或12位以下的小数位相加。

很快，这将导致您的寄存器溢出，或者该值超出了所采集样本的错误*号。四边形上的振动会加剧此问题。

解决方案是将积分偶尔或在超过某个限制（值或迭代）时重置为零。

试图通过有限的精度有限的长时间积分来保持绝对结果会失败。

您在多个位置具有的/ 1000也由于截断而导致精度下降-您始终四舍五入。我不会尝试通过四舍五入来解决此问题，但它可能会导致很多错误。

浮点数可能是一种选择？也许甚至用F4组件代替F103-您都可以获得与引脚兼容的组件。如果您没有使用SMD的经验，请把芯片送到当地的手机维修店。

您可能需要添加磁力计，以便重新设置为更好的估计值。