3. 运动学基础（下）：DH参数法建模、正运动学求解、逆运动学求解思路
好，咱们接着聊。上一节我们把旋转矩阵、齐次变换这些数学工具都捋了一遍，说白了就是给机器人运动学搭好了“脚手架”。这一节，咱们要真刀真枪地干——用DH参数法给六轴机器人建模，然后搞定正解和逆解。
我个人习惯，每次讲运动学前，都会先问自己一个问题：“我到底想让机器人末端走到哪？” 正运动学回答的是“给定关节角度，末端在哪”；逆运动学回答的是“给定末端位姿，关节角度是多少”。这两个问题，贯穿了机器人编程的始终。
3.1 DH参数法：给机器人关节“上户口”
DH参数法，说白了就是一套标准化的“户口登记”流程。每个关节、每个连杆，都用四个参数来描述。你想想看，六轴机器人有六个关节，六个连杆，要是没个统一标准，光坐标系定义就能把人绕晕。
我在项目中遇到过，有些同事自己定义坐标系，结果换个人来接手，根本看不懂。所以，DH参数法最大的价值，就是让沟通成本降到最低。
四个参数分别是：
- θ (关节角)：绕Z轴旋转的角度。对于旋转关节，这是变量。
- d (连杆偏距)：沿Z轴平移的距离。
- a (连杆长度)：沿X轴平移的距离，也就是两个Z轴之间的公垂线长度。
- α (连杆扭角)：绕X轴旋转的角度，也就是两个Z轴之间的夹角。
嗯，这里要注意：DH参数有两种约定——标准DH和改进DH。我个人习惯用改进DH（也叫Craig版本），因为它的坐标系定义更直观，尤其在处理树形结构或闭链时优势明显。六轴机器人是开链，两种都能用，但一旦选定了，就别中途换，否则坐标系全乱套。
3.2 六轴机器人DH参数表：实战案例
咱们拿一个典型的六轴工业机器人（比如KUKA、ABB那种）来举例。我一般会先画出它的简化结构图，然后逐关节填写参数表。
假设我们定义好坐标系后，得到如下DH参数表（改进DH）：
| 关节 i | θ_i (度) | d_i (mm) | a_i (mm) | α_i (度) |
|---|---|---|---|---|
| 1 | θ1 | d1 (基座高度) | a1 (0) | -90 |
| 2 | θ2 | 0 | a2 (大臂长度) | 0 |
| 3 | θ3 | 0 | a3 (小臂长度) | 0 |
| 4 | θ4 | d4 (腕部偏距) | 0 | -90 |
| 5 | θ5 | 0 | 0 | 90 |
| 6 | θ6 | d6 (腕部到法兰盘) | 0 | 0 |
这张表，就是机器人的“身份证”。有了它，任何两个相邻关节之间的变换矩阵，都能直接写出来。
3.3 正运动学求解：从关节空间到笛卡尔空间
正运动学，说白了就是“连连看”。把六个变换矩阵连乘起来，就得到了末端坐标系相对于基坐标系的位姿。
公式长这样：
T_0^6 = T_0^1 * T_1^2 * T_2^3 * T_3^4 * T_4^5 * T_5^6结果是一个4x4的齐次变换矩阵，里面包含了位置（px, py, pz）和姿态（用旋转矩阵R表示）。
我曾经在调试一个焊接机器人时，发现末端位置总是偏几毫米。查了半天，最后发现是DH参数表里d4的值写错了。所以，正运动学虽然计算简单，但参数必须精确。你想想看，一个参数差1毫米，六个关节累积下来，末端误差可能就超过5毫米了。
代码实现也很直接，用Python举个例：
import numpy as np
def dh_transform(theta, d, a, alpha):
"""计算单个DH变换矩阵"""
ct = np.cos(theta)
st = np.sin(theta)
ca = np.cos(alpha)
sa = np.sin(alpha)
return np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d],
[0, 0, 0, 1]
])
# 假设六个关节角度已知
joint_angles = [0.1, 0.2, -0.3, 0.0, 0.5, 0.0] # 弧度
# 从DH参数表取值
d_vals = [0.3, 0, 0, 0.1, 0, 0.05]
a_vals = [0, 0.4, 0.35, 0, 0, 0]
alpha_vals = [-np.pi/2, 0, 0, -np.pi/2, np.pi/2, 0]
T = np.eye(4)
for i in range(6):
T_i = dh_transform(joint_angles[i], d_vals[i], a_vals[i], alpha_vals[i])
T = T @ T_i
print("末端位姿矩阵：\n", T)
print("位置：", T[:3, 3])运行这段代码，你就能看到末端在空间中的精确位置和姿态。正运动学，就是这么简单直接。
3.4 逆运动学求解思路：从笛卡尔空间回到关节空间
逆运动学就麻烦多了。给定末端位姿，求六个关节角。这玩意儿没有通用解析解，而且解不唯一——同一个末端位姿，可能对应好几组关节角度（比如“肘部朝上”和“肘部朝下”）。
我刚开始做机器人时，觉得逆解就是解方程组。后来发现，六轴机器人的逆解，核心在于“腕部解耦”。什么意思？就是利用大多数工业机器人的结构特点——最后三个关节的轴线交于一点（称为腕点）。这样，位置和姿态就可以分开求解。
具体思路分三步：
- 求腕点位置：根据末端位姿和腕部结构，反算出腕点（关节4、5、6的交点）在基坐标系下的位置。
- 求前三个关节角：腕点位置只与前三个关节有关。通过几何法（比如投影、余弦定理）求出θ1、θ2、θ3。这一步通常有2-4组解。
- 求后三个关节角：已知前三个关节角，可以算出关节3到腕点的变换矩阵。再结合末端姿态，反解出θ4、θ5、θ6。这一步通常也有2组解（腕部翻转）。
所以，六轴机器人最多有8组逆解。实际应用中，我们会根据关节限位、避障、运动平滑性等条件，选一组最合适的。
3.5 知识体系总览
为了让你更直观地理解这一节的内容，我画了张图，把DH参数、正解、逆解的关系串起来：
这张图把逻辑关系理得很清楚：DH参数是基础，正解是“正向推导”，逆解是“反向求解”。实际项目中，正解用于仿真验证，逆解用于实时控制。两者缺一不可。
好了，运动学基础就讲到这里。记住，DH参数表是机器人的“身份证”，正解是“照镜子”，逆解是“猜拳”。多动手算几遍，自然就熟了。
公众号:蓝海资料掘金营，微信deep3321