URDF 与机器人状态发布：从关节采样推导整机位姿

机器人软件与具身智能 · ROS 2 运行机制 · 第 12 篇。用五连杆模型手算正向运动学，再在八界公开 URDF 上验证局部状态、整链时间与共同运动。

图 1　八界与机器人状态发布的概念示意。基于八界 URDF 和作者提供的实拍参考进行外观渲染。

八界模型已经显示出来，腕部相机基座到抓取参考点的 TF 也能查，为什么底盘到这两个点仍然不通？补齐关节后查询成功了，随后只更新升降，为什么升降连杆已经到 12 s，头部和腕部的整条路径仍停在 11 s？

这两个现象分别暴露了模型与状态、局部更新与全链时间的区别。URDF 描述每条关节边随关节量变化的几何关系，JointState 为某些关节提供某个时刻的数值，robot_state_publisher 计算并发布相应的局部 TF。最后由 TF2 按查询路径与时间组合整机位姿。1

要理解状态发布，先把这条链路写完整：

Xacro 展开 → URDF XML → 机构树与局部运动学
                                + 带采样戳的关节位置
                                → 局部 TF 边
                                → TF2 按时间组合路径

1. URDF 为每条边定义一个函数

一个 link 命名一个刚体及其参考坐标系，一个 joint 把父 link 与子 link 连接起来。对于本文的转动和平移关节，可以把局部关系写成：

T_parent_child(q) = T_origin × Q_axis(q)

T_origin 由 joint 的 origin xyz/rpy 给出，描述零位安装关系；Q_axis(q) 描述沿关节局部轴的运动。转动关节的 q 用弧度，移动关节用米。第 11 篇采用列向量，因此右侧的局部运动先作用，再换算到父系。24

保留一条升降、转动、工具安装的链，以及一条固定相机分支。它是为了手算建立的教学模型，不代表八界的尺寸。

图 2　固定相机分支启动后即可发布；工具路径还需要升降与转动两个关节的位置。

1.1 origin 和 axis 不能脱离坐标系阅读

教学模型的转动关节如下：

<joint name="yaw_joint" type="revolute">
  <parent link="lift_link"/>
  <child link="wrist_link"/>
  <origin xyz="0.2 0 0.1" rpy="0 0 1.5707963267948966"/>
  <axis xyz="0 0 1"/>
  <limit lower="-3.141592653589793" upper="3.141592653589793" effort="5" velocity="1"/>
</joint>

零位时，手腕原点已在升降台的 (0.20,0,0.10) m，坐标轴已有 π/2 偏航。再给 q=π/2，这个同轴例子的总偏航变成 π，手腕原点的位置保持不变。若把运动旋转放到安装变换左边，安装平移也被旋转，原点变成 (0,0.20,0.10) m，相当于让整套安装位置绕父系原点转动。

再看一个轴向反例：若移动关节的零位姿态绕父系 y 轴转 π/2，局部 axis="0 0 1" 在父系就沿 +x；增加 0.10 m 会沿父系 x 移动。直接往父系 z 坐标加关节值，就忽略了零位旋转。

Humble 的 kdl_parser 将 URDF 转成 KDL 时，会用零位旋转将局部轴转换到父系，并结合关节原点构造 KDL Joint 与 Segment。2 这是内部表示方式；阅读 KDL 中的轴向数值时，不能再把它当作原始 URDF 的局部轴重复转换。

link 内的 <visual><origin> 又属于另一层关系：它把显示网格放到 link 系里。调整视觉网格位置会改变外观，不会自动改写父子 link 的关节变换。因此，RViz 中"外壳看起来对齐"和 TF 坐标轴正确，需要分别检查。

1.2 模型给出几何映射，状态源负责给出有效的 q

关节 limit、碰撞和惯性可以供其他组件使用，但本篇的状态发布器主要依据输入位置计算局部变换。不能仅因为模型声明了角度限制，就认为输入消息已经被控制器校正或机械限位验证。编码器单位、零点偏置和方向错误，会直接变成 TF 几何错误。14

Xacro 允许复用尺寸、宏和标定参数，展开后仍是一份 URDF XML。中文示例 5 展示了宏展开与加载的基本流程。排查时应检查实际交给节点的展开结果，尤其是 include、参数和标定文件；编辑某个 Xacro 后，如果启动参数仍是旧 XML，当前节点不会自然使用新模型。

2. 固定边、活动边和完整姿态分别在哪里产生

robot_state_publisher 读取 robot_description，解析模型并把固定段与活动段存入不同集合。固定段按零运动量计算，通过 /tf_static 发布；模型 XML 通过 robot_description 话题提供。Humble 的这些发布通路使用 transient local 持久性，合适的晚加入读者可以取得仍被保留的数据。13

活动段由 JointState 驱动。消息中 name[i] 与 position[i] 对应同一关节，顺序可以由发布端决定，状态发布器按名字查找模型段。单条消息的关节状态共享 header.stamp；该时间应反映采样语义。4

只给 yaw_joint 时，发布的是 lift_link → wrist_link。固定工具边已经存在，但根到工具仍缺 base_link → lift_link 的动态边。

图 3　局部边可以独立产生；根到工具的查询需要整条路径在同一时刻都有值。

2.1 一条 JointState 并不自动刷新所有活动关节

在 Humble 3.0.3 的 callbackJointState() 中，通过筛选后构造的关节位置映射来自当前这一条消息 ；再按模型中的 mimic 关系补充可计算的从动量，传给 publishTransforms()。后者遍历这份映射，查找对应活动段，计算并发布这些父子边。1

因此，先收到"升降、转动"全量消息，再收到"仅升降"消息，后一次不会自动把此前记住的转动值改标为新时间重新发布。代码中的 last_publish_time_ 保存的是各关节用于发布筛选的时间记录，不是一份被持续合并、每次完整广播的关节配置向量。

这个行为有合理的时间含义：只测到了升降的新状态，并不能据此声称转动关节在新时刻也被测量过。若上游希望发布完整状态，需要定义如何取得或估计每个关节在同一采样时刻的位置。把旧值复制进新消息并标成当前时间，也等于引入"旧值仍代表当前状态"的假设。

2.2 发布频率如何参与消息筛选

Humble 3.0.3 的常规路径先找当前消息所列关节中最早的上次发布时间，再与本次消息时间比较。略去时钟回退处理和毫秒取整后，逻辑可概括为：1

last = 当前消息各关节的上次发布时间中的最小值
当 stamp ≥ last + 发布间隔，或 ignore_timestamp=true：
    计算本消息的关节变换
    输出 TF 的时间戳仍为本消息 stamp
    更新本消息所列关节的发布时间记录

这个频率参数参与消息处理筛选，并不会自动把缺失关节插值到定时器时刻。一个关节以同一个 stamp 重复上报，可能被筛掉；另一个此前较少更新的关节加入消息，又会改变整批筛选依据。所以分析漏发要同时保留消息中有哪些名字、它们的采样戳以及参数配置。

ignore_timestamp=true 可以跳过这个筛选条件，但输出仍使用原消息戳。它无法把过期数据变成新观测，也不能修复系统时间与仿真时间混用。需要在状态源层面确认时间语义。

2.3 mimic 减少独立输入，不会创建新的测量

对直接依赖已知源关节的 mimic 关系，从动位置由 q_m = multiplier × q_s + offset 得到。源关节值随当前消息进入时，状态发布器可以据此计算从动边。1 这是机构约束的应用，不能反向推断任意缺失关节的状态，也不包含机械间隙和负载形变。

八界夹爪模型描述了指部的联动，而 objects_link 是夹爪基座下的固定抓取参考点。8 夹爪开合时指部坐标可变化，这个固定参考点仍保持其安装关系。查到 objects_link 的位姿，只说明该参考系的位置，不能直接证明物体已经进入夹爪或被夹稳。

3. 用五连杆模型手算，再检查真实节点输出

教学模型在 t=11 s 输入 lift_joint=0.15 m、yaw_joint=π/2 rad。顺着工具链逐项计算：

升降连杆高度：0.20 + 0.15 = 0.35 m
手腕原点：   (0.20, 0, 0.35 + 0.10) = (0.20, 0, 0.45) m
手腕偏航：   π/2（安装）+ π/2（关节）= π
工具局部偏移：(0.30,0,0) 经 π 旋转变为 (−0.30,0,0)
工具在基座： (0.20,0,0.45) + (−0.30,0,0) = (−0.10,0,0.45) m

将平面转向与高度分别展开。把每一步对应到一条 joint，能够定位数值错误来自安装位置、零位方向还是运动量。

图 4　工具偏移在手腕局部系定义，先旋转再加到基座系下的手腕原点。

配套 examples/12_state_publisher.py 启动已安装的真实状态发布器，默认加载 examples/12_teaching_arm.urdf。脚本通过独立命名空间收集 TFMessage，填入 Buffer 后做查询和断言。6

# 已加载 ROS 2 Humble 环境，在本篇目录运行
ROS_LOCALHOST_ONLY=1 ROS_DOMAIN_ID=37 RMW_IMPLEMENTATION=rmw_fastrtps_cpp \
  python3 examples/12_state_publisher.py

实测环境为断网 Ubuntu 22.04 Humble 容器，robot_state_publisher 3.0.3、rclpy 3.3.21、rmw_fastrtps_cpp 6.2.10。脚本先验证晚加入时能读到固定边和模型，再发送局部与完整状态：

{
  "late_static_children": ["camera_link", "tool_link"],
  "late_robot_description_received": true,
  "partial_dynamic_children": ["wrist_link"],
  "partial_base_to_tool_available": false,
  "full_dynamic_children": ["lift_link", "wrist_link"],
  "tool_in_base_at_11_m": [-0.1, 0.0, 0.45],
  "lookup_at_10_after_full_update": false,
  "latest_after_lift_only_sec": 11,
  "lookup_at_12_before_yaw": false,
  "tool_in_base_at_12_m": [-0.1, 0.0, 0.5]
}

10 s 只给转动，根到工具未连通。11 s 补齐后，正好得到手算结果；回查 10 s 仍失败，因为升降边的最早样本是 11 s，后来补齐不会追溯生成过去状态。

12 s 先只把升降改成 0.20 m，整链最新共同时间仍为 11 s。补上同一个 12 s 戳的转动状态后，才得到 (-0.10,0,0.50) m。这一前后对照把"局部边有新值"与"完整路径可在新时刻求值"分开了。

4. 把同一个实验放到八界的实际机构树上

本次使用八界公开描述包提交 7be6a0a 展开主 Xacro。头部和机械臂在 lift_link 分叉，相关路径如下：8

base_footprint
└─ base_link
   └─ lift_base_link
      └─ lift_link                 lift_joint：局部 +z 平移
         ├─ head_pitch_link        pitch_joint：局部 −y 旋转
         │  └─ berxel_upper_base_link
         └─ arm_base_link
            └─ arm1_link ... arm6_link
               ├─ berxel_arm_base_link
               └─ gripper_link
                  └─ objects_link

图 5 把这棵树与三个输入阶段放在一起。观察量来自状态发布器的实际输出，所有关节值和时间戳都是合成输入，仓库外参采用原有占位配置。

图 5　头部与腕部共享升降路径。更新共享边后，分支时间覆盖仍决定整链能否查询新时刻。

4.1 先让分支完整，却故意缺少共同的升降边

在 10 s，只发送 pitch_joint 和 joint1 至 joint6，都取零。头部、腕部相机的固定安装边已经收到，腕部基座到 objects_link 也能查询。但从 base_footprint 到头部、腕部与抓取参考点全部失败，因为三条根路径都需要尚未提供的 lift_joint。

这比"看看 TF 图是不是一棵树"多了一层信息：模型定义了边的可能关系，Buffer 中是否有该时刻的动态边，必须由状态发布证据确认。

4.2 补齐一个采样时刻，再只更新共享关节

11 s 输入升降 0.10 m，同时输入俯仰与六个臂关节。三条根路径都可查。升降连杆在根系的高度可以直接从公开模型复算：

z_lift = 0.070 + 0.095 + 0.100 + 0.100 = 0.365 m
          底盘    升降底座  关节零位  本次升降量

12 s 先仅输入升降 0.20 m，该连杆高度变成 0.465 m。然而头部、腕部与抓取点相对根系的最新共同时间均为 11 s，显式查询 12 s 仍失败。腕部到抓取参考点的固定路径依旧可用。

随后补发采样戳同为 12 s 的俯仰与臂关节，位置仍为零。两路相机在根系的 z 都增加 0.10 m，它们之间的相对变换保持不变。这同时验证了共享升降运动、局部边按批更新，以及第 11 篇中共同祖先项抵消的推导。

4.3 用公开模型模式复现

同一个脚本新增 --bajie-urdf 模式，输入已经展开的 XML，运行上述步骤并输出模型内容的 SHA-256，方便核对输入。先按描述包说明使 bajie_robot 能被 ROS 2 找到，并安装 Xacro；本文测试依据为上述提交。8

xacro "$(ros2 pkg prefix --share bajie_robot)/urdf/bajie_robot.urdf.xacro" \
  > /tmp/bajie-model.urdf

ROS_LOCALHOST_ONLY=1 ROS_DOMAIN_ID=37 RMW_IMPLEMENTATION=rmw_fastrtps_cpp \
  python3 examples/12_state_publisher.py --bajie-urdf /tmp/bajie-model.urdf

模型模式先检查关节类型、轴与相关父子连接，再启动独立的状态发布进程，不依赖设备反馈。不同版本若调整机构或标定，应先检查模型差异，再解释数值变化。输出节选如下：

{
  "base_queries_head_wrist_grasp_at_t10": [false, false, false],
  "wrist_to_grasp_fixed_at_t10": true,
  "base_lift_z_m_at_t11_t12": [0.365, 0.465],
  "latest_after_t12_lift_only": {
    "berxel_upper_base_link": 11,
    "berxel_arm_base_link": 11,
    "objects_link": 11
  },
  "base_queries_head_wrist_grasp_at_t12_before_other_joints": [false, false, false],
  "wrist_to_grasp_fixed_at_t12": true,
  "head_delta_in_base_footprint_m": [0.0, 0.0, 0.1],
  "wrist_delta_in_base_footprint_m": [0.0, 0.0, 0.1],
  "head_to_wrist_transform_unchanged_after_lift": true
}

这份结果验证的是公开模型的运动学与节点行为。相机基座外参仍需实机标定，关节零位也需设备反馈约定；不能把模型模式中的坐标直接视为真实抓取精度。

5. 将模型、状态源和定位职责接起来

八界可视化 launch 展开主 Xacro，启动 robot_state_publisher、joint_state_publisher_gui 和 RViz。10 GUI 提供显示用关节状态，八界开发文档则将 /joint_states 列为设备反馈入口。9 接真实状态时要确认由哪个节点提供每个关节，避免演示输入与反馈同时争用相同关节名和时间线。

对根到相机查不通的问题，可以沿证据逐层缩小范围：

• 固定边尚未进入 Buffer： 核对实际加载的 XML、父子关系、静态发布端及读者持久性。先确定模型确实形成所需安装关系。
• 固定边可查，根路径缺失： 沿路径逐个核对 JointState 中的名字与位置。八界两路相机同时缺根路径时，共有的升降段是一个有依据的检查起点。
• 活动边都有，指定时间不通： 对比各边的时间区间、发布筛选和时钟域。分别以不同速率上报的关节不会自然获得同一个最新戳。
• 查询成功，几何结果不对： 先看零位安装，再单独变化一个 q，检查单位、轴方向和零点；最后检查外参与视觉网格偏置。逐关节变化比同时移动整条臂更容易识别误差来源。

主 Xacro 的 Berxel 基座还需与图像实际光学帧衔接，不能根据名字省略这段关系。URDF 也不会自行产生 map → odom 或 odom → base_footprint 的定位状态，外部定位与内部关节运动需要在同一 TF 系统里各自承担对应边。89

如果多个机器人共享 TF 通路，消息里的 frame ID 也要唯一。ROS 命名空间可隔离话题名，坐标系字符串还需通过 frame_prefix 或一致的命名方案安排，并与所有外部发布者和查询者对应。1

从几何函数、关节采样到局部边，再到整链查询，这几个步骤各自都有可检查的输入和输出。保留它们的对应关系，既能解释模型为何显示成当前姿态，也能判断这份姿态是否适合处理某个时刻的观测。欢迎关注科沃斯八界 AtomGit，相关代码将逐步释放，实际开放内容与使用说明以仓库为准。7

参考链接

1. 与实验对应的节点实现，查阅模型加载、固定与活动段发布和关节消息处理。robot_state_publisher 3.0.3
2. Humble 的 URDF 到 KDL 转换，查阅关节原点和轴如何参与段定义。kdl_parser.cpp
3. ROS 2 的固定与动态变换发布机制及参数说明。robot_state_publisher Humble
4. 关节状态消息定义，查阅位置单位与数组约定。JointState.msg
5. 中文 URDF/Xacro 与 TF 发布示例，适合查看宏展开与模型装载的完整命令。CSDN 开发者文章
6. 本专栏示例代码仓库入口。科沃斯八界示例仓库
7. 八界项目公开入口。科沃斯八界 AtomGit
8. 八界整机 Xacro，查阅相机基座、关节及标定属性的引入。本文模型实验使用 7be6a0a。bajie_robot.urdf.xacro
9. 八界开发文档，查阅 §6.3 的关节反馈话题。ROS 2 Humble 高阶开发 SOP
10. 八界可视化 launch，查阅 Xacro、robot_state_publisher、关节 GUI 与 RViz 的连接。display_bajie_robot_urdf.launch.py