3、运动学基础（下）：正运动学建模（DH参数法）、逆运动学求解（解析法与数值法）、雅可比矩阵与奇异性
好，咱们接着聊运动学。上一节我们把坐标系变换和位姿描述讲透了，这一节要动真格的了——正运动学、逆运动学、还有雅可比矩阵。这三个东西，说白了就是机器人的“手脚怎么动”、“脑子怎么想”、“什么时候会抽筋”。
我在项目里见过不少工程师，正运动学算得飞起，一到逆解就抓瞎。还有人把机器人跑到奇异点附近，结果关节速度瞬间爆表，差点把电机烧了。嗯，这些坑咱们今天都得填上。
3.1 正运动学建模：DH参数法
正运动学，就是已知关节角度，求末端位姿。你给机器人发指令：“肘关节弯30度，肩关节抬45度”，然后它告诉你手在哪儿、朝哪个方向。这就是正运动学。
建模方法有很多，但我个人最常用的是DH参数法。为什么？因为它规整、系统、不容易漏参数。你想想看，一个六轴机器人，六个关节，每个关节4个参数，总共24个参数就能完整描述。多清爽。
3.1.1 DH参数的定义
DH参数有四个：
- θ（关节角）：绕Z轴的旋转角
- d（连杆偏距）：沿Z轴的平移距离
- a（连杆长度）：沿X轴的平移距离
- α（连杆扭角）：绕X轴的旋转角
每个关节对应一组参数。对于旋转关节，θ是变量；对于移动关节，d是变量。其他参数都是固定的，由机械结构决定。
核心公式：相邻连杆的变换矩阵
T = Rot(z, θ) * Trans(z, d) * Trans(x, a) * Rot(x, α)展开后就是4x4的齐次变换矩阵，把上一关节的坐标系变换到下一关节。
3.1.2 建立DH坐标系的步骤
我记得第一次给六轴机器人建DH模型时，卡在坐标系建立上整整两天。后来总结出一套流程，分享给你：
- 找关节轴：每个关节的旋转轴（或移动轴）就是Z轴
- 定原点：相邻两关节轴的公垂线与Z轴的交点
- 定X轴：沿公垂线方向，从当前Z轴指向下一Z轴
- 定Y轴：右手定则，Z×X
- 填参数：按定义读出θ、d、a、α
小技巧：如果相邻两关节轴平行，公垂线不唯一。这时候我习惯取前一个关节的原点做公垂线起点，这样参数最规整。
3.1.3 一个简单的2自由度臂示例
咱们拿一个平面2R机械臂练练手。两个旋转关节，都在同一平面内。
# DH参数表
# 关节 θ d a α
# 1 q1 0 L1 0
# 2 q2 0 L2 0
# 正运动学计算
T01 = DH(q1, 0, L1, 0)
T12 = DH(q2, 0, L2, 0)
T02 = T01 * T12
# 末端位置
x = L1*cos(q1) + L2*cos(q1+q2)
y = L1*sin(q1) + L2*sin(q1+q2)你看，结果跟几何法算出来的一模一样。但DH法的优势在于——当机器人变成6轴、7轴时，你依然可以用同一套流程，不会乱。
3.2 逆运动学求解
逆运动学就反过来了：已知末端位姿，求关节角度。说白了就是“手到了这个位置，各个关节应该怎么摆”。
这玩意儿比正运动学难多了。为什么？因为正解是唯一的，逆解可能有多解、无解、甚至无穷多解。你想想看，一个六轴机器人，同一个末端位姿，可能有8组甚至16组关节角度解。
3.2.1 解析法
解析法，就是通过代数或几何推导，直接写出关节角度的表达式。速度快、精度高，是工业机器人最常用的方法。
但解析法有个前提：机器人必须满足Pieper准则——相邻的三个关节轴交于一点。大多数工业六轴机器人（比如典型的6R构型）都满足这个条件，因为它们的腕部三个关节轴交于一点。
解析法的典型步骤（以六轴机器人为例）：
- 用前三个关节确定腕部位置（位置逆解）
- 用后三个关节确定腕部姿态（姿态逆解）
- 组合得到8组解，再根据关节限位和避障需求筛选
我在项目中遇到过一个问题：解析法算出来的解，有时候关节角度跳变特别大。比如上一时刻q1=10°，下一时刻q1=350°。虽然数学上没错，但实际跑起来机器人会“甩”一下。所以后来我加了一步——最小行程筛选，选离当前关节角度最近的那组解。
3.2.2 数值法
如果机器人不满足Pieper准则，或者你懒得推导解析式，那就用数值法。数值法的核心思想是迭代逼近：先猜一组关节角，算正运动学得到末端位姿，跟目标位姿对比，然后修正关节角，再算，再修正……直到误差足够小。
最常用的数值法是牛顿-拉夫森法和雅可比伪逆法。这里给个伪代码：
q = q0 # 初始猜测
while error > threshold:
T = forward_kinematics(q) # 正运动学
e = target_pose - T # 位姿误差
J = jacobian(q) # 雅可比矩阵
dq = pinv(J) * e # 关节修正量
q = q + dq # 更新关节角
error = norm(e)注意：数值法有两个大坑。第一，初始猜测离真实解太远可能不收敛。第二，遇到奇异点时雅可比矩阵不可逆，算法直接崩掉。我曾经在调试时忘了处理奇异点，结果机器人“咔”一下卡住，吓得我赶紧按了急停。
3.2.3 解析法 vs 数值法
| 对比项 | 解析法 | 数值法 |
|---|---|---|
| 计算速度 | 快（微秒级） | 慢（毫秒级，需迭代） |
| 精度 | 理论精确 | 受迭代次数和阈值影响 |
| 适用性 | 需满足Pieper准则 | 通用，任意构型 |
| 多解处理 | 显式得到所有解 | 只得到一个解（依赖初值） |
| 实现难度 | 高（需推导） | 低（通用框架） |
我个人习惯是：能解析就解析，解析不了再用数值法。毕竟工业现场对实时性要求很高，解析法那几十微秒的计算时间，比数值法的几毫秒香太多了。
3.3 雅可比矩阵与奇异性
雅可比矩阵，是连接关节空间和操作空间的桥梁。它告诉你：关节转一点点，末端会动多少、往哪个方向动。
数学上，雅可比矩阵J满足：
v = J * dq其中v是末端的线速度和角速度（6维向量），dq是关节速度（n维向量）。J是6×n的矩阵。
3.3.1 雅可比矩阵的求法
有两种常用方法：
- 微分法：对正运动学表达式求偏导。适合解析式已知的情况。
- 矢量积法：用运动学递推公式，逐关节计算。适合编程实现。
矢量积法的公式很简单：
对于旋转关节i：
J_vi = Zi × (Pe - Pi)
J_wi = Zi
其中Zi是关节i的Z轴方向，Pe是末端位置，Pi是关节i的原点位置。3.3.2 奇异性分析
奇异性，就是雅可比矩阵的秩下降（行列式为0）的状态。这时候机器人会“失去”某个方向的运动能力。
常见的奇异位形有：
- 边界奇异：手臂完全伸直或完全折叠
- 腕部奇异：腕部三个关节轴共面（比如关节4和关节6的轴平行）
- 肩部奇异：肩关节和肘关节的轴共线
避坑指南：我曾经调试一个焊接机器人，在焊缝拐角处机器人突然剧烈抖动。查了半天，发现是路径刚好经过腕部奇异点。雅可比矩阵接近奇异，关节速度理论上趋近无穷大，控制器为了跟上轨迹拼命加速，结果就是抖动。后来我在路径规划中加了奇异点规避，问题解决。
3.3.3 奇异点的处理策略
遇到奇异点怎么办？有三种常用策略：
- 阻尼最小二乘法（DLS）：给雅可比矩阵加一个阻尼项，让逆矩阵在奇异点附近“软着陆”。公式是J^T*(J*J^T + λI)^(-1)。λ越大，越稳定，但精度越低。
- 奇异值分解（SVD）：把雅可比矩阵分解成U*Σ*V^T，把接近零的奇异值直接置零或加阻尼。
- 路径规划规避：在规划阶段就避开奇异区域，这是最根本的解决办法。
我的经验：实际项目中，我通常把策略1和策略3结合使用。路径规划时尽量远离奇异点，但万一靠近了，DLS能兜底。别指望单一策略解决所有问题。
3.4 本章知识体系
下面这张图把本章的核心逻辑串起来了。你看一遍，应该能理清正运动学、逆运动学、雅可比矩阵之间的关系。
嗯，这一章内容不少。正运动学是基础，逆运动学是核心，雅可比矩阵是进阶。三块内容环环相扣，缺一不可。你在实际项目中，大概率会反复用到这些知识——不管是做轨迹规划、运动控制，还是调试机器人。
记住一点：理论是死的，但应用是活的。别死磕公式，多想想“这个公式在机器人上到底对应什么物理意义”。想通了，你就能从“会用”变成“懂用”。
公众号:蓝海资料掘金营，微信deep3321