3. 坐标系与坐标变换：全局坐标系、局部坐标系、齐次变换矩阵、ROS中的TF树

各位同学，今天我们来聊聊坐标系与坐标变换。说实话，这可能是整个路径规划课程里最绕、也最容易出bug的部分。我当年刚入行时，就因为搞混了坐标系，让一台AGV在仓库里画了个完美的“8”字——嗯，不是规划好的路径，是失控了。从那以后，我对坐标系就格外敬畏。

你想想看，机器人要移动，总得知道“我在哪”、“要去哪”吧？坐标系就是用来回答这两个问题的。但问题来了——机器人的“眼睛”（激光雷达）看到的障碍物，和它的“脚”（轮子）要走的路径，用的不是同一个参考系。

举个例子：激光雷达装在车头，它检测到前方1米有障碍物。这个“前方1米”是相对于雷达自身的。但控制指令要发给轮子，轮子关心的是“相对于车体中心，障碍物在哪”。这就涉及坐标变换了。

  核心概念：坐标系就是给空间中的点一个“地址”。不同的坐标系，就是不同的“地址编码方式”。

我个人习惯把全局坐标系叫做“世界坐标系”，局部坐标系叫做“本体坐标系”。

全局坐标系（World Frame）：固定在地面上的一个参考系。比如地图的东北角是原点，x轴指向东，y轴指向北。所有静态障碍物的位置都用这个坐标系描述。
局部坐标系（Local Frame）：固定在机器人身上的参考系。原点在机器人中心，x轴指向车头方向。传感器数据、控制指令通常都在这个坐标系下。

我在项目中遇到过最典型的坑：把激光雷达的局部坐标数据直接当成全局坐标来用。结果就是——机器人明明看到左边有墙，却往左边撞了上去。为什么？因为雷达数据是相对于车身的，而地图是相对于世界的，两者没对齐。

避坑指南：我曾经在调试时发现，只要机器人一转弯，障碍物位置就全乱了。后来才意识到——局部坐标系会随着机器人旋转而旋转，而全局坐标系不会。所以，所有传感器数据在用于路径规划前，必须先变换到全局坐标系。

好了，既然要在不同坐标系之间来回切换，总得有个数学工具吧？齐次变换矩阵就是干这个的。

说白了，一个齐次变换矩阵 T 包含了两个信息：旋转和平移。它长这样：

T = | R   t |
    | 0   1 |

其中 R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。最后一行是 [0 0 0 1]，这是齐次坐标的固定格式。

为什么叫“齐次”？因为用 4x4 的矩阵，可以把旋转和平移统一成一次矩阵乘法。你想想看，如果不用齐次坐标，你得先旋转再平移，两步操作。用齐次矩阵，一步搞定。

举个例子：假设机器人在全局坐标系中的位置是 (x=2, y=3, θ=30°)。那么从局部坐标系到全局坐标系的变换矩阵就是：

T = | cos30°  -sin30°  2 |
    | sin30°   cos30°  3 |
    | 0        0        1 |

现在，激光雷达检测到局部坐标下的一点 (1, 0)，它在全局坐标下的位置就是：

| x_global |   | cos30°  -sin30°  2 |   | 1 |
| y_global | = | sin30°   cos30°  3 | * | 0 |
| 1        |   | 0        0        1 |   | 1 |

算出来是 (2.866, 3.5)。嗯，这就是那个障碍物在地图上的真实位置。

注意：齐次变换矩阵的乘法顺序很重要！T_global_to_local 和 T_local_to_global 互为逆矩阵。我见过太多人把顺序搞反，结果机器人越跑越偏。

在实际的机器人系统中，坐标系远不止两个。你可能有：

这么多坐标系，怎么管理它们之间的关系？ROS 给出了答案——TF树。

TF树本质上是一个有向无环图。每个节点代表一个坐标系，每条边代表一个变换关系。比如：

map → odom → base_link → laser
                    ↓
                 camera

这意味着：要知道激光雷达数据在地图上的位置，只需要沿着树从 laser 一路变换到 map 即可。

ROS 提供了 tf2 库，可以自动维护和查询这些变换。你只需要发布每个坐标系之间的变换关系，剩下的交给 TF 去算。

  我的经验：在写路径规划节点时，我习惯在回调函数开头先查一下 TF：transform = tf_buffer.lookup_transform("map", "base_link", ros::Time(0))。这样能确保所有数据都在同一个坐标系下处理，避免很多莫名其妙的bug。

下面这张图总结了本章的核心逻辑：

最后，分享几个我在实际项目中总结的经验：

统一时间戳：TF变换是有时间戳的。如果你拿1秒前的变换去变换当前的数据，结果会不准。我习惯用 ros::Time(0) 获取最新变换。
检查TF树是否完整：用 tf2_echo 工具可以查看任意两个坐标系之间的变换。如果报错“No transform available”，说明你的TF树没连上。
静态变换 vs 动态变换：激光雷达相对于车体的变换是固定的，用 static_transform_publisher 发布一次即可。而车体相对于地图的变换是动态的，需要实时更新。
不要手动计算变换：除非你在做底层算法，否则尽量用ROS的TF API。手动计算容易出错，而且代码可读性差。

小技巧：在调试时，可以用 rviz 的 TF 显示功能，直观地看到所有坐标系的位置和方向。如果某个坐标系飞到了天边，那肯定是变换矩阵算错了。

好了，坐标系与坐标变换就讲到这里。记住一句话：所有数据，先变换，再使用。这是避免路径规划bug的第一道防线。