机械臂运动学基础（上）：刚体变换、旋转矩阵、齐次变换矩阵、正运动学推导（以UR5为例）
大家好，我是你们的讲师。今天咱们来啃机械臂运动学这块硬骨头。说实话，我刚入行那会儿，觉得运动学就是一堆矩阵乘法，枯燥得很。直到第一次调试UR5，发现末端死活对不准目标点，才意识到——嗯，这玩意儿是机械臂的「灵魂」。你连它怎么动都不懂，怎么让它干活？
这一章，咱们只讲最核心的东西：刚体变换、旋转矩阵、齐次变换矩阵，最后拿UR5练手，把正运动学推一遍。别怕，我会用最「人话」的方式讲清楚。
1. 刚体变换：机械臂的「身体」怎么动？
机械臂的每个关节，本质上就是一个刚体。刚体变换，说白了就是描述一个刚体在空间里怎么「挪」和「转」。
你想想看，一个刚体有6个自由度：3个平移（x, y, z），3个旋转（绕x, y, z轴）。我们管这叫「位姿」——位置+姿态。
我个人习惯把位姿拆成两部分理解：
- 位置：刚体上某个点（比如关节中心）在空间中的坐标。
- 姿态：刚体本身朝哪个方向「看」。
举个例子。UR5的基座固定在地面上，它的位置是固定的。但第二个关节相对于第一个关节，既有平移又有旋转。这就是刚体变换要解决的问题。
2. 旋转矩阵：描述「转」的利器
旋转矩阵，是描述姿态最常用的工具。它是一个3x3的矩阵，每一列代表旋转后的坐标系轴在原始坐标系中的投影。
为什么会这样？因为旋转矩阵的本质是「基变换」。你把一个坐标系旋转了，它的三个轴在新的坐标系下就有了新的坐标。把这些坐标按列排好，就是旋转矩阵。
常用的旋转矩阵有三种：
- 绕X轴旋转：Rx(θ)
- 绕Y轴旋转：Ry(θ)
- 绕Z轴旋转：Rz(θ)
它们的公式长这样：
Rx(θ) = [[1, 0, 0],
[0, cosθ, -sinθ],
[0, sinθ, cosθ]]
Ry(θ) = [[cosθ, 0, sinθ],
[0, 1, 0],
[-sinθ, 0, cosθ]]
Rz(θ) = [[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]]注意看，绕哪个轴转，哪个轴对应的行和列就不变。其他四个元素是三角函数。这个规律记住了，写代码的时候不容易出错。
3. 齐次变换矩阵：把「挪」和「转」合二为一
旋转矩阵只能描述姿态，不能描述位置。那怎么办？齐次变换矩阵登场。
它是一个4x4的矩阵，结构如下：
T = [[R, t],
[0, 1]]其中R是3x3旋转矩阵，t是3x1平移向量。最后一行是[0, 0, 0, 1]。
为什么要加这一行？说白了，是为了让矩阵乘法能同时处理旋转和平移。你想想看，如果只用3x3矩阵，平移就得单独加，多麻烦。齐次变换矩阵把两者统一了，一个矩阵乘法搞定一切。
在UR5的运动学里，每个关节之间的变换就是一个齐次变换矩阵。从基座到末端，就是一连串矩阵的乘积：
T_0_n = T_0_1 * T_1_2 * ... * T_{n-1}_n这个乘积的结果，就是末端在基坐标系下的位姿。这就是正运动学的核心。
4. 正运动学推导：以UR5为例
好了，理论讲完了，咱们来点实战。UR5是一款6轴协作机器人，它的运动学参数可以用DH（Denavit-Hartenberg）法来描述。
UR5的DH参数表如下：
| 关节 i | a_{i-1} (mm) | α_{i-1} (rad) | d_i (mm) | θ_i (rad) |
|---|---|---|---|---|
| 1 | 0 | 0 | 89.459 | θ1 |
| 2 | -425 | -π/2 | 0 | θ2 |
| 3 | -392.25 | 0 | 0 | θ3 |
| 4 | 0 | -π/2 | 109.15 | θ4 |
| 5 | 0 | π/2 | 94.65 | θ5 |
| 6 | 0 | -π/2 | 82.3 | θ6 |
每个关节的齐次变换矩阵，根据DH参数可以写成：
T_i = [[cosθ_i, -sinθ_i, 0, a_{i-1}],
[sinθ_i * cosα_{i-1}, cosθ_i * cosα_{i-1}, -sinα_{i-1}, -d_i * sinα_{i-1}],
[sinθ_i * sinα_{i-1}, cosθ_i * sinα_{i-1}, cosα_{i-1}, d_i * cosα_{i-1}],
[0, 0, 0, 1]]把UR5的参数代进去，就能得到每个关节的变换矩阵。然后连乘，就得到末端位姿。
举个例子，假设所有关节角度都是0（即机械臂处于「直立」状态），那么末端位姿就是：
T_0_6 = [[1, 0, 0, -817.25],
[0, 1, 0, 0],
[0, 0, 1, 375.399],
[0, 0, 0, 1]]这个结果意味着：末端在基座前方817.25mm，高度375.399mm，姿态与基座一致。嗯，跟实际UR5的零位状态完全吻合。
5. 知识体系总览
为了让你更直观地理解这一章的知识结构，我画了一张图：
这张图把这一章的核心逻辑串起来了。从刚体变换出发，引出旋转矩阵和齐次变换矩阵，最后用UR5的DH参数做正运动学推导。每一步都是下一步的基础，缺一不可。
好了，这一章的内容就到这里。记住，运动学是机械臂控制的「地基」。地基不牢，后面做轨迹规划、动力学控制都会出问题。下一章咱们继续聊运动学的另一半——逆运动学，到时候你会看到，正运动学有多简单，逆运动学就有多「坑」。
可可精品资料库