STM32F103互补滤波器溢出

Question

我正在为我的 quad 编写飞行控制器。到目前为止它在速率模式下效果很好，所以我决定添加角度模式。这需要融合我的加速度计和陀螺仪数据以获得四边形在 space 中的真实位置。所以我能够将陀螺仪的 degrees/sec 整合成一个角度，并从加速器获得角度，如下所示：

微控制器：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 开始返回乱码（我认为它溢出），结果角度也 returns 乱码，因为其中使用了 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

很可能您正在进入集成溢出。这很正常，在许多类似情况下占 'drift'。

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

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

解决方案就是每隔一段时间或当它超过某个限制（值或迭代次数）时将积分重置为零。

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

你在几个地方的 /1000 也会因截断而导致精度下降——你总是向下舍入。我不会尝试通过四舍五入来解决这个问题，但它可能会导致很多错误。

浮点数可能是一个选项？甚至可能是一个 F4 组件来代替 F103 - 你可以获得一个引脚兼容的组件。如果您没有使用 SMD 的经验，请将芯片带到当地的手机维修店

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

STM32F103互补滤波器溢出

STM32F103 complementary filter overflow

overflow

accelerometer

stm32

gyroscope