跳到正文
孔乙己

输入关键词开始搜索…

↑↓选择
Esc

机械臂运动学：齐次变换、DH 参数与几何雅可比

从齐次变换与 DH 建模讲到几何雅可比的逐列构造与奇异性分析，给出机械臂运动学正问题与速度级的完整推导。

机器人,建模与标定3分钟阅读

运动学是机械臂软件栈里最底层的一块地基：正解回答“关节角是这样时末端在哪”，逆解回答“想让末端到那里关节角该是多少”，雅可比回答“关节速度和末端速度怎么换算”。规划器、笛卡尔伺服、力控、示教再现，全都建立在这三个问题的答案之上。

本文按依赖顺序推导前两块半：先把位姿和齐次变换说清楚，再用 DH 参数把串联机构参数化，然后构造雅可比并讨论奇异性。逆运动学本身是另一个体量的问题，只在这里给出结论，完整推导见配套的那篇。

刚体在空间中的位姿由位置 p∈R3\mathbf p \in \mathbb R^3 和姿态 R∈SO(3)\mathbf R \in SO(3) 组成，合写成齐次变换矩阵：

T=[Rp0⊤1]∈SE(3)\mathbf T = \begin{bmatrix} \mathbf R & \mathbf p \ \mathbf 0^\top & 1 \end{bmatrix} \in SE(3)

它的好处是复合就是矩阵乘法：坐标系 {B}\{B\} 相对 {A}\{A\}、{C}\{C\} 相对 {B}\{B\} 的位姿已知时，

ATC=ATB  BTC{}^A\mathbf T_C = {}^A\mathbf T_B ; {}^B\mathbf T_C

串联机械臂正是一串这样的复合。逆变换也有闭式：

T−1=[R⊤−R⊤p0⊤1]\mathbf T^{-1} = \begin{bmatrix} \mathbf R^\top & -\mathbf R^\top \mathbf p \ \mathbf 0^\top & 1 \end{bmatrix}

姿态的参数化（欧拉角、轴角、四元数）各有取舍：欧拉角直观但有万向锁；四元数无奇异、插值方便（slerp），是运动规划中的默认选择；旋转矩阵则用于一切需要复合和求导的推导。工程上常见的错误几乎都发生在约定不清：内旋还是外旋、四元数 wxyz 还是 xyzw、角度还是弧度——这些约定错了，后面全错。

串联机构相邻连杆之间的变换有 6 个自由度，但 Denavit 与 Hartenberg 发现：只要坐标系按规则架设（z 轴取关节轴，x 轴取相邻两关节轴的公垂线），相邻连杆变换只需要 4 个参数。

采用修正 DH（Modified DH，Craig 版约定），从坐标系 {i−1}\{i-1\} 到 {i}\{i\} 的变换为

i−1Ti=Rx(αi−1) Dx(ai−1) Rz(θi) Dz(di){}^{i-1}\mathbf T_i = \mathbf R_x(\alpha_{i-1}), \mathbf D_x(a_{i-1}), \mathbf R_z(\theta_i), \mathbf D_z(d_i)

展开成矩阵：

i−1Ti=[cos⁡θi−sin⁡θi0ai−1sin⁡θicos⁡αi−1cos⁡θicos⁡αi−1−sin⁡αi−1−disin⁡αi−1sin⁡θisin⁡αi−1cos⁡θisin⁡αi−1cos⁡αi−1dicos⁡αi−10001]{}^{i-1}\mathbf T_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}

四个参数的物理含义：

  • ai−1a_{i-1}（连杆长度）、αi−1\alpha_{i-1}（连杆扭角）：由机械结构决定的常数；
  • θi\theta_i（关节角）：转动关节的变量；
  • did_i（连杆偏距）：移动关节的变量。

注意标准 DH（Spong 版）与修正 DH 的下标和乘法顺序不同，两套约定的参数表不能混用。看论文或者对接现成 URDF 时第一件事是确认约定；URDF 本身不用 DH，而是任意的 joint origin + axis，从 URDF 提取 DH 参数并不总是唯一的。

正运动学就是把 nn 个连杆变换连乘：

0Tn(q)=0T1(q1)  1T2(q2)⋯n−1Tn(qn){}^0\mathbf T_n(\mathbf q) = {}^0\mathbf T_1(q_1); {}^1\mathbf T_2(q_2) \cdots {}^{n-1}\mathbf T_n(q_n)

这一步没有任何数值困难：给定 q\mathbf q，答案唯一、计算是 O(n)O(n) 次矩阵乘法。真正的难点全在反方向。

设末端线速度 ve\mathbf v_e、角速度 ωe\boldsymbol\omega_e，堆叠成 6 维空间速度。雅可比矩阵 J(q)∈R6×n\mathbf J(\mathbf q) \in \mathbb R^{6\times n} 定义了它与关节速度的线性映射：

[veωe]=J(q) q˙\begin{bmatrix} \mathbf v_e \ \boldsymbol\omega_e \end{bmatrix} = \mathbf J(\mathbf q),\dot{\mathbf q}

几何雅可比可以逐列构造，物理图像非常直接——第 ii 列就是“只动第 ii 个关节、单位速度”时末端的空间速度：

Ji={[zi−1×(pe−pi−1)zi−1]转动关节[zi−10]移动关节\mathbf J_i = \begin{cases} \begin{bmatrix} \mathbf z_{i-1} \times (\mathbf p_e - \mathbf p_{i-1}) \ \mathbf z_{i-1} \end{bmatrix} & \text{转动关节} \[10pt] \begin{bmatrix} \mathbf z_{i-1} \ \mathbf 0 \end{bmatrix} & \text{移动关节} \end{cases}

其中 zi−1\mathbf z_{i-1}、pi−1\mathbf p_{i-1} 分别是第 ii 个关节轴方向和轴上一点的位置（正解的中间结果，顺手就有）。

**这个逐列公式为什么成立？**分两步说清。

第一步，为什么可以一列一列地看。速度映射 Jq˙=∑iJiq˙i\mathbf J\dot{\mathbf q} = \sum_i \mathbf J_i \dot q_i 对 q˙\dot{\mathbf q} 是线性的，所以末端速度是“每个关节单独运动产生的速度”的叠加。冻结除 ii 外的所有关节，此时从关节 ii 到末端的整段机构是一个刚体，末端的运动完全由关节 ii 的运动决定——这就把问题化成了单关节的刚体运动学。

第二步，单个转动关节产生什么速度。关节 ii 以 q˙i\dot q_i 绕轴 zi−1\mathbf z_{i-1} 转动时，这个刚体的角速度是 ω=q˙i zi−1\boldsymbol\omega = \dot q_i\, \mathbf z_{i-1}（角速度沿转轴，与轴上取哪个点无关）；刚体上任意一点的线速度由刚体速度场公式给出——取轴上的点 pi−1\mathbf p_{i-1} 为参考点（它不动），末端点 pe\mathbf p_e 的速度是

ve=ω×(pe−pi−1)=q˙i zi−1×(pe−pi−1)\mathbf v_e = \boldsymbol\omega \times (\mathbf p_e - \mathbf p_{i-1}) = \dot q_i, \mathbf z_{i-1} \times (\mathbf p_e - \mathbf p_{i-1})

除以 q˙i\dot q_i 就是第 ii 列的上半块；下半块 zi−1\mathbf z_{i-1} 是角速度贡献（角速度矢量直接相加，这是角速度叠加定理）。移动关节更简单：整段刚体平移，线速度沿轴、角速度为零。雅可比不是求偏导求出来的，是刚体速度场拼出来的——这也是它叫“几何”雅可比的原因。

雅可比的用处远不止速度换算：

  • 静力学对偶：末端受外力旋量 F\mathbf F、关节出力矩 τ\boldsymbol\tau，系统准静态平衡时对任意虚位移 δq\delta\mathbf q 虚功为零：τ⊤δq−F⊤δx=0\boldsymbol\tau^\top \delta\mathbf q - \mathbf F^\top \delta\mathbf x = 0。代入 δx=J δq\delta\mathbf x = \mathbf J\,\delta\mathbf q 得 (τ−J⊤F)⊤δq=0(\boldsymbol\tau - \mathbf J^\top \mathbf F)^\top \delta\mathbf q = 0 对一切 δq\delta\mathbf q 成立，故 τ=J⊤F\boldsymbol\tau = \mathbf J^\top \mathbf F。速度正着映射、力反着映射（对偶），力控和碰撞检测都靠它；
  • 数值逆解：见第 4 节；
  • 奇异性分析：见下面。

当 J\mathbf J 丢秩时，末端在某些方向上瞬时失去运动能力，这就是运动学奇异。6 轴臂的典型奇异位形有三类：腕部奇异（4、6 轴共线，最常见）、肘部奇异（手臂完全伸直，即工作空间边界）、肩部奇异（腕心落在 1 轴轴线上）。

定量刻画用可操作度（manipulability）：

w(q)=det⁡ ⁣(JJ⊤)=σ1σ2⋯σ6w(\mathbf q) = \sqrt{\det!\left(\mathbf J \mathbf J^\top\right)} = \sigma_1 \sigma_2 \cdots \sigma_6

即奇异值之积，等于末端速度椭球的体积。接近奇异时最小奇异值 σmin→0\sigma_{min} \to 0，后果不是“动不了”，而是反过来：为了维持普通的末端速度，某些关节速度会飙向无穷。笛卡尔伺服在奇异附近炸掉的根源就在这里，处理手段（阻尼、限速、任务重构）见第 4 节。

逆运动学要解 Ttarget=0Tn(q)\mathbf T_{target} = {}^0\mathbf T_n(\mathbf q)，它和正解有本质不对称性：解可能不存在（目标在工作空间外）、可能有限多个（6 轴臂一般位形最多 16 个，球腕构型 8 个）、也可能无穷多（7 轴冗余臂）。这决定了逆解不是一个函数，而是一次带选择的搜索。

工程上分两条路：

  • 解析解：构型满足 Pieper 条件（相邻三轴交于一点，即球腕）时位置与姿态解耦，能写出全部 8 组闭式解——微秒级、可枚举全部分支，是轨迹规划的首选；
  • 数值解：不满足 Pieper 条件或冗余臂只能迭代（阻尼最小二乘 + 零空间投影），代价是只收敛到初值附近的一个解，且对关节限位一无所知。

两者的完整推导——球腕 8 组解每一步的代数、阻尼最小二乘的 SVD 分析、数值解的吸引盆结构，以及解析法与数值法的实测对比——见 逆运动学：从球腕解耦到数值解的吸引盆。

问题 方法 复杂度 解的性质
正解 连乘 DH 变换 O(n)O(n)，确定性 唯一
速度级 几何雅可比 O(n)O(n) 逐列构造 线性映射，奇异时丢秩
逆解（球腕） 解析：几何 + 欧拉角提取 微秒级 8 组，可全局选支
逆解（一般/冗余） DLS 迭代 + 零空间投影 每步一次 6×n 最小二乘 局部唯一，依赖初值

几条实践经验：

  1. 实时笛卡尔伺服用速度级 IK（直接积分 q˙=J+x˙\dot{\mathbf q} = \mathbf J^+ \dot{\mathbf x}），不要每个周期做完整位置级 IK——前者天然连续，后者可能跳解分支；
  2. 解的选择比解本身重要：8 组解析解里选哪组，标准是与当前构型加权关节距离最小，且整条轨迹不换支；
  3. 检查逆解库的姿态误差定义和关节卷绕策略，这两处不一致会表现为“偶发的关节大回转”，非常难排查；
  4. 奇异不可怕，可怕的是没有处理策略就闯进去——限最小奇异值、DLS 阻尼、或者规划阶段直接把奇异邻域当障碍。

  • J. J. Craig. Introduction to Robotics: Mechanics and Control. Pearson.（修正 DH 约定出处）
  • K. M. Lynch, F. C. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press.（旋量视角，可与 DH 对照）
  • B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo. Robotics: Modelling, Planning and Control. Springer.

评论