正运动学：DH 参数与 PoEForward Kinematics: DH vs. PoE

控制器每个周期要知道末端在哪里；逆运动学要反复调用它；碰撞检测要把每一节连杆放进世界；仿真器、可视化、手眼标定都先问同一个问题：给定 \(\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）。

A controller needs to know where the end effector is every cycle; inverse kinematics calls FK over and over; collision checking places every link in the world; simulators, visualisation and hand-eye calibration all start with the same question: given \(\boldsymbol\theta\), where is each frame? That is forward kinematics (FK). Lesson 5.10 already gave a clean answer, the product of exponentials \(T(\boldsymbol\theta)=\Exp(S_1\theta_1)\cdots\Exp(S_n\theta_n)M\).

But the documents you receive in practice are often in another form. Universal Robots and Franka publish DH parameter tables in their official documentation; the textbooks (Craig, Spong, Siciliano), a lot of legacy code and many calibration tools speak DH too. Modern libraries and simulators such as Pinocchio, Drake and Gazebo, on the other hand, read URDF, which is a third way of writing the same thing. To work in robotics you must read all three and know how to translate between them.

The route of this lesson: first take one DH link apart into four moves (FIG 1); then multiply out a real UR5 once with DH and once with PoE, watch the results agree to \(10^{-15}\), and learn to write \(S_i\) and \(M\) mechanically from a DH table (FIG 2); then see why DH misbehaves when joint axes become parallel or intersect (FIG 3); then change base and tool frames (FIG 4); finally see how a millimetre of parameter error in FK becomes several millimetres at the tool, and how robots are calibrated (FIG 5).

一般来说，两个坐标系之间的位姿有 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：同样四个数，顺序换了，得到另一个矩阵。

In general the pose between two frames has six degrees of freedom. The idea of DH is: since we are free to choose the frames, choose them cleverly so that two neighbouring frames differ by only four numbers. The recipe has three rules: the axis of joint \(i\) is taken as \(z_{i-1}\); \(x_i\) is perpendicular to \(z_{i-1}\); and \(x_i\) intersects \(z_{i-1}\). Under these perpendicularity and intersection constraints, getting from \(\{i-1\}\) to \(\{i\}\) takes exactly four moves, each one about or along an axis of the frame you have just arrived at:

1. rotate about \(z_{i-1}\) by \(\theta_i\);
2. slide along \(z_{i-1}\) by \(d_i\);
3. slide along the new \(x\) axis (that is \(x_i\), the common normal) by \(a_i\);
4. rotate about \(x_i\) by \(\alpha_i\), twisting \(z_{i-1}\) into \(z_i\).

\(\theta_i\) (revolute joint) or \(d_i\) (prismatic joint) is the joint variable; the other three numbers are geometric constants of the link. The lab below lets you set the four numbers and play the moves one by one. The dashed purple lines are the two joint axes: the first through the origin of \(\{i-1\}\) along \(z_{i-1}\), the second through the origin of \(\{i\}\) along \(z_i\). Red segments are the slide \(a\) along \(x\), blue segments the slide \(d\) along \(z\). The switch at the top changes to Craig's modified DH: the same four numbers in a different order, which gives a different matrix.

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

The UR5 is one of the most common collaborative arms, and its standard DH parameters are published by Universal Robots (metres and radians):

每一行的 \(\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\) 矩阵之差的范数。

The \(\theta_i\) in each row is the angle of joint \(i\) (zero offset 0). Multiply the six \(A_i(\theta_i)\) in order and you get the pose of the flange (frame 6) in the base. That is the textbook recipe, and what the UR documentation hands you.

PoE, however, asks for the six screw axes \(S_i\) at home and the home end-effector pose \(M\). What now? The answer is hidden in the construction of DH: the axis of joint \(i\) is \(z_{i-1}\), so once you compute the frame poses at \(\theta=0\), the direction of axis \(i\) is the \(z\) axis of frame \(\{i-1\}\) and a point on it is that frame's origin: \(\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)\), and \(M\) is simply frame 6 at home. Section 5 proves that this is exact. FIG 2 computes the same arm both ways; the readout shows the norm of the difference between the two \(4\times4\) matrices.

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\) 的各个分量。

The four DH parameters are determined by the common normal between two consecutive joint axes. Two lines in space generally have one common normal, but there are two special cases:

• Parallel axes: there are infinitely many common normals (slide along the axes), so \(d_i\) has no unique value; it is a free parameter. The usual habit is to pick the position that zeroes some other parameter (joints 2, 3, 4 of the UR5 are mutually parallel, so \(d_2=d_3=0\)).
• Intersecting axes: the common normal has length \(a_i=0\) and a definite position (through the intersection), but its direction is only fixed up to the normal of the plane spanned by the two axes, and both signs are allowed: this is a discrete sign convention, and flipping it flips the sign of \(\alpha_i\) and shifts \(\theta\) by \(\pi\).

Worse is "almost parallel". A real arm is never exactly parallel: assembly error tilts the axes by a fraction of a degree. The common normal is then unique, but its foot flies far away: \(d_i\) becomes 10 m, 100 m, and \(d_{i+1}\) is forced to a large value to compensate. The product is still right, but these two parameters have lost their physical meaning and can no longer be identified stably. FIG 3 tilts one axis slowly towards parallel and plots both the DH parameters and the components of the screw axis \(S\).

现实中的机械臂总有两个"额外"的坐标系：基座系（机械臂装在工作台上，相对世界系 \(\{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 同时用两种方式算，并显示哪些量变了。

A real arm always has two "extra" frames: the base frame (the arm is bolted to a table, at a pose \(T_{WB}\) relative to the world frame \(\{W\}\)) and the tool frame (a gripper is mounted on the flange, and the tool centre point TCP has a fixed pose \(T_{\rm tool}\) relative to it). When you swap the gripper or move the arm, how must FK change?

The two languages do it differently, and this is where PoE is convenient: changing the tool touches only \(M\) (\(M\mapsto M\,T_{\rm tool}\)) and every \(S_i\) stays as it is; changing the base touches every \(S_i\) (\(S_i\mapsto\operatorname{Ad}_{T_{WB}}S_i\), \(M\mapsto T_{WB}M\)). In DH you multiply one more fixed matrix on the left or on the right of the chain and leave the \(A_i\) alone. FIG 4 computes both ways and shows what changed.

三条规则（轴 \(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\) 的角。把四步按"每一步都在当前坐标系里"的规则（右乘）连起来：

With the three rules (axis \(z_{i-1}\) is the axis of joint \(i\); \(x_i\perp z_{i-1}\); \(x_i\) intersects \(z_{i-1}\)) in place, the parameters mean: \(\theta_i\) is the angle about \(z_{i-1}\) from \(x_{i-1}\) to \(x_i\); \(d_i\) is the distance along \(z_{i-1}\) from the origin of \(\{i-1\}\) to the point where \(x_i\) meets \(z_{i-1}\); \(a_i\) is the distance along \(x_i\) from that point to the origin of \(\{i\}\), the length of the common normal between the two axes; and \(\alpha_i\) is the angle about \(x_i\) from \(z_{i-1}\) to \(z_i\). Chaining the four moves with the rule "each move is in the current frame" (right multiplication) gives

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

Craig's version puts frame \(\{i\}\) on the axis of joint \(i\) (\(z_i\) is the axis of joint \(i\)) and puts the link parameters \(a_{i-1},\alpha_{i-1}\) before the joint:

把标准 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)\)，把常数因子一个一个挪到右边：

Split each standard-DH factor into "joint rotation × fixed geometry": \(A_i(\theta_i)=\mathrm{Rot}_z(\theta_i)\,\hat A_i\), where \(\hat A_i=\mathrm{Trans}_z(d_i)\mathrm{Trans}_x(a_i)\mathrm{Rot}_x(\alpha_i)\) is constant. Let \(P_i=\hat A_1\cdots\hat A_i\); this is exactly the pose \(T_{0i}(0)\) of frame \(\{i\}\) at \(\theta=0\), with \(P_0=I\). Also \(\mathrm{Rot}_z(\theta)=\Exp(Z\theta)\) with \(Z=(0,0,1;\,0,0,0)\). Using \(T\,\Exp(\xi)\,T^{-1}=\Exp(\operatorname{Ad}_T\xi)\) from Lesson 5.8, move the constant factors to the right one at a time:

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)\)。整条链是

URDF (the robot description file used by ROS and most simulators) gives each joint two things: a fixed transform from the parent link frame to the joint frame, origin xyz rpy, call it \(O_i\in\SE\); and an axis, a unit vector \(\hat{\mathbf a}_i\) expressed in the joint frame. After the joint turns by \(q_i\) the child link sits at \(O_i\,\Exp\bigl((\hat{\mathbf a}_i;\mathbf 0)\,q_i\bigr)\) relative to the parent. The whole chain is

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 把误差找回来。

FK is the foundation of the whole robot stack: IK, planning, visual servoing and learning all assume that \(T(\boldsymbol\theta)\) is right. But the \(a_i,d_i\) in a DH table are design values. A real arm has machining tolerances in its links, tilt in the assembly of its axes, and errors in its gearboxes and zeros. The result: repeatability can be excellent (going back to the same point is accurate to a fraction of a millimetre), yet absolute accuracy (commanding a Cartesian position and actually arriving there) can be off by several millimetres or more. Precision tasks (assembly, drilling, registration to a camera) need kinematic calibration: measure the true end-effector position in many postures with a laser tracker or a camera, then solve for corrections to each link parameter.

This is a least-squares problem (Gauss–Newton, Lesson 6.6; nonlinear least squares, Lesson 6.9): the unknowns are the parameter corrections \(\delta\mathbf p\), the residuals are "measured tool position − FK prediction", and the Jacobian is the derivative of FK with respect to the parameters. FIG 5 is a minimal version: you secretly add a few millimetres of manufacturing error to the UR5 (the hidden "real robot"), see how this error turns into tool deviation, and then let Gauss–Newton recover the errors from 20 measured postures.