运动学基础回顾：正逆运动学、雅可比矩阵、奇异性分析

各位同学，欢迎来到第二章。说实话，很多做轨迹优化的朋友，最后栽跟头都栽在运动学基础上。不是算法不够好，而是对机器人本身的运动学特性理解不够深。今天我们就来把这块地基夯实。

我个人习惯把运动学比作机器人的「骨架」。你想想看，如果连骨架怎么动都搞不清楚，谈何优化轨迹？所以这一章，我们重点回顾三个核心模块：正逆运动学、雅可比矩阵、奇异性分析。别觉得基础，这里面坑不少。

2.1 正运动学：从关节空间到任务空间

正运动学，说白了就是：给定关节角度，求末端位姿。这是最直观的映射关系。

我刚开始做机器人时，总觉得正运动学就是套 DH 参数公式，没什么难度。直到有一次，我在调试一个六轴协作臂时，发现末端位置总是偏了 2 毫米。查了半天，原来是 DH 参数中一个连杆的扭转角符号搞反了。嗯，从那以后，我每次建 DH 模型都会手动验证至少三个特殊位姿。

T_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_i)

其中：
θ_i — 关节角（变量）
d_i — 连杆偏距
a_i — 连杆长度
α_i — 连杆扭转角

正运动学的计算流程其实很固定：

1. 建立各连杆坐标系，确定 DH 参数表
2. 逐关节计算齐次变换矩阵 T_i
3. 连乘得到末端位姿矩阵 T_0n = T_1 · T_2 · ... · T_n
4. 从 T_0n 中提取位置向量和旋转矩阵

这里有个小技巧——我建议你在代码里把每个 T_i 单独打印出来，逐级验证。很多 bug 其实就藏在中间某一级。

2.2 逆运动学：从任务空间回到关节空间

逆运动学就麻烦多了。给定末端位姿，求关节角度。这是个多解、甚至无解的问题。

我在项目中遇到过最头疼的事：一个六轴机器人，末端要走到某个点，结果逆解出来 8 组解，其中 4 组关节超限，2 组会撞到工作台，剩下 2 组里还有 1 组会导致奇异。你说怎么选？

所以逆运动学不只是求解，更是选解。

2.3 雅可比矩阵：速度与力的桥梁

雅可比矩阵，这是轨迹优化里最核心的工具之一。它建立了关节速度与末端速度之间的线性映射关系。

公式很简单：ẋ = J(q) · q̇

但你别小看这个 J。它不仅是速度映射，还隐含了力映射（虚功原理）：τ = Jᵀ · F

我在实际项目中，经常用雅可比矩阵做两件事：

• 轨迹速度规划：根据末端期望速度，反算关节速度指令
• 力控/阻抗控制：通过 Jᵀ 将末端力映射到关节力矩

计算雅可比矩阵有两种常见方法：

1. 微分法：对正运动学方程求偏导，适合解析推导
2. 矢量法：利用运动学链的几何关系直接构造，适合编程实现

2.4 奇异性分析：轨迹优化的「雷区」

奇异性，说白了就是雅可比矩阵秩亏损的状态。此时机器人会丢失某个方向上的运动能力。

为什么会这样？因为当机器人处于某些特殊构型时，关节运动无法产生某个方向的末端运动。比如肘关节伸直时，你无法再让末端沿某个方向移动。

我遇到过最典型的案例：一个焊接机器人，在焊缝拐角处突然「卡住」，关节速度瞬间飙升到极限。后来一查，就是进入了奇异位形附近。

常见的奇异类型：

• 边界奇异：关节到达物理限位
• 内部奇异：关节轴线共线或共面（如腕部奇异、肘部奇异）
• 算法奇异：数值求解时雅可比矩阵接近奇异

判断奇异的方法很简单：计算 det(J · Jᵀ) 或条件数 cond(J)。当条件数趋近无穷大时，就说明离奇异不远了。

2.5 知识体系总览

下面这张图，是我自己梳理的运动学基础与轨迹优化的关系。你看完应该能明白，为什么我说运动学是轨迹优化的「地基」。

2.6 实战中的几个坑

最后，分享几个我踩过的坑，希望能帮你省点时间：

• DH 参数顺序别搞错：标准 DH 和改进 DH 的坐标系定义不同，混用会导致末端位姿完全错误。我建议你统一用一种，并在代码注释里标明。
• 逆解多解处理：不要只取第一组解。我习惯按「关节位移最小 → 远离奇异 → 避障」的优先级排序选解。
• 雅可比矩阵的维度：平面机器人是 3×n，空间机器人是 6×n。写代码时注意维度匹配，不然矩阵乘法会报错。
• 奇异附近不要硬算：当条件数 > 1000 时，直接用伪逆会放大噪声。改用阻尼最小二乘法，或者干脆绕路。