2.1 机器人运动学：从关节角到末端位姿
2.1 机器人运动学：从关节角到末端位姿
本节摘要：运动学只关心几何，不问力。正向运动学把关节角翻译成末端位姿，DH 参数是标准记法；逆运动学反向求解，会多解、会无解、还会撞奇异。本节给出 DH 建模全流程与一段可运行的正解代码。
「正向运动学」（Forward Kinematics）这个词第一次出现在课程表上时，多数人的反应是"不就是乘矩阵吗"。真正动手写示教程序才会发现它难在哪：六个关节角各自绕不同的轴，轴的方向又取决于前面所有关节的状态，矩阵一多，轴的归属就乱了。DH 参数法的价值正在于此——它用四个参数把任意关节的位姿变换标准化，让"哪根轴、转多少"变成填表题。
正运动学：DH 参数把混乱变成表格
Denavit-Hartenberg 约定给每个连杆配四个量：连杆长度 a、扭角 α、连杆偏距 d、关节角 θ。旋转关节的 θ 是变量，其余三个是结构常量；移动关节反过来。每个关节对应一个 4×4 齐次变换，连乘即末端位姿。
以平面三连杆臂（三旋转关节，z 轴全部竖直向上）为例，DH 表简单到极致：
| 关节 | θ | d | a | α |
|---|---|---|---|---|
| 1 | θ1 | 0 | L1 | 0 |
| 2 | θ2 | 0 | L2 | 0 |
| 3 | θ3 | 0 | L3 | 0 |
空间臂（比如带竖直腰部旋转 + 两个俯仰关节）的 α 列才会出现 ±90 度。建模时最常见的错误是坐标系原点没落在"公垂线与轴的交点"上，导致 d、a 列全是补丁值——DH 表长得越别扭，越要回头查坐标系。
import numpy as np def dh_matrix(theta, d, a, alpha): """单个关节的 DH 齐次变换""" ct, st = np.cos(theta), np.sin(theta) ca, sa = np.cos(alpha), np.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 ]]) # 三连杆平面臂，L1=0.3, L2=0.25, L3=0.1 米 L = [0.3, 0.25, 0.1] thetas = np.radians([30, -45, 20]) # 给定关节角 T = np.eye(4) for th, Li in zip(thetas, L): T = T @ dh_matrix(th, 0, Li, 0) print(T[:3, 3]) # 末端位置 # 输出: [0.41199 0.29868 0.] print(T[:3, :3]) # 末端姿态（此处为绕 z 转 5 度）
末端姿态是三段旋转的叠加：30 − 45 + 20 = 5 度。平面臂的姿态只有这一个自由度，这正是它逆运动学可以用几何法解的原因。
图：正运动学与逆运动学的双向关系
逆运动学：多解、无解与奇异
逆运动学是给定末端位姿反解关节角。平面两连杆臂可以完全手推，也是理解多解的最小模型。令目标点与基座距离为 r，由余弦定理：
cos θ2 = (r² − L1² − L2²) / (2·L1·L2)
根号前正负两个符号，对应"肘向上"与"肘向下"两支解——同一个目标点，臂可以撅着肘够，也可以压着肘够。r > L1 + L2 时根号内为负，无解：目标在工作空间之外。这两条几何事实在六轴臂上原样放大：腕部三个关节交于一点的标准构型可以把位置与姿态解耦，位置用几何法、姿态用欧拉角反解，但手肘与腕部的"翻转"组合能给出八支解；车间布线、管缆走向决定你只能用其中一支。
奇异是逆解的第三道坎。雅可比矩阵 J 把关节角速度映射为末端速度；J 不满秩时，某些方向的末端速度无论关节怎么转都产生不了。平面两连杆臂在 θ2 = 0（臂完全伸直）时奇异：此时末端只能沿臂方向退回，垂直方向的瞬时速度为零。数值法若不加防护，在奇异点附近求 J 的逆会让关节速度指令飙到天文数字。工业控制器的通行做法是限流限速加阻尼最小二乘（DLS），用 λ²I 项给 JᵀJ 加垫：
def ik_step_dls(x_current, x_target, J, lam=0.05): """带阻尼最小二乘的一步逆解迭代""" e = x_target - x_current # 末端误差 JT = J.T dq = JT @ np.linalg.solve(J @ JT + lam**2 * np.eye(6), e) return dq # 关节增量，代替 J^-1 @ e
数值法每次迭代算一次 J 与一步增量，工程上用得最多；它的软肋是对初值敏感——初值落在另一支解的吸引域里，最后收敛到的支就不同，机械臂会在半空突然换肘。所以轨迹级的逆解通常"以上一时刻的解作初值"，保证解的支别连续。
本节要点
- DH 四参数把关节位姿变换标准化，DH 表是正运动学的"源代码"，建模错多数错在坐标系原点。
- 正解唯一且廉价；逆解多解、可能无解，解的支别由肘部与腕部构型决定。
- 奇异的本质是雅可比降秩：某些方向末端瞬时不可动，数值逆解必须加阻尼防护。
- 轨迹级逆解用上一时刻的解作初值，防止机械臂在两支解之间跳变。
工作空间：正运动学的包络
把所有可达关节角的正运动学解画出来，得到工作空间——机械臂"够得着的范围"。它有两个值得记住的层次：可达工作空间（末端能以任意姿态到达）与灵活工作空间（末端能以全部姿态到达）。前者像锅底，后者像锅底里的小锅。工程布置工位时，抓取点应尽量放进灵活空间内部：贴着可达空间边缘的抓取，姿态选择余地趋零，奇异性风险陡增。工作空间还由杆长比决定：等比放大所有杆长只放大包络不改变形状，而把大臂缩短、小臂加长，会在损失外圈范围的同时改善近身灵活性——布局优化就是在这类取舍里做的。
一个 DH 建模的自测练习
取一支笔和一张纸，为"腰部旋转加肩部俯仰加肘部俯仰"的三关节空间臂建 DH 表。提示：腰轴竖直，肩轴水平，两者异面垂直，α 列会出现一个 90 度；肘轴与肩轴平行。建完后用 2.1 节的代码函数连乘，给定三个角度验证末端坐标是否符合几何直觉（腰转 0 度、肩肘各 90 度时，末端应在最远处水平伸出）。这个练习能暴露绝大多数建模坑：坐标系原点没落在轴交点上、公垂线方向搞反、z 轴没有沿转动轴。DH 表错一处，后面全错——所以建表时多花五分钟画坐标系草图，永远比调试半天划算。
问题：为什么六轴臂腕部三个轴要交于一点
这是被几十年工程验证过的最优构型。三轴交于一点（球腕）后，末端姿态只由最后三个关节决定，位置只由前三个关节决定——逆解因此解耦：先用位置解前三角（几何法），再解姿态（欧拉角反算），每个子问题都很简单。若三轴不交于一点，位置与姿态耦合，六维非线性方程必须整体数值求解，速度与可靠性都吃亏。球腕的代价是腕部机构紧凑、走线困难——工程上没有免费的解耦。