第4章动态TF变换

动态TF变换，说白了就是坐标系之间的关系会随时间变化。比如机械臂的关节在转动，移动机器人在前进，激光雷达装在云台上左右扫……这些场景下，坐标系之间的相对位姿每一帧都在变。你想想看，如果还用静态TF那一套，写死一个固定变换，那机器人肯定要撞墙。

4.1 动态变换的概念

动态变换的核心是“实时发布”。TF树里有一个父坐标系和一个子坐标系，子坐标系相对于父坐标系的位置和姿态随着时间变化。发布器需要以较高的频率（通常10~100Hz）不断广播最新的变换数据。

我个人习惯把动态变换分成两类：一类是连续运动，比如轮子转动、关节旋转；另一类是离散跳变，比如相机被重新安装后更新外参。但后者其实更接近静态变换的重新发布，真正意义上的动态变换，指的是连续平滑的运动轨迹。

            关键区别： 静态变换只发布一次（或极少更新），动态变换需要持续发布，且时间戳必须精确对应数据采集时刻。
        

4.2 动态变换发布器编写

在ROS1里，我们用 tf::TransformBroadcaster；ROS2里用 tf2_ros::TransformBroadcaster。写法其实差不多，核心就三步：构造变换、填时间戳、发布。

我直接给你看一个ROS1的典型例子——模拟一个机械臂关节转动。嗯，这里要注意：时间戳一定要用数据采集的时刻，不要用 ros::Time::now() 随便填，否则TF的缓冲机制会出乱子。

// ROS1 动态TF发布器示例 (C++)
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "dynamic_tf_publisher");
    ros::NodeHandle nh;
    tf::TransformBroadcaster br;
    ros::Rate rate(50);  // 50Hz

    double angle = 0.0;
    while (ros::ok()) {
        angle += 0.02;  // 模拟关节转动
        tf::Transform transform;
        transform.setOrigin(tf::Vector3(0.3, 0.0, 0.2));
        tf::Quaternion q;
        q.setRPY(0, 0, angle);  // 绕Z轴旋转
        transform.setRotation(q);

        // 发布 dynamic_link 相对于 base_link 的变换
        br.sendTransform(
            tf::StampedTransform(transform, ros::Time::now(),
                                 "base_link", "dynamic_link"));
        rate.sleep();
    }
    return 0;
}

💡 避坑指南： 我曾经在项目里直接用 ros::Time(0) 发布动态变换，结果TF监听器一直报“extrapolation error”。后来才意识到，时间戳为0表示“最新可用”，但动态变换必须带真实时间戳，否则TF的插值逻辑会崩溃。

ROS2的写法稍微有点区别，但思路一样。下面这个Python片段展示了如何在ROS2里发布动态变换：

# ROS2 动态TF发布器 (Python)
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped

class DynamicTFPublisher(Node):
    def __init__(self):
        super().__init__('dynamic_tf_pub')
        self.br = TransformBroadcaster(self)
        self.timer = self.create_timer(0.02, self.publish_tf)  # 50Hz
        self.angle = 0.0

    def publish_tf(self):
        self.angle += 0.02
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'base_link'
        t.child_frame_id = 'dynamic_link'
        t.transform.translation.x = 0.3
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.2
        # 绕Z轴旋转
        t.transform.rotation.z = sin(self.angle / 2)
        t.transform.rotation.w = cos(self.angle / 2)
        self.br.sendTransform(t)

4.3 动态变换在机械臂运动中的应用

机械臂是动态TF最典型的战场。每个关节都有一个坐标系，关节角度一变，末端执行器（tool0）相对于基座（base_link）的变换就变了。你想想看，如果不用动态TF，机械臂的逆运动学根本算不准。

我记得有一次调试六轴机械臂，末端总是差几毫米抓不到工件。查了半天，发现是某个关节的TF发布频率太低（只有5Hz），导致运动规划器用的变换是“过时”的。后来我把发布频率提到100Hz，问题立刻解决。

            机械臂动态TF的关键点：
            每个运动关节都需要一个独立的动态TF发布器（或合并到一个节点里）。
发布频率至少是控制频率的2倍以上（比如控制频率50Hz，TF发布100Hz）。
时间戳必须与关节状态读取时刻对齐，不要用定时器随意生成。

        

下面这张表总结了动态TF在机械臂中常见的坐标系对：

父坐标系	子坐标系	变换含义	典型更新频率
base_link	link1	关节1旋转	100 Hz
link1	link2	关节2旋转	100 Hz
link2	link3	关节3旋转	100 Hz
link6	tool0	末端工具偏移（固定或动态）	10~100 Hz

⚠️ 注意： 不要把所有关节的TF放在一个超大循环里用同一个时间戳发布。每个关节的变换应该对应各自关节的读取时刻。我曾经见过有人图省事，所有关节共用一个时间戳，结果在高速运动时出现明显的轨迹抖动。

最后说一个我自己的习惯：在机械臂的TF树里，我通常会在 tool0 下面再挂一个 gripper_frame，用来表示夹爪中心。这个变换可能是静态的（如果夹爪固定），也可能是动态的（如果夹爪有开合运动）。动态变换让整个TF树变得非常灵活，你几乎可以表达任何运动学链。

第4章 动态TF变换

4.1 动态变换的概念

4.2 动态变换发布器编写

4.3 动态变换在机械臂运动中的应用

第4章动态TF变换