欧拉角、四元数与旋转矩阵的C语言转换实现

欧拉角、四元数与旋转矩阵的C语言转换实现

在3D图形学、机器人控制和航空航天等领域，欧拉角（Euler Angles）、四元数（Quaternions）和旋转矩阵（Rotation Matrix）是最常用的旋转表示方法。本文将提供它们之间的完整C语言转换代码，并解释关键步骤。

1. 基本数据结构定义

首先定义三种旋转表示的数据结构：

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

// 欧拉角（弧度制，Z-Y-X顺序）
typedef struct {
    float roll;   // X轴旋转 (ϕ)
    float pitch;  // Y轴旋转 (θ)
    float yaw;    // Z轴旋转 (ψ)
} EulerAngles;

// 四元数 (w + xi + yj + zk)
typedef struct {
    float w, x, y, z;
} Quaternion;

// 3x3旋转矩阵 (列主序)
typedef struct {
    float m[3][3];
} RotationMatrix;

2. 欧拉角 → 旋转矩阵

按Z-Y-X顺序（航空航天常用）：

RotationMatrix eulerToMatrix(EulerAngles euler) {
    float c1 = cosf(euler.yaw);    // cos(ψ)
    float s1 = sinf(euler.yaw);    // sin(ψ)
    float c2 = cosf(euler.pitch);  // cos(θ)
    float s2 = sinf(euler.pitch);  // sin(θ)
    float c3 = cosf(euler.roll);   // cos(ϕ)
    float s3 = sinf(euler.roll);   // sin(ϕ)

    RotationMatrix R;
    R.m[0][0] = c1 * c2;
    R.m[0][1] = c1 * s2 * s3 - s1 * c3;
    R.m[0][2] = c1 * s2 * c3 + s1 * s3;
    
    R.m[1][0] = s1 * c2;
    R.m[1][1] = s1 * s2 * s3 + c1 * c3;
    R.m[1][2] = s1 * s2 * c3 - c1 * s3;
    
    R.m[2][0] = -s2;
    R.m[2][1] = c2 * s3;
    R.m[2][2] = c2 * c3;
    
    return R;
}

3. 旋转矩阵 → 欧拉角

EulerAngles matrixToEuler(RotationMatrix R) {
    EulerAngles euler;
    
    // 计算俯仰角 (θ)
    euler.pitch = asinf(-R.m[2][0]);
    
    // 避免万向锁情况（cosθ ≈ 0）
    if (cosf(euler.pitch) > 1e-6) {
        euler.roll = atan2f(R.m[2][1], R.m[2][2]);
        euler.yaw = atan2f(R.m[1][0], R.m[0][0]);
    } else {
        // 万向锁时，滚转和偏航角退化
        euler.roll = atan2f(-R.m[0][1], R.m[1][1]);
        euler.yaw = 0.0f;
    }
    
    return euler;
}

4. 四元数 → 旋转矩阵

RotationMatrix quaternionToMatrix(Quaternion q) {
    float xx = q.x * q.x;
    float yy = q.y * q.y;
    float zz = q.z * q.z;
    float xy = q.x * q.y;
    float xz = q.x * q.z;
    float yz = q.y * q.z;
    float wx = q.w * q.x;
    float wy = q.w * q.y;
    float wz = q.w * q.z;

    RotationMatrix R;
    R.m[0][0] = 1.0f - 2.0f * (yy + zz);
    R.m[0][1] = 2.0f * (xy - wz);
    R.m[0][2] = 2.0f * (xz + wy);
    
    R.m[1][0] = 2.0f * (xy + wz);
    R.m[1][1] = 1.0f - 2.0f * (xx + zz);
    R.m[1][2] = 2.0f * (yz - wx);
    
    R.m[2][0] = 2.0f * (xz - wy);
    R.m[2][1] = 2.0f * (yz + wx);
    R.m[2][2] = 1.0f - 2.0f * (xx + yy);
    
    return R;
}

5. 旋转矩阵 → 四元数

Quaternion matrixToQuaternion(RotationMatrix R) {
    Quaternion q;
    float trace = R.m[0][0] + R.m[1][1] + R.m[2][2];
    
    if (trace > 0.0f) {
        float s = 0.5f / sqrtf(trace + 1.0f);
        q.w = 0.25f / s;
        q.x = (R.m[2][1] - R.m[1][2]) * s;
        q.y = (R.m[0][2] - R.m[2][0]) * s;
        q.z = (R.m[1][0] - R.m[0][1]) * s;
    } else {
        // 选择最大对角元素
        if (R.m[0][0] > R.m[1][1] && R.m[0][0] > R.m[2][2]) {
            float s = 2.0f * sqrtf(1.0f + R.m[0][0] - R.m[1][1] - R.m[2][2]);
            q.w = (R.m[2][1] - R.m[1][2]) / s;
            q.x = 0.25f * s;
            q.y = (R.m[0][1] + R.m[1][0]) / s;
            q.z = (R.m[0][2] + R.m[2][0]) / s;
        } else if (R.m[1][1] > R.m[2][2]) {
            float s = 2.0f * sqrtf(1.0f + R.m[1][1] - R.m[0][0] - R.m[2][2]);
            q.w = (R.m[0][2] - R.m[2][0]) / s;
            q.x = (R.m[0][1] + R.m[1][0]) / s;
            q.y = 0.25f * s;
            q.z = (R.m[1][2] + R.m[2][1]) / s;
        } else {
            float s = 2.0f * sqrtf(1.0f + R.m[2][2] - R.m[0][0] - R.m[1][1]);
            q.w = (R.m[1][0] - R.m[0][1]) / s;
            q.x = (R.m[0][2] + R.m[2][0]) / s;
            q.y = (R.m[1][2] + R.m[2][1]) / s;
            q.z = 0.25f * s;
        }
    }
    
    // 归一化（可选，但推荐）
    float norm = sqrtf(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
    q.w /= norm;
    q.x /= norm;
    q.y /= norm;
    q.z /= norm;
    
    return q;
}

6. 欧拉角 → 四元数

Quaternion eulerToQuaternion(EulerAngles euler) {
    float cy = cosf(euler.yaw * 0.5f);
    float sy = sinf(euler.yaw * 0.5f);
    float cp = cosf(euler.pitch * 0.5f);
    float sp = sinf(euler.pitch * 0.5f);
    float cr = cosf(euler.roll * 0.5f);
    float sr = sinf(euler.roll * 0.5f);

    Quaternion q;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;
    
    return q;
}

7. 四元数 → 欧拉角

EulerAngles quaternionToEuler(Quaternion q) {
    EulerAngles euler;
    
    // Roll (X-axis)
    float sinr_cosp = 2.0f * (q.w * q.x + q.y * q.z);
    float cosr_cosp = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
    euler.roll = atan2f(sinr_cosp, cosr_cosp);
    
    // Pitch (Y-axis)
    float sinp = 2.0f * (q.w * q.y - q.z * q.x);
    if (fabs(sinp) >= 1.0f) {
        euler.pitch = copysignf(M_PI / 2.0f, sinp); // 处理±90°奇异点
    } else {
        euler.pitch = asinf(sinp);
    }
    
    // Yaw (Z-axis)
    float siny_cosp = 2.0f * (q.w * q.z + q.x * q.y);
    float cosy_cosp = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
    euler.yaw = atan2f(siny_cosp, cosy_cosp);
    
    return euler;
}

8. 测试示例

int main() {
    // 初始化一个欧拉角（单位：弧度）
    EulerAngles euler = {0.1f, 0.2f, 0.3f};
    
    // 欧拉角 → 旋转矩阵
    RotationMatrix R = eulerToMatrix(euler);
    
    // 旋转矩阵 → 四元数
    Quaternion q = matrixToQuaternion(R);
    
    // 四元数 → 欧拉角
    EulerAngles euler_new = quaternionToEuler(q);
    
    printf("原始欧拉角: roll=%.3f, pitch=%.3f, yaw=%.3f\n", euler.roll, euler.pitch, euler.yaw);
    printf("转换后欧拉角: roll=%.3f, pitch=%.3f, yaw=%.3f\n", euler_new.roll, euler_new.pitch, euler_new.yaw);
    
    return 0;
}

9. 关键注意事项

1. 万向锁问题：欧拉角在俯仰角±90°时退化，需特殊处理。
2. 归一化：四元数和旋转矩阵应保持归一化，避免数值误差。
3. 旋转顺序：欧拉角的计算顺序必须明确（如Z-Y-X）。
4. 性能优化：在嵌入式系统中，可查表法优化三角函数计算。

10. 总结

本文提供了欧拉角、四元数、旋转矩阵之间的完整C语言转换代码，适用于：

• 机器人姿态控制（如无人机、机械臂）。
• 3D图形渲染（如OpenGL/DirectX）。
• 传感器融合（如IMU+视觉SLAM）。

完整代码可集成到您的项目中，欢迎反馈优化建议！ 🚀