坐标系与变换基础：刚体变换、齐次坐标、旋转矩阵、四元数、TF树在ROS中的使用
做机械臂地图构建，绕不开一个核心问题：坐标系。
你想想看，机械臂的每个关节都有自己的坐标系，激光雷达装在末端也有自己的坐标系，地图又有世界坐标系。这些坐标系之间怎么换算？一个点在激光雷达下是 (0.5, 0.2, 0.3)，到了机械臂基座下是多少？
这就是本章要解决的事。我做了这么多年机器人系统，见过太多新手在这上面栽跟头。坐标系搞不清楚，后面建图、定位、路径规划全都会乱套。
刚体变换：说白了就是“挪”和“转”
刚体变换，就是物体在空间里移动和旋转，但形状、大小都不变。你拿一本书，从桌上拿到书架上——书没变形，只是位置和朝向变了。这就是刚体变换。
在机器人里，刚体变换由两部分组成：
- 平移：从A点挪到B点，用三维向量 (x, y, z) 表示
- 旋转：物体朝向变了，用旋转矩阵或四元数表示
我刚开始做SLAM时，总把平移和旋转分开算，结果代码越写越乱。后来才明白，得把它们合在一起处理——这就是齐次坐标的由来。
齐次坐标：把“挪+转”变成一次矩阵乘法
普通三维坐标 (x, y, z) 没法直接表示旋转加平移。你试试看：一个点先旋转再平移，得先乘旋转矩阵，再加平移向量。两步操作，代码里就得写两行。
齐次坐标的做法很简单：给三维坐标加一维，变成 (x, y, z, 1)。然后用一个 4x4 的变换矩阵，一次乘法就把旋转和平移全搞定。
齐次变换矩阵长这样：
| R11 R12 R13 | tx |
| R21 R22 R23 | ty |
| R31 R32 R33 | tz |
| 0 0 0 | 1 |左上角 3x3 是旋转矩阵 R，右边 3x1 是平移向量 t。最后一行固定是 [0 0 0 1]。
用齐次坐标的好处很明显：多个变换可以连乘。比如从激光雷达到机械臂基座，中间可能经过好几个坐标系，把它们的变换矩阵乘在一起，一次计算就得到最终结果。
我的习惯：在代码里统一用 4x4 齐次矩阵，别混着用。我曾经在一个项目里同时用了旋转矩阵+平移向量和四元数+平移向量，结果调试了整整两天才发现是数据格式不统一。
旋转矩阵：最直观，但容易出问题
旋转矩阵是个 3x3 的正交矩阵，行列式为 +1。每一列代表旋转后的坐标轴在原坐标系下的方向。
举个例子，绕 Z 轴旋转 θ 角：
R_z(θ) = | cosθ -sinθ 0 |
| sinθ cosθ 0 |
| 0 0 1 |旋转矩阵的好处是直观，看一眼就知道是绕哪个轴转了多少度。但坏处也很明显：
- 参数冗余：9 个元素，实际自由度只有 3
- 容易不满足正交性：数值计算累积误差会让矩阵慢慢“变形”
- 插值困难：两个旋转矩阵之间做线性插值，结果往往不是有效的旋转矩阵
我在做机械臂轨迹规划时就吃过这个亏。两个姿态之间做平滑过渡，用旋转矩阵插值，结果末端执行器走了一条奇怪的弧线。后来换成四元数球面插值，问题才解决。
四元数：轻量、稳定、好插值
四元数是个超复数，形式是 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。在机器人里，我们通常用单位四元数表示旋转。
四元数相比旋转矩阵的优势：
| 特性 | 旋转矩阵 | 四元数 |
|---|---|---|
| 参数数量 | 9 个 | 4 个 |
| 奇异性 | 有（万向锁） | 无 |
| 插值 | 困难 | 容易（球面线性插值） |
| 数值稳定性 | 需定期正交化 | 只需归一化 |
| 直观性 | 高 | 低 |
四元数唯一的缺点是不直观。你看到 q = (0.707, 0, 0, 0.707)，很难立刻想到这是绕 X 轴转 90 度。所以我个人的习惯是：调试时用旋转矩阵或欧拉角，正式代码里用四元数。
避坑指南：ROS 里四元数的顺序是 (x, y, z, w)，不是 (w, x, y, z)。我刚开始用 ROS 时，按数学书的习惯写了 (w, x, y, z)，结果 tf 树一直报错，查了半天才发现是顺序搞反了。
TF树：ROS里的坐标系管理神器
前面讲的都是数学工具。在 ROS 里，这些工具被封装成了 TF（Transform）树。
TF 树本质上是一个有向无环图，每个节点代表一个坐标系，每条边代表两个坐标系之间的变换关系。比如：
world→base_link（机器人基座在世界坐标系下的位姿）base_link→arm_base（机械臂基座在机器人基座下的位姿）arm_base→arm_shoulder→arm_elbow→arm_wrist→arm_tool（机械臂各关节）arm_tool→laser_frame（激光雷达在机械臂末端下的位姿）
你只需要发布这些父子变换关系，TF 库会自动帮你维护整棵树。任何时候你想知道“激光雷达下的点 (0.5, 0.2, 0.3) 在世界坐标系下是多少”，一行代码就能搞定。
TF 树的核心操作：
// 监听变换
tf::StampedTransform transform;
listener.lookupTransform("world", "laser_frame",
ros::Time(0), transform);
// 变换点坐标
tf::Vector3 point_laser(0.5, 0.2, 0.3);
tf::Vector3 point_world = transform * point_laser;这里有个关键点：lookupTransform("world", "laser_frame") 返回的是从 laser_frame 到 world 的变换。也就是说，它告诉你怎么把激光雷达坐标系下的点变换到世界坐标系下。
我见过有人搞反了参数顺序，结果地图建出来全是歪的。嗯，这里要注意：第一个参数是目标坐标系，第二个参数是源坐标系。
本章知识体系总览
下面这张图把本章的核心概念串起来了。你可以看到，从刚体变换出发，衍生出平移和旋转两种操作；旋转又有三种表示方式；最后所有这些都汇聚到 TF 树这个工程工具上。
小结
这一章的内容，说白了就是一件事：怎么用一个统一的数学工具，描述机器人各个部件之间的位置和朝向关系。
- 刚体变换 = 平移 + 旋转
- 齐次坐标把变换变成 4x4 矩阵乘法
- 旋转矩阵直观但冗余，四元数轻量但抽象
- TF 树是 ROS 里管理这些变换的工程工具
这些概念会贯穿整个课程。后面讲机械臂运动学、激光雷达建图、多传感器融合，全都离不开坐标系变换。你现在花时间把基础打牢，后面会省很多事。
嗯，我记得有个学员跟我说，他之前做项目时一直搞不懂为什么地图会“漂移”。后来发现是 TF 树里一个坐标系的父子关系搞反了。这种问题，排查起来真的很头疼。所以，坐标系变换这件事，值得你认真对待。