如何解决欧拉角偏航角计算中的数值跳变及角速度异常问题？

1. 核心问题：欧拉角奇异性（万向锁） #

当俯仰角接近±π/2时，偏航角与滚转角会出现耦合，导致数值跳变，这是欧拉角的固有缺陷。你的代码中theta的变化会触发该奇异性，加上np.arctan的单象限取值限制，进一步放大了跳变问题。

2. 针对性修复步骤 #

步骤1：修正欧拉角计算逻辑 #

用np.arctan2替代np.arctan，它支持全象限角度计算，避免单象限的不连续问题，同时确保unwrap逻辑正确：

def Euler_angles(phi, theta):
    roll = np.arctan2(-np.cos(phi)*np.sin(theta), np.cos(theta))
    pitch = np.arcsin(np.sin(phi)*np.sin(theta))
    # arctan2自动适配全象限，避免角度截断
    yaw = np.arctan2(-np.sin(phi)*np.cos(theta), np.cos(phi))
    # 先计算全角度再unwrap，保证连续性
    yaw = np.unwrap(yaw)
    return roll, pitch, yaw

步骤2：优化角速度计算 #

放弃手动判断跳变的逻辑，改用numpy向量运算结合unwrap处理差分，避免循环中的边界错误：

def Angular_speeds(roll, pitch, yaw, dt):
    # 边缘补值，保证中心差分的边界计算准确
    roll_pad = np.pad(roll, 1, mode='edge')
    pitch_pad = np.pad(pitch, 1, mode='edge')
    yaw_pad = np.pad(yaw, 1, mode='edge')
    
    # 中心差分计算角速度
    omegax = (roll_pad[2:] - roll_pad[:-2]) * 0.5 / dt
    omegay = (pitch_pad[2:] - pitch_pad[:-2]) * 0.5 / dt
    # 对yaw差分先unwrap再计算，彻底消除跳变影响
    yaw_diff = np.unwrap(np.diff(yaw_pad))
    omegaz = -yaw_diff[1:] * 0.5 / dt
    
    return omegax, omegay, omegaz

步骤3：规避万向锁的根本方案 #

如果场景允许，彻底放弃欧拉角，改用四元数表示姿态——四元数无奇异点，能完全避免角度跳变：

import quaternion

def phi_theta_to_quaternion(phi, theta):
    # 根据你的旋转顺序，将phi、theta转换为四元数
    q_phi = quaternion.from_euler_angles(0, 0, phi)
    q_theta = quaternion.from_euler_angles(0, theta, 0)
    return q_phi * q_theta

# 生成四元数序列
quats = phi_theta_to_quaternion(phi, theta)
# 直接从四元数差分计算角速度，稳定性拉满
omega = quaternion.as_euler_angles(np.diff(quats) / dt)
omegax, omegay, omegaz = omega[:,0], omega[:,1], omega[:,2]

3. 验证建议 #

替换代码后，先观察yaw曲线的连续性，再检查角速度跳变是否消失。若为万向锁导致的跳变，四元数方案能彻底解决问题。

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