两个四元数计算XYZ欧拉角时Y轴(theta)存在大量噪声

公式校验结论 #

你手写的转换公式是**XYZ固定轴顺规（外旋，旋转顺序X→Y→Z）**的标准欧拉角转换公式，不存在书写错误。你遇到的两个问题和磁场干扰无关，是欧拉角本身的固有特性+边界逻辑缺失导致的：

• Y轴角度只能获取±90°以内数据：公式中Y轴角度theta由math.asin()计算得到，而asin函数的天然返回值域就是[-π/2, π/2]（即-90°~90°），超出这个范围的旋转会触发万向锁奇点。
• 绕Y轴运动时其余轴出现大量噪声：当theta接近±90°时，X轴和Z轴的旋转轴会完全重合，原本独立的phi（X轴角）和psi（Z轴角）会进入耦合状态，两个角度的计算值会出现无物理意义的跳变，视觉表现和噪声类似。

排查&修正方案 #

按优先级依次排查以下问题：

• 校验四元数计算逻辑一致性
  你通过q_upper.conjugate * q_lower计算得到的是q_upper到q_lower的相对旋转四元数，需要确认两点：
  1. 输入的q1、q2的四元数存储顺序和pyquaternion的要求一致（pyquaternion初始化顺序为w,x,y,z，如果你的输入是x,y,z,w顺序会直接导致全角度计算错误）
  2. 相对旋转的参考坐标系和你需要的欧拉角参考坐标系匹配，机体坐标系/全局坐标系搞反会出现角度符号、顺规不匹配的问题。
• 补全转换逻辑的边界处理
  标准欧拉角转换必须加浮点误差兜底和万向锁奇点判断，否则不仅会出现奇点跳变，还可能因为浮点精度问题导致asin输入超出[-1,1]范围直接报错。修正后的转换代码如下：

  # 先对输入四元数做归一化，避免非单位四元数带来的计算偏差
  q_upper = q_upper.normalised
  q_lower = q_lower.normalised
  qd = q_upper.conjugate * q_lower
  
  sin_theta = 2 * (qd.w * qd.y - qd.z * qd.x)
  # 截断浮点误差，保证asin输入在合法范围内
  sin_theta = max(min(sin_theta, 1.0), -1.0)
  theta = math.asin(sin_theta)
  
  # 万向锁阈值，可根据精度需求调整，0.9999对应角度约87.7°
  if abs(sin_theta) > 0.9999:
      # 奇点状态下phi和psi耦合，固定phi为0计算psi即可
      phi = 0.0
      psi = math.atan2(2 * (qd.x * qd.y - qd.w * qd.z), 2 * (qd.w * qd.x + qd.y * qd.z))
  else:
      phi = math.atan2(2 * (qd.w * qd.x + qd.y * qd.z), 1 - 2 * (qd.x ** 2 + qd.y ** 2))
      psi = math.atan2(2 * (qd.w * qd.z + qd.x * qd.y), 1 - 2 * (qd.y ** 2 + qd.z ** 2))
  

• 验证干扰来源
  磁场干扰带来的角度误差是全角度范围的随机漂移，不会仅在绕Y轴旋转到±90°附近时集中出现。你可以构造一组纯Y轴旋转的仿真四元数输入到转换代码中，如果复现同样的角度跳变，即可完全排除传感器侧的干扰问题。

场景适配建议 #

如果你的应用场景中绕Y轴的旋转范围会超过±90°，不要使用XYZ顺规的欧拉角做姿态表示，直接使用四元数或旋转矩阵做运算，或更换不存在对应位置奇点的欧拉角顺规，从根源上避免万向锁问题。

内容的提问来源于stack exchange，提问作者Heavy Metal Drummer