TF2 的原理与使用：把坐标、运动和观测时间一起算清楚

机器人软件与具身智能 · ROS 2 运行机制 · 第 11 篇。以八界头部与腕部相机的关系提出问题，用可手算的变换和 Humble Buffer 验证几何与时间语义。

图 1　八界与坐标变换的概念示意。基于八界 URDF 和作者提供的实拍参考进行外观渲染。

八界先用头部相机定位目标，再调整升降和机械臂，用腕部相机复核。目标可能没动，两个相机相对底盘的位姿却变了。若把旧图像中的点套用到新的相机位姿上，程序就会把机器人的运动算进目标位置。78

这种错误不一定产生异常。本文的教学例子中，观测时间为 11 s，查询最新变换得到 12 s，两个查询都成功；同一个相机坐标点却被放到相差 0.443508 m 的位置。要判断一次 TF 查询是否适合当前数据，需要同时明确变换方向、路径和时间。

1. 一条 TF 描述谁在谁里面

约定用 T_A_B 表示"把 B 系坐标变换到 A 系"的刚体变换，点使用列向量。R_A_B 是 B 系各轴在 A 系中的方向，t_A_B 是 B 系原点在 A 系中的位置：

p_A = R_A_B × p_B + t_A_B

          [ R_A_B  t_A_B ]
T_A_B  =  [   0      1   ]

三维旋转 R 为 3×3 矩阵，平移 t 为 3×1，齐次变换 T 为 4×4。给点补上最后一维 1，就能用矩阵乘法同时表达旋转和平移。

对于 TransformStamped，父系在 header.frame_id，子系在 child_frame_id；消息描述子系在父系中的位姿。lookup_transform(target, source, time) 返回把 source 中的点变到 target 所需的变换。因此查 ("odom", "camera_link", t) 得到 T_odom_camera(t)。12

1.1 组合顺序由坐标落在哪个系中决定

图 2 的相机固定在底盘正 x 方向 0.5 m，两者坐标轴一致。相机看见的点为 (1,0) m。先得到底盘坐标 p_base=(1.5,0)，再用底盘在 odom 中的姿态，得到 odom 坐标。

p_odom = T_odom_base × T_base_camera × p_camera

先作用的矩阵写在右边。可以通过相邻下标检查组合：右侧先输出 base，左侧正好以 base 为输入。交换顺序，会改变平移在哪个系中旋转，通常得到另一个几何问题。

图 2　相机点先经过安装变换，再经过底盘位姿。图示使用后文 t=11 s 的插值结果。

逆变换也可以直接推导。由 p_A=R p_B+t 移项，再利用旋转矩阵的逆为转置：

p_B = Rᵀ × (p_A − t)
T_B_A 的旋转 = Rᵀ
T_B_A 的平移 = −Rᵀ × t

所以把平移取负、保持旋转不变，通常无法得到逆变换。配套脚本在正向变换后查询反方向，再将结果点变回 (1,0)，用一个闭环检查方向与乘法顺序。

2. 树结构让计算有路径，也限定了责任

TF 按父子关系组织坐标系。对于稳定的模型，每个子系有明确的父系；查询向上寻找共同祖先，在两条支路上组合正向或逆向变换。共同祖先之上的边不属于这次相对查询所需的路径。2

八界主模型中，头部基座 berxel_upper_base_link 固定在 head_pitch_link，腕部基座 berxel_arm_base_link 固定在 arm6_link。从 base_footprint 到它们都先经过升降机构，随后头部走俯仰分支，腕部走六轴机械臂分支。79

"相机安装是固定的"说的是相机基座与它安装的连杆之间的边。相机相对底盘的整条路径仍包含活动关节。另一方面，腕部基座到抓取参考点 objects_link 在 arm6_link 处就遇到共同祖先，途经的安装边全是固定边，因此这项查询可以在升降或其他臂关节状态缺失时仍然成功。

如果用 L 表示共同的升降路径，H、W 表示升降连杆到头部、腕部的路径，同一时刻的相对变换为：

T_base_head  = L × H
T_base_wrist = L × W
T_head_wrist = (L × H)⁻¹ × (L × W) = H⁻¹ × W

共同项在相对变换中抵消。第 12 篇的八界公开模型实验只改变升降、保持其他关节不变：两路相机在底盘系都上升 0.10 m，头部到腕部的相对变换保持不变。这个结论来自机构关系；模型中的占位外参并未被当作实机标定结果。

底盘的外部参考又有不同职责。REP 105 区分连续的 odom 与可能随全局定位校正的 map。3 URDF 描述机器人内部机构，定位和里程计通常负责它与这些外部参考之间的边。明确每条边由谁发布，可以避免两个节点竞争同一个子系。中文移动机器人接入记录 4 可作为把传感器接到实际 TF 树的辅助阅读。

3. TF2 如何在两个采样时刻之间取一个值

TF2 为动态变换保存带时间戳的历史。本文先向 Buffer 放入同一条 odom → base_link 的两份样本：

t₀ = 10 s：平移 (1,0) m，偏航 0°
t₁ = 12 s：平移 (3,0) m，偏航 90°

查询 t=11 s 时，插值比例是 α=(t−t₀)/(t₁−t₀)=0.5。在父系一致的相邻样本之间，Humble TF2 对平移线性插值，对旋转采用四元数球面线性插值。5

t(α) = (1−α) × t₀_vector + α × t₁_vector

这里 t₀_vector、t₁_vector 表示两份平移向量，区别于前面的采样时间。四元数使用单位四元数；q 与 −q 代表相同姿态，因此插值要处理符号并选择短弧。在非退化的短弧情况下，其数学形式为：

θ = acos(q₀ · q₁)          必要时先对一个四元数取负，使点积非负
q(α) = sin((1−α)θ)/sinθ × q₀ + sin(αθ)/sinθ × q₁

θ 是四元数单位球面上的夹角，两姿态的旋转角差为 2θ。两端非常接近时可用等价的数值稳定处理；实际调用直接使用 TF2 的插值结果。不要把一般三维姿态的 roll、pitch、yaw 分别取平均作为替代，角度跨越和旋转耦合会改变结果。

本例只绕 z 轴，半程姿态为 45°，平移为 (2,0) m。相机安装偏移加上观测点之后是 (1.5,0) m，因此：

p_odom(11) = (2,0) + (1.5 cos45°, 1.5 sin45°)
           = (3.060660..., 1.060660...) m

这验证了实现采用的插值模型，不能证明真实机器人在两份采样之间严格匀速。采样间隔大、运动加速度高时，几何时间戳完全合法，插值仍可能偏离真实轨迹。TF 样本提供位姿历史，没有自动携带可用于任意预测的运动模型。

4. "最新"为什么必须是整条路径的共同时间

零时间 Time() 表示查询路径的最新共同时间。对目前只有一条动态边的路径，最新是 12 s。此时相机点变成 (3,1.5) m。它描述按 12 s 相机姿态解释这个坐标的结果；若输入来自 11 s 的图像，查询虽成功，配对的物理时刻已经改变。12

再加入 map → odom，在 9 s 和 11 s 各有一个平移 10 m 的样本。到 map 的整条路径同时受两条动态边限制。

图 3　两条动态边共同覆盖 [10,11] s，零时间查询返回共同时间 11 s。静态安装边不缩短这个区间。

对父子关系稳定、相邻历史可以插值的路径，令各动态边的可查询区间为 [aᵢ,bᵢ]。整条路径的共同区间满足：

A = max(aᵢ)，B = min(bᵢ)
A ≤ B 时，共同区间为 [A,B]，最新共同时间为 B
A > B 时，不存在共同时间

本例 A=max(9,10)=10、B=min(11,12)=11。于是底盘那条边也必须在 11 s 取值，即使它已经有 12 s 样本。正确的 map 坐标是 (13.06066,1.06066) m。

脚本还故意分别查询两段"各自最新"，再手工相乘：map → odom 用 11 s，底盘与相机用 12 s。得到 (13,1.5) m，与共同时间结果相差 0.443508 m。这组乘法在矩阵维度上完全合法，但两个因子没有描述同一时刻。

再把第一条动态边改成 [13,15] s，另一条保持 [10,12] s。单查每条最新都成功；整条查询没有交集。Humble 先得到候选最新时间 12 s，沿途取值时发现它早于 map 边的最早样本，最终抛出外推错误。25 因此，时间零也无法保证任意已知拓扑都能查通。

在八界模型上，只把升降更新到 12 s，而俯仰与臂关节最新仍为 11 s，两路相机到底盘的整链最新时间就仍停在 11 s。应用应读取返回变换的时间戳；如果只看到"lookup 成功"就继续动作，会漏掉这种整体陈旧状态。

5. 将旧观测表示在新时刻的坐标系中

如果问题本来就是"把 11 s 相机观察到的点，表示在 12 s 底盘系中"，两个时间应该都保留。lookup_transform_full() 提供源时间、目标时间和固定参考系：1

transform = buffer.lookup_transform_full(
    "base_link", stamp(12), "camera_link", stamp(11), "odom"
)

它对应下面的组合：

T_base(12)_camera(11) = T_base_odom(12) × T_odom_camera(11)

先在观测时刻把点放到 odom，得到 (3.06066,1.06066)；再减去 12 s 底盘原点 (3,0)，并旋转 −90°，结果为 (1.06066,−0.06066) m。源与目标时间分别用于各自路径，不要求两条路径都覆盖同一个查询时刻，但每次所需的历史必须存在。2

这只是把旧观测重新表达。若要把结果解释成物体在 12 s 的当前位置，还隐含了物体相对所选固定参考不动的假设。物体自行运动时，需要新观测或运动估计；选择 map 作参考而期间发生定位跳变时，也要考虑这种坐标校正的语义。3

八界 SDK 的 eco_captureImages() 文档明确区分了两类 tf_goal：头部 HEAD 以 map 为参考，手臂 ARM 以 base_footprint 为参考。8 使用返回值前，应先确认它已经在哪个坐标系、对应哪个时刻。已在 map 中的点不应再套一遍相机到 map 的变换；相机系点则需要保留观测戳，再按路径变换。识别输出是像素、相机系三维点还是已有位姿，决定了下一步需要的计算。

6. 时间误差怎样变成空间误差

对一个距离相机约 r 的静止目标，短时间内错误使用相差 Δt 的相机位姿，会同时引入平移与旋转误差。设相机平移速度量级为 v，角速度量级为 ω，小角度条件下可作一阶估算：

平移项 ≈ v × |Δt|
旋转项 ≈ r × ω × |Δt|
误差上界的量级 ≈ (v + rω) × |Δt|

其中 r 用米、v 用米/秒、ω 用弧度/秒。两项方向可能相消或叠加；上式是忽略高阶项、加速度和其他误差后的预算，不是设备测量精度公式。

取教学参数 v=0.2 m/s、ω=0.5 rad/s、r=1 m、Δt=50 ms，平移项约 1 cm，旋转项约 2.5 cm，合起来可达 3.5 cm 量级。目标越远，相同角度误差引起的位置偏差越大。这给第 09 篇的 RGBD 时间差与帧龄预算提供了几何依据。

坐标轴约定同样重要。REP 103 的机体系通常是 x 前、y 左、z 上，相机 optical 系则是 z 前、x 右、y 下。10 八界主 Xacro 中的两个 Berxel 基座不能仅凭名称当作光学帧；应读取图像 header.frame_id，核实驱动或描述文件如何连接到基座，模型外参也需使用实际标定。79

7. 外推、等待和缓存分别能解决什么

LookupException 指向未知帧名，ConnectivityException 指向已知帧之间缺少连通路径，ExtrapolationException 指向所需动态边缺少对应时刻的样本。25 调试先保留源帧、目标帧、查询戳和报错边，再比较最早与最新历史。

向未来外推时，等待新的 TF 可能使查询成功；但如果 TF 监听器与查询共享一个被同步等待占满的线程，数据无法进入 Buffer，等待本身也没有帮助。向过去外推时，延长 timeout 通常不会重新创造已被清理的历史；应检查观测处理延迟、回放方式与缓存长度。所有时间戳还必须处于一致的 ROS 时间配置中。1

本次版本的一个具体缓存边界。cache_time=5 s，先放 10 s 和 12 s 样本。插入 18 s 后仍可查 11 s，再插入 19 s 后失败。geometry2 0.25.20 的插入函数先按插入前的最新样本时间清理，再写新样本：第一次按 12−5=7 清理，第二次按 18−5=13 清理。5

图 4　这是 geometry2 0.25.20 实测的插入顺序。偶然还能查询到更旧历史，不应当作超出配置窗口的保留承诺。

如果观测从采集到查询经历 L 秒，还存在抖动 J，缓存配置至少需要围绕 L+J 及采样边界规划，并由实际历史覆盖验证。加大缓存增加内存，只能帮助过去时间的查询，无法修复未知帧、断开的树或两个时钟域之间的巨大偏差。

8. 运行与复算

配套 examples/11_tf2_history.py 直接使用真实 TF2 Buffer，验证插值、逆变换、两时刻查询、混用最新值、空时间交集与缓存清理。合成样本让每个结果都可以手算。6

# 已加载 ROS 2 Humble 环境，在本篇目录运行
python3 examples/11_tf2_history.py

验证使用 tf2_ros_py 0.25.20。关键输出如下，其余输出列出各类预期异常：

{
  "point_in_odom_at_observation": [3.06066, 1.06066],
  "point_in_odom_using_latest": [3.0, 1.5],
  "point_in_base_at_12_from_camera_at_11": [1.06066, -0.06066],
  "latest_common_stamp_map_camera": 11,
  "point_in_map_at_common_time": [13.06066, 1.06066],
  "point_in_map_mixing_latest_edges": [13.0, 1.5],
  "mixed_time_error_m": 0.443508,
  "disjoint_history": {
    "odom_base_latest": 12,
    "map_odom_latest": 15,
    "map_camera_latest_error": "ExtrapolationException"
  },
  "history_still_available_after_insert_at_18": true
}

在八界上接入一份观测，可以依次核对：输入是什么坐标、对应哪个采集时刻、路径经过哪些活动关节、返回时间是否满足业务要求。随后再检查模型的轴向与标定。这样，"能查到 TF"才会成为几何正确性检查的一部分。欢迎关注科沃斯八界 AtomGit，相关代码将逐步释放，实际开放内容与使用说明以仓库为准。67

参考链接

1. geometry2 0.25.20 的 Python Buffer 查询、等待与双时刻接口。tf2_ros/buffer.py
2. geometry2 0.25.20 的 TF2 BufferCore 树遍历、共同时间和变换组合。buffer_core.cpp
3. ROS 坐标系职责约定，查阅 map、odom 与 base_link 的关系。REP 105
4. Gitee 上的 Humble 移动机器人接入记录，查阅作者如何接入雷达并组织 TF 链。willianfu/ros2_humble_main
5. 与实验版本相同的插值、外推判断和缓存清理实现。geometry2 0.25.20 · cache.cpp
6. 本专栏示例代码仓库入口。科沃斯八界示例仓库
7. 八界描述包，查阅两路相机基座、升降分支、抓取参考点与光学帧的接入说明。本文模型实验使用 7be6a0a。ecovacs-bajie-urdf
8. 八界 Python SDK 文档，查阅 §3.3 抓放流程与 §5.3.1 两类相机返回位姿的参考系。bajie_sdk Python 文档
9. 八界整机 Xacro，查阅 lift_joint、head_rgbd_joint、gripper_rgbd_joint 和 object_joint 的父子关系。bajie_robot.urdf.xacro
10. 坐标轴与单位约定，包含机体系和相机 optical 系。REP 103