第4章 TF坐标变换与多机器人定位

第4章：TF坐标变换与多机器人定位

说到多机器人协同，绕不开的一个话题就是——坐标变换。

我刚开始接触ROS那会儿，觉得TF就是个“黑盒子”。反正发布个变换，订阅一下，位置就对了。直到我第一次做双机器人协作抓取，两个机器人明明在同一个地图里，却互相“看不见”对方抓取的目标点。查了半天，才发现是TF树没管理好。

嗯，从那以后，我再也不敢小看TF了。

4.1 TF2树形结构：为什么是“树”？

TF2的核心思想，就是用一棵树来管理所有坐标系之间的关系。

你想想看，一个机器人身上有底盘、激光雷达、摄像头、机械臂末端……每个部件都有自己的坐标系。如果这些坐标系之间没有清晰的父子关系，那定位就乱套了。

TF2树的特点：

有根节点：通常用 map 或 odom 作为根
无环结构：每个坐标系只有一个父节点
单向变换：从父节点到子节点，变换关系是确定的

举个例子，一个典型的单机器人TF树长这样：

map → odom → base_footprint → base_link
                              → laser_link
                              → camera_link
                              → gripper_link

每个箭头代表一个变换关系。你只要知道 base_link 相对于 odom 的位置，再知道 laser_link 相对于 base_link 的位置，就能算出激光雷达在地图中的坐标。

我个人习惯，在设计多机器人系统时，先把每个机器人的内部TF树画清楚。这一步省了，后面全是坑。

4.2 多机器人坐标系管理：别让树变成森林

多机器人场景下，TF树会变得复杂。每个机器人都有自己的内部坐标系，但它们又共享同一个全局坐标系（比如 map）。

我见过最典型的错误，就是两个机器人用了相同的坐标系名称。比如两个机器人都有 base_link，结果TF2不知道这个 base_link 到底属于谁。

避坑指南：

我曾经在一个项目中，两个机器人同时发布 base_link 到 odom 的变换。结果TF2直接报错，说检测到多个发布者。后来我改用命名空间前缀，比如 robot1/base_link、robot2/base_link，问题才解决。

多机器人坐标系管理的几个原则：

全局唯一命名：每个坐标系名称加上机器人ID前缀
共享根节点：所有机器人的TF树最终都连接到同一个 map 或 world
独立内部树：每个机器人内部的TF树自己维护，互不干扰

一个典型的多机器人TF树结构：

map
├── robot1/odom
│   └── robot1/base_footprint
│       ├── robot1/base_link
│       ├── robot1/laser_link
│       └── robot1/camera_link
└── robot2/odom
    └── robot2/base_footprint
        ├── robot2/base_link
        ├── robot2/laser_link
        └── robot2/gripper_link

你看，这样每个机器人的坐标系都带上了自己的名字，TF2就不会搞混了。

4.3 广播与监听TF：发布者和订阅者

TF2的工作机制，说白了就是广播和监听。

广播者负责发布坐标系之间的变换关系。监听者负责接收这些变换，并计算出任意两个坐标系之间的相对位置。

4.3.1 广播TF变换

在C++中，广播一个变换很简单：

#include <tf2_ros/transform_broadcaster.h>

tf2_ros::TransformBroadcaster broadcaster;
geometry_msgs::msg::TransformStamped transformStamped;

transformStamped.header.stamp = this->get_clock()->now();
transformStamped.header.frame_id = "robot1/odom";
transformStamped.child_frame_id = "robot1/base_footprint";
transformStamped.transform.translation.x = 1.0;
transformStamped.transform.translation.y = 0.5;
transformStamped.transform.rotation.w = 1.0;

broadcaster.sendTransform(transformStamped);

嗯，这里要注意：header.frame_id 是父坐标系，child_frame_id 是子坐标系。方向别搞反了，我刚开始就犯过这个错。

4.3.2 监听TF变换

监听变换，最常用的就是 tf2_ros::Buffer 和 tf2_ros::TransformListener：

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>

tf2_ros::Buffer tf_buffer(this->get_clock());
tf2_ros::TransformListener tf_listener(tf_buffer);

geometry_msgs::msg::TransformStamped transformStamped;
try {
    transformStamped = tf_buffer.lookupTransform(
        "robot2/base_link",
        "robot1/laser_link",
        tf2::TimePointZero);
} catch (tf2::TransformException &ex) {
    RCLCPP_WARN(this->get_logger(), "Could not transform: %s", ex.what());
}

这段代码的意思是：我想知道 robot1/laser_link 在 robot2/base_link 坐标系下的位置。TF2会自动沿着树查找路径，计算出变换结果。

小技巧：

我个人习惯在监听时加上超时处理。如果变换不可用，不要硬等，先跳过这一帧。否则程序容易卡死。

4.4 使用robot_state_publisher：自动发布关节变换

手动发布每个关节的变换太累了。尤其是机器人有多个关节时，比如机械臂有6个关节，你总不能一个一个写吧？

这时候 robot_state_publisher 就派上用场了。

它读取机器人的URDF模型，自动计算每个关节的变换，并发布到TF2。你只需要提供关节角度（joint states），剩下的它全包了。

启动方式很简单：

ros2 run robot_state_publisher robot_state_publisher --ros-args -p robot_description:="$(cat my_robot.urdf)"

或者直接在launch文件中配置：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[{'robot_description': open('my_robot.urdf').read()}]
        )
    ])

多机器人场景下，每个机器人需要启动一个独立的 robot_state_publisher 实例，并指定不同的命名空间：

Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    namespace='robot1',
    parameters=[{'robot_description': open('robot1.urdf').read()}]
)

Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    namespace='robot2',
    parameters=[{'robot_description': open('robot2.urdf').read()}]
)

这样，每个机器人的关节变换都会自动带上 robot1/ 或 robot2/ 前缀，完美避免命名冲突。

4.5 实战经验总结

说了这么多，我总结几条实战中容易踩的坑：

常见问题	原因	解决方案
TF树断开	某个坐标系没有发布变换	用 `tf2_tools` 查看树结构，检查缺失节点
变换时间戳不匹配	广播和监听的时间戳不一致	统一使用 `now()`，或启用TF2的时间插值
坐标系名称冲突	多机器人用了相同名称	加命名空间前缀，如 `robot1/base_link`
变换延迟过大	广播频率太低或网络延迟	提高广播频率，或使用 `static_transform_publisher` 发布静态变换

最后说一句：TF2其实不复杂，但一定要养成“先画树、再写代码”的习惯。我在项目里吃过亏，现在每做一个多机器人系统，第一件事就是画TF树结构图。

嗯，这一章就到这里。下一章我们聊聊多机器人之间的通信机制，那又是另一片天地了。

📚 课程目录