第三章 运动学基础（下）：DH参数法、正运动学建模、逆运动学求解
各位同学，咱们接着聊运动学。上一节我们把刚体变换和旋转矩阵讲透了，这一节要动真格的了——怎么给机器人腿建立数学模型，怎么算它末端的位置，又怎么反过来求关节角度。
说实话，我刚入行那会儿，最头疼的就是逆运动学。正运动学还好说，给角度算位置，计算机一秒钟算几万次。逆运动学就不一样了，它是个多解、甚至无解的问题。我踩过的坑，今天都给你们摆出来。
3.1 DH参数法：给关节建个“身份证”
DH参数法，说白了就是给每个关节和连杆建立一套标准化的坐标系。你想想看，人形机器人一条腿有髋关节、膝关节、踝关节，每个关节的运动都会影响脚的位置。怎么统一描述？DH参数就是干这个的。
我个人习惯用改进型DH（Modified 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是变量；对于移动关节，di是变量。人形机器人基本都是旋转关节，所以θi就是我们要控制的量。
核心公式：相邻连杆的变换矩阵
T = Rot(x, αi-1) · Trans(x, ai-1) · Rot(z, θi) · Trans(z, di)写成矩阵形式就是：
| cosθi -sinθi 0 ai-1 |
| sinθicosαi-1 cosθicosαi-1 -sinαi-1 -disinαi-1 |
| sinθisinαi-1 cosθisinαi-1 cosαi-1 dicosαi-1 |
| 0 0 0 1 |我的经验：建立DH参数表时，一定要画图！我见过太多人直接对着机器人硬套公式，结果坐标系方向搞反了。先在纸上画出每个关节的z轴方向，再确定x轴，最后填参数表。这一步省不了。
3.2 正运动学建模：从关节空间到操作空间
正运动学，就是已知关节角度，求末端位姿。说白了，你告诉机器人“髋关节转30度，膝关节转60度”，它得知道脚在哪儿。
对于一条人形机器人腿，假设有6个自由度（髋3个、膝1个、踝2个），正运动学就是把这6个变换矩阵乘起来：
T06 = T01 · T12 · T23 · T34 · T45 · T56这个T06矩阵里，左上角3×3是姿态（旋转矩阵），右上角3×1是位置（脚的位置）。
实际项目中的坑：我曾经在调试双足机器人时，发现正运动学算出来的脚位置和实际差了5厘米。查了两天，最后发现是髋关节的DH参数里，ai-1的符号写反了。所以，算完之后一定要做验证——把每个关节转到零位，看看末端是不是在预期的初始位置。
正运动学的计算其实很机械，就是矩阵乘法。但要注意计算效率——控制周期通常是1kHz，你必须在1毫秒内算完所有运动学。我建议用符号推导，提前把矩阵乘法的结果写成解析表达式，运行时直接代入数值。
3.3 逆运动学求解：解析法与数值法
逆运动学才是真正考验人的地方。已知脚的位置和姿态，反求6个关节角度。这问题难在哪儿？
- 多解性：同一个脚的位置，可能有多种关节角度组合（比如膝盖朝前还是朝后）
- 奇异性：某些位置下，关节速度会突变（比如腿伸直时）
- 无解：目标位置超出了机器人的工作空间
求解方法分两大类：解析法和数值法。
3.3.1 解析法
解析法，就是通过代数或几何方法，直接求出关节角度的闭式表达式。对于人形机器人腿，通常可以解耦成位置和姿态两部分。
举个例子，对于一条6自由度腿，我们可以把前3个关节（髋）用于定位，后3个关节（膝、踝）用于定向。这样就把6自由度问题拆成了两个3自由度问题。
// 解析法求解膝关节角度（简化版）
// 已知：脚的位置 (x, y, z)，大腿长度 L1，小腿长度 L2
// 求：膝关节角度 θ_knee
double d = sqrt(x*x + y*y + z*z); // 脚到髋的距离
double cos_knee = (L1*L1 + L2*L2 - d*d) / (2*L1*L2);
double θ_knee = acos(cos_knee); // 注意多解：±θ_knee注意：解析法虽然快，但只适用于特定构型的机器人。如果你的机器人髋关节不是标准的球关节，或者有偏置，解析法可能就不适用了。我曾经给一个髋关节有偏置的机器人写解析解，折腾了两周才搞定。
3.3.2 数值法
数值法，说白了就是迭代逼近。最常用的是雅可比矩阵法（也叫牛顿-拉夫森法）。
核心思想：
- 给定一个初始关节角度猜测
- 计算当前末端位姿与目标位姿的误差
- 用雅可比矩阵的逆（或伪逆）修正关节角度
- 重复直到误差足够小
// 数值法逆运动学伪代码
q = q_initial; // 初始猜测
for (i = 0; i < max_iter; i++) {
T_current = forward_kinematics(q); // 正运动学
e = T_target - T_current; // 位姿误差
if (norm(e) < tolerance) break;
J = compute_jacobian(q); // 计算雅可比矩阵
dq = pinv(J) * e; // 用伪逆求关节修正量
q = q + dq; // 更新关节角度
}我的建议：数值法通用性强，但要注意几点：第一，初始猜测要合理，否则可能收敛到错误解；第二，遇到奇异位形时，雅可比矩阵会降秩，要用阻尼最小二乘法（DLS）代替伪逆；第三，设置最大迭代次数，防止死循环。
3.4 解析法 vs 数值法：怎么选？
| 对比项 | 解析法 | 数值法 |
|---|---|---|
| 计算速度 | 极快（微秒级） | 较慢（毫秒级） |
| 通用性 | 依赖机器人构型 | 通用，任意构型 |
| 多解处理 | 可显式给出所有解 | 只能得到一个解 |
| 实现难度 | 数学推导复杂 | 编程实现简单 |
| 奇异性处理 | 需单独处理 | 可用阻尼法自动处理 |
我个人在实际项目中是这样用的：对于离线轨迹规划，用解析法，因为它能给出所有可能的解，方便做最优选择。对于实时控制，用数值法，因为它通用、鲁棒，而且现代处理器算力足够在1毫秒内完成迭代。
3.5 本章知识体系
下面这张图，把本章的核心逻辑串起来了。你仔细看，从DH参数到正运动学，再到逆运动学的两种解法，每一步都是环环相扣的。
核心要点回顾：
- DH参数是运动学建模的基石，四个参数定天下
- 正运动学就是矩阵连乘，但要注意计算效率
- 逆运动学解析法快但局限，数值法通用但慢
- 实际项目中，我通常混合使用：离线用解析，在线用数值
好了，运动学基础就讲到这里。这些内容看着多，但真正用起来就是几行代码的事。关键是要理解背后的几何意义——你想想看，机器人每条腿的运动，本质上就是一系列旋转和平移的组合。把这个想通了，后面学步态规划就轻松多了。
课后动手：找一个人形机器人模型（哪怕是纸上的），手动建立它的DH参数表，然后写一个正运动学函数。算完之后，用实际测量验证一下。这一步走扎实了，后面学动力学和步态控制会顺畅很多。