如何检查 IMU 输出角度在机器人坐标中是否正确？

Question

I ROS封装并与凉亭仿真集成。

Answer 1

您可以使用此代码获得欧拉角

import math

def quaternion2euler(q0, q1, q2, q3):
# q0: scalar part

angles = {"roll": 0, "pitch": 0, "yaw": 0}
denom_r = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
numerator_r = 2 * (q0 * q1 + q2 * q3)
roll = math.atan2(numerator_r, denom_r)
angles["roll"] = roll

pitch = math.asin(2 * (q0 * q2 - q1 * q3))
angles["pitch"] = pitch

numerator_y = 2 * (q0 * q3 + q1 * q2)
denom_y = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
yaw = math.atan2(numerator_y, denom_y)
angles["yaw"] = yaw

return angles

您可以在Wikipedia上阅读更多内容。