旋转矩阵与欧拉角转换数学公式与代码详解-深圳市維司達科技有限公司

下面分三大部分说明：

数学推导与公式（欧拉角 ↔ 四元数）
C++ 实现代码
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>structQuaternion{doublew,x,y,z;};// 输入：roll, pitch, yaw (弧度)// 输出：单位四元数 (w, x, y, z)// 约定：Z-Y-X (yaw-pitch-roll) 旋转顺序Quaternioneuler_to_quaternion(doubleroll,doublepitch,doubleyaw){doublehalf_roll=roll*0.5;doublehalf_pitch=pitch*0.5;doublehalf_yaw=yaw*0.5;doublec1=std::cos(half_roll);doubles1=std::sin(half_roll);doublec2=std::cos(half_pitch);doubles2=std::sin(half_pitch);doublec3=std::cos(half_yaw);doubles3=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;returnq;}

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

#include<cmath>#include<tuple>structQuaternion{doublew,x,y,z;};// 输出：roll, pitch, yaw (弧度)// 约定与 euler_to_quaternion 保持一致：Z-Y-X (yaw-pitch-roll)std::tuple<double,double,double>quaternion_to_euler(constQuaternion&q){doublew=q.w;doublex=q.x;doubley=q.y;doublez=q.z;// roll (x-axis rotation)doublesinr_cosp=2.0*(w*x+y*z);doublecosr_cosp=1.0-2.0*(x*x+y*y);doubleroll=std::atan2(sinr_cosp,cosr_cosp);// pitch (y-axis rotation)doublesinp=2.0*(w*y-z*x);doublepitch;if(std::fabs(sinp)>=1.0){// use 90 degrees if out of rangepitch=std::copysign(M_PI/2.0,sinp);}else{pitch=std::asin(sinp);}// yaw (z-axis rotation)doublesiny_cosp=2.0*(w*z+x*y);doublecosy_cosp=1.0-2.0*(y*y+z*z);doubleyaw=std::atan2(siny_cosp,cosy_cosp);returnstd::make_tuple(roll,pitch,yaw);}

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

如果你更习惯用角度制：

constexprdoublePI=3.14159265358979323846;inlinedoubledeg2rad(doubledeg){returndeg*PI/180.0;}inlinedoublerad2deg(doublerad){returnrad*180.0/PI;}

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

doubleroll_deg=30.0;doublepitch_deg=10.0;doubleyaw_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：欧拉角 → 四元数

importnumpyasnpdefeuler_to_quaternion(roll,pitch,yaw):""" 欧拉角 (roll, pitch, yaw) -> 四元数 (w, x, y, z) 约定：Z-Y-X (yaw-pitch-roll)，弧度制 """half_roll=roll*0.5half_pitch=pitch*0.5half_yaw=yaw*0.5c1=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*c3returnnp.array([w,x,y,z],dtype=float)

3.2 Python：四元数 → 欧拉角

importnumpyasnpdefquaternion_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)ifnp.abs(sinp)>=1.0:pitch=np.sign(sinp)*np.pi/2.0else: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)returnroll,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) 非常接近（浮点误差级别）。

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

保持约定统一
- 坐标系：右手还是左手？
- 欧拉角顺序：XYZ、ZYX、ZXY 等？
- 旋转是主动还是被动（坐标系旋转）？
  使用不同库时，一定确认文档，不然很容易出现角度“看起来不对”的问题。
单位统一（度 / 弧度）
C/C++ 中的三角函数使用弧度，Python 的numpy也是弧度；
如果你的业务逻辑习惯用度数，记得在接口处集中转换。
归一化四元数
由于数值误差，反复运算后的四元数可能不再是单位长度，需要定期做一次归一化：
```
doublenorm=std::sqrt(w*w+x*x+y*y+z*z);w/=norm;x/=norm;y/=norm;z/=norm;
```
避免频繁使用欧拉角插值
在动画、控制、轨迹规划中，如果要在姿态之间插值，尽量使用四元数插值（slerp），而不是直接线性插值欧拉角。