欧拉角转轴角：代码结果与在线工具不符，求问题排查

你的代码目前存在几个关键问题，导致结果和在线工具不一致，下面逐一说明并给出修正方案：

核心错误：把四元数直接当成轴角输出 #

你当前的函数计算的是四元数（分量为w, x, y, z），但你直接将其作为轴角的(x, y, z, theta)输出——这是完全错误的。轴角的正确表示是「单位轴向量(x,y,z) + 旋转角theta」，必须从四元数进一步转换得到。

四元数转轴角的正确逻辑 #

从四元数(qw, qx, qy, qz)转换为轴角的步骤如下：

1. 旋转角 theta = 2 * acos(qw)
2. 计算轴向量的模长 scale = sqrt(qx² + qy² + qz²)
3. 若scale接近0（即旋转角趋近于0），轴向量可设为任意单位向量（比如(1,0,0)）；否则将qx、qy、qz分别除以scale，得到归一化的单位轴向量。

其他可能导致结果不符的因素 #

除了上述核心错误，还有两个常见坑：

• 旋转顺序不匹配：在线工具可能采用不同的欧拉角旋转顺序（比如常用的Yaw-Pitch-Roll即Z-Y-X外旋，而你代码里是X-Y-Z内旋），或者区分了内旋/外旋。必须确认你的欧拉角定义（Pitch X、Yaw Y、Roll Z）对应的旋转顺序，和在线工具保持一致。
• 弧度/角度混淆：C语言的cos()/sin()只接受弧度参数，如果你的输入值是角度（比如-6.017度），必须先乘以M_PI/180转换为弧度，否则计算结果会完全偏离预期。

修正后的完整代码 #

#include <math.h>
#include <stdio.h>

#define EPSILON 1e-8  // 用于判断极小值

// 欧拉角转四元数（假设顺序为X(Pitch)→Y(Yaw)→Z(Roll)内旋，输入为弧度）
void eulerToQuaternion(double euler[3], double quat[4]) {
    double cy = cos(euler[1] * 0.5);
    double sy = sin(euler[1] * 0.5);
    double cp = cos(euler[0] * 0.5);
    double sp = sin(euler[0] * 0.5);
    double cr = cos(euler[2] * 0.5);
    double sr = sin(euler[2] * 0.5);

    quat[0] = cy * cp * cr + sy * sp * sr;  // qw
    quat[1] = cy * cp * sr - sy * sp * cr;  // qx
    quat[2] = sy * cp * sr + cy * sp * cr;  // qy
    quat[3] = sy * cp * cr - cy * sp * sr;  // qz
}

// 四元数转轴角
void quaternionToAxisAngle(double quat[4], double axis_angle[4]) {
    double qw = quat[0];
    double qx = quat[1];
    double qy = quat[2];
    double qz = quat[3];

    // 计算旋转角（弧度）
    axis_angle[3] = 2 * acos(qw);
    // 计算轴向量模长
    double scale = sqrt(qx*qx + qy*qy + qz*qz);

    if (scale < EPSILON) {
        // 旋转角接近0，轴向量取默认单位向量
        axis_angle[0] = 1.0;
        axis_angle[1] = 0.0;
        axis_angle[2] = 0.0;
    } else {
        // 归一化轴向量
        axis_angle[0] = qx / scale;
        axis_angle[1] = qy / scale;
        axis_angle[2] = qz / scale;
    }
}

int main() {
    // 注意：如果输入是角度，需要先转弧度，比如：
    // double euler_input[3] = {-6.017 * M_PI/180, 0.487 * M_PI/180, -0.412 * M_PI/180};
    double euler_input[3] = {-6.017, 0.487, -0.412};  // 当前假设输入为弧度
    double quat[4];
    double axis_angle_output[4];

    eulerToQuaternion(euler_input, quat);
    quaternionToAxisAngle(quat, axis_angle_output);

    printf("轴向量: x=%.6f, y=%.6f, z=%.6f\n", 
           axis_angle_output[0], axis_angle_output[1], axis_angle_output[2]);
    printf("旋转角(弧度): %.6f | (角度): %.6f\n", 
           axis_angle_output[3], axis_angle_output[3] * 180/M_PI);

    return 0;
}

验证步骤 #

1. 先确认输入值的单位：如果是角度，务必在传入转换函数前转为弧度。
2. 核对在线工具的旋转顺序：如果工具用的是Z-Y-X外旋（常见的航模/机器人坐标系），你需要调整四元数的计算公式——不同旋转顺序的四元数组合方式完全不同。

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