机器人 / 系统课时
对齐 URDF 与 DH 模型输出
对齐 URDF 与 DH 模型输出
同一组关节角输入 URDF 与 DH 模型后,如果末端位置或朝向不同,不能只调整连杆长度让数字凑齐。应先统一基座与工具坐标系,再逐项检查固定关节、零位偏置和旋转轴。本篇用可运行的双关节算例建立对照,并给出定位真实机器人模型误差的方法。
学习目标
- 明确 URDF 与标准 DH 正运动学结果可以直接比较的前提。
- 运行同一关节配置的双模型计算,得到位置和姿态误差。
- 依据残差变化区分参考系、安装偏置、零位及关节轴问题。
先确定比较的是同一对坐标系
URDF 的 joint/origin 描述父链接到关节零位坐标系的固定变换,axis 指定关节自身坐标系中的运动轴;运动变量在此固定变换之后施加。标准 DH 则通过连杆参数构造相邻坐标系变换。若末端帧不同,两套计算即使都正确也会得到不同结果。
| 检查项 | 本例 URDF | 本例标准 DH |
|---|---|---|
| 起点 | base 链接坐标系 |
第 0 坐标系,与 base 重合 |
| 终点 | tool 链接坐标系 |
第 2 坐标系,与 tool 重合 |
| 关节输入 | j1、j2,单位 rad |
同一物理关节角,单位 rad |
| 几何单位 | origin/xyz 使用 m |
a、d 使用 m |
不要把 visual/origin 的网格偏移当成末端位姿,也不要把 URDF 的 arm2 原点直接拿来对比 DH 的工具末端。若 DH 第 0 坐标系与机器人基座不同,还要先乘入固定的基座对齐变换。
对齐两套模型的关节链
取平面二旋转关节机械臂,两段长度均为 1 m。URDF 中 j1 位于基座原点,j2 在第一段局部 X 轴上距离 1 m;第二段末端由 tool_mount 固定关节再沿局部 X 轴偏移 1 m。两个转动轴都是局部正 Z,关节零位时连杆均指向正 X。
标准 DH 取 m、、、、,其中单节变换为 。URDF 必须保留最后的固定工具偏置,才能与 DH 第 2 坐标系重合。

在关节角 q1=30°、q2=-45° 时,末端朝向为 -15°,位置按 、 计算,约为 (1.831951 m, 0.241181 m)。这三个值可作为手算基准。
运行 URDF 与 DH 的正运动学对照
下面是 Python 3.9+ 标准库示例。将代码保存为 compare_fk.py,执行 python compare_fk.py。代码直接解析内嵌的最小 URDF,按父子关系遍历从 base 到 tool 的单链;为便于核算,只支持平面、绕正 Z 旋转的关节,不适用于任意六轴机构。
import math
from xml.etree import ElementTree as ET
URDF = """<robot name="planar2">
<link name="base"/><link name="arm1"/>
<link name="arm2"/><link name="tool"/>
<joint name="j1" type="continuous">
<parent link="base"/><child link="arm1"/>
<origin xyz="0 0 0" rpy="0 0 0"/><axis xyz="0 0 1"/>
</joint>
<joint name="j2" type="continuous">
<parent link="arm1"/><child link="arm2"/>
<origin xyz="1 0 0" rpy="0 0 0"/><axis xyz="0 0 1"/>
</joint>
<joint name="tool_mount" type="fixed">
<parent link="arm2"/><child link="tool"/>
<origin xyz="1 0 0" rpy="0 0 0"/>
</joint>
</robot>"""
def mul(a, b):
return [[sum(a[i][k] * b[k][j] for k in range(3))
for j in range(3)] for i in range(3)]
def tf(x=0.0, y=0.0, angle=0.0):
c, s = math.cos(angle), math.sin(angle)
return [[c, -s, x], [s, c, y], [0.0, 0.0, 1.0]]
def urdf_fk(q):
root = ET.fromstring(URDF)
chain = {j.find("parent").get("link"): j
for j in root.findall("joint")}
t, link = tf(), "base"
while link != "tool":
joint = chain[link]
xyz = list(map(float, joint.find("origin").get("xyz").split()))
rpy = list(map(float, joint.find("origin").get("rpy").split()))
assert xyz[2] == 0 and rpy[0] == rpy[1] == 0
# URDF:先施加固定 origin,再施加关节运动
t = mul(t, tf(xyz[0], xyz[1], rpy[2]))
if joint.get("type") != "fixed":
axis = list(map(float, joint.find("axis").get("xyz").split()))
assert axis == [0.0, 0.0, 1.0]
t = mul(t, tf(angle=q[joint.get("name")]))
link = joint.find("child").get("link")
return t
def dh_fk(q):
t = tf()
for length, angle in ((1.0, q["j1"]), (1.0, q["j2"])):
# 标准 DH 在 d=alpha=0 时为 Rz(theta) 再 Tx(a)
t = mul(mul(t, tf(angle=angle)), tf(x=length))
return t
def yaw(t):
return math.atan2(t[1][0], t[0][0])
for degrees in ((0, 0), (30, -45)):
q = dict(zip(("j1", "j2"), map(math.radians, degrees)))
u, d = urdf_fk(q), dh_fk(q)
ep = math.hypot(u[0][2] - d[0][2], u[1][2] - d[1][2])
delta = yaw(u) - yaw(d)
ea = math.degrees(abs(math.atan2(math.sin(delta), math.cos(delta))))
print(f"q={degrees} URDF: x={u[0][2]:.6f} y={u[1][2]:.6f} yaw={math.degrees(yaw(u)):.3f}deg")
print(f"q={degrees} DH: x={d[0][2]:.6f} y={d[1][2]:.6f} yaw={math.degrees(yaw(d)):.3f}deg")
print(f"位置误差={ep:.6f}m 姿态误差={ea:.6f}deg")
assert ep < 1e-9 and ea < 1e-9预期输出:
q=(0, 0) URDF: x=2.000000 y=0.000000 yaw=0.000deg
q=(0, 0) DH: x=2.000000 y=0.000000 yaw=0.000deg
位置误差=0.000000m 姿态误差=0.000000deg
q=(30, -45) URDF: x=1.831951 y=0.241181 yaw=-15.000deg
q=(30, -45) DH: x=1.831951 y=0.241181 yaw=-15.000deg
位置误差=0.000000m 姿态误差=0.000000deg这里的零残差是两套实现使用同一几何定义的结果,不表示机器人标定误差为零。角度差通过 atan2(sin(delta), cos(delta)) 归一化,避免把 -179° 与 179° 误判为相差 358°。
按误差特征定位模型差异
先测全零位,再只改变一个关节,最后测多关节组合。不要只看末端误差数值,还要记录第一个出现差异的链接坐标系。
| 观测现象 | 优先检查 | 可执行的修正 |
|---|---|---|
| 零位时位置差 1 m、朝向相同 | tool_mount 固定变换遗漏 |
补回沿工具前向 1 m 的固定偏置;本例不加该偏置时末端只有 (1,0) m |
零位一致,转动 j2 后才分离 |
axis 符号、DH 角度方向 |
单独输入 j2=+10° 与 -10°,核对正向旋转 |
| 每个姿态都存在固定的相对旋转 | 基座或 TCP 坐标架安装方向 | 明确乘入固定基座变换或工具安装旋转,而非改动连杆长度 |
| 零位符合,但多关节姿态误差随角度变大 | 零位偏置、角度单位或 DH 约定混用 | 核对 rad 与 deg、q+offset,确认使用标准 DH 而非改进 DH |
URDF 的 axis 缺省值是关节局部正 X,不是正 Z;模型编写时若省略该标签,平面旋转轴可能悄悄改变。origin/rpy 使用弧度,且固定关节仍要参与连乘。不要通过任意修改 a 值来掩盖轴向或坐标系定义错误。
把验证方法扩展到真实六轴模型
六轴机械臂应使用完整的 4×4 刚体变换,保留 URDF 的三维 xyz/rpy、各关节的局部轴及所有固定关节;上面的 3×3 平面程序不能直接套用。实际验收可采用以下顺序:
- 确定 URDF 的
base_link与tool0(或实际 TCP),写明它们分别对应哪个 DH 坐标架,并补入必要的基座、法兰与工具固定变换。 - 在关节限位内测试全零位、每轴单独正负 10°、至少三组多轴混合角;对有移动关节的机构改用米为单位的单轴位移。
- 对每组配置计算平移向量的欧氏距离和相对旋转矩阵的转角。离线模型初验可暂用 位置误差不超过 0.1 mm、姿态误差不超过 0.01°,再按实际数值精度和项目要求收紧或放宽。
- 若末端不一致,逐段比较对应关节坐标架,找到第一个误差增长的位置;注意 DH 坐标架不一定与 URDF 链接坐标架天然重合,比较前必须先做固定坐标变换。
模型计算一致只证明两套几何描述对齐,不代表实机已完成编码器零点、弹性变形或 TCP 标定。只有在统一参考系后得到的残差,才适合继续用于标定和误差分析。