3. 运动学基础（上）：正运动学——从关节角度到末端位姿的推导（以2轴/3轴机械臂为例）

各位同学，大家好。今天我们进入一个非常核心的话题——正运动学。

说白了，正运动学就是回答一个问题：“我给了机器人每个关节一个角度，它的末端到底在哪儿？朝向如何？”

这个问题听起来简单，但它是所有运动控制的基础。你想想看，如果连末端位置都算不准，后面的轨迹规划、速度控制、力控就全是空中楼阁。我在刚入行那会儿，就吃过这个亏——有一次调试一个六轴协作臂，死活走不到目标点，查了半天，原来是正运动学模型里一个连杆长度参数写错了。嗯，从那以后，我每次建模型都会反复核对DH参数表。

3.1 为什么需要正运动学？

我们做多轴运动控制，最终目的是让机械臂的末端执行器（比如焊枪、夹爪、摄像头）到达指定的空间位置和姿态。

但电机只能控制关节角度，不能直接控制末端位置。所以，我们必须建立一个数学桥梁：

• 输入：每个关节的角度值（θ₁, θ₂, θ₃, ...）
• 输出：末端在笛卡尔空间中的位置（x, y, z）和姿态（α, β, γ）

这个桥梁，就是正运动学方程。

3.2 2轴平面机械臂——最简单的例子

我们先从最简单的2轴平面机械臂入手。它只有两个旋转关节，都在同一个平面内运动。你想想看，这就像你的上臂和前臂，肩关节和肘关节。

假设：

• 连杆1长度：L₁
• 连杆2长度：L₂
• 关节1角度：θ₁（相对于水平轴）
• 关节2角度：θ₂（相对于连杆1的延长线）

那么，末端位置 (x, y) 怎么算？

其实很简单，就是两次坐标旋转和平移。我个人习惯用几何法推导，直观又好记：

x = L₁ * cos(θ₁) + L₂ * cos(θ₁ + θ₂)
y = L₁ * sin(θ₁) + L₂ * sin(θ₁ + θ₂)

你看，这就是正运动学方程。给定任意一组 (θ₁, θ₂)，我们就能算出末端在平面上的精确位置。

3.3 3轴平面机械臂——增加一个自由度

现在，我们在2轴的基础上再加一个旋转关节，变成3轴平面机械臂。这在实际中很常见，比如一些SCARA机器人的前三个关节。

假设：

• 连杆1长度：L₁
• 连杆2长度：L₂
• 连杆3长度：L₃
• 关节1角度：θ₁
• 关节2角度：θ₂
• 关节3角度：θ₃

推导思路完全一样，只是多叠加一次变换：

x = L₁*cos(θ₁) + L₂*cos(θ₁+θ₂) + L₃*cos(θ₁+θ₂+θ₃)
y = L₁*sin(θ₁) + L₂*sin(θ₁+θ₂) + L₃*sin(θ₁+θ₂+θ₃)

注意，这里的θ₃是相对于连杆2的延长线。如果你用的是绝对角度，公式会略有不同，但本质是一样的——逐级叠加。

3.4 从平面到空间——引入齐次变换矩阵

平面机械臂用几何法推导还算简单。但一旦进入三维空间（比如6轴工业机器人），几何法就变得非常繁琐。这时候，我们需要一个更强大的工具——齐次变换矩阵。

齐次变换矩阵是一个4×4的矩阵，它同时包含了旋转和位移信息：

| R(3×3)   T(3×1) |
| 0 0 0     1     |

其中：

• R 是3×3的旋转矩阵，描述姿态
• T 是3×1的平移向量，描述位置

对于多轴机械臂，正运动学就是依次将每个关节的齐次变换矩阵相乘：

T_末端 = T_01 * T_12 * T_23 * ... * T_(n-1)n

这个乘积的结果，就是末端相对于基座的位姿。

3.5 知识体系结构图

下面我用一张SVG图来总结本章的核心逻辑，方便你建立整体认知：

3.6 实际项目中的注意事项

讲完了理论，我想分享几个在实际项目中容易踩的坑：

1. 角度单位：我见过太多人把度（°）和弧度（rad）搞混。在代码里，我建议统一用弧度，只在人机交互界面显示度数。
2. 奇异点：当两个关节的轴线平行或共线时，运动学方程会退化。比如2轴机械臂在θ₂=0°时，末端只能沿一个方向移动。这在规划轨迹时一定要避开。
3. 数值精度：矩阵连乘会累积浮点误差。对于高精度应用（比如医疗手术机器人），建议使用双精度浮点数，并定期做归一化处理。

3.7 小结

正运动学是机器人控制的基石。从2轴到3轴，从平面到空间，核心思想始终不变：通过关节角度，逐级推导出末端位姿。

对于简单构型，几何法足够用；对于复杂构型，齐次变换矩阵是标准答案。我个人建议，无论你用什么方法，都要先吃透2轴和3轴的推导过程，因为这是理解更复杂运动学的基础。

好了，这一章就到这里。记住，正运动学是“正向”的——从关节到末端。下一章我们会讲它的逆过程，也就是逆运动学，那才是真正考验功力的地方。

---