C++ Python使用常用库时如何做欧拉角 ⇄ 四元数转换

下面在原有推导与基础代码的基础上，专门补充使用常用库时如何做欧拉角 ⇄ 四元数转换，分别针对：

• C++：Eigen、ROS tf2、GLM（游戏/图形常用）
• Python：SciPy、ROS tf_transformations、NumPy-Quaternion（可选）

仍然使用上一版的约定（如无特别说明）：

• 右手坐标系
• 欧拉角顺序：Z-Y-X（yaw-pitch-roll）
• 欧拉角含义：
  • roll：绕 x 轴
  • pitch：绕 y 轴
  • yaw：绕 z 轴
• 四元数：((w, x, y, z))，单位四元数

非常关键：不同库对欧拉顺序和四元数分量顺序的约定不完全一样，下面会在每个库的示例里明确写清楚，避免“方向反了”“绕错轴”等问题。

一、C++ 中用常见库进行转换

1.1 使用 Eigen（C++ 线性代数库）

Eigen 在机器人学和图形学领域非常常用。
核心类型：

• Eigen::Quaterniond：四元数，内部存储顺序为(x, y, z, w)
• Eigen::Matrix3d：3x3 旋转矩阵
• Eigen::AngleAxisd和Eigen::Transform也可以描述旋转

Eigen没有直接“给 roll, pitch, yaw 就生成四元数”的单函数，但可以通过AngleAxis或eulerAngles组合。

1.1.1 欧拉角 → 四元数（Eigen，ZYX）

想要 ZYX（yaw, pitch, roll），可以通过旋转矩阵中间步骤实现：

#include<Eigen/Core>#include<Eigen/Geometry>#include<iostream>// 欧拉角 -> 四元数// 输入：roll, pitch, yaw（弧度）// 旋转顺序：Z-Y-X（yaw-pitch-roll）Eigen::Quaterniondeuler_to_quaternion_eigen(doubleroll,doublepitch,doubleyaw){// 按 Z-Y-X 生成旋转矩阵Eigen::Matrix3d R;R=Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());// 从旋转矩阵生成四元数（内部为 (x,y,z,w)）Eigen::Quaterniondq(R);q.normalize();// 以防数值误差returnq;}

Eigen::Quaterniond的coeffs()顺序是(x, y, z, w)，如果你需要(w, x, y, z)，要手动取：

Eigen::Quaterniond q=euler_to_quaternion_eigen(roll,pitch,yaw);doublew=q.w();doublex=q.x();doubley=q.y();doublez=q.z();

1.1.2 四元数 → 欧拉角（Eigen，ZYX）

Eigen 提供matrix().eulerAngles(0,1,2)等方法从旋转矩阵中提取欧拉角，不过索引顺序比较绕，所以推荐一种更直观的形式：直接用RotationMatrix+eulerAngles(2,1,0)表示 ZYX。

#include<Eigen/Core>#include<Eigen/Geometry>#include<tuple>// 四元数 -> 欧拉角 (roll, pitch, yaw)std::tuple<double,double,double>quaternion_to_euler_eigen(constEigen::Quaterniond&q){// 转为旋转矩阵Eigen::Matrix3d R=q.normalized().toRotationMatrix();// eulerAngles 的参数是轴索引：0->X, 1->Y, 2->Z// Z-Y-X（yaw, pitch, roll），因此使用 (2,1,0)Eigen::Vector3d euler=R.eulerAngles(2,1,0);doubleyaw=euler[0];doublepitch=euler[1];doubleroll=euler[2];returnstd::make_tuple(roll,pitch,yaw);}

如果你要用X-Y-Z顺序，只需要对应改成R.eulerAngles(0,1,2)等即可，但要与自己定义的数学公式对齐。

1.2 使用 ROS tf2（ROS 机器人框架）

在 ROS1 中常见的是tf；在 ROS2 或新代码中推荐tf2和tf2_geometry_msgs。

常用类型：

• tf2::Quaternion：四元数（内部是(x, y, z, w)）
• tf2::Matrix3x3：3x3 旋转矩阵风格的类

1.2.1 欧拉角 → 四元数（tf2）

tf2::Quaternion提供了setRPY(roll, pitch, yaw)方法，顺序为：R-P-Y，对应 X-Y-Z，即默认是X-Y-Z 内部实现顺序为 yaw-pitch-roll？
但从使用角度看，你只要把 roll, pitch, yaw 直接填进去即可，tf2会按其内部的一套约定生成姿态。关键是“正反变换要用同一套 tf2 函数”，不混用别的库的 ZYX 公式即可。

#include<tf2/LinearMath/Quaternion.h>#include<tf2/LinearMath/Matrix3x3.h>// 欧拉角 -> 四元数tf2::Quaternioneuler_to_quaternion_tf(doubleroll,doublepitch,doubleyaw){tf2::Quaternion q;q.setRPY(roll,pitch,yaw);// 参数顺序：roll, pitch, yawq.normalize();returnq;}

1.2.2 四元数 → 欧拉角（tf2）

#include<tf2/LinearMath/Quaternion.h>#include<tf2/LinearMath/Matrix3x3.h>#include<tuple>// 四元数 -> 欧拉角std::tuple<double,double,double>quaternion_to_euler_tf(consttf2::Quaternion&q_in){tf2::Quaternion q=q_in;q.normalize();tf2::Matrix3x3m(q);doubleroll,pitch,yaw;m.getRPY(roll,pitch,yaw);// 输出 roll, pitch, yawreturnstd::make_tuple(roll,pitch,yaw);}

重要：setRPY和getRPY是成对使用，互相兼容的；对于绝大多数 ROS 应用，你只需要遵循这对 API，而不必手动去记某种“ZYX/XYZ”的数学定义。

1.3 使用 GLM（OpenGL / 游戏常用数学库）

GLM（OpenGL Mathematics）广泛用于游戏和图形引擎里，API 风格类似 GLSL。

常用类型：

• glm::quat：四元数，分量顺序是(w, x, y, z)
• glm::vec3：向量
• glm::mat3/mat4：矩阵

GLM 约定默认是右手坐标系，弧度制（建议开启GLM_FORCE_RADIANS）。

1.3.1 欧拉角 → 四元数（GLM）

GLM 提供了glm::quat(glm::vec3(eulerAngles))的构造重载，但其内部欧拉角顺序为Z-Y-X（pitch-yaw-roll 的某种约定），并且有版本差异与坑。
更推荐“明确轴旋转”的写法：

#include<glm/glm.hpp>#include<glm/gtc/quaternion.hpp>#include<glm/gtx/quaternion.hpp>// 欧拉角->四元数 (roll, pitch, yaw) 弧度制，Z-Y-X 旋转顺序glm::quateuler_to_quaternion_glm(floatroll,floatpitch,floatyaw){glm::quat qx=glm::angleAxis(roll,glm::vec3(1.0f,0.0f,0.0f));glm::quat qy=glm::angleAxis(pitch,glm::vec3(0.0f,1.0f,0.0f));glm::quat qz=glm::angleAxis(yaw,glm::vec3(0.0f,0.0f,1.0f));// 先绕 X, 再绕 Y, 再绕 Z => q = qz * qy * qxglm::quat q=qz*qy*qx;returnglm::normalize(q);}

1.3.2 四元数 → 欧拉角（GLM）

可以用glm::eulerAngles(q)，返回的 vec3 中的含义要看文档（一般为 XYZ 顺序的欧拉角）。如果你保证创建时用的是 ZYX 组合方式，正反方向保持一致即可：

#include<glm/glm.hpp>#include<glm/gtc/quaternion.hpp>#include<glm/gtx/quaternion.hpp>glm::vec3quaternion_to_euler_glm(constglm::quat&q_in){glm::quat q=glm::normalize(q_in);// 返回的三分量通常可认为是 (pitchX, yawY, rollZ) 或 (roll, pitch, yaw) 视版本而定glm::vec3 euler=glm::eulerAngles(q);// 如果你要明确 roll, pitch, yaw 的含义，建议用约定统一它returneuler;}

GLM 对欧拉角的标注相对含糊，如想绝对严谨，推荐：只在 GLM 内使用四元数与矩阵，不在其中大量用欧拉角，或者写清自己的封装并测试正反变换。

二、Python 中使用常见库

2.1 使用 SciPy（推荐）

如果你可以使用scipy，强烈推荐scipy.spatial.transform.Rotation，设计良好、文档清晰。

安装（如需）：

pipinstallscipy

2.1.1 欧拉角 → 四元数（SciPy）

fromscipy.spatial.transformimportRotationasRimportnumpyasnp# 欧拉角 -> 四元数defeuler_to_quaternion_scipy(roll,pitch,yaw):""" roll, pitch, yaw: 弧度 约定：Z-Y-X (yaw, pitch, roll)，'zyx' 表示按照给定顺序应用旋转 SciPy 的 from_euler('zyx', [yaw, pitch, roll]) 是最自然的写法 """r=R.from_euler('zyx',[yaw,pitch,roll])# 注意顺序是 [z, y, x] = [yaw, pitch, roll]# SciPy 的 as_quat() 返回顺序是 (x, y, z, w)x,y,z,w=r.as_quat()returnnp.array([w,x,y,z])# 转为 (w, x, y, z)

如果你要批量处理（N×3 的欧拉角数组），from_euler也支持批量。

2.1.2 四元数 → 欧拉角（SciPy）

fromscipy.spatial.transformimportRotationasRimportnumpyasnpdefquaternion_to_euler_scipy(q):""" q: 长度为 4 的数组/列表，顺序 (w, x, y, z) 返回：roll, pitch, yaw（弧度） """q=np.asarray(q,dtype=float)w,x,y,z=q# SciPy Rotation 需要 (x, y, z, w)r=R.from_quat([x,y,z,w])# 'zyx' 对应 [yaw, pitch, roll]yaw,pitch,roll=r.as_euler('zyx')returnroll,pitch,yaw

小结：

• SciPy 内部的as_quat始终是(x, y, z, w)
• from_euler('zyx', [yaw, pitch, roll])明确地表示 Z-Y-X 顺序
• 你只需要在“自己的函数接口”层面，用好(w, x, y, z)和(roll, pitch, yaw)这套约定即可。

2.2 使用 ROS 的 tf_transformations（Python 版）

在 ROS1 中常见tf.transformations模块，许多 Python 节点会直接用它。

安装（如果不是在 ROS 环境，可以通过 pip 装一个兼容版本tf-transformations）：

pipinstalltf-transformations

2.2.1 欧拉角 → 四元数（tf_transformations）

importtf_transformationsastfdefeuler_to_quaternion_tf(roll,pitch,yaw):""" tf.transformations.euler_from_quaternion / quaternion_from_euler 使用的欧拉顺序默认为 (roll, pitch, yaw) 对应 XYZ """# 返回顺序为 (x, y, z, w)x,y,z,w=tf.quaternion_from_euler(roll,pitch,yaw)return(w,x,y,z)

2.2.2 四元数 → 欧拉角（tf_transformations）

importtf_transformationsastfdefquaternion_to_euler_tf(q):""" q: (w, x, y, z) 返回 roll, pitch, yaw（弧度） """w,x,y,z=q# euler_from_quaternion 接受 (x, y, z, w)roll,pitch,yaw=tf.euler_from_quaternion([x,y,z,w])returnroll,pitch,yaw

与 C++ 的tf2一样，quaternion_from_euler与euler_from_quaternion是强绑定的一对，正反使用不会有旋转约定不一致的问题。

2.3 仅用 NumPy + 自实现（方便嵌入）

上一版已经提供了纯公式 + numpy的实现，对多数不想依赖 SciPy 的场景就足够了：

• euler_to_quaternion(roll, pitch, yaw)
• quaternion_to_euler(q)

如果你只需要在一个无 ROS、无 SciPy 的环境中做数学转换，这一版即可；如果你已经有 SciPy / ROS，则优先用库函数，以减少出错风险。

三、如何在工程中选择/对齐这些库

1. 在 C++ 机器人系统里（ROS）

   • 建议：直接使用tf2::Quaternion::setRPY与Matrix3x3::getRPY
   • 如果还用到了 Eigen（路径规划/控制算法），则在接口边界使用 Eigen ↔ tf2 的转换（把(x,y,z,w)显式地对号入座）。

2. 在 C++ 纯数学/控制项目里（无 ROS）

   • 强烈建议使用 Eigen 的Quaterniond+AngleAxisd+eulerAngles
   • 不再自己写三角公式，除非你要做极致优化。

3. 在游戏/图形渲染里（C++/GLM）

   • 始终用glm::quat+glm::mat4
   • 尽量避免大量欧拉角逻辑，只在输入输出/编辑器界面做“欧拉角接口”；内部用四元数和矩阵。

4. 在 Python 科学运算/姿态处理里

   • 有 SciPy：用scipy.spatial.transform.Rotation
   • 在 ROS 中：用tf.transformations或tf2的接口，并在边界处转换为 SciPy/Eigen/自定义数学结构。

5. 统一几个关键点（无论你选哪个库都需要确认）：

   • 坐标系：右手/左手
   • 旋转顺序：XYZ / ZYX / …
   • 角度单位：度 / 弧度
   • 四元数分量顺序：是(x,y,z,w)还是(w,x,y,z)

一旦选定，就在项目里写一份简单的文档 + 封装函数，把所有库的外表现象统一成你自己定义的接口形式（例如：统一成(w, x, y, z)+(roll, pitch, yaw, ZYX, 弧度)），避免在工程中混杂着不同语义的函数。