第4章 运动学基础（上）：正运动学——从关节角到末端位姿

大家好，欢迎来到运动学部分。

说实话，很多初学者一听到「运动学」三个字就头大。我当年也一样，觉得这玩意儿全是公式推导，离实际应用很远。直到我第一次调试机械臂，发现末端抓不准位置，才意识到——嗯，运动学这东西，绕不过去。

这一章我们只聊一件事：正运动学。说白了就是——我知道每个关节转了多少角度，我想知道机械臂末端到底在哪儿、朝哪个方向。

4.1 什么是正运动学？

正运动学，英文叫 Forward Kinematics，简称 FK。

它的输入是：关节角度（比如关节1转了30°，关节2转了45°）。
输出是：末端位姿（位置 + 姿态）。

我习惯用一个比喻来理解：你站在一个房间里，你的肩膀、肘部、手腕各有一个角度。正运动学就是——我知道你肩膀抬了多少、肘弯了多少，然后算出你的手指尖在房间里的坐标。

听起来是不是很直观？

4.2 为什么需要正运动学？

你可能要问：我直接量一下末端位置不就行了？

嗯，理论上可以。但实际中，你不可能在每个时刻都拿尺子去量。而且，机械臂的末端位置会随着关节运动实时变化。我们需要一个数学模型，能根据关节角度实时计算出末端位姿。

我在项目中遇到过这样一个场景：我们需要让机械臂末端沿着一条直线轨迹运动。如果不知道正运动学，你根本不知道每个关节该转多少。正运动学是逆运动学的基础，而逆运动学才是轨迹规划的起点。

4.3 DH参数法简介

讲正运动学，绕不开一个经典方法——DH参数法（Denavit-Hartenberg 参数法）。

DH参数法的核心思想是：
把机械臂的每个连杆用一个4×4的齐次变换矩阵表示。然后把这些矩阵乘起来，就得到了末端相对于基座的位姿。

每个连杆需要4个参数：

参数	符号	含义
连杆长度	a	相邻两关节轴线的公垂线长度
连杆扭角	α	相邻两关节轴线的夹角
连杆偏距	d	沿关节轴线的偏移量
关节角	θ	绕关节轴线的旋转角度

我个人习惯把 DH 参数想象成「搭积木」——每个积木块有固定的形状（a 和 α），但可以沿着轴线滑动（d）和旋转（θ）。你把这些积木按顺序拼起来，就得到了整个机械臂。

4.4 简单2连杆机械臂正解计算

理论讲完了，我们来动手算一个最简单的例子——2连杆机械臂。

假设：

• 连杆1长度 L1
• 连杆2长度 L2
• 关节1角度 θ1
• 关节2角度 θ2

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

其实很简单，用三角函数就行：

x = L1 * cos(θ1) + L2 * cos(θ1 + θ2)
y = L1 * sin(θ1) + L2 * sin(θ1 + θ2)

为什么是 θ1 + θ2？因为第二个连杆的方向不仅取决于 θ2，还取决于第一个关节的朝向。你想想看，如果第一个关节转了30°，第二个关节再转45°，那第二个连杆的实际方向就是75°。

我曾经在调试一个2自由度机械臂时，发现末端位置总是算不对。后来发现，是我把 θ2 当成了角度，而不是相对角度。嗯，这个坑我踩过，你注意一下。

4.5 用齐次变换矩阵表示

上面的三角函数方法只适用于2连杆。对于更复杂的机械臂，我们需要用矩阵。

每个连杆的变换矩阵长这样：

T = [cosθ   -sinθ*cosα   sinθ*sinα   a*cosθ
     sinθ    cosθ*cosα  -cosθ*sinα   a*sinθ
       0        sinα        cosα        d
       0          0           0         1]

把每个连杆的 T 矩阵乘起来，就得到末端相对于基座的变换矩阵：

T_0_n = T_0_1 * T_1_2 * ... * T_(n-1)_n

这个矩阵的最后一列就是末端位置，左上角的3×3矩阵就是末端姿态。

我刚开始学的时候，觉得这个矩阵乘法很麻烦。后来写了个 Python 脚本，用 numpy 一行代码搞定。你如果感兴趣，可以自己试试。

4.6 本章知识体系

为了让你更直观地理解本章的知识结构，我画了一张图：

4.7 本章小结

这一章我们聊了：

• 正运动学的定义：从关节角到末端位姿
• DH参数法的四个参数：a, α, d, θ
• 2连杆机械臂的正解计算：三角函数法
• 齐次变换矩阵的通用方法

说实话，正运动学是机器人学里最「友好」的部分。因为它有确定的数学公式，你只要代入角度，就能得到唯一的位置。不像逆运动学，有时候解不唯一，甚至无解。

下一章我们会深入逆运动学，到时候你会发现，正运动学学得好不好，直接决定了逆运动学能不能算对。

可可精品资料库