STM32陀螺仪角度跟踪

Question

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

如有错误请指正，

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

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

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

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

Answer 1

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

您应该避免使用浮点数（尤其是在没有 FPU 的情况下），尤其是当此代码位于定时器中断处理程序中时。
您应该避免在每个样本上不必要地转换为 degrees/sec - 这种转换仅用于演示，因此您应该仅在需要该值时执行它 - 在内部积分器应该在陀螺仪样本中工作单位。

此外，如果您同时在 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).

Answer 2

不幸的是，MEM 传感器数学计算相当复杂。我个人会使用 STM https://www.st.com/en/embedded-software/x-cube-mems1.html.

提供的现成库

我实际用过，效果很好

STM32陀螺仪角度跟踪

STM32 Gyroscope angle tracking

embedded

stm32

gyroscope

stm32f4discovery