如何解决STM32 陀螺仪角度跟踪
如果有错误,请纠正我,
我首先读取 3 个轴的值 High 和 Low 并将它们连接起来。然后我将每个值乘以 0.07
以将它们转换为 DPS。
我的主要目标是随着时间的推移跟踪角度,所以我简单地实现了一个 Timer,它每隔 dt = 10 ms
读取一次数据
要集成 ValueInDPS * 10ms
,这是我正在使用的代码行:
angleX += (resultGyroX)*dt*0.001; //0.001 to get dt in [seconds]
这应该给我们以 [度] 为单位的角度值,我说得对吗? 问题是我得到的值有点奇怪,例如当我旋转 90° 时,我得到了 70° 之类的东西......
解决方法
你的方法是不精确和累积误差的秘诀。
- 您应该避免使用浮点数(尤其是在没有 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)。
不幸的是,MEMS 传感器数学计算相当复杂。 我个人会使用 STM https://www.st.com/en/embedded-software/x-cube-mems1.html 提供的现成库。
我实际使用过,效果非常好。
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。