MPU6050 - 自平衡机器人的陀螺仪倾角测量？

Question

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

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

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

Psuedo Code

• 每0.1秒测量一次Gx。
• 在灵敏度因子和偏差校正之后，我乘以180 / PI转换为degress。
• 然后我按频率除以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：我知道有一个类似here的问题，但它没有回答我的问题，所以请帮助我。

Answer 1

10Hz采样似乎太低了，无论如何你在每个样本上都做了不必要的工作。应用原始偏置偏移并整合原始值 - 如果需要，转换为度/秒可以在演示时完成，并且中间转换为弧度/秒无用。

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

Gx_Correction如何确定 - 它会随着热漂移而随时间变化;如果它不正确或以某种方式未被跟踪，您的积分器将放大错误。

请注意，SPI上的采样率可能不高 - 这就是片上DPM的用途。

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

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

Gx_Theta += GYRO_PLAY.Gyroscope_X - Gx_Zero_Bias;

Answer 2

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