📚 课程目录第2章机械臂运动学基础（上）

2. 机械臂运动学基础（上）

大家好，我是你们的讲师。今天咱们来啃机械臂运动学这块硬骨头。说实话，我刚入行那会儿，觉得运动学就是一堆矩阵乘法，枯燥得很。直到第一次调试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

这个乘积的结果，就是末端在基坐标系下的位姿。这就是正运动学的核心。

  重点：齐次变换矩阵的乘法顺序不能乱。从左到右，依次是基座到关节1、关节1到关节2……一旦顺序错了，结果就完全不对。

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的零位状态完全吻合。

我的习惯：写正运动学代码时，我会先写一个函数，输入6个关节角，输出4x4齐次变换矩阵。然后写一个测试用例，用零位验证。如果结果不对，八成是DH参数符号搞反了。UR5的DH参数里，α有正有负，很容易写错。

5. 知识体系总览

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

这张图把这一章的核心逻辑串起来了。从刚体变换出发，引出旋转矩阵和齐次变换矩阵，最后用UR5的DH参数做正运动学推导。每一步都是下一步的基础，缺一不可。

好了，这一章的内容就到这里。记住，运动学是机械臂控制的「地基」。地基不牢，后面做轨迹规划、动力学控制都会出问题。下一章咱们继续聊运动学的另一半——逆运动学，到时候你会看到，正运动学有多简单，逆运动学就有多「坑」。

  课后练习：用Python或C++实现UR5的正运动学函数。输入6个关节角，输出末端位姿（位置+姿态）。用零位验证结果是否正确。

可可精品资料库