3. 数学基础（二）：运动学基础
运动学，说白了就是研究机器人怎么动，但不关心为什么动。你给它一组关节角度，它告诉你末端在哪儿——这叫正运动学。反过来，你告诉它末端要去哪儿，它算出一组关节角度——这叫逆运动学。这两件事，是轨迹规划的基石。
我记得刚入行那会儿，总觉得运动学是理论课才用的东西。直到第一次调工业机器人，发现末端走不准，才意识到DH参数表里一个符号写反了，整个轨迹全偏。嗯，从那以后我再也不敢小看这章内容了。
3.1 正运动学：DH参数法
正运动学解决的是：已知关节角度，求末端位姿。最经典的方法就是Denavit-Hartenberg参数法，简称DH法。
DH法的核心，是在每个关节上建立一个坐标系，然后用四个参数描述相邻坐标系之间的变换关系：
| 参数 | 符号 | 物理含义 |
|---|---|---|
| 连杆长度 | ai-1 | 沿xi-1轴，从zi-1到zi的距离 |
| 连杆扭角 | αi-1 | 绕xi-1轴，从zi-1到zi的转角 |
| 连杆偏距 | di | 沿zi轴，从xi-1到xi的距离 |
| 关节转角 | θi | 绕zi轴，从xi-1到xi的转角 |
有了这四个参数，相邻坐标系的变换矩阵就是：
i-1T_i = Rot(x, α_{i-1}) · Trans(x, a_{i-1}) · Rot(z, θ_i) · Trans(z, d_i)展开成4×4矩阵：
i-1T_i =
[ cosθ_i -sinθ_i·cosα_{i-1} sinθ_i·sinα_{i-1} a_{i-1}·cosθ_i ]
[ sinθ_i cosθ_i·cosα_{i-1} -cosθ_i·sinα_{i-1} a_{i-1}·sinθ_i ]
[ 0 sinα_{i-1} cosα_{i-1} d_i ]
[ 0 0 0 1 ]把每个关节的变换矩阵连乘，就得到末端相对于基座的位姿：
0T_n = 0T_1 · 1T_2 · ... · n-1T_n3.2 逆运动学：解析法与数值法
逆运动学比正运动学麻烦得多。你想想看，正运动学是唯一解，逆运动学可能无解、有唯一解、有多解，甚至无穷多解。我曾在项目里遇到过六轴机器人末端走奇异点，逆解直接报错，整个产线停了半小时……
求解逆运动学，主流方法有两种：
3.2.1 解析法
解析法就是通过代数或几何推导，直接写出关节角度的闭式表达式。优点是快、准、稳，缺点是需要针对每个机器人单独推导。
常见的解析法套路：
- 代数法：利用矩阵方程两边对应元素相等，解出角度。比如从0Tn的(1,4)和(2,4)元素解θ1。
- 几何法：把机器人投影到平面，用余弦定理等几何关系求解。适合前三个关节决定位置、后三个关节决定姿态的构型。
- Pieper准则：如果机器人后三个关节轴线交于一点（球形腕），则前三个关节的位置解和后三个关节的姿态解可以分离求解。
3.2.2 数值法
数值法不依赖具体构型，通用性强。最常用的是牛顿-拉夫森法：
给定目标位姿 T_des，当前关节角度 q_cur
while ||Δx|| > ε:
计算当前正运动学 T_cur = f(q_cur)
计算位置误差 Δx = log(T_des · T_cur^{-1})
计算雅可比矩阵 J(q_cur)
解线性方程 J · Δq = Δx
更新 q_cur = q_cur + Δq数值法的优缺点很明显：
| 对比项 | 解析法 | 数值法 |
|---|---|---|
| 计算速度 | 快（微秒级） | 慢（毫秒级，需迭代） |
| 通用性 | 差（每款机器人单独推导） | 好（换机器人只需改DH参数） |
| 奇异点处理 | 可提前规避 | 雅可比矩阵奇异时需阻尼处理 |
| 多解选择 | 可显式枚举所有解 | 依赖初值，可能收敛到非期望解 |
3.3 雅可比矩阵与速度运动学
雅可比矩阵把关节速度映射到末端速度。它是运动学里最强大的工具之一，也是轨迹规划中速度、加速度约束的基础。
定义很简单：
ẋ = J(q) · q̇
其中：
ẋ = [v_x, v_y, v_z, ω_x, ω_y, ω_z]^T —— 末端速度（线速度+角速度）
q̇ = [q̇_1, q̇_2, ..., q̇_n]^T —— 关节速度
J(q) ∈ R^{6×n} —— 雅可比矩阵雅可比矩阵的每一列，对应一个关节对末端速度的贡献。对于旋转关节i：
J_i = [ z_{i-1} × (p_n - p_{i-1}) ]
[ z_{i-1} ]其中zi-1是关节i的旋转轴方向，pn是末端位置，pi-1是关节i的位置。
有了雅可比矩阵，我们能做很多事：
- 速度正解：给定关节速度，算末端速度。直接矩阵乘法就行。
- 速度逆解：给定末端速度，算关节速度。需要求J的逆或伪逆。
- 奇异性检测：当J的行列式接近0时，机器人处于奇异位形。
- 可操作度：用sqrt(det(J·JT))衡量机器人当前位形的灵活性。
- 力/力矩映射：τ = JT · F，把末端力映射到关节力矩。
3.4 本章知识体系
下面这张图把本章的核心逻辑串起来了。你从左上角开始看，顺着箭头走一遍，就能理解运动学在轨迹规划中的位置。
从这张图你能看到：正运动学是基础，雅可比矩阵是连接位置和速度的桥梁，逆运动学是实际控制中反复调用的核心模块。三者缺一不可。
我个人习惯在写轨迹规划代码时，先把正运动学和雅可比矩阵封装成独立模块，逆运动学基于它们实现。这样换机器人时，只需要改DH参数表和雅可比矩阵的解析式，上层代码几乎不用动。