机器人 / 系统课时

对齐 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 取 a1=a2=1a_1=a_2=1 m、d1=d2=0d_1=d_2=0、α1=α2=0\alpha_1=\alpha_2=0、θ1=q1\theta_1=q_1、θ2=q2\theta_2=q_2,其中单节变换为 Ai=Rz(θi)Tz(di)Tx(ai)Rx(αi)A_i=R_z(\theta_i)T_z(d_i)T_x(a_i)R_x(\alpha_i)。URDF 必须保留最后的固定工具偏置,才能与 DH 第 2 坐标系重合。

双关节 URDF 和 DH 坐标系及工具偏置对照

在关节角 q1=30°、q2=-45° 时,末端朝向为 -15°,位置按 x=cos⁡q1+cos⁡(q1+q2)x=\cos q_1+\cos(q_1+q_2)、y=sin⁡q1+sin⁡(q1+q2)y=\sin q_1+\sin(q_1+q_2) 计算,约为 (1.831951 m, 0.241181 m)。这三个值可作为手算基准。

运行 URDF 与 DH 的正运动学对照

下面是 Python 3.9+ 标准库示例。将代码保存为 compare_fk.py,执行 python compare_fk.py。代码直接解析内嵌的最小 URDF,按父子关系遍历从 base 到 tool 的单链;为便于核算,只支持平面、绕正 Z 旋转的关节,不适用于任意六轴机构。

python
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

预期输出:

text
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 平面程序不能直接套用。实际验收可采用以下顺序:

  1. 确定 URDF 的 base_link 与 tool0(或实际 TCP),写明它们分别对应哪个 DH 坐标架,并补入必要的基座、法兰与工具固定变换。
  2. 在关节限位内测试全零位、每轴单独正负 10°、至少三组多轴混合角;对有移动关节的机构改用米为单位的单轴位移。
  3. 对每组配置计算平移向量的欧氏距离和相对旋转矩阵的转角。离线模型初验可暂用 位置误差不超过 0.1 mm、姿态误差不超过 0.01°,再按实际数值精度和项目要求收紧或放宽。
  4. 若末端不一致,逐段比较对应关节坐标架,找到第一个误差增长的位置;注意 DH 坐标架不一定与 URDF 链接坐标架天然重合,比较前必须先做固定坐标变换。

模型计算一致只证明两套几何描述对齐,不代表实机已完成编码器零点、弹性变形或 TCP 标定。只有在统一参考系后得到的残差,才适合继续用于标定和误差分析。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

对齐 URDF 与 DH 模型输出

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通