STM32陀螺仪角度跟踪

Posted

技术标签:

【中文标题】STM32陀螺仪角度跟踪【英文标题】:STM32 Gyroscope angle tracking 【发布时间】:2021-09-02 10:45:00 【问题描述】:

我正在使用具有 2000DPS 的陀螺仪 (L3GD20)

如有错误请指正,

我首先读取 3 个轴的值 High 和 Low 并将它们连接起来。然后我将每个值乘以 0.07 以将它们转换为 DPS。

我的主要目标是随着时间的推移跟踪角度,所以我简单地实现了一个计时器,它每隔dt = 10 ms 读取一次数据 集成ValueInDPS * 10ms,这是我正在使用的代码行:

angleX += (resultGyroX)*dt*0.001;   //0.001 to get dt in [seconds]

这应该给我们以 [度] 为单位的角度值,对吗? 问题是我得到的值有点奇怪,例如当我旋转 90° 时,我得到的值类似于 70°...

【问题讨论】:

您已经描述了很多代码,在这些代码中显示代码就不会那么模棱两可了。 【参考方案1】:

您的方法会导致不精确和累积错误。

您应该避免使用浮点数(尤其是在没有 FPU 的情况下),尤其是如果此代码在定时器中断处理程序中。 您应该避免在每个样本上不必要地转换为度/秒 - 仅在演示时才需要转换,因此您应该只在需要该值时执行它 - 在内部,积分器应该以陀螺仪样本单位工作。李>

此外,如果您在 ISR 和普通线程中都进行浮点运算并且您有一个 FPU,您也可能会遇到不相关的错误,因为 FPU 寄存器不会在中断处理程序。总而言之,浮点数只能谨慎使用。

所以让我们假设你有一个函数 gyroIntegrate() 每 10 毫秒调用一次精确

static int32_t ax = 0
static int32_t ay = 0
static int32_t az = 0

void gyroIntegrate( int32_t sample_x, int32_t sample_y, int32_t sample_z)

    ax += samplex ;
    ay += sampley ;
    az += samplez ;

ax 等不是原始样本值的积分,因此与相对于起始位置的角度成正比。

ax 转换为度数:

度数 = ax × r-1 × s

地点:

r 是以度/秒为单位的陀螺仪分辨率 (0.07) s 是采样率 (100)。

现在你最好避免使用浮点数,这里完全没有必要; r-1 x s 是一个常数(在本例中为 1428.571)。所以要读取积分器表示的当前角度,你可能有一个函数:

#define GYRO_SIGMA_TO_DEGREESx10 14286

void getAngleXYZ( int32_t* int32_t, int32_t* ydeg, int32_t* zdeg )

    *xdeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;
    *ydeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;
    *zdeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;

getAngleXYZ() 应该在您需要结果时从应用程序层调用 - 而不是从积分器中调用 - 您在需要时进行数学运算,并留出 CPU 周期来做更多有用的事情。

请注意,在上面我忽略了积分器算术溢出的可能性。因为它适用于大约 +/-150 万度 +/-4175 旋转),因此在某些应用中可能不是问题。您可以使用int64_t,或者如果您对旋转次数不感兴趣,则可以在积分器中使用绝对角度:

ax += samplex ;
ax %= GYRO_SIGMA_360 ;

其中GYRO_SIGMA_360 等于 514286 (360 x s / r)。

【讨论】:

要复杂得多:( @0___________ 集成陀螺仪并不复杂。处理零点、漂移、传感器融合等当然更复杂。但是,这将提供比 OP 方法更好的结果。他提供的信息太少,无法确定其他错误来源,或者“DC 偏差”是否已消除。 对于 mems,我实际上使用了那些 STM 库——它们工作得很好。 @0___________ 同意了,但答案不那么有趣。这里讨论的技术和原则有更广泛的应用。【参考方案2】:

不幸的是,MEMS 传感器的数学运算相当复杂。 我个人会使用 STM https://www.st.com/en/embedded-software/x-cube-mems1.html 提供的现成库。

我实际使用过,效果很好。

【讨论】:

以上是关于STM32陀螺仪角度跟踪的主要内容,如果未能解决你的问题,请参考以下文章

STM32--MPU6050 DMP读角度总结

STM32实现六轴姿态测量陀螺仪模块JY61P(标准库与HAL库实现)

STM32实现六轴姿态测量陀螺仪模块JY61P(标准库与HAL库实现)

l3gd20 陀螺仪在 stm32f3discovery 上发回重复字节

玩转X-CTR100 l STM32F4 l MPU6050加速度陀螺仪传感器

MPU6050工作原理及STM32控制MPU6050