3

我通过以下公式计算了加速度计的倾斜角：

Angle_Accel = atan(Ax/sqrt(Ay*Ay+Az*Az))*(180/PI)

我现在想从陀螺仪计算倾斜角，我正在使用 Gx 坐标进行如下积分，但我的结果不正确。

伪代码

  • 每 0.1 秒测量一次 Gx。
  • 在灵敏度因子和偏差校正后，我乘以 180/PI 以转换为度数。
  • 然后我除以频率，即 10 并将其添加到最终角度。

C代码

Gx = (((float)GYRO_PLAY.Gyroscope_X  )* GYRO_PLAY.Gyro_Mult)-Gx_Correction;

Gy = (((float)GYRO_PLAY.Gyroscope_Y  )*  GYRO_PLAY.Gyro_Mult)-Gy_Correction;

Gz = (((float)GYRO_PLAY.Gyroscope_Z  )* GYRO_PLAY.Gyro_Mult)-Gz_Correction;

Gx_temp = (Gx*degrees)/10.0; //degrees = 180/PI

Gx_Theta = Gx_Theta + Gx_temp;

我的角度不对。我应该如何整合？任何帮助深表感谢。

PS：我知道这里有这样的问题，但它没有回答我的问题，所以请帮助我。

4

2 回答 2

4

10Hz 采样似乎太低了，无论如何您都在对每个采样做不必要的工作。应用原始偏差偏移并整合原始值 - 如果需要，可以在演示时转换为度/秒，而中间转换为弧度/秒没有任何意义。

机器人不关心甚至不理解你不需要任何转换的单位；只是符号和幅度 - 灵敏度可以通过您的闭环控制器系数来处理。

如何Gx_Correction确定 - 它会随着时间的推移随着热漂移而变化；如果它不正确或没有以某种方式跟踪，您的集成商将放大错误。

请注意，可能无法通过 SPI 获得更高的采样率——这就是片上 DPM 的用途。

另一个可能的错误来源是使用float. STM32F4 具有单精度 FPU，因此操作将在硬件中完成 - 但是，如果您在中断或线程上下文中使用浮点，请注意浮点寄存器不太可能在上下文之间保留，除非您已明确实现它，因此例如，如果被执行浮点运算的中断中断，浮点运算将被破坏。

如果积分器只需要处理原始数据值，则浮点是不必要的：

Gx_Theta += GYRO_PLAY.Gyroscope_X - Gx_Zero_Bias;
于 2018-01-10T16:17:19.377 回答
0

必须整合 Gy 而不是 Gx。此外，陀螺仪测量的角度以度/秒为单位，因此无需乘以degrees。积分频率也需要为 50Hz。

于 2018-01-11T12:55:17.137 回答