3、机械臂运动学基础（一）：正运动学原理、DH参数建模、使用Python实现简单的正运动学解算
大家好，我是你们的讲师。今天我们来聊聊机械臂运动学里最基础、也最核心的一块——正运动学。
说白了，正运动学就是回答一个问题：“我告诉每个关节转多少度，机械臂的手（末端）到底在哪儿？”
这个问题听起来简单，但背后有一套严谨的数学逻辑。我当年刚入行时，觉得这玩意儿就是矩阵乘法，没啥了不起。直到有一次在产线上调试一个六轴机器人，发现末端位置差了2毫米，死活找不到原因。后来才发现，是某个关节的坐标系方向搞反了。嗯，从那以后，我再也不敢小看DH参数建模了。
3.1 正运动学到底在干什么？
先别急着看公式。我们想想看，一个机械臂由多个连杆和关节组成。每个关节转动，都会带动后面的连杆一起动。正运动学要做的，就是把这些“局部运动”一步步传递下去，最终算出末端在“全局坐标系”下的位置和姿态。
这个过程，就像你站在一个队伍里，前面的人向左转，你也跟着向左转。你要知道队伍最前面那个人最终面朝哪个方向，就得一层层算下去。
核心思想： 从基座到末端，每个关节的变换矩阵相乘，就得到了末端相对于基座的位姿。
3.2 DH参数建模——给机械臂“上户口”
DH参数是Denavit和Hartenberg两位大佬提出的。我个人习惯叫它“给每个关节和连杆建立身份证”。
每个关节i，我们定义四个参数：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | ai-1 | 从Zi-1到Zi沿Xi-1的距离 |
| 连杆扭角 | αi-1 | 从Zi-1到Zi绕Xi-1的旋转角 |
| 连杆偏距 | di | 从Xi-1到Xi沿Zi的距离 |
| 关节角 | θi | 从Xi-1到Xi绕Zi的旋转角 |
你可能会问：“这四个参数怎么来的？”
其实，它们描述的是相邻两个坐标系之间的变换关系。我建议你记住一个口诀：“先沿X走，再绕X转，然后沿Z走，最后绕Z转”。这个顺序千万别搞反，我曾经因为搞反了顺序，在仿真里让机械臂“拧成了麻花”。
3.3 标准DH vs 改进DH
这里有个坑，我得提醒你。DH参数有两种主流定义方式：标准DH（Classic DH）和改进DH（Modified DH）。
区别在于坐标系附着的位置：
- 标准DH：坐标系i附着在连杆i的前端（靠近基座一侧）
- 改进DH：坐标系i附着在连杆i的后端（靠近末端一侧）
我个人更常用改进DH，因为它在处理树状结构或闭链机构时更直观。但如果你用的是URDF或者ROS里的tf，标准DH也很常见。记住，选了一种就别中途换，否则你会被坐标系搞疯。
3.4 变换矩阵——数学的“翻译官”
有了DH参数，我们就可以写出相邻两个坐标系之间的变换矩阵了。公式长这样：
i-1T_i = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)展开成4x4矩阵：
| cosθ_i -sinθ_i·cosα_{i-1} sinθ_i·sinα_{i-1} a_{i-1}·cosθ_i |
| sinθ_i cosθ_i·cosα_{i-1} -cosθ_i·sinα_{i-1} a_{i-1}·sinθ_i |
| 0 sinα_{i-1} cosα_{i-1} d_i |
| 0 0 0 1 |看着有点吓人，对吧？其实你不需要死记硬背。我每次写代码时都会把公式贴在旁边，用多了自然就熟了。
3.5 用Python实现正运动学
好了，理论说完了，我们来点实际的。下面是一个简单的Python实现，假设我们有一个2自由度的平面机械臂。
import numpy as np
def dh_transform(a, alpha, d, theta):
"""
计算单个DH参数的变换矩阵
"""
ct = np.cos(theta)
st = np.sin(theta)
ca = np.cos(alpha)
sa = np.sin(alpha)
T = np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d],
[0, 0, 0, 1]
])
return T
def forward_kinematics(joint_angles, dh_params):
"""
正运动学解算
:param joint_angles: 关节角度列表 [θ1, θ2, ...]
:param dh_params: DH参数列表 [(a1, alpha1, d1), (a2, alpha2, d2), ...]
:return: 末端位姿矩阵 (4x4)
"""
T = np.eye(4)
for i, (a, alpha, d) in enumerate(dh_params):
theta = joint_angles[i]
T_i = dh_transform(a, alpha, d, theta)
T = T @ T_i
return T
# 示例：2自由度平面臂
# 连杆1: a=1, alpha=0, d=0
# 连杆2: a=1, alpha=0, d=0
dh_params = [(1.0, 0.0, 0.0), (1.0, 0.0, 0.0)]
joint_angles = [np.radians(30), np.radians(45)]
T_end = forward_kinematics(joint_angles, dh_params)
print("末端位姿矩阵：")
print(T_end)
print(f"末端位置：({T_end[0,3]:.3f}, {T_end[1,3]:.3f})")运行这段代码，你会得到末端的位置。我建议你手动算一遍，验证一下结果。相信我，手算一次比看十遍代码都管用。
小技巧： 调试时，可以把所有关节角设为0，看看末端位置是不是在“初始姿态”下。如果不对，八成是DH参数符号搞反了。
3.6 知识体系结构图
为了让你更直观地理解本章的知识脉络，我画了一张图：
注意： 正运动学是逆运动学的基础。如果你正运动学没搞明白，后面做轨迹规划、避障时一定会出问题。我见过太多人跳过这一步直接做逆解，结果调试了三天发现是DH参数错了。
好了，这一章的内容就到这里。正运动学虽然基础，但它是整个机械臂控制的基石。你想想看，如果连末端在哪儿都不知道，还谈什么避障呢？
下一章我们会继续深入，聊聊逆运动学——也就是“我知道末端要去哪儿，关节该怎么转”。不过在那之前，我建议你把今天的代码跑一遍，改改参数，看看末端位置怎么变化。动手试试，比什么都强。
可可精品资料库