3. 坐标系与空间变换
刚体运动、坐标变换、四元数、旋转矩阵、TF2树……这些概念几乎是SLAM的“空气”。你想想看，机器人每时每刻都在问自己：“我在哪？我朝哪看？” 而答案就藏在这些变换里。这一章，我带你从直觉到代码，把空间变换彻底吃透。
3.1 刚体运动与旋转矩阵
说白了，刚体运动就是“不拧麻花”的移动加旋转。在SLAM里，我们假设机器人是一个刚体——轮子不会自己扭曲，激光雷达也不会自己变形。旋转矩阵 R 是描述朝向最直接的方式，3x3 的矩阵，每一列都是单位正交的。
核心性质： RTR = I ，det(R) = +1。这意味着旋转矩阵属于特殊正交群 SO(3)。
我记得刚做SLAM那会儿，总喜欢把旋转矩阵写成 4x4 齐次矩阵，方便连乘。其实在代码里，Eigen 或 tf2 都帮你封装好了。你只需要理解：p' = R * p + t，这就是刚体变换的全部秘密。
3.2 四元数与欧拉角
欧拉角很直观，但有个臭名昭著的毛病——万向锁。我在项目里遇到过，用欧拉角做云台控制，俯仰到 ±90° 时，航向和翻滚突然耦合，机器人原地转圈…… 从那以后，我内部计算一律用四元数。
四元数 q = w + xi + yj + zk，模长为1。ROS2 里 geometry_msgs::msg::Quaternion 就是标准载体。我个人习惯用 tf2::Quaternion 做运算，非常顺手。
// C++ 示例：四元数旋转点
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
tf2::Quaternion q;
q.setRPY(0.0, 0.0, 1.57); // 绕Z轴转90度
tf2::Vector3 p(1.0, 0.0, 0.0);
tf2::Vector3 p_rotated = tf2::quatRotate(q, p);
// p_rotated ≈ (0.0, 1.0, 0.0)3.3 TF2 树在 ROS2 中的实现
TF2 是ROS2的坐标变换骨架。每个坐标系都是一个节点，变换关系就是边。整个结构是一棵有向树，从 map 到 odom 到 base_link 再到各个传感器帧。
你想想看，激光雷达的数据是在 laser_frame 下测量的，要变成地图上的点，就得沿着TF树一直变换到 map。TF2 帮你自动做链式乘法。
# 命令行工具：查看变换
ros2 run tf2_ros tf2_echo map base_link
# 输出：Translation: [0.12, 0.05, 0.00], Rotation: in Quaternion [0.00, 0.00, 0.07, 0.99]| TF2 组件 | 作用 | ROS2 包 |
|---|---|---|
| Broadcaster | 发布坐标系之间的变换 | tf2_ros::TransformBroadcaster |
| Listener | 缓存并查询变换 | tf2_ros::Buffer + TransformListener |
| StaticBroadcaster | 发布固定不变的变换（如激光与底座） | tf2_ros::StaticTransformBroadcaster |
TF2 最佳实践： 所有传感器帧都挂在 base_link 下，odom 到 base_link 由里程计发布，map 到 odom 由 SLAM 算法发布。千万不要把 map 直接连到 base_link，否则你的定位会失去全局修正能力。
3.4 欧拉角万向锁问题
万向锁不是bug，是欧拉角表示法的数学缺陷。当俯仰角为 ±90° 时，第一次旋转和第三次旋转的轴重合，丢失一个自由度。在SLAM中，如果你用欧拉角做图优化，可能会让优化器陷入奇异。
我建议：存储和优化用四元数或旋转矩阵，只在可视化或人机交互时转成欧拉角。 ROS2 的 tf2::getYaw() 只提取偏航角，不会触发万向锁，因为它是从四元数计算的。
嗯，这一章的内容其实很密集。刚体运动、旋转矩阵、四元数、TF2树，每一个都是SLAM的基石。你在写代码时，多想想“这个变换是在哪个坐标系下表达的？” 习惯之后，你会发现空间变换就像呼吸一样自然。