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

Question

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

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：我知道有类似here 的问题，但它没有回答我的问题，所以请帮助我。

Answer 1

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

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

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

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

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

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

Gx_Theta += GYRO_PLAY.Gyroscope_X - Gx_Zero_Bias;

Answer 2

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