4、坐标变换与TF树：TF（Transform）库原理、静态与动态坐标变换、TF树查看与调试、使用tf2_ros进行坐标广播与监听

做SLAM的人，天天跟坐标打交道。你想想看，激光雷达扫到的点，是在雷达坐标系下的；IMU测到的姿态，是在IMU坐标系下的；机器人底盘的运动，又是在odom坐标系下的。这些坐标系之间怎么换算？谁是谁的父级？谁先动谁后动？

嗯，这就是TF要干的事。TF全称Transform，说白了就是帮你维护一棵坐标系之间的变换树。我刚开始接触ROS时，觉得TF就是个黑盒子，直到有一次在项目中死活对不准地图和激光数据，折腾了两天才发现是TF树里少了一个静态变换。从那以后，我再也不敢轻视TF了。

4.1 TF库原理：坐标系变换的本质

TF库解决的核心问题很简单：已知A坐标系到B坐标系的变换，以及B到C的变换，求A到C的变换。 这就是坐标变换的链式法则。

数学上，每个变换都是一个4x4的齐次变换矩阵，包含旋转和平移。TF库帮你把这些矩阵串起来，形成一棵树。注意，是树，不是图——每个坐标系只能有一个父坐标系，但可以有多个子坐标系。为什么？因为如果有环，变换关系就矛盾了。

我在项目中遇到过最典型的场景：机器人有base_link、laser、camera、imu四个坐标系。base_link是根，laser和camera挂在base_link下，imu挂在base_link下。这样，只要知道laser到base_link的变换，以及base_link到map的变换，就能算出laser到map的变换。TF自动帮你做乘法。

4.2 静态坐标变换 vs 动态坐标变换

TF里有两类变换：静态和动态。区别很简单——静态变换不变，动态变换会变。

类型	特点	典型场景	发布频率
静态变换	固定不变	激光雷达到机器人中心	一次发布，或低频发布
动态变换	随时间变化	机器人位姿、关节角度	高频发布（10-100Hz）

静态变换，说白了就是那些物理安装位置固定的传感器。比如激光雷达装在机器人正前方10cm处，这个变换装好之后就不会变了。我建议用static_transform_publisher或者写一个节点只发布一次。

动态变换呢？机器人每时每刻都在移动，odom到base_link的变换就在不断变化。这个必须高频发布，否则导航模块拿到的位姿就是过时的。

4.3 TF树查看与调试

TF树长什么样？别猜，直接看。ROS提供了几个非常好用的工具。

第一个：tf_monitor

这个命令会打印所有坐标系之间的变换关系，以及发布频率、延迟等信息。我调试时第一件事就是跑这个，看看有没有坐标系断链。

rosrun tf tf_monitor

第二个：tf_echo

想知道任意两个坐标系之间的实时变换？用这个。比如我想知道激光雷达坐标系下的点(1,0,0)在map坐标系下的坐标：

rosrun tf tf_echo laser map

第三个：view_frames

这个最直观，生成一张PDF格式的TF树图。我每次调试完都会跑一下，看看树结构对不对。

rosrun tf view_frames
evince frames.pdf

第四个：Rviz里的TF面板

在Rviz中直接添加TF显示，就能看到所有坐标系和它们之间的连线。哪个坐标系没连上，一眼就能看出来。我个人习惯在Rviz里把坐标系显示调成不同颜色，这样更清晰。

4.4 使用tf2_ros进行坐标广播与监听

好了，理论讲完了，上代码。ROS里现在主流用的是tf2_ros，比老版tf更高效、更易用。

4.4.1 广播坐标变换

假设你要广播一个从base_link到laser的静态变换。激光雷达装在机器人前方0.1米处，没有旋转。

import rospy
import tf2_ros
import geometry_msgs.msg

rospy.init_node('laser_tf_broadcaster')
br = tf2_ros.TransformBroadcaster()

t = geometry_msgs.msg.TransformStamped()
t.header.frame_id = "base_link"
t.child_frame_id = "laser"
t.transform.translation.x = 0.1
t.transform.translation.y = 0.0
t.transform.translation.z = 0.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0

rate = rospy.Rate(10)
while not rospy.is_shutdown():
    t.header.stamp = rospy.Time.now()
    br.sendTransform(t)
    rate.sleep()

注意，即使是静态变换，我也建议在循环里发布，只不过频率可以低一些。为什么？因为有些订阅者可能启动得晚，你只发布一次它可能收不到。

4.4.2 监听坐标变换

监听更简单。比如你想知道laser坐标系下的点，在map坐标系下是什么位置：

import rospy
import tf2_ros
import tf2_geometry_msgs

rospy.init_node('tf_listener_example')
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)

rate = rospy.Rate(10)
while not rospy.is_shutdown():
    try:
        # 获取laser到map的变换
        trans = tf_buffer.lookup_transform('map', 'laser', rospy.Time(0))
        # 变换一个点
        point_laser = geometry_msgs.msg.PointStamped()
        point_laser.header.frame_id = 'laser'
        point_laser.point.x = 1.0
        point_laser.point.y = 0.0
        point_laser.point.z = 0.0
        
        point_map = tf2_geometry_msgs.do_transform_point(point_laser, trans)
        rospy.loginfo("Point in map: %f, %f, %f", 
                      point_map.point.x, point_map.point.y, point_map.point.z)
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, 
            tf2_ros.ExtrapolationException):
        rospy.logwarn("TF lookup failed")
        continue
    rate.sleep()

这里有个关键点：rospy.Time(0)表示获取最新可用的变换。如果你需要获取历史某个时刻的变换，可以传具体时间戳。但要注意，TF buffer默认只缓存10秒的数据。

4.5 TF树的核心逻辑图

下面这张图展示了TF树的核心结构。map是根，odom是中间节点，base_link是机器人本体，下面挂着各种传感器。每个箭头代表一个变换，箭头方向从父坐标系指向子坐标系。

这张图里，map到odom是定位模块发布的变换，odom到base_link是里程计发布的变换，base_link到各个传感器是静态变换。整个树结构清晰，没有环，每个节点只有一个父节点。

4.6 实战建议与避坑指南

• 命名规范：坐标系名称统一用小写+下划线，比如base_link、laser_front。别用大写，别用空格，否则后面调试会疯。
• 时间戳同步：广播变换时，时间戳一定要填对。如果填了0，TF会认为这是过时的数据。我习惯用rospy.Time.now()。
• 缓存大小：默认TF buffer缓存10秒。如果你的机器人跑得慢，或者变换更新频率低，可以调大缓存时间。
• 不要用tf_old：新项目一律用tf2_ros。老版tf已经过时了，API也不友好。

可可精品资料库