坐标系与变换：世界坐标系、机器人坐标系、TF树，坐标变换的常见坑
坐标系这东西，说白了就是给机器人一个「我在哪」的答案。很多新手一上来就怼着算法调参，结果机器人满屋子乱窜——最后发现是坐标系搞反了。嗯，我当年也干过这种事。
咱们先理清楚三个最核心的概念：世界坐标系、机器人坐标系、还有那个让人又爱又恨的TF树。
世界坐标系 vs 机器人坐标系
世界坐标系，就是地图的参考系。你可以把它想象成房间的墙角——原点固定，方向固定。不管机器人怎么转，世界坐标系纹丝不动。
机器人坐标系，也叫本体坐标系。原点在机器人中心，x轴朝前，y轴朝左（或者朝右，看你的约定）。机器人一转身，这个坐标系就跟着转。
这两个坐标系之间的转换，就是机器人定位的核心。我见过最典型的错误，是把传感器数据直接当世界坐标用。比如激光雷达扫到一个点，距离2米，角度30度——这是机器人坐标系下的值。你要是不做变换就扔给全局路径规划，那机器人肯定撞墙。
核心原则：传感器数据永远先转换到机器人坐标系，再通过TF树转换到世界坐标系。跳过任何一步，都会出问题。
TF树：坐标变换的骨架
TF树是ROS里管理坐标变换的机制。它本质上是一棵有向树，每个节点代表一个坐标系，每条边代表一个变换关系。
举个例子，一个差速轮机器人，典型的TF树长这样：
map → odom → base_footprint → base_link
├── laser_frame
├── camera_frame
└── imu_frame每个箭头都代表一个变换：位置偏移 + 旋转四元数。TF树会随时间更新，所以你能查到任意时刻任意两个坐标系之间的变换。
我个人习惯，在写任何导航代码之前，先跑一遍 tf2_monitor 看看树结构对不对。这一步能省掉后面80%的调试时间。
小技巧：用 tf2_echo 实时查看两个坐标系之间的变换。比如 rosrun tf2_ros tf2_echo map base_link，能直接看到位置和姿态的数值变化。
坐标变换的常见坑
我在项目里踩过的坑，列出来给你参考。每一个都是真金白银换来的教训。
坑1：四元数顺序搞反
ROS里四元数的顺序是 (x, y, z, w)，但有些传感器或者第三方库用的是 (w, x, y, z)。你想想看，一旦搞混，旋转方向就全错了。
我曾经调试一个AGV，明明指令是直行，它却原地转圈。查了两天，最后发现是IMU驱动里四元数顺序写反了。从那以后，我每次拿到新传感器，第一件事就是打印四元数，手动验证一下。
# 正确的ROS四元数顺序
from geometry_msgs.msg import Quaternion
q = Quaternion(x=0.0, y=0.0, z=0.707, w=0.707) # 绕z轴旋转90度坑2：时间戳不同步
TF变换是有时间戳的。如果你拿一个10毫秒前的激光数据，去查当前时刻的TF变换，结果就是错的。为什么？因为机器人在这10毫秒里可能已经移动了。
我记得有一次，机器人定位突然跳变，排查下来发现是激光雷达的驱动没给时间戳，TF默认用了当前时间。激光数据是旧的，TF是新的，两者对不上。解决方案很简单：给传感器数据打上正确的时间戳，或者用 tf2::BufferCore::lookupTransform() 时指定时间。
警告：不要用 lookupTransform("map", "base_link", ros::Time(0)) 这种写法。ros::Time(0) 取的是最新变换，但你的传感器数据可能是旧的。一定要用传感器数据的时间戳去查TF。
坑3：父坐标系和子坐标系搞混
TF树是有方向的。从父坐标系到子坐标系的变换，和反过来是不一样的。很多人写代码时，把 transformStamped.transform.translation 的符号搞反，结果位置直接翻倍或者归零。
我建议你记住这个口诀：「从父到子，平移旋转；从子到父，取逆变换」。ROS的 tf2 库提供了 transform.inverse() 方法，别自己手动取反。
坑4：静态变换发布频率过高
有些坐标系之间的变换是固定的，比如激光雷达和机器人中心。很多人用 static_transform_publisher 每10毫秒发一次。其实没必要，静态变换发布一次就够了，TF树会自动缓存。
我见过一个项目，CPU占用率莫名其妙高，查了半天发现是静态TF发布频率设成了100Hz。改成1Hz之后，CPU占用直接降了15%。
坐标系变换的数学基础
虽然ROS帮你封装了大部分变换逻辑，但理解背后的数学还是有必要的。说白了就是两个操作：平移和旋转。
| 操作 | 数学表达 | ROS实现 |
|---|---|---|
| 平移 | p' = p + t | transform.translation |
| 旋转 | p' = R * p | transform.rotation (四元数) |
| 刚体变换 | p' = R * p + t | tf2::Transform |
你不需要手写这些矩阵，但要知道：变换是可逆的，也是可组合的。从map到laser_frame的变换，等于从map到base_link，再乘上从base_link到laser_frame。
SVG：坐标系与TF树结构图
下面这张图展示了典型的导航系统坐标系关系。你可以看到世界坐标系、机器人坐标系、传感器坐标系之间的层级结构。
避坑指南：我总结的坐标系检查清单
每次搭建新机器人，我都会按这个清单过一遍。你直接拿去用：
- 检查TF树完整性：运行
rosrun tf2_tools view_frames.py，看所有坐标系是否连接成树，有没有孤立节点。 - 验证静态变换：用
tf2_echo查看固定坐标系之间的变换，确认数值和预期一致。 - 检查时间戳：所有传感器消息的时间戳必须和TF查询的时间戳匹配。
- 测试简单运动：让机器人前进1米，看odom坐标系下的位移是不是1米。如果不是，检查轮距、编码器分辨率或者TF变换。
- 验证旋转方向：让机器人原地转90度，看map坐标系下的姿态变化是否正确。我曾经在这里翻过车——四元数符号反了，转了90度显示-90度。
血的教训：有一次我在现场调试，机器人定位一直漂移。查了三天，最后发现是base_footprint到base_link的z轴偏移设成了0.1米，但实际机器人底盘高度是0.05米。就这么5厘米的误差，导致整个定位系统不稳定。所以，所有变换参数必须实测，不要靠猜。
坐标系和变换，看起来是基础，但恰恰是这些基础最容易出问题。你想想看，一个导航系统几十个坐标系，任何一个变换错了，后面的定位、规划、控制全都会跟着错。所以，花时间把TF树理清楚，比急着调算法参数要划算得多。