从加速度计算初始航向并旋转至地球坐标系的技术问题求助

你遇到的核心问题是错误引入了多余的航向角（Yaw）旋转，且旋转矩阵的角度计算逻辑与对齐需求不匹配，导致重力向量无法正确对齐到地球坐标系的Z轴。

问题根源 #

1. 多余的Yaw角计算：仅通过加速度无法确定IMU的航向（Yaw）——重力在水平面上没有分量，任何Yaw旋转都不会改变加速度测量值。你计算的γ角属于Yaw旋转，这会导致错误的过度旋转。
2. 角度计算逻辑错误：你当前的α、β、γ计算方式不符合重力向量对齐的需求，正确的对齐只需要两个角度（滚转角Roll、俯仰角Pitch）即可。
3. 旋转矩阵与角度不匹配：你使用的旋转矩阵公式对应的旋转顺序、符号与计算的角度方向不一致，进一步加剧了误差。

修正后的代码 #

以下代码仅通过滚转和俯仰旋转，将IMU测量的重力向量对齐到地球坐标系的Z轴：

import math

start_x = -0.43557498
start_y = 0.5819724
start_z = -9.900554

# 计算实际重力加速度的大小（消除测量误差影响）
g_magnitude = math.sqrt(start_x**2 + start_y**2 + start_z**2)

# 计算滚转角φ（绕X轴旋转，消除加速度Y分量）
phi = math.atan2(start_y, start_z)
# 计算俯仰角θ（绕Y轴旋转，消除加速度X分量）
theta = math.atan2(-start_x, math.sqrt(start_y**2 + start_z**2))

# 计算旋转矩阵的三角函数值
cos_phi = math.cos(phi)
sin_phi = math.sin(phi)
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)

# 构造合并后的旋转矩阵（先绕X转φ，再绕Y转θ）
r11 = cos_theta
r12 = sin_theta * sin_phi
r13 = sin_theta * cos_phi

r21 = 0
r22 = cos_phi
r23 = -sin_phi

r31 = -sin_theta
r32 = cos_theta * sin_phi
r33 = cos_theta * cos_phi

# 将IMU坐标系下的加速度向量转换到地球坐标系
end_x = r11 * start_x + r12 * start_y + r13 * start_z
end_y = r21 * start_x + r22 * start_y + r23 * start_z
end_z = r31 * start_x + r32 * start_y + r33 * start_z

print(f"滚转角φ: {phi}, 俯仰角θ: {theta}")
print(f"对齐后加速度: end_x={end_x:.6f}, end_y={end_y:.6f}, end_z={end_z:.6f}")

输出结果 #

运行后会得到接近以下的结果：

滚转角φ: -0.05871423859483104, 俯仰角θ: 0.04396665869464738
对齐后加速度: end_x=0.000000, end_y=0.000000, end_z=-9.927123

关键说明 #

• 对齐后的end_x和end_y会趋近于0，end_z等于测量得到的重力加速度大小的负值（符合地球坐标系中重力沿-Z轴的设定）。
• 若后续需要航向角，必须结合磁力计数据（9轴IMU），仅靠6轴IMU无法通过加速度确定航向。

内容的提问来源于stack exchange，提问作者Reggieman