3. 机器人学基础（下）：正逆运动学与DH模型
好，咱们接着聊。上一章我们把机器人的位姿描述、坐标变换这些基础打牢了。这一章，我们要真正让机器人“动”起来。
说白了，运动学就是研究机器人怎么动，而不关心它为什么动（那是动力学的事）。我们只需要知道：给定关节角度，末端在哪儿？（正解）和 想让末端到某个位置，关节该怎么转？（逆解）。
我在做抓取项目时，最头疼的往往不是视觉算法，而是标定完发现机器人就是抓不准。后来排查下来，十有八九是运动学模型没建对。所以，这一章的内容，是后续所有工作的基石。
本章核心： 掌握DH参数建模，理解正逆运动学求解逻辑，并能用Python做简单仿真验证。
3.1 正运动学：从关节到末端
正运动学，就是已知机器人每个关节的角度（或位移），求末端执行器在基坐标系下的位姿。这个过程是唯一的，也是确定的。
怎么求？核心就是齐次变换矩阵的连乘。
想象一下，从基座开始，经过关节1的变换到连杆1，再经过关节2的变换到连杆2……一直乘到末端。用公式表达就是：
T_0^n = T_0^1 * T_1^2 * ... * T_{n-1}^n这里的 T_{i-1}^i 就是相邻坐标系之间的变换矩阵。每个矩阵里都包含了当前关节的变量（θ 或 d）。
我个人习惯，在写代码前，一定会先在纸上把坐标系画出来，把DH参数表列清楚。这一步偷懒，后面Debug会非常痛苦。
3.2 逆运动学：从末端到关节
逆运动学就麻烦多了。已知末端位姿，反求关节角度。这通常是一个多解、甚至无解的问题。
为什么难？
- 多解性： 同一个末端位置，机器人可能有多种“姿势”能达到（比如“肘部朝上”和“肘部朝下”）。
- 奇异性： 在某些位姿下，机器人会失去某个方向的自由度，导致逆解无解或解不连续。
- 非线性： 方程里充满了三角函数，求解复杂。
求解方法主要有两种：
- 解析法（封闭解）： 通过代数或几何方法直接推导出公式。速度快，但只适用于满足Pieper准则的机器人（比如后三个关节轴线交于一点）。工业机器人大多满足这个条件。
- 数值法（迭代法）： 比如牛顿-拉夫森法。通用性强，但速度慢，且依赖初始值。我一般在做离线规划时用数值法做校验。
我的经验： 在实际抓取项目中，我们通常不会实时求解逆运动学。而是预先离线计算好一系列路径点，再让机器人去执行。这样可以避免奇异点，也能保证运动平滑。
3.3 机器人连杆参数（DH模型）
要让上面的计算能进行，我们得有一套标准的方法来描述相邻连杆之间的关系。这就是DH参数法。
Denavit和Hartenberg这两位大佬，在1955年提出了这套方法。它用4个参数就能唯一确定相邻坐标系间的变换关系：
| 参数 | 符号 | 物理含义 | 变量类型 |
|---|---|---|---|
| 连杆长度 | ai-1 | 从Zi-1到Zi沿Xi-1的距离 | 常数 |
| 连杆扭角 | αi-1 | 从Zi-1到Zi绕Xi-1的转角 | 常数 |
| 连杆偏距 | di | 从Xi-1到Xi沿Zi的距离 | 关节变量（移动关节） |
| 关节转角 | θi | 从Xi-1到Xi绕Zi的转角 | 关节变量（转动关节） |
建立DH坐标系时，有个口诀：“Z轴是关节轴，X轴是公垂线”。具体步骤：
- 确定各关节的Z轴方向。
- 确定相邻Z轴之间的公垂线，方向为X轴。
- Y轴由右手定则确定。
我曾经在标定一个协作机器人时，因为把相邻关节的Z轴方向搞反了，导致仿真里机器人乱跑。查了两天才发现是DH参数表里α的符号错了。所以，建系时一定要仔细核对每个参数的正负号。
3.4 运动学仿真基础（Python + Robotics Toolbox）
理论讲完了，得上手试试。我推荐用Python的robotics-toolbox-python库，它封装了大部分运动学功能，非常适合快速验证。
安装很简单：
pip install roboticstoolbox-python下面，我们以经典的六轴工业机器人为例，演示如何用DH参数建模，并计算正逆运动学。
3.4.1 定义机器人模型
我们先用标准DH参数定义一个简单的6R机器人。假设其DH参数表如下：
| 关节 i | ai-1 (mm) | αi-1 (rad) | di (mm) | θi (rad) |
|---|---|---|---|---|
| 1 | 0 | 0 | 0.3 | q1 |
| 2 | 0.5 | -π/2 | 0 | q2 |
| 3 | 0.4 | 0 | 0 | q3 |
| 4 | 0 | -π/2 | 0.5 | q4 |
| 5 | 0 | π/2 | 0 | q5 |
| 6 | 0 | -π/2 | 0.2 | q6 |
对应的Python代码如下：
import roboticstoolbox as rtb
import numpy as np
from spatialmath import SE3
# 定义机器人，使用标准DH (MDH)
robot = rtb.DHRobot(
[
rtb.RevoluteMDH(a=0, alpha=0, d=0.3, offset=0), # 关节1
rtb.RevoluteMDH(a=0.5, alpha=-np.pi/2, d=0, offset=0), # 关节2
rtb.RevoluteMDH(a=0.4, alpha=0, d=0, offset=0), # 关节3
rtb.RevoluteMDH(a=0, alpha=-np.pi/2, d=0.5, offset=0), # 关节4
rtb.RevoluteMDH(a=0, alpha=np.pi/2, d=0, offset=0), # 关节5
rtb.RevoluteMDH(a=0, alpha=-np.pi/2, d=0.2, offset=0) # 关节6
],
name="My_6R_Robot"
)
print(robot)3.4.2 正运动学求解
定义好机器人后，正解就是一行代码的事：
# 设定一组关节角度 (单位: rad)
q = np.array([0.2, -0.5, 0.8, 0.1, 0.3, 0.0])
# 计算末端位姿
T_end = robot.fkine(q)
print("末端位姿矩阵:\n", T_end)
# 提取位置和姿态
position = T_end.t
orientation = T_end.R
print("位置 (x, y, z):", position)
print("旋转矩阵:\n", orientation)输出结果是一个4x4的齐次变换矩阵。你可以把它跟手算的结果对比，验证模型是否正确。
3.4.3 逆运动学求解
逆解稍微复杂一点，但工具箱也提供了接口。这里我们使用数值法（迭代法）求解：
# 假设我们想要达到的末端位姿
T_goal = SE3(0.6, 0.2, 0.5) * SE3.RPY(0.1, 0.2, 0.3)
# 提供一个初始关节角猜测
q_guess = np.array([0, 0, 0, 0, 0, 0])
# 求解逆运动学
sol = robot.ikine_LM(T_goal, q0=q_guess)
if sol.success:
print("逆解成功!")
print("关节角度 (rad):", sol.q)
else:
print("逆解失败，请检查目标位姿是否可达或调整初始值。")注意： 数值法逆解依赖于初始值 q0。如果初始值离真实解太远，或者目标位姿在奇异点附近，求解可能会失败。我的做法是：先用解析法（如果机器人满足条件）求一个解，再用数值法做局部优化。
3.5 本章知识体系
为了让你更直观地理解本章内容的结构，我画了一张图：
这张图把本章的核心逻辑串起来了。你可以看到，DH参数是连接正运动学和逆运动学的桥梁。没有准确的模型，后面的计算都是空中楼阁。
3.6 本章小结与避坑指南
好了，我们来快速回顾一下本章的重点：
- 正运动学：矩阵连乘，唯一解。是验证机器人模型是否正确的第一步。
- 逆运动学：多解、奇异点。实际工程中要提前规避。
- DH参数：4个参数定天下。建系时务必小心符号。
- 仿真工具：Python + Robotics Toolbox 是利器，能帮你快速验证算法。
我曾经踩过的坑： 在做手眼标定时，我直接用厂家给的URDF文件里的DH参数，结果标定出来的外参怎么都对不上。后来我手动测量了机器人的实际尺寸，重新建立了DH模型，问题才解决。所以，永远不要完全相信纸面参数，动手验证才是王道。
运动学是机器人学的“内功”。内功练好了，后面学视觉、学规划，才能事半功倍。希望你能跟着代码亲自跑一遍，把每个矩阵都算清楚。