📚 课程目录第4章坐标变换与TF2

坐标变换与TF2：理解机器人坐标系、TF2静态与动态变换、在算法中的应用

核心原理：机器人为什么需要“坐标变换”

机器人的感知、规划与控制都依赖坐标系。激光雷达测得的是雷达坐标系下的点云，摄像头输出的是相机坐标系下的像素，底盘里程计给出的是里程计坐标系下的位姿，而机械臂末端执行器又处于基座坐标系下。要让这些数据在同一个空间框架下对齐，就必须建立坐标系之间的变换关系。

在ROS2中，TF2（Transform Library 2）负责维护一棵坐标变换树。每个坐标系称为一个frame，任意两个frame之间只要有路径相连，就可以通过树中累积的变换矩阵，把一个坐标系下的点或位姿转换到另一个坐标系下。变换的本质是平移向量 + 旋转四元数，即一个6自由度的刚体变换。

关键认知：TF2不存储“物体在哪”，而是存储“坐标系A相对于坐标系B的位姿”。所有数据都挂靠在某个frame下，算法层只负责消费变换关系，不关心传感器物理安装位置。

方法流程：静态与动态变换的发布与使用

TF2中变换分为两类：静态变换（fixed frame之间关系不变）和动态变换（随时间变化）。静态变换通常描述传感器与机器人基座的相对位姿，动态变换描述机器人相对世界/里程计的运动。

1. 静态变换发布

静态变换用tf2_ros::StaticTransformBroadcaster发布，频率只需一次。在ROS2中也可直接用命令行工具发布：

ros2 run tf2_ros static_transform_publisher \
  --x 0.2 --y 0.0 --z 0.4 \
  --qx 0 --qy 0 --qz 0 --qw 1 \
  --frame-id base_link --child-frame-id laser

上述命令表示激光雷达安装在机器人基座前方0.2米、上方0.4米处，无旋转。静态变换发布后，TF树中base_link与laser之间就有了固定连接。

2. 动态变换发布

动态变换通常由里程计节点或定位模块发布。核心代码模式如下：

// 在循环中创建变换消息
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "odom";
t.child_frame_id = "base_link";
t.transform.translation.x = odom_x;
t.transform.translation.y = odom_y;
t.transform.rotation = quat_from_yaw(yaw);
broadcaster_->sendTransform(t);

动态变换的时间戳必须与传感器数据时间同步，否则TF2会因时间不匹配而拒绝查询。

3. 监听与查询变换

算法节点通常使用tf2_ros::Buffer和TransformListener来获取变换：

tf2_ros::Buffer tf_buffer;
tf2_ros::TransformListener tf_listener(tf_buffer);

// 等待变换可用
auto transform = tf_buffer.lookup_transform(
  "base_link", "laser", rclcpp::Time(0));
// 将激光点转换到base_link坐标系
geometry_msgs::msg::PointStamped pt_in_laser, pt_in_base;
tf2::doTransform(pt_in_laser, pt_in_base, transform);

  推荐流程：先启动静态变换发布器（或使用URDF自动生成），再启动动态变换源（里程计/定位），最后启动算法节点。算法节点中统一通过Buffer查询变换，不要自行维护坐标关系。

案例/场景：激光雷达数据用于代价地图

假设机器人在室内导航，激光雷达安装在底盘中心前方0.3米、高度0.35米处。代价地图节点需要将激光点云转换到base_link坐标系下进行障碍物标记。

坐标系	作用	变换类型
map	全局地图固定原点	静态（由AMCL或SLAM发布）
odom	里程计累积位姿	动态（由轮式里程计发布）
base_link	机器人本体中心	动态（由里程计发布）
laser	激光雷达安装位置	静态（由URDF或static_transform_publisher发布）

实际处理时，代价地图节点订阅/scan话题，消息头中frame_id = "laser"。节点调用tf_buffer.transform(point, "base_link")将每个激光点转换到机器人基座坐标系。若激光雷达安装位置有偏差而未配置静态变换，则障碍物会出现系统性偏移——例如墙面向左偏移0.3米，导致路径规划撞墙。

在机械臂抓取场景中，视觉识别到物体在camera坐标系下的坐标后，需要查询camera → base_link → gripper的完整变换链，才能控制机械臂末端移动到物体位置。此时若TF树断裂（例如缺少base_link到gripper的静态变换），lookup_transform会抛出异常，程序必须做容错处理。

误区与复盘：TF2使用中的常见问题

误区1：所有变换都用动态发布。传感器安装位置是固定的，用动态发布不仅浪费资源，还会引入时间戳抖动。正确做法是静态发布一次或通过URDF描述。
误区2：忽略时间戳同步。动态变换的时间戳必须与传感器数据时间戳匹配。使用Time(0)查询时，TF2会返回最新可用变换，但若数据延迟过大，仍可能得到错误结果。
误区3：TF树断裂不处理。当lookup_transform抛出异常时，很多初学者直接忽略或退出。正确做法是等待几毫秒重试，或检查frame_id拼写是否正确。
误区4：在回调函数中做耗时变换查询。TF2的Buffer内部有锁，频繁查询会阻塞其他线程。建议在回调中只做轻量查询，或使用async方式。

复盘教训：一次导航调试中，机器人定位漂移严重。排查发现odom→base_link的变换由两个节点同时发布，一个来自轮式里程计，一个来自IMU融合节点，两者频率不同导致TF树抖动。最终只保留一个权威发布源，问题解决。TF2的单一父坐标系原则必须严格遵守——每个frame只能有一个父frame。

在算法开发中，建议使用tf2_echo工具快速验证变换关系：

ros2 run tf2_ros tf2_echo base_link laser
# 输出示例:
# At time 123.456
# - Translation: [0.300, 0.000, 0.350]
# - Rotation: in Quaternion [0, 0, 0, 1]

确认输出与预期一致后，再启动上层算法。这能避免大量因坐标关系错误导致的排查时间浪费。