3、运动学基础（下）：正运动学建模（DH参数法）、逆运动学求解（解析法与数值法）
好，咱们接着聊运动学。上一节我们把坐标系变换和旋转矩阵讲透了，这一节直接上硬菜——正运动学和逆运动学。说白了，就是解决两个问题：“我让关节动，末端到底在哪？” 和 “我要末端到某处，关节该怎么转？”
这两个问题，贯穿了我做机器人算法这几年几乎所有的项目。我记得刚入行那会儿，调一个六轴机械臂的抓取动作，正运动学算得飞快，一到逆运动学就卡壳。后来才明白，不是公式难，是没选对方法。今天我就把这两种方法掰开揉碎了讲给你听。
3.1 正运动学建模：DH参数法
正运动学，就是已知关节角度，求末端位姿。这活儿听着简单，但每个关节的坐标系怎么建、怎么转，很容易乱。业界最通用的方法就是 DH参数法（Denavit-Hartenberg）。
我个人习惯把DH参数法理解成一套“标准化流程”。你只要按步骤填好四个参数，剩下的矩阵乘法交给计算机就行。这四个参数分别是：
- ai-1：连杆长度——从Zi-1到Zi沿Xi-1的距离
- αi-1：连杆扭转角——从Zi-1到Zi绕Xi-1的转角
- di：连杆偏距——从Xi-1到Xi沿Zi的距离
- θi：关节角——从Xi-1到Xi绕Zi的转角
你想想看，这四个参数是不是刚好描述了两个相邻关节之间的空间关系？对，就是这么巧妙。
核心公式：相邻连杆变换矩阵
T = Rotx(αi-1) · Transx(ai-1) · Rotz(θi) · Transz(di)把每个关节的T矩阵连乘，就得到末端相对于基座的位姿：
T0_n = T0_1 · T1_2 · ... · Tn-1_n
我在项目中遇到过最典型的坑是什么？是DH参数的坐标系定义不统一。有的资料用标准DH，有的用改进DH，混着用直接算错。嗯，这里要注意：我们通常用改进DH（Craig版），因为它在处理树形结构时更自然。
下面我画了一张图，帮你理清DH参数法的整体逻辑：
我的小技巧：写代码时，先把DH参数表定义成一个结构体数组，每个元素包含a、α、d、θ四个字段。这样后续修改参数或调试都特别方便。我曾经在一个六轴焊接机器人项目里，就是靠这个结构体快速迭代了三种不同构型的DH参数。
3.2 逆运动学求解：解析法
正运动学是“已知关节求末端”，逆运动学反过来——已知末端位姿，求关节角度。这问题可就复杂多了。为什么？因为解可能不存在、可能不唯一、甚至可能有无穷多解。
解析法，也叫封闭解法。它的核心思想是：利用机械臂的几何结构，通过代数运算直接求出关节角的表达式。说白了，就是手算公式。
解析法有两个前提条件：
- 机械臂必须满足 Pieper准则（相邻三个关节轴交于一点，或平行）
- 通常适用于6自由度及以下的工业机器人
我记得有一次调试一款协作机器人，它的腕部三个关节正好交于一点。我当时心里一喜——这不就是典型的Pieper构型吗？直接用解析法，几行公式就把逆解算出来了，速度比数值法快了两个数量级。
解析法的典型步骤：
- 分离变量：把末端位姿矩阵拆成多个子问题
- 求解前三个关节：利用位置约束，解出θ₁、θ₂、θ₃
- 求解后三个关节：利用姿态约束，解出θ₄、θ₅、θ₆
- 选择最优解：从多组解中选一个（比如关节行程最小、能耗最低）
举个栗子：对于典型的六轴工业机器人，解析法通常能给出8组解。但实际中我们只选其中1-2组。我一般会优先选“关节角度变化最小”的那组，这样机械臂运动更平滑。
注意：解析法虽然快，但只适用于特定构型。如果你的机械臂是非Pieper构型，或者自由度超过6，解析法就无能为力了。这时候，数值法才是你的好朋友。
3.3 逆运动学求解：数值法
数值法，说白了就是“猜+迭代”。它不依赖机械臂的几何结构，而是通过不断逼近的方式找到解。最常用的方法是 牛顿-拉夫森法 和 雅可比矩阵伪逆法。
数值法的核心思路：
- 给定一个初始猜测的关节角度 q₀
- 计算当前末端位姿与目标位姿的误差 Δx
- 利用雅可比矩阵 J 求出关节角修正量 Δq = J⁺ · Δx
- 更新关节角 q = q + Δq
- 重复直到误差小于阈值
你想想看，这其实就是一个优化问题。我刚开始用数值法时，总遇到迭代不收敛的情况。后来发现，问题出在初始值上——初始值离真实解越近，收敛越快。所以我一般会先用解析法（如果适用）算一个近似解，再用数值法精调。
避坑指南：我曾经在一个7自由度冗余机械臂项目里，只用数值法做逆解。结果在奇异点附近，雅可比矩阵的伪逆变得不稳定，导致关节角跳变。后来我加了阻尼因子（Levenberg-Marquardt方法），问题就解决了。嗯，数值法一定要处理好奇异点。
下面这个表格，帮你快速对比解析法和数值法：
| 对比项 | 解析法 | 数值法 |
|---|---|---|
| 计算速度 | 极快（微秒级） | 较慢（毫秒级，取决于迭代次数） |
| 适用性 | 仅限Pieper构型、低自由度 | 任意构型、任意自由度 |
| 解的完整性 | 可得到所有解 | 只能得到一个解（依赖初始值） |
| 奇异点处理 | 天然避开奇异点 | 需要额外处理（阻尼、SVD等） |
| 代码实现 | 公式推导复杂，但代码简洁 | 公式简单，但需要迭代控制 |
在实际项目中，我个人的习惯是：能用解析法就用解析法，因为它快、准、稳。但如果机械臂构型特殊，或者需要处理冗余自由度，那就老老实实用数值法。两种方法不是互斥的，而是互补的。
总结一下：正运动学是基础，DH参数法是标准工具；逆运动学是难点，解析法和数值法各有千秋。你只要记住——正运动学算位置，逆运动学算关节，剩下的就是多练、多调、多踩坑。
好了，这一节的内容就到这里。运动学基础讲完，下一节我们就要进入动力学了。到时候再聊力矩、惯量那些事儿。