前言
在描述物体的姿态时，常用的方法有三种：欧拉角、四元数、旋转矩阵。它们有这各自的特点且可以互相转换。
描述姿态
欧拉角
定义：用三个绕特定坐标轴的连续旋转角度来描述姿态。最常见的顺序是**偏航（Yaw）- 俯仰（Pitch）- 滚动（Roll）。
- 偏航（ψ）：绕Z轴旋转，指向前后方向的变化。
- 俯仰（θ）：绕Y轴旋转，指向上下方向的变化。
- 滚动（φ）：绕X轴旋转，绕自身前后轴线的旋转。 优点：
- 非常直观：飞行员和工程师可以直接理解“机头抬升20度，右倾10度”这样的描述。 缺点：
- 万向节死锁：这是欧拉角最致命的缺陷。当俯仰角为±90度时，偏航和滚转的旋转轴会重合，丢失一个自由度，导致系统出现奇异性。
- 计算复杂：进行多次旋转叠加时，计算效率低。
旋转矩阵
旋转矩阵是一个3x3的正交矩阵
定义：它的每一列代表新坐标系的X、Y、Z轴单位向量在原坐标系下的坐标。可以直接用来旋转三维向量或进行坐标系变换。
优点：
- 无奇异性：没有万向节死锁问题。
- 旋转向量直接：与向量乘法即可完成旋转，非常方便。
- 组合简单：多个旋转可以直接通过矩阵乘法连接。 缺点：
- 冗余：9个数字只表示3个自由度，存在6个约束（正交且行列式为1）。
- 效率：存储和计算相比四元数更耗费资源。
- 数值误差：多次运算后可能不再严格正交，需要重新正交化。
四元数
四元数是一种扩展的复数，由一个实部和三个虚部组成，通常表示为 或
定义： 用于表示旋转的单位四元数满足 w² + x² + y² + z² = 1。它可以被解释为绕一个轴 [x, y, z] 旋转一个角度 θ，其中：
-
优点：
- 紧凑：只有4个元素，比旋转矩阵更节省存储空间。
- 无奇异性：没有万向节死锁问题。
- 计算高效：组合旋转和旋转向量所需的运算量比矩阵少。
- 数值稳定：比旋转矩阵更容易保持单位性质。 缺点：
- 不直观：人类很难直接理解一个四元数的几何意义。
转换方法
欧拉角与旋转矩阵
欧拉角 → 旋转矩阵
基本信息：
以z y x为顺序
偏航ψ， 俯仰θ， 横滚φ
以z y x为顺序
偏航ψ， 俯仰θ， 横滚φ
R=[
[cosψ*cosθ, cosψ*sinθ*sinφ - sinψ*cosφ, cosψ*sinθ*cosφ + sinψ*sinφ],
[sinψ*cosθ, sinψ*sinθ*sinφ + cosψ*cosφ, sinψ*sinθ*cosφ - cosψ*sinφ],
[-sinθ, cosθ*sinφ, cosθ*cosφ ]
]
旋转矩阵 → 欧拉角
基本信息：
以z y x为顺序
偏航ψ， 俯仰θ， 横滚φ
z y x为顺序
偏航ψ， 俯仰θ， 横滚φ
θ = -arcsin(R[2][0]) // 俯仰角
ψ = atan2(R[1][0], R[0][0]) // 偏航角
φ = atan2(R[2][1], R[2][2]) // 横滚角
四元数与旋转矩阵
四元数 → 旋转矩阵
归一化四元数q = [w, x, y, z]
旋转矩阵R = [
[1 - 2y² - 2z², 2xy - 2wz, 2xz + 2wy],
[2xy + 2wz, 1 - 2x² - 2z², 2yz - 2wx],
[2xz - 2wy, 2yz + 2wx, 1 - 2x² - 2y²]
]
旋转矩阵 → 四元数
正交矩阵旋转矩阵 R = [
[m00, m01, m02],
[m10, m11, m12],
[m20, m21, m22]
]
1. 先算四个分量的平方值：
实部平方 = (1 + m00 + m11 + m22) / 4
虚部x平方 = (1 + m00 - m11 - m22) / 4
虚部y平方 = (1 - m00 + m11 - m22) / 4
虚部z平方 = (1 - m00 - m11 + m22) / 4
2. 找最大的平方值，分四种情况：
情况1：实部平方最大
实部 w = 开方(实部平方)
除数 = 1 / (4 × w)
虚部 x = (m21 - m12) × 除数
虚部 y = (m02 - m20) × 除数
虚部 z = (m10 - m01) × 除数
情况2：虚部x平方最大
虚部 x = 开方(虚部x平方)
除数 = 1 / (4 × x)
实部 w = (m21 - m12) × 除数
虚部 y = (m01 + m10) × 除数
虚部 z = (m02 + m20) × 除数
情况3：虚部y平方最大
虚部 y = 开方(虚部y平方)
除数 = 1 / (4 × y)
实部 w = (m02 - m20) × 除数
虚部 x = (m01 + m10) × 除数
虚部 z = (m12 + m21) × 除数
情况4：虚部z平方最大
虚部 z = 开方(虚部z平方)
除数 = 1 / (4 × z)
实部 w = (m10 - m01) × 除数
虚部 x = (m02 + m20) × 除数
虚部 y = (m12 + m21) × 除数
欧拉角与四元数
欧拉角 → 四元数
以z y x为顺序
偏航ψ， 俯仰θ， 横滚φ
以z y x为顺序
偏航ψ， 俯仰θ， 横滚φ
四元数_x = [cos(φ/2), sin(φ/2), 0, 0]
四元数_y = [cos(θ/2), 0, sin(θ/2), 0]
四元数_z = [cos(ψ/2), 0, 0, sin(ψ/2)]
四元数 = 四元数_z × 四元数_y × 四元数_x
/*********************************************/
实部 = cos(ψ/2)cos(θ/2)cos(φ/2) + sin(ψ/2)sin(θ/2)sin(φ/2)
虚部x = cos(ψ/2)cos(θ/2)sin(φ/2) - sin(ψ/2)sin(θ/2)cos(φ/2)
虚部y = cos(ψ/2)sin(θ/2)cos(φ/2) + sin(ψ/2)cos(θ/2)sin(φ/2)
虚部z = sin(ψ/2)cos(θ/2)cos(φ/2) - cos(ψ/2)sin(θ/2)sin(φ/2)
四元数 → 欧拉角
以z y x为顺序
偏航ψ， 俯仰θ， 横滚φ
以z y x为顺序
偏航ψ， 俯仰θ， 横滚φ
先归一化四元数 q = [w, x, y, z]
欧拉角 [ψ, θ, φ]（偏航、俯仰、横滚）
θ = asin(2(wy - xz))
φ = atan2(2(wx + yz), 1 - 2(x² + y²))
ψ = atan2(2(wz + xy), 1 - 2(y² + z²))
// 更准确的：
θ = asin(2(wy - xz))
ψ = atan2(2(wz + xy), 1 - 2(y² + z²)) // 偏航角
φ = atan2(2(wx + yz), 1 - 2(x² + y²)) // 横滚角
/************************************************/
四元数 = [实部, 虚部x, 虚部y, 虚部z]
欧拉角 [偏航角, 俯仰角, 横滚角]
俯仰角 = 反正弦(2(实部×虚部y - 虚部x×虚部z))
横滚角 = 反正切2(2(实部×虚部x + 虚部y×虚部z), 1 - 2(虚部x² + 虚部y²))
偏航角 = 反正切2(2(实部×虚部z + 虚部x×虚部y), 1 - 2(虚部y² + 虚部z²))
示例代码
#include <stdio.h>
#include <math.h>
#define PI 3.14159265358979323846
typedef struct {
double yaw; // 偏航角 ψ (Z轴)
double pitch; // 俯仰角 θ (Y轴)
double roll; // 横滚角 φ (X轴)
} EulerAngles;
typedef struct {
double m[3][3];
} Matrix3x3;
typedef struct {
double w, x, y, z;
} Quaternion;
// 安全数学函数
double safe_asin(double x) {
if (x >= 1.0) return PI/2;
if (x <= -1.0) return -PI/2;
return asin(x);
}
// 弧度与度数转换函数
double rad_to_deg(double radians) {
return radians * 180.0 / PI;
}
double deg_to_rad(double degrees) {
return degrees * PI / 180.0;
}
// 欧拉角度数转换为弧度
EulerAngles euler_deg_to_rad(EulerAngles euler_deg) {
EulerAngles euler_rad;
euler_rad.yaw = deg_to_rad(euler_deg.yaw);
euler_rad.pitch = deg_to_rad(euler_deg.pitch);
euler_rad.roll = deg_to_rad(euler_deg.roll);
return euler_rad;
}
// 欧拉角弧度转换为度数
EulerAngles euler_rad_to_deg(EulerAngles euler_rad) {
EulerAngles euler_deg;
euler_deg.yaw = rad_to_deg(euler_rad.yaw);
euler_deg.pitch = rad_to_deg(euler_rad.pitch);
euler_deg.roll = rad_to_deg(euler_rad.roll);
return euler_deg;
}
// 归一化四元数
Quaternion normalize_quaternion(Quaternion q) {
double norm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
if (norm > 1e-6) {
q.w /= norm;
q.x /= norm;
q.y /= norm;
q.z /= norm;
}
return q;
}
// ZYX顺序：偏航ψ→俯仰θ→横滚φ
// 欧拉角 → 旋转矩阵
Matrix3x3 euler_to_matrix(EulerAngles euler) {
double cos_psi = cos(euler.yaw);
double sin_psi = sin(euler.yaw);
double cos_theta = cos(euler.pitch);
double sin_theta = sin(euler.pitch);
double cos_phi = cos(euler.roll);
double sin_phi = sin(euler.roll);
Matrix3x3 R;
// ZYX顺序的旋转矩阵：R = R_z(ψ) * R_y(θ) * R_x(φ)
R.m[0][0] = cos_psi * cos_theta;
R.m[0][1] = cos_psi * sin_theta * sin_phi - sin_psi * cos_phi;
R.m[0][2] = cos_psi * sin_theta * cos_phi + sin_psi * sin_phi;
R.m[1][0] = sin_psi * cos_theta;
R.m[1][1] = sin_psi * sin_theta * sin_phi + cos_psi * cos_phi;
R.m[1][2] = sin_psi * sin_theta * cos_phi - cos_psi * sin_phi;
R.m[2][0] = -sin_theta;
R.m[2][1] = cos_theta * sin_phi;
R.m[2][2] = cos_theta * cos_phi;
return R;
}
// 旋转矩阵 → 欧拉角 (ZYX顺序)
EulerAngles matrix_to_euler(Matrix3x3 R) {
EulerAngles euler;
// 俯仰角 θ = -arcsin(R[2][0])
double sin_pitch = -R.m[2][0];
euler.pitch = safe_asin(sin_pitch);
double cos_theta = cos(euler.pitch);
if (fabs(cos_theta) > 1e-6) {
// 修正：不需要除以cosθ
euler.yaw = atan2(R.m[1][0], R.m[0][0]); // 偏航角
euler.roll = atan2(R.m[2][1], R.m[2][2]); // 横滚角
} else {
// 万向锁处理保持不变
if (euler.pitch > 0) {
euler.roll = 0.0;
euler.yaw = atan2(R.m[0][1], R.m[1][1]);
} else {
euler.roll = 0.0;
euler.yaw = atan2(-R.m[0][1], -R.m[1][1]);
}
}
return euler;
}
// 四元数 → 旋转矩阵
Matrix3x3 quaternion_to_matrix(Quaternion q) {
q = normalize_quaternion(q);
Matrix3x3 R;
double xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
double xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
double wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;
R.m[0][0] = 1 - 2 * (yy + zz);
R.m[0][1] = 2 * (xy - wz);
R.m[0][2] = 2 * (xz + wy);
R.m[1][0] = 2 * (xy + wz);
R.m[1][1] = 1 - 2 * (xx + zz);
R.m[1][2] = 2 * (yz - wx);
R.m[2][0] = 2 * (xz - wy);
R.m[2][1] = 2 * (yz + wx);
R.m[2][2] = 1 - 2 * (xx + yy);
return R;
}
// 旋转矩阵 → 四元数
Quaternion matrix_to_quaternion(Matrix3x3 R) {
double m00 = R.m[0][0], m01 = R.m[0][1], m02 = R.m[0][2];
double m10 = R.m[1][0], m11 = R.m[1][1], m12 = R.m[1][2];
double m20 = R.m[2][0], m21 = R.m[2][1], m22 = R.m[2][2];
Quaternion q;
double tr = m00 + m11 + m22;
if (tr > 0) {
double S = sqrt(tr + 1.0) * 2;
q.w = 0.25 * S;
q.x = (m21 - m12) / S;
q.y = (m02 - m20) / S;
q.z = (m10 - m01) / S;
} else if ((m00 > m11) && (m00 > m22)) {
double S = sqrt(1.0 + m00 - m11 - m22) * 2;
q.w = (m21 - m12) / S;
q.x = 0.25 * S;
q.y = (m01 + m10) / S;
q.z = (m02 + m20) / S;
} else if (m11 > m22) {
double S = sqrt(1.0 + m11 - m00 - m22) * 2;
q.w = (m02 - m20) / S;
q.x = (m01 + m10) / S;
q.y = 0.25 * S;
q.z = (m12 + m21) / S;
} else {
double S = sqrt(1.0 + m22 - m00 - m11) * 2;
q.w = (m10 - m01) / S;
q.x = (m02 + m20) / S;
q.y = (m12 + m21) / S;
q.z = 0.25 * S;
}
return normalize_quaternion(q);
}
// 欧拉角 → 四元数 (ZYX顺序)
Quaternion euler_to_quaternion(EulerAngles euler) {
double half_psi = euler.yaw * 0.5;
double half_theta = euler.pitch * 0.5;
double half_phi = euler.roll * 0.5;
double cos_psi = cos(half_psi);
double sin_psi = sin(half_psi);
double cos_theta = cos(half_theta);
double sin_theta = sin(half_theta);
double cos_phi = cos(half_phi);
double sin_phi = sin(half_phi);
Quaternion q;
// ZYX顺序：q = q_z(ψ) * q_y(θ) * q_x(φ)
q.w = cos_psi * cos_theta * cos_phi + sin_psi * sin_theta * sin_phi;
q.x = cos_psi * cos_theta * sin_phi - sin_psi * sin_theta * cos_phi;
q.y = cos_psi * sin_theta * cos_phi + sin_psi * cos_theta * sin_phi;
q.z = sin_psi * cos_theta * cos_phi - cos_psi * sin_theta * sin_phi;
return normalize_quaternion(q);
}
// 四元数 → 欧拉角 (ZYX顺序) - 修正版本
EulerAngles quaternion_to_euler(Quaternion q) {
q = normalize_quaternion(q);
EulerAngles euler;
// 俯仰角 θ = asin(2*(w*y - x*z))
double sin_pitch = 2.0 * (q.w * q.y - q.x * q.z);
euler.pitch = safe_asin(sin_pitch);
// 检查万向锁 (俯仰角接近 ±90°)
double cos_theta = cos(euler.pitch);
if (fabs(cos_theta) > 1e-6) {
// 非万向锁情况
// 偏航角 ψ = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))
euler.yaw = atan2(2.0 * (q.w * q.z + q.x * q.y),
1.0 - 2.0 * (q.y*q.y + q.z*q.z));
// 横滚角 φ = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
euler.roll = atan2(2.0 * (q.w * q.x + q.y * q.z),
1.0 - 2.0 * (q.x*q.x + q.y*q.y));
} else {
// 万向锁情况：俯仰角接近 ±90°
// 此时只能确定 ψ - φ 或 ψ + φ
if (euler.pitch > 0) { // θ = +90°
euler.roll = 0.0;
euler.yaw = 2.0 * atan2(q.x, q.w);
} else { // θ = -90°
euler.roll = 0.0;
euler.yaw = -2.0 * atan2(q.x, q.w);
}
}
return euler;
}
// 打印函数
void print_euler_rad(EulerAngles euler) {
printf("偏航(ψ): %.6f rad, 俯仰(θ): %.6f rad, 横滚(φ): %.6f rad\n",
euler.yaw, euler.pitch, euler.roll);
}
void print_euler_deg(EulerAngles euler) {
EulerAngles euler_deg = euler_rad_to_deg(euler);
printf("偏航(ψ): %.6f°, 俯仰(θ): %.6f°, 横滚(φ): %.6f°\n",
euler_deg.yaw, euler_deg.pitch, euler_deg.roll);
}
void print_matrix(Matrix3x3 R) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.6f ", R.m[i][j]);
}
printf("\n");
}
}
void print_quaternion(Quaternion q) {
printf("w: %.6f, x: %.6f, y: %.6f, z: %.6f\n", q.w, q.x, q.y, q.z);
}
// 测试函数
int main() {
printf("=== ZYX顺序转换测试 ===\n");
// 测试1：普通情况
printf("\n--- 测试1：普通情况 ---\n");
EulerAngles euler_rad = {0.5, 0.3, 0.2}; // ψ, θ, φ
printf("原始欧拉角: ");
print_euler_rad(euler_rad);
print_euler_deg(euler_rad);
// 欧拉角 → 旋转矩阵
Matrix3x3 R = euler_to_matrix(euler_rad);
printf("\n旋转矩阵:\n");
print_matrix(R);
// 旋转矩阵 → 欧拉角
EulerAngles euler_back = matrix_to_euler(R);
printf("\n矩阵转换回的欧拉角: ");
print_euler_rad(euler_back);
// 欧拉角 → 四元数
Quaternion q = euler_to_quaternion(euler_rad);
printf("\n四元数: ");
print_quaternion(q);
// 四元数 → 欧拉角
EulerAngles euler_from_q = quaternion_to_euler(q);
printf("四元数转换回的欧拉角: ");
print_euler_rad(euler_from_q);
// 一致性检查
printf("\n一致性误差:\n");
printf("矩阵转换: ψ=%.6f, θ=%.6f, φ=%.6f\n",
fabs(euler_rad.yaw - euler_back.yaw),
fabs(euler_rad.pitch - euler_back.pitch),
fabs(euler_rad.roll - euler_back.roll));
printf("四元数转换: ψ=%.6f, θ=%.6f, φ=%.6f\n",
fabs(euler_rad.yaw - euler_from_q.yaw),
fabs(euler_rad.pitch - euler_from_q.pitch),
fabs(euler_rad.roll - euler_from_q.roll));
// 测试2：万向锁情况
printf("\n--- 测试2：万向锁情况 (俯仰角=90°) ---\n");
EulerAngles gimbal_lock = {0.8, PI/2, 0.4}; // 精确的90度
printf("原始欧拉角: ");
print_euler_deg(gimbal_lock);
Matrix3x3 R_lock = euler_to_matrix(gimbal_lock);
EulerAngles euler_lock_back = matrix_to_euler(R_lock);
printf("矩阵转换回的欧拉角: ");
print_euler_deg(euler_lock_back);
Quaternion q_lock = euler_to_quaternion(gimbal_lock);
EulerAngles euler_lock_q = quaternion_to_euler(q_lock);
printf("四元数转换回的欧拉角: ");
print_euler_deg(euler_lock_q);
// 测试3：接近万向锁情况
printf("\n--- 测试3：接近万向锁情况 (俯仰角≈90°) ---\n");
EulerAngles near_gimbal_lock = {0.8, PI/2 - 0.001, 0.4};
printf("原始欧拉角: ");
print_euler_deg(near_gimbal_lock);
Matrix3x3 R_near_lock = euler_to_matrix(near_gimbal_lock);
EulerAngles euler_near_back = matrix_to_euler(R_near_lock);
printf("矩阵转换回的欧拉角: ");
print_euler_deg(euler_near_back);
Quaternion q_near_lock = euler_to_quaternion(near_gimbal_lock);
EulerAngles euler_near_q = quaternion_to_euler(q_near_lock);
printf("四元数转换回的欧拉角: ");
print_euler_deg(euler_near_q);
return 0;
}
