旋转矩阵与欧拉角转换数学公式与代码详解

下面分三大部分说明：

1. 数学推导与公式（欧拉角 ↔ 四元数）
2. C++ 实现代码
3. Python 实现代码

为便于说明，统一采用常用的航空/机器人学约定：

• 欧拉角顺序：Z-Y-X（yaw-pitch-roll）
• 旋转顺序：先绕 X 轴转 roll，再绕 Y 轴转 pitch，最后绕 Z 轴转 yaw
  R=Rz(yaw) Ry(pitch) Rx(roll) R = R_z(\text{yaw}) \, R_y(\text{pitch}) \, R_x(\text{roll}) R=Rz(yaw)Ry(pitch)Rx(roll)
• 欧拉角记作：
  • roll：绕 x轴 旋转，记为 ϕ\phiϕ 或 roll
  • pitch：绕 y轴 旋转，记为 θ\thetaθ 或 pitch
  • yaw：绕 z轴 旋转，记为 ψ\psiψ 或 yaw
• 四元数记作：
  q=w+x i+y j+z k q = w + x\,\mathbf{i} + y\,\mathbf{j} + z\,\mathbf{k} q=w+xi+yj+zk
  通常写为 (w,x,y,z)(w, x, y, z)(w,x,y,z)，为单位四元数。

注意：不同库（ROS、Unity、DirectX、GLM、Eigen 等）可能有不同的轴定义、欧拉角顺序和左/右手坐标系，要确保公式与使用的库约定一致。下面是右手坐标系，主动旋转（rotate vector）语境。

---

一、欧拉角 ↔ 四元数：数学推导与公式

1.1 基本概念回顾

欧拉角（Euler Angles）

• 使用三个有序旋转（绕坐标轴）描述三维空间中的任意姿态：
  • roll（绕 x）
  • pitch（绕 y）
  • yaw（绕 z）
• 优点：直观（类似飞机的滚转、俯仰、偏航）
• 缺点：存在万向节锁（gimbal lock）、插值不平滑、组合复杂。

四元数（Quaternion）

• 单位四元数 q=(w,x,y,z)q = (w, x, y, z)q=(w,x,y,z) 描述旋转
• 也可用轴角形式表达：
  q=(cos⁡θ2,  sin⁡θ2 u^x,  sin⁡θ2 u^y,  sin⁡θ2 u^z) q = \left( \cos\frac{\theta}{2}, \; \sin\frac{\theta}{2}\,\hat{u}_x , \; \sin\frac{\theta}{2}\,\hat{u}_y , \; \sin\frac{\theta}{2}\,\hat{u}_z \right) q=(cos2θ,sin2θu^x,sin2θu^y,sin2θu^z)
  其中 u^\hat{u}u^ 为单位旋转轴，θ\thetaθ 为旋转角度
• 优点：无万向节锁、易于插值（slerp）、组合为四元数乘法。

---

1.2 单轴旋转的四元数

绕坐标轴的旋转（右手定则）：

• 绕 x 轴转角 α\alphaα：
  qx(α)=(cos⁡α2, sin⁡α2, 0, 0) q_x(\alpha) = \left( \cos\frac{\alpha}{2},\ \sin\frac{\alpha}{2},\ 0,\ 0 \right) qx(α)=(cos2α, sin2α, 0, 0)
• 绕 y 轴转角 β\betaβ：
  qy(β)=(cos⁡β2, 0, sin⁡β2, 0) q_y(\beta) = \left( \cos\frac{\beta}{2},\ 0,\ \sin\frac{\beta}{2},\ 0 \right) qy(β)=(cos2β, 0, sin2β, 0)
• 绕 z 轴转角 γ\gammaγ：
  qz(γ)=(cos⁡γ2, 0, 0, sin⁡γ2) q_z(\gamma) = \left( \cos\frac{\gamma}{2},\ 0,\ 0,\ \sin\frac{\gamma}{2} \right) qz(γ)=(cos2γ, 0, 0, sin2γ)

旋转组合对应四元数乘法（右乘，约定为"后旋转写在左边"或"先执行的在右边"要统一）：

对于
R=Rz(ψ) Ry(θ) Rx(ϕ) R = R_z(\psi)\,R_y(\theta)\,R_x(\phi) R=Rz(ψ)Ry(θ)Rx(ϕ)

对应的四元数是
q=qz(ψ) qy(θ) qx(ϕ) q = q_z(\psi)\, q_y(\theta)\, q_x(\phi) q=qz(ψ)qy(θ)qx(ϕ)

---

1.3 欧拉角 (roll, pitch, yaw) → 四元数 (w, x, y, z)

令（单位：弧度）：

• roll = ϕ\phiϕ
• pitch = θ\thetaθ
• yaw = ψ\psiψ

先计算半角的三角函数：

cϕ=cos⁡ϕ2,sϕ=sin⁡ϕ2cθ=cos⁡θ2,sθ=sin⁡θ2cψ=cos⁡ψ2,sψ=sin⁡ψ2 \begin{aligned} c_\phi &= \cos\frac{\phi}{2}, & s_\phi &= \sin\frac{\phi}{2} \\ c_\theta &= \cos\frac{\theta}{2}, & s_\theta &= \sin\frac{\theta}{2} \\ c_\psi &= \cos\frac{\psi}{2}, & s_\psi &= \sin\frac{\psi}{2} \end{aligned} cϕcθcψ=cos2ϕ,=cos2θ,=cos2ψ,sϕsθsψ=sin2ϕ=sin2θ=sin2ψ

按照
q=qz(ψ) qy(θ) qx(ϕ) q = q_z(\psi)\, q_y(\theta)\, q_x(\phi) q=qz(ψ)qy(θ)qx(ϕ)

推导结果为（Z-Y-X，yaw-pitch-roll）：

w=cϕcθcψ+sϕsθsψx=sϕcθcψ−cϕsθsψy=cϕsθcψ+sϕcθsψz=cϕcθsψ−sϕsθcψ \begin{aligned} w &= c_\phi c_\theta c_\psi + s_\phi s_\theta s_\psi \\ x &= s_\phi c_\theta c_\psi - c_\phi s_\theta s_\psi \\ y &= c_\phi s_\theta c_\psi + s_\phi c_\theta s_\psi \\ z &= c_\phi c_\theta s_\psi - s_\phi s_\theta c_\psi \end{aligned} wxyz=cϕcθcψ+sϕsθsψ=sϕcθcψ−cϕsθsψ=cϕsθcψ+sϕcθsψ=cϕcθsψ−sϕsθcψ

习惯上返回顺序为 (w, x, y, z)。

这是极其常用的一组公式，例如 ROS、Eigen、许多游戏/机器人代码都采用此约定的某种变体。如果你的应用使用了不同顺序（XYZ、ZYX 等），公式会相应改变，不要混用。

---

1.4 四元数 (w, x, y, z) → 欧拉角 (roll, pitch, yaw)

现在给定单位四元数
q=(w,x,y,z) q = (w, x, y, z) q=(w,x,y,z)

目标是恢复 Z-Y-X 顺序的欧拉角（yaw-pitch-roll）。

先给出最终常用公式，再解释细节。

常用公式（Z-Y-X）

roll=ϕ=atan2⁡(2(wx+yz), 1−2(x2+y2))pitch=θ=arcsin⁡(2(wy−zx))yaw=ψ=atan2⁡(2(wz+xy), 1−2(y2+z2)) \begin{aligned} \text{roll} &= \phi = \operatorname{atan2}\big( 2(w x + y z),\ 1 - 2(x^2 + y^2) \big) \\ \text{pitch} &= \theta = \arcsin\big( 2(w y - z x) \big) \\ \text{yaw} &= \psi = \operatorname{atan2}\big( 2(w z + x y),\ 1 - 2(y^2 + z^2) \big) \end{aligned} rollpitchyaw=ϕ=atan2(2(wx+yz), 1−2(x2+y2))=θ=arcsin(2(wy−zx))=ψ=atan2(2(wz+xy), 1−2(y2+z2))

但是要注意数值误差导致的越界问题：

中间项
s=2(wy−zx) s = 2(w y - z x) s=2(wy−zx)

理论上 s∈[−1,1]s \in [-1, 1]s∈[−1,1]，实际浮点运算可能略超出，所以通常需要：

s = 2 * (w*y - z*x)
if s >= 1: pitch = +pi/2
elif s <= -1: pitch = -pi/2
else: pitch = asin(s)

这是为了处理接近万向节锁时的稳健性。

简要推导思路（可略读）

从四元数得到旋转矩阵 (R)，再从矩阵提取欧拉角。

单位四元数对应的旋转矩阵为：

R=[1−2(y2+z2)2(xy−zw)2(xz+yw)2(xy+zw)1−2(x2+z2)2(yz−xw)2(xz−yw)2(yz+xw)1−2(x2+y2)] R = \begin{bmatrix} 1 - 2(y^2 + z^2) & 2(x y - z w) & 2(x z + y w) \\ 2(x y + z w) & 1 - 2(x^2 + z^2) & 2(y z - x w) \\ 2(x z - y w) & 2(y z + x w) & 1 - 2(x^2 + y^2) \end{bmatrix} R= 1−2(y2+z2)2(xy+zw)2(xz−yw)2(xy−zw)1−2(x2+z2)2(yz+xw)2(xz+yw)2(yz−xw)1−2(x2+y2)

在 Z-Y-X 约定下：
R=Rz(ψ)Ry(θ)Rx(ϕ) R = R_z(\psi) R_y(\theta) R_x(\phi) R=Rz(ψ)Ry(θ)Rx(ϕ)

对该矩阵逐项比较，就可以推导出上述 atan2 / asin 公式。由于推导较繁琐，在工程代码中通常直接使用已知的标准形式。

---

二、C++ 实现示例

下面给出不依赖第三方库 的简单 C++ 实现（仅使用 <cmath>），约定：

• 输入输出的欧拉角单位为弧度
• 欧拉角顺序：输入/输出按 (roll, pitch, yaw)
• 四元数顺序：(w, x, y, z)

如需使用度数，可额外写转换函数 deg2rad / rad2deg。

2.1 C++：欧拉角 → 四元数

#include <cmath>
#include <tuple>

struct Quaternion
{
    double w, x, y, z;
};

// 输入：roll, pitch, yaw (弧度)
// 输出：单位四元数 (w, x, y, z)
// 约定：Z-Y-X (yaw-pitch-roll) 旋转顺序
Quaternion euler_to_quaternion(double roll, double pitch, double yaw)
{
    double half_roll  = roll  * 0.5;
    double half_pitch = pitch * 0.5;
    double half_yaw   = yaw   * 0.5;

    double c1 = std::cos(half_roll);
    double s1 = std::sin(half_roll);
    double c2 = std::cos(half_pitch);
    double s2 = std::sin(half_pitch);
    double c3 = std::cos(half_yaw);
    double s3 = std::sin(half_yaw);

    Quaternion q;
    q.w = c1 * c2 * c3 + s1 * s2 * s3;
    q.x = s1 * c2 * c3 - c1 * s2 * s3;
    q.y = c1 * s2 * c3 + s1 * c2 * s3;
    q.z = c1 * c2 * s3 - s1 * s2 * c3;

    return q;
}

2.2 C++：四元数 → 欧拉角

#include <cmath>
#include <tuple>

struct Quaternion
{
    double w, x, y, z;
};

// 输出：roll, pitch, yaw (弧度)
// 约定与 euler_to_quaternion 保持一致：Z-Y-X (yaw-pitch-roll)
std::tuple<double, double, double> quaternion_to_euler(const Quaternion& q)
{
    double w = q.w;
    double x = q.x;
    double y = q.y;
    double z = q.z;

    // roll (x-axis rotation)
    double sinr_cosp = 2.0 * (w * x + y * z);
    double cosr_cosp = 1.0 - 2.0 * (x * x + y * y);
    double roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2.0 * (w * y - z * x);
    double pitch;
    if (std::fabs(sinp) >= 1.0)
    {
        // use 90 degrees if out of range
        pitch = std::copysign(M_PI / 2.0, sinp);
    }
    else
    {
        pitch = std::asin(sinp);
    }

    // yaw (z-axis rotation)
    double siny_cosp = 2.0 * (w * z + x * y);
    double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
    double yaw = std::atan2(siny_cosp, cosy_cosp);

    return std::make_tuple(roll, pitch, yaw);
}

2.3 可选：角度与弧度转换（C++）

如果你更习惯用角度制：

constexpr double PI = 3.14159265358979323846;

inline double deg2rad(double deg) { return deg * PI / 180.0; }
inline double rad2deg(double rad) { return rad * 180.0 / PI; }

使用时注意统一单位，例如：

double roll_deg = 30.0;
double pitch_deg = 10.0;
double yaw_deg = 45.0;

Quaternion q = euler_to_quaternion(
    deg2rad(roll_deg),
    deg2rad(pitch_deg),
    deg2rad(yaw_deg)
);

---

三、Python 实现示例

Python 实现会更简洁，下面给出纯 numpy 实现 ，不依赖外部库。如果你使用 scipy 或 transformations.py，可以直接调用现成函数，不过这里展示"从零实现"的版本以说明公式。

3.1 Python：欧拉角 → 四元数

import numpy as np

def euler_to_quaternion(roll, pitch, yaw):
    """
    欧拉角 (roll, pitch, yaw) -> 四元数 (w, x, y, z)
    约定：Z-Y-X (yaw-pitch-roll)，弧度制
    """
    half_roll  = roll  * 0.5
    half_pitch = pitch * 0.5
    half_yaw   = yaw   * 0.5

    c1 = np.cos(half_roll)
    s1 = np.sin(half_roll)
    c2 = np.cos(half_pitch)
    s2 = np.sin(half_pitch)
    c3 = np.cos(half_yaw)
    s3 = np.sin(half_yaw)

    w = c1 * c2 * c3 + s1 * s2 * s3
    x = s1 * c2 * c3 - c1 * s2 * s3
    y = c1 * s2 * c3 + s1 * c2 * s3
    z = c1 * c2 * s3 - s1 * s2 * c3

    return np.array([w, x, y, z], dtype=float)

3.2 Python：四元数 → 欧拉角

import numpy as np

def quaternion_to_euler(q):
    """
    四元数 (w, x, y, z) -> 欧拉角 (roll, pitch, yaw)
    约定与 euler_to_quaternion 一致：Z-Y-X (yaw-pitch-roll)，弧度制
    """
    q = np.asarray(q, dtype=float)
    w, x, y, z = q

    # roll (x-axis)
    sinr_cosp = 2.0 * (w * x + y * z)
    cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    # pitch (y-axis)
    sinp = 2.0 * (w * y - z * x)
    if np.abs(sinp) >= 1.0:
        pitch = np.sign(sinp) * np.pi / 2.0
    else:
        pitch = np.arcsin(sinp)

    # yaw (z-axis)
    siny_cosp = 2.0 * (w * z + x * y)
    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

3.3 简单测试（Python）

你可以做一个往返测试，验证实现是否一致：

if __name__ == "__main__":
    # 测试用的欧拉角（弧度）
    roll  = np.deg2rad(30.0)
    pitch = np.deg2rad(20.0)
    yaw   = np.deg2rad(60.0)

    q = euler_to_quaternion(roll, pitch, yaw)
    print("Quaternion:", q)

    r2, p2, y2 = quaternion_to_euler(q)
    print("Recovered (deg):", np.rad2deg([r2, p2, y2]))

输出中恢复的角度应该与原始 (30, 20, 60) 非常接近（浮点误差级别）。

---

四、工程实践中的注意事项（简要）

1. 保持约定统一

   • 坐标系：右手还是左手？
   • 欧拉角顺序：XYZ、ZYX、ZXY 等？
   • 旋转是主动还是被动（坐标系旋转）？
     使用不同库时，一定确认文档，不然很容易出现角度"看起来不对"的问题。

2. 单位统一（度 / 弧度）

   C/C++ 中的三角函数使用弧度，Python 的 numpy 也是弧度；

   如果你的业务逻辑习惯用度数，记得在接口处集中转换。

3. 归一化四元数

   由于数值误差，反复运算后的四元数可能不再是单位长度，需要定期做一次归一化：

   double norm = std::sqrt(w*w + x*x + y*y + z*z);
   w /= norm; x /= norm; y /= norm; z /= norm;

4. 避免频繁使用欧拉角插值

   在动画、控制、轨迹规划中，如果要在姿态之间插值，尽量使用四元数插值（slerp），而不是直接线性插值欧拉角。