如何处理ZYX欧拉角俯仰穿越±90°时的分支翻转以保持连续性

你当前用Force Dimension Omega.7设备提取ZYX欧拉角的代码，会因为asin的范围限制（仅返回[−π/2, +π/2]），导致实际俯仰角超出±90°时出现角度回绕——本质是同一个3D姿态对应两组等价欧拉角：
$$(𝜓, 𝜃, 𝜙) ≡ (𝜓 ± 𝜋, 𝜋 − 𝜃, 𝜙 ± 𝜋)$$

要保持角度连续性，核心思路是结合历史帧角度，选择与上一帧变化最小的等价欧拉角组合，具体实现如下：

步骤1：角度归一化工具函数 #

先实现一个函数，把角度统一归一到[-π, π]区间，避免跨周期的差值计算错误：

#include <cmath>

double normalize_angle(double angle) {
    while (angle > M_PI) angle -= 2 * M_PI;
    while (angle < -M_PI) angle += 2 * M_PI;
    return angle;
}

步骤2：欧拉角提取与分支选择逻辑 #

假设你维护了上一帧的欧拉角（prev_yaw, prev_pitch, prev_roll），每帧执行以下逻辑：

// 1. 计算原始欧拉角（保留你原有的提取逻辑）
double yaw = std::atan2(R[1][0], R[0][0]);
double pitch = std::asin(-R[2][0]);
double roll = std::atan2(R[2][1], R[2][2]);

// 2. 归一化原始角度到[-π, π]
yaw = normalize_angle(yaw);
pitch = normalize_angle(pitch);
roll = normalize_angle(roll);

// 3. 计算等价分支的欧拉角（应用姿态等价公式）
double yaw_alt = normalize_angle(yaw + M_PI);
double pitch_alt = normalize_angle(M_PI - pitch);
double roll_alt = normalize_angle(roll + M_PI);

// 4. 对比两组角度与上一帧的总变化量，选择变化更小的组合
// （第一帧无历史值时，直接跳过此判断，用原始角度即可）
if (/* 不是第一帧 */) {
    double delta_original = fabs(normalize_angle(yaw - prev_yaw)) + 
                            fabs(normalize_angle(pitch - prev_pitch)) + 
                            fabs(normalize_angle(roll - prev_roll));
    
    double delta_alt = fabs(normalize_angle(yaw_alt - prev_yaw)) + 
                       fabs(normalize_angle(pitch_alt - prev_pitch)) + 
                       fabs(normalize_angle(roll_alt - prev_roll));
    
    if (delta_alt < delta_original) {
        // 切换到等价分支，保证角度连续性
        yaw = yaw_alt;
        pitch = pitch_alt;
        roll = roll_alt;
    }
}

// 5. 更新历史帧角度，供下一帧使用
prev_yaw = yaw;
prev_pitch = pitch;
prev_roll = roll;

逻辑说明 #

• 当实际俯仰角超出±90°时，原始提取的角度会出现跳变（比如从-87°跳到87°），同时偏航/滚转角会伴随180°突变，此时等价分支的角度与上一帧的总变化量会远小于原始组，触发分支切换。
• 若实际姿态未进入俯仰翻转区间，原始组的变化量会更小，直接保留原始提取值即可。
• 你之前尝试的“检测偏航/滚转大幅变化”思路，本质是这个逻辑的简化版，但通过总变化量判断更可靠，能覆盖所有分支切换场景。

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