第四节：坐标系与TF变换

自动驾驶里，坐标系这事儿，说大不大，说小不小。但你要是搞错了，车可能直接往墙上怼。

我刚开始做系统集成那会儿，就吃过这个亏。激光雷达的点云和摄像头图像怎么都对不上，折腾了两天，最后发现是TF树里一个旋转方向写反了。嗯，从那以后，我对坐标系就特别敏感。

4.1 车体、激光雷达、摄像头的坐标系定义

先说说最常见的几个坐标系。你想想看，一辆车上装了那么多传感器，每个传感器都有自己的“小世界”。

• 车体坐标系（base_link / base_footprint）：一般以车辆后轴中心为原点，x轴朝前，y轴朝左，z轴朝上。这是整车的“锚点”。
• 激光雷达坐标系（lidar_link）：原点在雷达的物理中心。x轴通常指向雷达的正前方，z轴朝上。注意，有些雷达的y轴方向可能跟车体相反。
• 摄像头坐标系（camera_link / camera_optical_frame）：这个最容易搞混。摄像头坐标系里，z轴是沿着光轴朝前的，x轴朝右，y轴朝下。对，你没看错，y轴是朝下的。这是为了跟图像坐标系（u轴向右，v轴向下）对齐。

我个人习惯，在定义这些坐标系时，会先画一张草图。把每个传感器的位置和朝向标清楚，再写进URDF文件里。别偷懒，这一步省了，后面调试能让你怀疑人生。

4.2 TF树调试：从入门到放弃（不是）

TF树说白了，就是坐标系之间的“亲戚关系图”。每个坐标系都知道自己相对于父坐标系的位姿。

调试TF树，我常用的工具就两个：tf2_monitor 和 tf2_echo。

4.2.1 用 tf2_monitor 看整体结构

这个命令会打印出TF树里所有坐标系之间的变换关系，以及发布频率。如果某个变换断了，或者频率不对，一眼就能看出来。

ros2 run tf2_tools tf2_monitor

输出大概长这样：

Transform tree:
/base_link -> /lidar_link: 100.0 Hz
/base_link -> /camera_link: 50.0 Hz
/camera_link -> /camera_optical_frame: 100.0 Hz

如果某个变换的发布频率是0，那说明发布节点挂了，或者没启动。

4.2.2 用 tf2_echo 查具体变换

当你怀疑某个坐标系之间的变换不对时，就用这个命令。它会实时打印出平移和旋转的四元数。

ros2 run tf2_ros tf2_echo base_link lidar_link

输出示例：

At time 1234567890.123
- Translation: [0.0, 0.0, 1.5]
- Rotation: in Quaternion [0.0, 0.0, 0.0, 1.0]

看到平移是 [0, 0, 1.5]，说明激光雷达安装在车体正上方1.5米处。旋转是单位四元数，说明朝向跟车体一致。如果旋转值不对，那就要检查URDF或者静态变换发布节点了。

4.3 坐标变换常见错误排查

这部分是我踩坑最多的。我总结了几类典型错误，你遇到了可以对照着查。

4.3.1 旋转方向搞反了

这是最常见的错误。比如摄像头坐标系里，y轴是朝下的。如果你在写变换时，把y轴写成了朝上，那图像就会上下颠倒。

排查方法：用 rviz2 可视化。把点云和图像同时显示出来，看它们是否对齐。如果点云投影到图像上，位置偏了，那大概率是旋转矩阵或者四元数写反了。

4.3.2 平移量写错了

传感器安装位置没量准，或者URDF里写错了。比如激光雷达实际装在车顶中央，但你在URDF里写成了装在车头。那融合出来的障碍物位置就会整体偏移。

排查方法：用卷尺量一下实际位置，再跟URDF里的值对比。别太相信记忆，我吃过这个亏。

4.3.3 TF树断链

某个坐标系找不到父坐标系，或者父坐标系没发布。比如你发布了 lidar_link 到 base_link 的变换，但 base_link 本身没有发布到 map 的变换，那 lidar_link 的数据就无法在全局地图中定位。

排查方法：运行 tf2_monitor，看是否有“No transform from ... to ...”的报错。如果有，说明断链了。

4.3.4 时间戳不同步

传感器数据的时间戳跟TF变换的时间戳对不上。比如摄像头图像是时间戳A，但TF变换是时间戳B，那变换就会失败。

排查方法：检查传感器驱动和TF发布节点的时间戳是否一致。我一般会在代码里打印出时间戳，手动对比一下。

4.4 知识体系与核心逻辑

下面这张图，是我自己总结的坐标系与TF变换的核心逻辑。你把它理解了，调试起来会顺手很多。

这张图的核心逻辑很简单：每个传感器都有自己的坐标系，通过静态TF变换到车体坐标系，车体再通过动态TF变换到全局坐标系。静态TF是固定的，比如雷达装在车顶哪个位置。动态TF是变化的，比如车在移动时，车体相对于地图的位置。

4.5 避坑指南

最后，分享几个我踩过的坑，希望能帮你省点时间。

• 我曾经在URDF里把摄像头坐标系的y轴写反了，结果图像投影到点云上，所有障碍物都偏了半米。排查了整整一天，最后发现是四元数里的符号写错了。
• 我曾经在回放rosbag时，忘了加 --clock 参数，结果TF树的时间戳全是0，所有变换都失败。嗯，从那以后，我每次回放都会检查时间戳。
• 我曾经在调试时，用 tf2_echo 查了半天，发现变换是对的，但数据就是不对。最后发现是传感器驱动发布的数据本身就有问题，跟TF树无关。所以，排查问题时，别只盯着TF树，也要看看数据源。