4. 速度运动学与雅可比矩阵：线速度与角速度、雅可比矩阵的构造与奇异性分析
好，我们进入第四章。这一章可以说是运动学里最核心、也最实用的部分——速度运动学。
前面几章我们聊了位置和姿态怎么描述，怎么用 DH 参数把机器人的“骨架”搭起来。但机器人动起来之后呢？关节转得快慢，末端到底能跑多快？关节空间和操作空间的速度是怎么映射的？
答案就是——雅可比矩阵。
我个人习惯把雅可比矩阵看作是机器人的“速度传动比”。它告诉你：你给关节一个很小的速度，末端会以什么样的线速度和角速度响应。搞懂了它，你才能做轨迹规划、速度控制，甚至判断机器人在哪些位姿下会“卡住”。
4.1 线速度与角速度：先从刚体运动说起
在讲雅可比之前，我们得先把速度本身说清楚。
一个刚体在空间中的运动，可以分解为两部分：
- 线速度：刚体上某一点相对于参考系的位置变化率，单位 m/s。
- 角速度：刚体绕某轴旋转的快慢，单位 rad/s。
你想想看，人形机器人的躯干、手臂、腿，每个连杆都是一个刚体。末端执行器（比如手）的运动，就是这些刚体运动叠加的结果。
我记得刚入行时，有个前辈跟我说：“别把线速度和角速度分开看，它们是一体的。” 后来做项目才明白，末端的速度其实是线速度 + 角速度 × 力臂的合成。用公式表达就是：
v_末端 = v_基座 + ω × r其中 r 是从基座到末端的向量。ω × r 就是角速度引起的切向速度分量。
4.2 雅可比矩阵的构造：从关节空间到操作空间
好，现在进入正题。雅可比矩阵 J 是一个 m × n 的矩阵，其中 m 是操作空间的维度（通常 6：3 个线速度 + 3 个角速度），n 是关节数。
它的定义很简单：
ẋ = J(q) · q̇其中 ẋ 是末端速度向量（6×1），q̇ 是关节速度向量（n×1），J(q) 是雅可比矩阵（6×n）。
说白了，雅可比矩阵就是关节速度到末端速度的线性映射。但注意，这个映射是位姿相关的——机器人姿势不同，J 就不同。
4.2.1 构造方法：两种思路
我在项目中用过两种构造雅可比的方法，各有适用场景：
- 微分法：对正运动学方程直接求偏导。适合解析表达式简单的机器人。
- 几何法：利用每个关节对末端速度的物理贡献来构造。适合链式结构复杂的机器人，比如人形机器人的腿。
几何法更直观。对于旋转关节 i，它对末端线速度和角速度的贡献是：
J_vi = z_i × (p_end - p_i)
J_ωi = z_i其中 z_i 是关节 i 的旋转轴方向向量，p_i 是关节 i 的位置，p_end 是末端位置。
对于移动关节，贡献是：
J_vi = z_i
J_ωi = 0嗯，这里要注意：人形机器人大多是旋转关节，所以移动关节的情况相对少见，但髋关节的某些设计会用到。
4.2.2 一个简单的 2R 机械臂例子
我们拿一个平面 2R 机械臂来演示。两个旋转关节，关节角 q1、q2，杆长 L1、L2。
末端位置：
x = L1·cos(q1) + L2·cos(q1+q2)
y = L1·sin(q1) + L2·sin(q1+q2)对时间求导，得到雅可比矩阵：
J = [ -L1·sin(q1) - L2·sin(q1+q2), -L2·sin(q1+q2) ;
L1·cos(q1) + L2·cos(q1+q2), L2·cos(q1+q2) ]这个 2×2 的矩阵，就是 2R 机械臂的雅可比。你看，它只和关节角度有关，和速度无关。
4.3 奇异性分析：机器人为什么会“卡住”
这是我最想跟你聊的部分。奇异性，说白了就是雅可比矩阵秩亏的时候——矩阵不满秩，行列式为 0。
为什么会这样？
当机器人处于某些特殊位姿时，关节的运动方向变得“重复”或“无效”，导致末端在某些方向上失去运动能力。这时候，你让关节转得飞快，末端可能纹丝不动，或者需要无穷大的关节速度才能实现微小的末端运动。
我曾经在一个焊接机器人项目里遇到过这个问题。机器人焊到某个位置时，末端突然抖得厉害，焊缝直接报废。排查了半天，发现就是奇异位姿导致的——雅可比矩阵接近奇异，关节速度指令被放大了十几倍，电机直接震荡。
4.3.1 常见的奇异类型
人形机器人常见的奇异情况有：
| 奇异类型 | 发生场景 | 后果 |
|---|---|---|
| 边界奇异 | 关节到达物理限位 | 末端无法继续沿某个方向运动 |
| 内部奇异 | 两个或多个关节轴共线 | 失去一个自由度，末端运动受限 |
| 腕部奇异 | 腕关节的轴 4 和轴 6 共线 | 末端无法实现某些姿态变化 |
| 肘部奇异 | 肘关节完全伸直或完全弯曲 | 末端在径向方向失去运动能力 |
对于人形机器人，走路时膝盖打直就是一种典型的肘部奇异。所以你看人形机器人走路，膝盖总是微弯的——不光是为了减震，也是为了避开奇异。
4.3.2 如何检测奇异
最直接的方法：计算雅可比矩阵的行列式。如果行列式的值接近 0，就说明接近奇异。
对于 6 自由度机器人，雅可比是 6×6 的方阵，行列式可以直接算。但对于人形机器人这种冗余自由度系统（比如一条腿 6 个自由度，但操作空间只有 6 维），雅可比可能是长方阵，这时候要用奇异值分解（SVD）。
SVD 会把雅可比矩阵分解成：
J = U · Σ · V^T其中 Σ 是对角矩阵，对角线上的值就是奇异值。最小的奇异值如果接近 0，就说明机器人接近奇异。
4.4 知识体系总览
下面这张图把本章的核心逻辑串起来了。从刚体速度开始，到雅可比构造，再到奇异性分析与处理，是一条完整的技术链路。
4.5 实际控制中的注意事项
最后，分享几个我在实际项目中踩过的坑：
- 雅可比矩阵要实时更新：别偷懒用上一帧的 J。机器人动得快的时候，J 变化也快，用旧值会导致速度控制误差累积。
- 奇异值阈值别设太死：我见过有人把阈值设成 0.001，结果机器人稍微靠近奇异就开始抖。建议设成 0.05~0.1，留点余量。
- 冗余自由度是朋友：人形机器人自由度多，遇到奇异时可以靠自运动调整姿态避开。别死磕一个构型。
- 仿真里先跑一遍：每次改轨迹，我都会先在仿真里跑一遍，看雅可比行列式的最小值在哪。提前标记出危险区域，省得现场出问题。
嗯，这一章的内容就到这里。雅可比矩阵是连接关节控制和末端运动的桥梁，搞懂了它，你才能真正理解机器人“怎么动”。