📌 前言

很多初学者（包括之前的我）喜欢用欧拉角（Roll-Pitch-Yaw）来描述旋转，因为它直观。但在实际工程中，ROS 2 和 MuJoCo 底层强制使用 四元数 (Quaternion)。为什么？因为欧拉角有著名的“万向节死锁”问题。

这两天，我利用 Eigen 3 数学库和 ROS 2 TF2 工具，打通了从“数学推导”到“可视化验证”的全流程。本文将深入浅出地讲解背后的数学原理，并附上完整的 C++ 实现代码。

---

一、 理论核心：机器人是怎么“定位”的？

在写代码之前，必须先搞懂两个核心数学概念。

1. 齐次变换矩阵 (Homogeneous Transformation)

机器人身上的每个部件（雷达、相机、脚尖）都有自己的坐标系。我们要解决的问题通常是：已知障碍物在雷达坐标系 {B}\{B\}{B} 里的位置 ${}^{B}P$，求它在世界坐标系 {A}\{A\}{A} 里的位置 ${}^{A}P$。

通用公式如下：

$${}^{A}P={}_B^{A}T\cdot {}^{B}P$$

这里的 ${}_B^{A}T$ 是一个 $4\times 4$ 的大矩阵，它包含了两部分信息：

• 旋转 ($R$)：$3\times 3$ 矩阵，描述 {B}\{B\}{B} 姿态相对于 {A}\{A\}{A} 转了多少。
• 平移 ($P$)：$3\times 1$ 向量，描述 {B}\{B\}{B} 原点在 {A}\{A\}{A} 中的位置。

2. 为什么要用四元数 (Quaternion)？

我们在 ROS 2 的 msg.pose.orientation 里看到的 x, y, z, w 就是四元数。既然欧拉角（Roll, Pitch, Yaw）这么直观，为什么不用它？

💡 深度解析：什么是“万向节死锁” (Gimbal Lock)？

很多教程只说“欧拉角会导致死锁”，但很少解释清楚为什么。这里用一个**“高射炮打蚊子”**的例子来直观理解。

想象你操控一台双轴高射炮：

1. 底座 (Yaw)：负责水平 360° 旋转。
2. 炮管 (Pitch)：负责上下抬起。

正常情况：
蚊子在前方 45° 天空。你转动底座（左右移），抬起炮管（上下移）。“左”和“上”是两个独立的维度，你可以轻松瞄准任何位置。

死锁情况：
蚊子飞到了你的正头顶（天顶）。你把炮管垂直向上抬起 90度。
此时，如果你想让炮口往“原本的左边”稍微偏一点点（去追稍微飞偏的蚊子），你会发现做不到！

• 转动底座？炮口只是在原地自转（像螺旋桨一样），并没有指向新的方向。
• 转动炮管？只能让炮口降下来，离开天顶。

结论：当 Pitch = 90° 时，“水平转动”这个功能失效了（或者说它和“自转”重合了）。我们原本拥有两个自由度，现在丢失了一个，这就叫死锁。

四元数为何能解？
欧拉角是分步走（先转底座，再抬炮管），这导致了步骤之间的依赖。而四元数是一步到位：它直接定义空间中的一根轴（Vector），让物体绕着这根轴一次性转过去。既然是一次性成型，就不存在“谁卡住谁”的问题，从而彻底解决了死锁。

---

二、 代码实战 A：Eigen 3 与 ROS 2 的结合

C++ 标准库没有矩阵运算，所以我们必须引入 Eigen 3。这是机器人领域的标配数学库。

1. CMake 配置 (避坑点)

Eigen 是 Header-only (纯头文件) 库，不需要链接 .so 文件，但必须告诉编译器头文件在哪里。

CMakeLists.txt 关键配置：

find_package(Eigen3 REQUIRED)

# 关键：${Eigen3_INCLUDE_DIRS} 变量有时会失效
# 如果报错，可以直接指定系统路径：/usr/include/eigen3
target_include_directories(math_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  ${Eigen3_INCLUDE_DIRS} 
)

2. 核心算法：欧拉角转四元数

利用 Eigen 的几何模块 (Eigen::Geometry)，我们可以一行代码完成转换，避免手写复杂的三角函数。

#include <Eigen/Dense>
#include <Eigen/Geometry>

// 1. 定义欧拉角 (例如：绕 Z 轴旋转)
// ⚠️ 注意：计算机只认弧度，不认角度！务必转换！
double yaw_rad = 90.0 * M_PI / 180.0;

// 2. 使用 Axis-Angle (轴角) 构建旋转
// Vector3d::UnitZ() 代表 Z 轴 (0, 0, 1)
Eigen::AngleAxisd t_V(yaw_rad, Eigen::Vector3d::UnitZ());

// 3. 转换为四元数
Eigen::Quaterniond q(t_V);

// 4. 打印 (Eigen 顺序是 w, x, y, z)
RCLCPP_INFO(this->get_logger(), "四元数: w=%.2f, x=%.2f, y=%.2f, z=%.2f", 
            q.w(), q.x(), q.y(), q.z());

---

三、 代码实战 B：TF2 广播与 Rviz 可视化

为了验证我们的四元数是对的，我写了一个 ROS 2 节点，利用 tf2_ros 把计算结果广播出去，然后在 Rviz 里看箭头转动。

1. TF 广播器代码要点

// 创建 TF 消息
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();

// ⚠️ 关键设置：父子坐标系不能搞反！
t.header.frame_id = "world";   // 父坐标系 (不动)
t.child_frame_id = "my_dog";   // 子坐标系 (旋转的物体)

// 填充我们算出来的四元数
t.transform.rotation.w = q.w();
t.transform.rotation.x = q.x();
// ...

// 发送！
tf_broadcaster_->sendTransform(t);

2. 成果展示

在 Rviz2 中添加 TF 插件，将 Fixed Frame 设为 world，成功看到了一根离地 1 米的坐标轴正在绕 Z 轴旋转。

---

---

四、 总结与展望

• 数学 是机器人的灵魂。
• ROS 2 是机器人的神经。
• Eigen 是连接两者的桥梁。