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开始返回乱码（我认为它溢出），结果角度也返回gabage因为使用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也会因截断而导致精度损失 - 你总是四舍五入。我不会尝试通过舍入来解决这个问题，但它可能会导致很多错误。

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

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

STM32F103互补滤波器溢出

1 个答案: