Part II · 第 7 章 结构与运动学 · 7.3

正运动学：DH 参数与 PoE

给定关节角，每一个连杆坐标系在世界里的哪里？机器人界有两门通用语言：几十年来写在厂商手册里的 Denavit–Hartenberg (DH) 参数表，和 5.10 节学过的指数积公式 (PoE)。本课把它们放在同一台 UR5 上对比：一个 DH 连杆是哪四步动作，两种写法为什么算出同一个结果，DH 在哪里会出问题，以及基座、工具和标定怎么处理。

约 35 分钟 前置：5.3 SE(3) · 5.6 指数映射 · 5.10 指数积 相关：7.1 关节 · 7.4 速度运动学 5 个交互

    1为什么机器人需要它

    控制器每个周期要知道末端在哪里；逆运动学要反复调用它；碰撞检测要把每一节连杆放进世界；仿真器、可视化、手眼标定都先问同一个问题：给定 \(\boldsymbol\theta\)，每个坐标系在哪？这就是正运动学 (FK)。5.10 节已经给出一个干净的答案：指数积公式 \(T(\boldsymbol\theta)=\Exp(S_1\theta_1)\cdots\Exp(S_n\theta_n)M\)。

    可是你在现实中拿到的资料，往往是另一种形式。Universal Robots 和 Franka 在官方文档里公布的是 DH 参数表；教科书（Craig、Spong、Siciliano）、很多老代码、不少标定软件说的也是 DH。反过来，Pinocchio、Drake、Gazebo 这类现代库和仿真器读的是 URDF，那是第三种写法。要做机器人，你必须能读懂这三种语言，并且知道它们怎样互相翻译。

    本课的路线：先用四步动作拆开一个 DH 连杆（FIG 1）；再把真实的 UR5 用 DH 乘一遍、用 PoE 乘一遍，看两个结果在 \(10^{-15}\) 内重合，并学会从 DH 表机械地写出 \(S_i\) 和 \(M\)（FIG 2）；然后看 DH 在轴平行或相交时为什么会出问题（FIG 3）；最后讨论基座与工具坐标系怎么改（FIG 4），以及为什么 FK 里一毫米的参数误差会变成末端的几毫米误差，机器人怎样标定它（FIG 5）。

    2直觉：一个 DH 连杆 = 四步动作

    一般来说，两个坐标系之间的位姿有 6 个自由度。DH 的想法是：既然我们可以自己选坐标系，就选得巧一点，让相邻两个坐标系之间只需要 4 个数。办法是规定三件事：关节 \(i\) 的转轴取作 \(z_{i-1}\)；\(x_i\) 垂直于 \(z_{i-1}\)；\(x_i\) 与 \(z_{i-1}\) 相交。满足这两条垂直和相交的约束后，从坐标系 \(\{i-1\}\) 到 \(\{i\}\) 恰好可以用下面四步依次完成（每一步都是绕着或沿着刚刚到达的坐标系的轴）：

    1. 绕 \(z_{i-1}\) 转 \(\theta_i\)；
    2. 沿 \(z_{i-1}\) 平移 \(d_i\)；
    3. 沿新的 \(x\) 轴（即 \(x_i\)，也就是公共法线）平移 \(a_i\)；
    4. 绕 \(x_i\) 转 \(\alpha_i\)，把 \(z_{i-1}\) 扭到 \(z_i\)。

    这里 \(\theta_i\)（转动关节）或 \(d_i\)（移动关节）是关节变量，其余三个数是连杆本身的几何常数。下面的实验让你调这四个数，并且一步一步地播放。紫色虚线是两根关节轴：第一根穿过 \(\{i-1\}\) 的原点沿 \(z_{i-1}\)，第二根穿过 \(\{i\}\) 的原点沿 \(z_i\)。红色线段是沿 \(x\) 的平移 \(a\)，蓝色线段是沿 \(z\) 的平移 \(d\)。顶部的开关可以切换到 Craig 的改进 DH：同样四个数，顺序换了，得到另一个矩阵。

    试一试
    • 点"播放四步"，看完整个过程。哪两步让原点移动？哪两步只转动坐标系？
    • 选标准 DH，设 \(\theta=0\)、\(a=0\)、\(\alpha=90^\circ\)（UR5 的第 1 个连杆就是这样）。新的 \(z_i\)（蓝色轴）指向哪个方向？把读数里的矩阵第三列和你的判断对一下。
    • 在 \(a>0.3\)、\(|\alpha|>30^\circ\) 时，来回切换标准 DH 和改进 DH。同样的四个数，终点坐标系是一样的吗？

    3动手玩：同一台 UR5，两种描述

    UR5 是最常见的协作臂之一，它的标准 DH 参数是 Universal Robots 官方公布的（单位米与弧度）：

    i\(\theta_i\)\(d_i\) (m)\(a_i\) (m)\(\alpha_i\)

    每一行的 \(\theta_i\) 就是第 \(i\) 个关节的角度（零偏为 0）。把六个 \(A_i(\theta_i)\) 依次相乘，就得到法兰盘（第 6 个坐标系）在基座里的位姿。这是教科书的做法，也是 UR 官方文档给你的做法。

    PoE 要的却是零位时的六根关节轴 \(S_i\) 和零位末端位姿 \(M\)。怎么办？答案藏在 DH 的构造里：关节 \(i\) 的转轴就是 \(z_{i-1}\)，所以只要把 \(\theta=0\) 时各个坐标系的位姿算出来，第 \(i\) 根轴的方向就是坐标系 \(\{i-1\}\) 的 \(z\) 轴，轴上一点就是它的原点：\(\boldsymbol\omega_i = R_{0,i-1}\hat{\mathbf z}\)，\(\mathbf q_i=\mathbf p_{0,i-1}\)，\(S_i=(\boldsymbol\omega_i,\,-\boldsymbol\omega_i\times\mathbf q_i)\)，而 \(M\) 就是零位时的第 6 个坐标系。第 5 节会证明这是严格成立的。FIG 2 同时用两种方法算同一台机械臂，右上角读数是两个 \(4\times4\) 矩阵之差的范数。

    FIG 2UR5：DH 连乘 vs. 指数积
    拖动画面旋转视角
    预设
    "DH 坐标系"模式：每个 \(\{i\}\) 都画成小坐标系，蓝线是沿 \(z_{i-1}\) 的 \(d_i\)，红线是沿 \(x_i\) 的 \(a_i\)。"PoE 轴"模式：紫色虚线是 \(\theta=0\) 时的六根螺旋轴，它们不动；机械臂由部分乘积 \(\prod_{j\le i}\Exp(S_j\theta_j)\) 把零位的点搬到当前位置得到。两个模式画出的手臂完全一样，因为它们是同一个乘积。读数里的 \(\|T_{\rm DH}-T_{\rm PoE}\|\) 是两个 \(4\times4\) 矩阵之差的 Frobenius 范数。
    i\(\boldsymbol\omega_i\)\(\mathbf q_i\)\(\mathbf v_i=-\boldsymbol\omega_i\times\mathbf q_i\)

    上表由 DH 表在 \(\theta=0\) 处自动算出（把鼠标移到表的某一行，FIG 2 会高亮对应的关节）。注意 \(\boldsymbol\omega_i\) 全是 \(\pm\) 坐标轴方向，因为 UR5 的所有关节轴都平行或垂直于基座的坐标轴。

    试一试
    • 点"零位"。工具位置是 \((-0.817,\,-0.191,\,-0.005)\)。这三个数各自来自 DH 表里的哪些参数？（提示：\(a_2+a_3\)、\(d_4+d_6\)、\(d_1-d_5\)。）
    • 只把 \(\theta_1\) 设成 \(90^\circ\)，其余为 0。把零位的 \((-0.817,-0.191)\) 绕 \(z\) 转 \(90^\circ\) 得到 \((0.191,-0.817)\)。读数一致吗？为什么 \(z\) 不变？
    • 让至少三个关节都偏离 \(30^\circ\) 以上，再用"DH 坐标系"和"PoE 轴"两个模式看。两种算法的差有多大？它会随姿态变大吗？
    核心洞见

    DH 和 PoE 不是两种不同的运动学，而是同一个乘积的两种分组方式。DH 把每个关节的转动和它后面的固定几何捆在一起：\(\mathrm{Rot}_z(\theta_1)\hat A_1\cdot\mathrm{Rot}_z(\theta_2)\hat A_2\cdots\)；PoE 把固定几何全部挪到最右边，剩下的转动被"共轭"成了世界系里的螺旋轴 \(\Exp(S_i\theta_i)\)。从 DH 到 PoE 只是做了一次代数整理，没有近似。

    4再看两面：DH 的软肋与坐标系的记账

    DH 在哪里出问题

    DH 的四个参数由相邻两根关节轴之间的公共法线决定。两根空间中的直线一般有唯一的公共法线，可是有两种特殊情形：

    • 两轴平行：公共法线有无穷多条（沿着轴滑动都是），\(d_i\) 没有唯一值，是一个自由参数。习惯做法是挑一个让别的参数为 0 的位置（UR5 的关节 2、3、4 轴互相平行，所以 \(d_2=d_3=0\)）。
    • 两轴相交：公共法线长度 \(a_i=0\)，位置确定（过交点），但方向只能取垂直于两轴的平面法线，\(\pm\) 都可以：这是一个离散的符号约定，选哪个会翻转 \(\alpha_i\) 的符号并让 \(\theta\) 差 \(\pi\)。

    更麻烦的是"差一点平行"。真实的机械臂不可能做到严格平行：装配误差让轴有零点几度的倾斜，此时公共法线是唯一的，可它的落脚点会飞到很远，\(d_i\) 变成 \(10\) 米、\(100\) 米，下一个连杆的 \(d_{i+1}\) 也被迫取一个大数来补偿，乘积仍然正确，但这两个参数已经没有物理含义，也无法稳定地辨识。FIG 3 把一根轴慢慢掰到与另一根平行，同时画出 DH 参数和螺旋轴 \(S\) 的各个分量。

    FIG 3DH 参数 vs. 螺旋轴：轴趋于平行或相交
    拖动画面旋转视角
    DH 参数（m 或 rad）
    螺旋轴 S = (ω, v) 的六个分量
    固定的轴 A 是 \(z\) 轴（关节 \(i\)），可动的轴 B 是关节 \(i+1\)。"近平行"：B 过点 \((0.5,\,0.2,\,0.3)\)，绕 \(x\) 方向倾斜 \(\varepsilon\)；"近相交"：B 与 A 夹角固定为 \(40^\circ\)，穿过 \((g,\,0,\,0.3)\)，平移 \(g\)。横轴是对数刻度（滑块一致），\(0\) 在正中。琥珀色线段是公共法线。左图 \(d\)（蓝）在 \(\varepsilon\to0\) 时飞出图外；右图六个分量始终光滑。
    试一试
    • 选"近平行"，把 \(|\varepsilon|\) 拉到 \(0.1^\circ\) 以下（或点"趋于 0"）。\(d\) 变成多少？公共法线现在在哪里？右图的曲线有任何异常吗？
    • 选"近相交"，把 \(g\) 从正数慢慢拖到负数，穿过 0。左图里哪些参数发生了跳变？物理上轴 B 动了多少？

    坐标系的记账：基座与工具

    现实中的机械臂总有两个"额外"的坐标系：基座系（机械臂装在工作台上，相对世界系 \(\{W\}\) 有位姿 \(T_{WB}\)）和工具系（法兰盘上装了夹爪，工具中心点 TCP 相对法兰有固定位姿 \(T_{\rm tool}\)）。换一个夹爪，或者把机械臂挪个位置，FK 怎么改？

    两种写法的改法并不一样，这正是 PoE 方便的地方：改工具只动 \(M\)（\(M\mapsto M\,T_{\rm tool}\)），所有 \(S_i\) 原样不动；改基座要动所有 \(S_i\)（\(S_i\mapsto\operatorname{Ad}_{T_{WB}}S_i\)，\(M\mapsto T_{WB}M\)）。DH 里则是在乘积的左边或右边多乘一个固定矩阵，\(A_i\) 不变。FIG 4 同时用两种方式算，并显示哪些量变了。

    FIG 4基座系与工具系：谁变了？
    拖动画面旋转视角
    姿态
    粗坐标系：世界 \(\{W\}\)（灰）、基座 \(\{B\}\)、法兰 \(\{6\}\)、工具 \(\{T\}\)（粗）。琥珀色线是法兰到 TCP 的偏移。读数中的 \(\|\Delta S_i\|\) 是新旧螺旋轴之差：改工具时全为 0；改基座时一般每根轴都变（只绕世界 \(z\) 偏航时 \(S_1\) 恰好不变，因为它就在转轴上）。
    试一试
    • 选"改工具"，把 TCP 沿法兰 \(z\) 伸出 \(0.15\) m 以上。\(S_1,\dots,S_6\) 变了吗？\(M\) 变了多少？
    • 选"改基座"，把基座绕世界 \(z\) 轴转超过 \(20^\circ\)。哪些 \(S_i\) 变了？有没有哪个 \(\|\Delta S_i\|\) 恰好为 0？为什么？（想一想 \(S_1\) 的轴方向。）

    5再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 或 FIG 2 中对应的元素会被高亮。

    标准 DH 矩阵

    三条规则（轴 \(z_{i-1}\) 是关节 \(i\) 的转轴；\(x_i\perp z_{i-1}\)；\(x_i\) 与 \(z_{i-1}\) 相交）确定后，四个参数的含义是：\(\theta_i\) 是绕 \(z_{i-1}\) 从 \(x_{i-1}\) 转到 \(x_i\) 的角；\(d_i\) 是沿 \(z_{i-1}\) 从 \(\{i-1\}\) 的原点到 \(x_i\) 与 \(z_{i-1}\) 交点的距离；\(a_i\) 是沿 \(x_i\) 从交点到 \(\{i\}\) 原点的距离，也就是两轴之间公共法线的长度；\(\alpha_i\) 是绕 \(x_i\) 从 \(z_{i-1}\) 转到 \(z_i\) 的角。把四步按"每一步都在当前坐标系里"的规则（右乘）连起来：

    $$ A_i = \mathrm{Rot}_z(\hl{th}{\theta_i})\,\mathrm{Trans}_z(\hl{d}{d_i})\,\mathrm{Trans}_x(\hl{a}{a_i})\,\mathrm{Rot}_x(\hl{al}{\alpha_i}) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i\\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i\\ 0 & \sin\alpha_i & \cos\alpha_i & d_i\\ 0&0&0&1\end{bmatrix}. $$

    读这个矩阵：最后一列是 \(\{i\}\) 的原点在 \(\{i-1\}\) 里的位置，\((a_i\cos\theta_i,\ a_i\sin\theta_i,\ d_i)\)，先升 \(d_i\)，再朝转过 \(\theta_i\) 的方向走 \(a_i\)；左上的 \(3\times3\) 是 \(R_z(\theta_i)R_x(\alpha_i)\)。整台机械臂是 \(T_{0n}(\boldsymbol\theta)=A_1(\theta_1)A_2(\theta_2)\cdots A_n(\theta_n)\)，每个因子相对前一个坐标系，所以全部右乘。这与 PoE 相反：PoE 的指数全部相对固定的世界系，所以左乘（5.10 节）。

    改进 DH（Craig）

    Craig 的版本把坐标系 \(\{i\}\) 放在关节 \(i\) 的轴上（\(z_i\) 是关节 \(i\) 的转轴），并把连杆参数 \(a_{i-1},\alpha_{i-1}\) 放在关节之前：

    $$ A_i^{\rm mod} = \mathrm{Rot}_x(\alpha_{i-1})\,\mathrm{Trans}_x(a_{i-1})\,\mathrm{Rot}_z(\theta_i)\,\mathrm{Trans}_z(d_i) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 & a_{i-1}\\ \sin\theta_i\cos\alpha_{i-1} & \cos\theta_i\cos\alpha_{i-1} & -\sin\alpha_{i-1} & -d_i\sin\alpha_{i-1}\\ \sin\theta_i\sin\alpha_{i-1} & \cos\theta_i\sin\alpha_{i-1} & \cos\alpha_{i-1} & d_i\cos\alpha_{i-1}\\ 0&0&0&1\end{bmatrix}. $$

    两个版本描述的是同一类机械臂，但每个连杆的坐标系放法不同，表格的数值与下标都对不上。标准 DH 表里第 \(i\) 行的 \((a_i,\alpha_i)\) 描述 \(z_{i-1}\to z_i\)，改进 DH 表里第 \(i\) 行的 \((a_{i-1},\alpha_{i-1})\) 也是描述 \(z_{i-1}\to z_i\)，只是挂在了第 \(i\) 个关节的名下。这个下标错位是两个版本混用时最常见的 bug（误区 1）。Franka 公布的是改进 DH，UR 是标准 DH。

    从 DH 到 PoE：一次代数整理

    把标准 DH 的每个因子拆成"关节转动 × 固定几何"：\(A_i(\theta_i)=\mathrm{Rot}_z(\theta_i)\,\hat A_i\)，其中 \(\hat A_i=\mathrm{Trans}_z(d_i)\mathrm{Trans}_x(a_i)\mathrm{Rot}_x(\alpha_i)\) 是常数。记 \(P_i=\hat A_1\cdots\hat A_i\)，它正是 \(\theta=0\) 时坐标系 \(\{i\}\) 的位姿 \(T_{0i}(0)\)，\(P_0=I\)。\(\mathrm{Rot}_z(\theta)=\Exp(Z\theta)\)，\(Z=(0,0,1;\,0,0,0)\)。利用 5.8 节的 \(T\,\Exp(\xi)\,T^{-1}=\Exp(\operatorname{Ad}_T\xi)\)，把常数因子一个一个挪到右边：

    $$ \begin{aligned} T_{0n}(\boldsymbol\theta) &= \Exp(Z\theta_1)\,\hat A_1\,\Exp(Z\theta_2)\,\hat A_2\cdots\\ &= \Exp(Z\theta_1)\,\bigl[\hat A_1\Exp(Z\theta_2)\hat A_1^{-1}\bigr]\,\hat A_1\hat A_2\,\Exp(Z\theta_3)\cdots\\ &= \Exp(Z\theta_1)\,\Exp(\operatorname{Ad}_{P_1}Z\,\theta_2)\,\Exp(\operatorname{Ad}_{P_2}Z\,\theta_3)\cdots\Exp(\operatorname{Ad}_{P_{n-1}}Z\,\theta_n)\,P_n. \end{aligned} $$

    于是得到一份可以直接写成代码的转换算法：

    $$ \hl{S}{S_i} = \operatorname{Ad}_{P_{i-1}}Z=\begin{bmatrix}\boldsymbol\omega_i\\ -\boldsymbol\omega_i\times\mathbf q_i\end{bmatrix},\quad \boldsymbol\omega_i=R(P_{i-1})\,\hat{\mathbf z},\ \ \mathbf q_i=\mathbf p(P_{i-1}),\qquad \hl{M}{M}=P_n=T_{0n}(\mathbf 0). $$

    用 \(\operatorname{Ad}_{(R,\mathbf p)}(\boldsymbol\omega,\mathbf v)=(R\boldsymbol\omega,\ \mathbf p\times R\boldsymbol\omega+R\mathbf v)\) 验证 \(\mathbf p\times\boldsymbol\omega=-\boldsymbol\omega\times\mathbf p\)，与 5.10 节的 \(\mathbf v=-\boldsymbol\omega\times\mathbf q\) 一致。移动关节则取 \(Z=(0,0,0;\,0,0,1)\)。如果表里有零偏 \(\theta_i=q_i+\delta_i\)，把 \(\mathrm{Rot}_z(\delta_i)\) 并进 \(\hat A_i\) 即可，\(q_i\) 就是编码器读数。改进 DH 的推导完全一样，只是转轴在坐标系 \(\{i\}\) 上而不是 \(\{i-1\}\)：\(S_i=\operatorname{Ad}_{P_i}Z\)，其中 \(P_i\) 此时是改进 DH 里零位时 \(\{i\}\) 的位姿。反过来，从 \(S_i\)、\(M\) 回到 DH 参数要先求相邻两轴的公共法线（FIG 3 的内容）并选定符号约定，结果不唯一。

    第三种语言：URDF

    URDF（ROS 与大多数仿真器使用的机器人描述文件）给每个关节两样东西：从父连杆坐标系到关节坐标系的固定变换 origin xyz rpy，记作 \(O_i\in\SE\)；和一个转轴 axis，单位向量 \(\hat{\mathbf a}_i\)，在关节坐标系里表示。关节转 \(q_i\) 之后，子连杆相对父连杆的位姿是 \(O_i\,\Exp\bigl((\hat{\mathbf a}_i;\mathbf 0)\,q_i\bigr)\)。整条链是

    $$ T(\mathbf q)=O_1\Exp(Z_1q_1)\,O_2\Exp(Z_2q_2)\cdots O_n\Exp(Z_nq_n)\quad\Longrightarrow\quad S_i=\operatorname{Ad}_{O_1\cdots O_i}Z_i,\quad M=O_1\cdots O_n, $$

    其中 \(Z_i=(\hat{\mathbf a}_i;\mathbf 0)\)。这与改进 DH 的结构完全一样（先固定几何，再转关节），只是固定变换用完整的 6 个数 \((x,y,z,r,p,y)\) 而不是靠约定压缩到 4 个，所以不需要任何坐标系放置规则，也没有平行轴的歧义。用同一段推导就能转成 PoE。

    参数个数

    空间中一条直线有 4 个自由度。螺旋轴 \(S=(\boldsymbol\omega,\mathbf v)\) 有 6 个数，但转动关节满足两个约束 \(\|\boldsymbol\omega\|=1\) 和 \(\boldsymbol\omega\cdot\mathbf v=0\)，所以独立的只有 4 个（这就是 Plücker 坐标）。一行 DH 也是 4 个数，其中 \(\theta\) 的零偏同样要在标定里估计。所以对 \(n\) 个转动关节，两种写法的"自由度"相当：前人的经典结论是，带任意工具位姿的最小连续模型需要 \(4n+6\) 个参数（6R 臂是 30 个）。差别不在个数，而在参数化是否连续：螺旋轴随几何光滑变化，DH 在平行/相交处没有定义，在附近则病态。PoE 要额外多记一个 \(M\)，但它同时也确定了关节零位。

    一个乘积，三种分组

    标准 DH：\(\mathrm{Rot}_z\,\mathrm{Trans}_z\,\mathrm{Trans}_x\,\mathrm{Rot}_x\)，每行 4 个数，靠建系约定省掉两个自由度。URDF / 改进 DH：固定变换在前、关节转动在后，URDF 用满 6 个数。PoE：把所有固定变换挪到最右边，关节变成世界系中的螺旋轴。三者算的是同一个 \(T(\boldsymbol\theta)\)；选哪种取决于你手里的资料和你要做的事：读厂商手册用 DH，读仿真和 ROS 用 URDF，推导速度、标定和理论分析用 PoE。

    6机器人中的它：运动学标定

    FK 是整个机器人栈的地基，所有后面的东西（IK、规划、视觉伺服、学习）都假设 \(T(\boldsymbol\theta)\) 是对的。但是 DH 表里的 \(a_i,d_i\) 是设计值。真实机械臂的连杆有加工公差，轴有装配倾斜，减速器和零点有偏差。结果是：重复定位精度可以很好（同一点来回走，误差零点几毫米以内），但绝对定位精度（命令到某个笛卡尔位置，真的到了没有）可能差几毫米甚至更多。精密作业（装配、钻孔、与相机配准）需要运动学标定：用激光跟踪仪或相机测量末端在很多姿态下的真实位置，再反推出每个连杆参数的修正值。

    它是一个最小二乘问题（Gauss–Newton，6.6 节；非线性最小二乘，6.9 节）：未知数是参数修正量 \(\delta\mathbf p\)，残差是"测得的末端位置 − FK 预测"，雅可比是 FK 对参数的导数。FIG 5 做一个最小的版本：你给 UR5 悄悄加入几毫米的制造误差（隐藏的"真机"），先看这点误差怎样变成末端的偏差，再用 20 个测量姿态让 Gauss–Newton 把误差找回来。

    FIG 5运动学误差与标定
    拖动画面旋转视角
    标定
    灰色是设计值（名义）模型，琥珀色是带误差的"真机"。红色箭头从名义工具位置指向真实工具位置，长度放大了 20 倍。"平均/最大误差"在 300 个随机姿态上统计。点"标定"：在 20 个随机姿态上"测量"真机的工具位置（可加噪声），用 Gauss–Newton 辨识四个参数 \((\delta a_2,\delta a_3,\delta d_4,\delta\theta_2)\)；绿色是标定后的模型。
    试一试
    • 只把 \(\delta a_2\) 设成 \(5\) mm（其余为 0）。工具偏差是多少？换几个姿态看看：偏差总是正好 5 mm，为什么？（提示：连杆长度误差只是把它下游的所有坐标系沿 \(x_2\) 平移 \(\delta a_2\)。）对比下一项的零点误差，它的影响随姿态变化。
    • 只给关节 2 一个 \(0.3^\circ\) 的零点误差。末端偏差有多大？（\(0.3^\circ\approx0.0052\) rad，乘以关节 2 到工具的距离，约 \(0.8\) m。）
    • 设定几项误差，点"标定"。残差平方根 (RMS) 经过几次迭代降到多少？加上 \(0.5\) mm 的测量噪声再试一次：标定后的验证误差还能到 0 吗？
    真实系统里

    厂商标定。UR 机械臂出厂时每台都做过运动学标定，修正值（相对标准 DH 的 \(\delta a,\delta d,\delta\alpha,\delta\theta\)）存在控制器里，ROS 的 ur_calibration 可以把它导出来给 URDF 使用。ROS / tf。robot_state_publisher 读 URDF 和 joint_states，每个周期做一遍 FK，把所有连杆的位姿发布成 tf 树。库。Pinocchio、KDL、MoveIt 的 RobotModel 与 Drake 都从 URDF 构造运动树，用"固定变换 × 关节变换"递推 FK，一台 6 自由度臂的 FK 只要大约一微秒量级，所以规划器每秒可以调用百万次。学习。可微分 FK 层（如 PyTorch 的 pytorch_kinematics）把 \(\boldsymbol\theta\mapsto T(\boldsymbol\theta)\) 做成网络里的一层，使末端位姿、关键点（夹爪指尖、肘部）的损失可以直接对关节角求导，用于模仿学习的损失、逆运动学网络和人体到机器人的动作重定向。标定学。PoE 形式的参数在辨识时没有平行轴的奇点，所以被用于运动学标定（Okamura & Park, 1996）。

    标准 DH改进 DHURDFPoE
    每个关节的数据\((\theta,d,a,\alpha)\)\((\alpha_{i-1},a_{i-1},\theta,d)\)\(O_i\)（6 个数）+ 轴向量\(S_i\)（6 个数，4 个自由度）
    需要的坐标系每连杆一个，按规则放每连杆一个，按规则放每连杆一个，随意放只有世界系和 \(M\)
    平行轴\(d\) 自由；邻近病态同左（加 \(\beta\) 修正）无问题无问题
    改工具 / 改基座右 / 左乘一个矩阵同左加一个固定关节 / 改根连杆只改 \(M\) / 所有 \(S_i\) 与 \(M\)
    常见于UR 文档、Spong、Siciliano、很多老代码Franka 文档、CraigROS、Gazebo、Pinocchio、Drake（MuJoCo 的 MJCF 结构类似）Modern Robotics、标定论文
    哪里最方便紧凑的参数表，手算同左，关节坐标系贴着关节带几何、惯量的完整模型雅可比、标定、理论推导

    7常见误区

    误区 1：标准 DH 和改进 DH 混用

    把 Franka 的表（改进 DH）塞进标准 DH 的公式，程序照样跑，输出的也是一个合法的 \(4\times4\) 位姿，只是不对。两者的差别有两层：乘法顺序（\(\mathrm{Rot}_z\mathrm{Trans}_z\mathrm{Trans}_x\mathrm{Rot}_x\) 对 \(\mathrm{Rot}_x\mathrm{Trans}_x\mathrm{Rot}_z\mathrm{Trans}_z\)）和 下标（\(a_i\) 对 \(a_{i-1}\)）。不会有任何异常，旋转矩阵仍然正交，位置的量级也合理，所以不容易一眼看出来。防范办法：先问文档用的是哪种约定；然后在至少两个非零位形下，和厂商给出的 FK 值（或仿真器）对一遍。

    误区 2：零位、零偏和基座系因厂商而异

    UR5 的 \(\boldsymbol\theta=0\) 是手臂向 \(-x\) 方向水平伸直；有的机器人把"直立"当零位；很多 DH 表里写着 \(\theta_2=q_2-90^\circ\) 这类零偏，需要并进 \(\hat A_i\)（第 5 节）。基座系也不统一：ROS 的 ur_description 里 base_link 与 UR 控制器文档里的 base 差一个绕 \(z\) 的 \(\pi\) 旋转，两边的 FK 位置 \(x,y\) 正好差一个符号。症状：手算对了，放到机器人上镜像或偏转 \(180^\circ\)。

    误区 3：\(\alpha\) 的符号、度与弧度

    \(\alpha_i\) 的方向是绕 \(x_i\) 用右手定则从 \(z_{i-1}\) 转到 \(z_i\)。\(\alpha=+90^\circ\) 和 \(-90^\circ\) 让 \(z_i\) 指向相反的方向，是两台不同的手臂。UR5 的表里 \(\alpha_4=+\pi/2\) 而 \(\alpha_5=-\pi/2\)，抄错一个符号，腕部就扭向另一边。另外，UR 的表是弧度；把 90 当弧度传给 sin/cos，得到的是一个完全不相关的角（\(90\) rad \(\approx 5157^\circ\)，去掉整圈后约 \(117^\circ\)）：程序不会报错，只是 FK 全错。FIG 1 里把 \(\alpha\) 从 \(+90^\circ\) 翻成 \(-90^\circ\)，看 \(z_i\) 指向哪个方向。

    误区 4：乘法顺序，DH 向右、PoE 向左

    DH 的因子是"相对前一个坐标系"，T = A1 @ A2 @ ... @ A6；PoE 空间形式的因子是"相对世界系"，T = E1 @ E2 @ ... @ E6 @ M，其中 \(M\) 在最右边。把 DH 的 \(\hat A_i\) 转成 \(S_i\) 时漏掉共轭，或者把 \(M\) 放到左边，在 \(\boldsymbol\theta=0\) 处都对（所有 \(\Exp\) 是单位阵），非零位形才露馅（5.10 节误区 2）。所以任何 FK 实现都要有一个非零位形的单元测试，FIG 2 读数里的两者之差就是这样的测试。

    误区 5：把 DH 参数当成物理测量量

    UR5 的 \(a_2=-0.425\) 为负，并不是"连杆长度是负的"，只是 \(x_2\) 朝了相反的方向；平行轴之间的 \(d\) 可以随便挪动；近平行时 \(d\) 是一个大数（FIG 3）。这些数是建系约定的产物，不是你能用尺子量的东西。做标定时，用普通 DH 去估计近平行轴的参数会病态（Hayati & Mirmirani 提出过给平行轴多加一个绕 \(y\) 的转角 \(\beta\)）；PoE 或 URDF 的参数没有这个问题。

    8小练习

    1. 标准 DH 的一个连杆：\(\theta=90^\circ\)，\(d=0.2\)，\(a=0.3\)，\(\alpha=0\)。坐标系 \(\{i\}\) 的原点在 \(\{i-1\}\) 里的位置是？
    最后一列是 \((a\cos\theta,\ a\sin\theta,\ d)=(0.3\cos90^\circ,\ 0.3\sin90^\circ,\ 0.2)=(0,\,0.3,\,0.2)\)。几何上：先沿 \(z\) 升 0.2，\(x\) 轴已经转到了 \(+y\)，再沿它走 0.3。选项 A 忘了 \(\theta\)，B 把转向搞反，D 把 \(a\) 和 \(d\) 搞混。
    2. 用标准 DH 表写 PoE 时，第 \(i\) 个关节的螺旋轴方向 \(\boldsymbol\omega_i\)（零位）是？
    标准 DH 约定关节 \(i\) 的转轴是 \(z_{i-1}\)。选项 A 是改进 DH 的约定（那里 \(S_i=\operatorname{Ad}_{P_i}Z\)），这正是两者下标错位的来源；D 只对关节 1 成立。
    3. 平面 3R 臂，标准 DH：所有 \(d=0\)、\(\alpha=0\)，\(a_1=1\)、\(a_2=1\)、\(a_3=0.5\)。\(\boldsymbol\theta=(90^\circ,\,-90^\circ,\,0)\) 时，工具原点（在 \(xy\) 平面内）在？
    平面链中各连杆的绝对方向是 \(\theta\) 的累加：\(90^\circ\)、\(90^\circ-90^\circ=0^\circ\)、\(0^\circ\)。位置 \(=a_1(\cos90^\circ,\sin90^\circ)+a_2(\cos0^\circ,\sin0^\circ)+a_3(\cos0^\circ,\sin0^\circ)=(0,1)+(1,0)+(0.5,0)=(1.5,\,1)\)。
    4. 关于两根关节轴"差一点平行"，下面哪一句是对的？
    公共法线的落脚点随倾角 \(\varepsilon\) 以约 \(1/\varepsilon\) 飞走，DH 的 \(d\) 随之发散，而相邻连杆的参数会取大数补偿，所以 FK 本身没事（C 错）。\(S=(\boldsymbol\omega,-\boldsymbol\omega\times\mathbf q)\) 是 \(\boldsymbol\omega\) 和 \(\mathbf q\) 的光滑函数（B 错）。
    5. 给一台 PoE 描述的机械臂换一个夹爪，TCP 相对法兰的固定位姿为 \(T_{\rm tool}\)。应该怎么改？
    工具固定在法兰上，在法兰系里偏移，所以右乘：\(T=\Exp(S_1\theta_1)\cdots\Exp(S_6\theta_6)\,[M\,T_{\rm tool}]\)。关节轴没动。选项 C 的左乘会让工具偏移相对世界系而不是法兰系，那是改基座一类的操作；A 把工具的操作当作了基座的操作。

    9延伸阅读

    • J. Denavit & R. S. Hartenberg, "A kinematic notation for lower-pair mechanisms based on matrices", ASME Journal of Applied Mechanics 22(2):215–221, 1955 — DH 参数的原始论文
    • J. J. Craig, Introduction to Robotics: Mechanics and Control, Ch. 3 — 改进 DH 的约定，建系步骤有大量例题
    • K. Lynch & F. Park, Modern Robotics, Ch. 4 (Forward Kinematics) and Appendix C — PoE、URDF，以及 DH 参数与 PoE 的关系
    • M. W. Spong, S. Hutchinson, M. Vidyasagar, Robot Modeling and Control, Ch. 3 — 标准 DH 的建系规则（本课的约定）
    • B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling, Planning and Control, Ch. 2 — 运动学与 DH 的完整处理
    • S. Hayati & M. Mirmirani, "Improving the absolute positioning accuracy of robot manipulators", Journal of Robotic Systems 2(4):397–413, 1985 — 指出平行轴会让 DH 标定病态，并提出多一个转角的修正
    • F. C. Park, "Computational aspects of the product-of-exponentials formula for robot manipulators", IEEE Transactions on Automatic Control 39(3):643–647, 1994 — PoE 在 FK 与雅可比计算上的效率
    • A. M. Okamura & F. C. Park, "Kinematic calibration using the product of exponentials formula", Robotica 14(4):415–421, 1996 — 用 PoE 做标定
    • R. Murray, Z. Li, S. Sastry, A Mathematical Introduction to Robotic Manipulation, Ch. 3 — 螺旋理论下的正运动学