3. 机械臂基础
各位同学，欢迎来到第三章。这一章我们正式进入机械臂的核心——运动学。
说实话，很多初学者一听到「运动学」三个字就头大，觉得全是矩阵和三角函数。我当年刚入行时也这么想。直到我在一个焊接项目里，因为逆解算错了一个关节角，导致焊枪直接怼到了工件上……嗯，从那以后，我再也不敢轻视运动学了。
这一章，我会把正解、逆解、DH参数和URDF模型串起来讲。你不需要一次性记住所有公式，但一定要理解它们之间的逻辑关系。
3.1 机械臂运动学：它在解决什么问题？
说白了，运动学就是研究「关节怎么动，末端去哪」以及「末端要去哪，关节怎么动」这两件事。
- 正运动学（Forward Kinematics, FK）： 已知每个关节的角度（或位移），求末端执行器在空间中的位置和姿态。
- 逆运动学（Inverse Kinematics, IK）： 已知末端执行器在空间中的目标位姿，反求每个关节的角度。
你想想看，在视觉定位里，相机告诉你「目标在坐标 (x, y, z) 处」，机械臂要抓过去。相机给的是正解结果吗？不是。相机给的是目标位姿，你需要用逆解算出关节角，才能让机械臂动过去。这就是视觉与机械臂协同的第一个关键接口。
3.2 DH参数：给机械臂建立「身份证」
DH参数（Denavit-Hartenberg参数）是运动学建模的通用语言。它用四个参数来描述相邻两个关节坐标系之间的变换关系。
| 参数 | 符号 | 物理含义 | 我的理解 |
|---|---|---|---|
| 连杆长度 | ai-1 | 沿 Xi-1 轴，从 Zi-1 到 Zi 的距离 | 两个关节轴之间的「硬距离」 |
| 连杆扭转角 | αi-1 | 绕 Xi-1 轴，从 Zi-1 到 Zi 的转角 | 两个关节轴之间的「扭角」 |
| 连杆偏距 | di | 沿 Zi 轴，从 Xi-1 到 Xi 的距离 | 关节之间的「偏移量」 |
| 关节角 | θi | 绕 Zi 轴，从 Xi-1 到 Xi 的转角 | 关节的「变量」 |
我曾经在一个六轴机器人项目里，因为把 a 和 d 搞反了，仿真里机械臂直接「飞」出去了。排查了整整两天才发现是参数顺序写错了。所以，建立DH参数表时，一定要对照机械臂的实物或CAD模型，一个参数一个参数地核对。
下面是一个标准六轴工业机器人的DH参数表示例（以常见的 ER3A-C60 构型为例）：
# DH参数表 (标准型)
# 关节 a (mm) α (deg) d (mm) θ (deg)
# -----------------------------------------
1 0 0 350 θ1
2 270 -90 0 θ2
3 130 0 0 θ3
4 0 -90 320 θ4
5 0 90 0 θ5
6 0 -90 80 θ6
3.3 正运动学：从关节角到位姿矩阵
有了DH参数，正解就是套公式。每个相邻关节的变换矩阵 Ti 可以写成：
T_i = Rot_z(θ_i) * Trans_z(d_i) * Trans_x(a_i) * Rot_x(α_i)
把所有变换矩阵连乘，就得到末端相对于基座的位姿：
T_0_n = T_1 * T_2 * ... * T_n
这个 T_0_n 是一个 4x4 的齐次变换矩阵，包含了位置（平移向量）和姿态（旋转矩阵）。
我在实际项目中，很少手算这个矩阵。一般用 Python 的 sympy 或 robotics-toolbox 来符号推导。但你要理解它的物理意义：正解就是把关节空间映射到笛卡尔空间。
- 验证机械臂的几何参数是否正确
- 在仿真中显示机械臂的当前姿态
- 作为逆解迭代的初始值
3.4 逆运动学：从目标位姿回到关节角
逆解比正解复杂得多。对于六轴机器人，理论上最多有 16 组解（但实际中很多解会因为关节限位被排除）。
逆解的求解方法主要有两种：
- 解析法（封闭解）： 利用机械臂的几何结构，直接推导出关节角的表达式。速度快，但只适用于特定构型（如满足 Pieper 准则的机器人）。
- 数值法（迭代法）： 用牛顿-拉夫森或雅可比伪逆等方法迭代求解。通用性强，但速度慢，且可能陷入局部最优。
我个人习惯：优先用解析法。 因为数值法在靠近奇异位形时容易发散。我曾经在一个打磨项目里，用数值法做逆解，结果在某个姿态下算法直接不收敛，机械臂原地抖动。后来换成解析法，问题立刻解决。
下面是一个简单的解析逆解伪代码（以 2R 平面机械臂为例）：
# 已知末端位置 (x, y)，求关节角 θ1, θ2
# 连杆长度 L1, L2
cos_θ2 = (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2)
θ2 = atan2(±sqrt(1 - cos_θ2^2), cos_θ2) # 两个解（肘上/肘下）
k1 = L1 + L2 * cos_θ2
k2 = L2 * sin_θ2
θ1 = atan2(y, x) - atan2(k2, k1)
3.5 URDF模型构建：把数学变成「看得见」的机器人
URDF（Unified Robot Description Format）是ROS里描述机器人模型的XML格式。它把DH参数、连杆几何、关节类型、碰撞模型等信息统一起来。
一个完整的URDF文件包含：
- <link>： 描述每个连杆的惯性、视觉、碰撞属性
- <joint>： 描述关节的类型（旋转/平移/固定）、父连杆、子连杆、原点变换、关节限位
- <robot>： 根元素，包含所有 link 和 joint
下面是一个简单的两连杆机械臂URDF片段：
<robot name="two_link_arm">
<link name="base_link">
<visual>
<geometry>
<box size="0.1 0.1 0.05"/>
</geometry>
</visual>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="10" velocity="1"/>
</joint>
<link name="link1">
<visual>
<geometry>
<cylinder length="0.3" radius="0.02"/>
</geometry>
<origin xyz="0 0 0.15" rpy="0 0 0"/>
</visual>
</link>
<joint name="joint2" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 0.3" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-2.09" upper="2.09" effort="10" velocity="1"/>
</joint>
<link name="link2">
<visual>
<geometry>
<cylinder length="0.3" radius="0.02"/>
</geometry>
<origin xyz="0 0 0.15" rpy="0 0 0"/>
</visual>
</link>
</robot>
<origin> 是相对于父连杆的变换，不是相对于世界坐标系。很多新手在这里犯错，导致模型在 RViz 里「散架」。我的建议是：先画一个连杆坐标系图，再写 URDF，坐标系图比代码更容易发现错误。
3.6 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
3.7 本章小结
这一章我们走完了从「机械臂几何结构」到「数学建模」再到「统一描述」的完整链路。你不需要记住所有公式，但一定要理解：
- DH参数是运动学建模的「通用语言」，写错了后面全错。
- 正解是基础工具，用来验证和仿真。
- 逆解是执行工具，视觉定位的结果最终要靠逆解驱动机械臂。
- URDF是把数学模型变成「看得见」的机器人的桥梁。