状态：草稿（2026-10-18），实测。 数据直接在笔记本电脑（Apple M1 Max，macOS 26.5）上通过 Python 3.9.6 及 numpy 2.0.2 计算得出。 时间为 7 次重复的中间值（舍弃首次）。原始数据与代码：工作目录
measure/robot-arm-basics/的arm.py,m03_fk.py,out/m03_fk.json。示例臂为虚拟设计，无实际机器人实测值（未进行硬件实测）。参考资料查阅于 2026-10-18。 撰写规则：以实测数据为基础，未能测得的标为“未实测”，无法确认的标为“未确认”。
目录
1. 直觉：从简单的方向开始
只要知道手臂的所有关节角度，球拍的位置就会唯一确定。从肩膀到手肘比划一把尺子，再从手肘到手腕比划一把尺子， 不断重复这个过程即可。这就是正运动学（forward kinematics）。答案始终唯一且计算简便。
相反的方向，即“为了把球拍放在那个位置、以那个角度放置，关节应该转动多少度”，则可能存在多个答案或无解，因而难得多。这就是第 4 篇中的 逆运动学。必须先准确建立正运动学，才能校验逆运动学的答案。
2. 图示：本系列的示例臂
示例臂的结构类似于人手臂。
- 第 1 个关节是绕垂直轴旋转的肩偏航（Yaw），第 2、3 个是同向平行的肩俯仰（Pitch）与肘俯仰。这三个关节决定球拍的 位置。
- 第 4、5、6 个是三轴交于一点的球面手腕（spherical wrist）。这三个关节决定球拍的 方向。三轴交汇的点称为手腕中心。
- 长度分别为 d1 = 0.30, a2 = 0.40, d4 = 0.35, d6 = 0.15 m。d6 是从手腕中心到球拍面中心的距离。手臂伸直时，从肩膀到手腕中心为 0.75 m。
在开源硬件方面，放弃该结构而采用 在 2 轴正交龙门架上搭载 2~3 轴手腕的设计也是一个强有力的候选方案。Google DeepMind 的系统也将 6 自由度工业机械臂安装在了 2 轴线性龙门架上（arXiv:2408.03906 II-A 节）。由于龙门架的正运动学和逆运动学几乎只涉及加法，我们将在第 4 篇中单独计算其优势。
3. 计算
3.1 双连杆平面臂
连杆长度为 L1、L2，关节角度为 q1、q2 的平面臂末端点如下。
x = L1·cos q1 + L2·cos(q1 + q2)
y = L1·sin q1 + L2·sin(q1 + q2)
该公式将成为第 4 篇逆运动学、第 5 篇雅可比矩阵以及第 9 篇动力学的共同出发点。示例臂中由第 2、3 个关节构成的平面问题正是如此。
3.2 DH 参数
如果关节有六个，就需要依次连接坐标系六次。DH（Denavit–Hartenberg）惯例仅需为每根连杆提供四个数字即可定义下一个坐标系。
| 符号 | 含义 |
|---|---|
| θ_i | 绕 z 轴旋转的角度（若是旋转关节，这就是关节变量） |
| d_i | 沿 z 轴方向的平移 |
| a_i | 沿新 x 轴方向的平移（连杆长度） |
| α_i | 绕新 x 轴旋转的角度（下一个关节轴的扭转） |
本文采用“绕 z 轴旋转 θ 并平移 d，再沿 x 轴平移 a 并旋转 α”的顺序（通常称为标准 DH）。根据教科书的不同，也有顺序和下标各异的修正 DH。Modern Robotics 的附录 C 定义的顺序为 α_{i−1}, a_{i−1}, d_i, φ_i。即使是相同的机械臂，如果惯例不同，表中的数字也会不同，因此 如果从他人处获取表格，必须首先确认其所用的惯例。
def dh_T(a, alpha, d, th): # 표준 DH: Rz(θ)·Tz(d)·Tx(a)·Rx(α)
ct, st, ca, sa = cos(th), sin(th), cos(alpha), sin(alpha)
return np.array([[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d ],
[0, 0, 0, 1.0 ]])
def fk_dh(q):
T = np.eye(4)
for (a, alpha, d), qi in zip(DH, q):
T = T @ dh_T(a, alpha, d, qi)
return T # 라켓 좌표계 {E} 를 로봇 바닥 {B} 에서 본 4×4
3.3 指数积（PoE）
DH 需要为每根连杆指定坐标系，且指定规则较为繁琐。指数积（product of exponentials, PoE）公式仅使用两个坐标系：基座坐标系以及所有关节为 0 时的球拍位姿 M。并且，它将每一个关节视为“绕某一轴旋转的螺旋运动（screw motion）”。
T(q) = e^[S1]q1 · e^[S2]q2 · … · e^[S6]q6 · M
S_i = (ω_i, v_i), ω_i = 관절 축 방향 단위 벡터, v_i = −ω_i × p_i (p_i 는 축 위의 아무 점)
所有的 S_i 和 M 都可以 仅凭零位时的单一状态 写出。这就是 Modern Robotics 第 4 章的空间形式 PoE 公式（4.14），该书也将不需要连杆坐标系作为其优点。指数矩阵具有罗德里格斯公式形式的闭式解。
def exp6(S, th):
w, v = S[:3], S[3:]
W = skew(w)
R = np.eye(3) + sin(th)*W + (1 - cos(th))*W@W
G = np.eye(3)*th + (1 - cos(th))*W + (th - sin(th))*W@W
T = np.eye(4); T[:3, :3] = R; T[:3, 3] = G @ v
return T
本系列的代码并非手写 S_i 和 M，而是通过 DH 表计算零位坐标系，并从中提取各关节的 z 轴和原点。这样便能直接校验这两种方法是否表示同一个机械臂。
3.4 球面手腕与手腕中心
如果手腕的三个轴交于一点，就会带来便利。无论第 4、5、6 个关节如何转动， 手腕中心的位置都不会改变。 手腕中心仅由第 1、2、3 个关节决定，从球拍位姿反推手腕中心也很简单。
손목 중심 = 라켓 중심 − d6 · (라켓 법선 방향, 곧 R 의 셋째 열)
第 4 篇将利用这一特性，将 6 自由度逆运动学拆分为“3 个位置”和“3 个方向”并通过闭式解求解。
4. 实测
4.1 DH 与 PoE 是否得出相同答案
在 −π 到 π 之间随机抽取 10,000 次关节角度，并逐元素比较两个函数的 4×4 结果矩阵。
| 项目 | 值 |
|---|---|
| 最大元素差 | 5.6×10⁻¹⁶ |
| 零位 M（位置） | (0.40, 0, −0.20) m，球拍法向量向下（−z） |
误差处于 float64 的舍入水平。在零位时球拍位于地面以下（−0.20 m），是因为肘部角度为 0 时下臂呈垂直下垂姿态。在实际机器人中，将这个“0 的含义”与编码器零点对齐是校准的第一步（4.3 节）。
4.2 计算时间
| 计算 | 单次耗时（µs） |
|---|---|
| 双连杆平面闭式解（纯 Python） | 0.41 |
| 6 自由度 DH（numpy 4×4 乘法 6 次） | 15.2 |
| 6 自由度 PoE（numpy，6 个指数矩阵） | 71.7 |
| 6 自由度雅可比矩阵（第 5 篇，在 DH 坐标系中） | 102.6 |
PoE 比 DH 慢约 4.7 倍。这是因为除了各关节的正弦和余弦外，还需要进行多次 3×3 矩阵乘法和加法。1 kHz 的控制周期为 1,000 µs，因此即使在 Python 中，两者也都足够快。我们认为，当移植到微控制器时，像 DH 那样预先展开的公式会更具优势（未进行硬件实测）。
4.3 微小误差在球拍末端会放大为多少
即使正运动学准确，如果手臂的实际尺寸和编码器存在偏差也无济于事。我们引入了四种误差，在 5,000 个随机姿态下测量了球拍中心位置和球拍法向量方向的偏差程度。
| 误差原因 | 位置误差 中位值 / 95% 分位值 | 法向量方向误差 中位值 / 95% 分位值 |
|---|---|---|
| 连杆长度 ±1 mm（均匀分布，各关节独立） | 0.98 / 1.94 mm | 0° |
| 关节零点 ±0.1° | 0.62 / 1.43 mm | 0.09 / 0.19° |
| 12 位编码器（4,096 格/转）量化 | 0.27 / 0.65 mm | 0.04 / 0.08° |
| 14 位编码器（16,384 格/转）量化 | 0.07 / 0.16 mm | 0.01 / 0.02° |
- 连杆长度误差只会改变位置而不会改变方向，因为长度不会改变角度。
- 关节零点 0.1° 在位置上约为 1 mm 左右，但在 方向上为 0.1~0.2° 。在第 7 篇中测得，如果球拍法向量倾斜 1°，球的落点将产生 6 cm 的中位偏差。仅零点误差就会导致损失大约 1 cm 的精度。
- 若使用 14 位编码器，量化误差可以忽略不计。12 位编码器的位置精度虽然足够，但 0.04° 的方向误差会成为累积误差的一部分。
5. 局限性
- 我们假设关节和连杆为刚体。皮带及减速机的背隙与弹性、连杆的弯曲属于正运动学之外的误差，本次未进行测量（未进行硬件实测）。
- 误差以相互独立的均匀分布加入。实际的校准误差可能相互交织，其大小可能会有所不同。
- 由于无法查阅正文，未能确认 Craig 和 Siciliano 之外的教科书所使用的 DH 惯例（未确认）。
6. 总结
- 正运动学是从关节角度推导球拍位姿的顺畅方向，也是对第 4、5、9 篇所有计算进行校验的工具。
- DH 用每根连杆四个数字，PoE 用一个零位状态和六个关节轴来表示同一个机械臂。两种方法在 10⁻¹⁶ 的量级上保持一致。
- 球面手腕通过固定手腕中心，使得拆分逆运动学成为可能。
- 从硬件角度来看，相比于编码器分辨率， 关节零点校准对球拍方向误差的影响更大。
乒乓小结： 比划六次尺子就能得出球拍位置。但是，如果尺子的零点哪怕只偏了 0.1°，球就会偏离目标约 1 cm，所以一定要先对准零点！
参考资料
所有资料查阅于 2026-10-18。
- Lynch, Park, Modern Robotics, 2017, 公开预印本，第 4 章正运动学（4.1 PoE，式 4.14），附录 C（DH 参数）。
- D'Ambrosio 等, Achieving Human Level Competitive Robot Table Tennis, arXiv:2408.03906, II-A 节（手臂与龙门架配置）。
- Craig, Introduction to Robotics: Mechanics and Control 第 4 版（Pearson）, Siciliano 等, Robotics: Modelling, Planning and Control（Springer 2009）：仅确认出版社章节目录，正文未查阅。