第2章运动学基础（上）

2、运动学基础（上）：刚体运动描述、坐标变换、齐次变换矩阵、旋转矩阵与欧拉角

各位同学，大家好。我是你们的老朋友，一个在机器人堆里摸爬滚打多年的工程师。今天咱们开始聊运动学，这是机器人轨迹规划的根基。说白了，你要让机器人动起来，首先得知道它“在哪儿”、“往哪儿去”。

这一章，我们先把基础打牢。我会带着大家把刚体运动描述、坐标变换、齐次变换矩阵、旋转矩阵和欧拉角这几个概念，一个一个掰开揉碎了讲清楚。别怕，都是纸老虎。

机器人身上的每一个部件，比如一个机械臂的连杆，我们都可以把它看作一个“刚体”。刚体嘛，就是不会变形的物体。描述一个刚体在空间中的状态，需要两样东西：位置和姿态。合起来，我们叫它位姿。

我个人习惯把位姿想象成“你手里拿着一个手机”。手机的位置是你手在空间中的坐标，而手机的姿态是屏幕是朝上还是朝下，摄像头对着哪个方向。嗯，就是这么回事。

在机器人系统里，有好多坐标系。比如世界坐标系、机器人基座坐标系、末端执行器坐标系、相机坐标系等等。同一个点，在不同坐标系下的坐标值是不一样的。坐标变换，就是解决“怎么从一个坐标系下的坐标，换算到另一个坐标系下”的问题。

举个例子。我在项目里调试一个视觉抓取系统，相机看到零件在 (100, 200, 50) 的位置（相机坐标系下）。但机器人得知道这个点在它自己基座坐标系下的坐标，才能去抓。这就得做坐标变换。

坐标变换分两种：

平移变换：两个坐标系之间只有位置的偏移，没有旋转。比如 {B} 坐标系的原点在 {A} 坐标系下的坐标是 P_A_B，那么点 P 在 {A} 下的坐标就是它在 {B} 下的坐标加上这个偏移量。
旋转变换：两个坐标系之间只有姿态的旋转，没有平移。这就要用到旋转矩阵了。

核心思想：坐标变换的本质，就是用一个矩阵（旋转矩阵）和一个向量（平移向量），把一个点从一个坐标系“搬”到另一个坐标系。

旋转矩阵，是一个 3x3 的正交矩阵，行列式为 +1。它描述了从一个坐标系到另一个坐标系的旋转变换。

假设我们有一个坐标系 {B}，它相对于坐标系 {A} 发生了旋转。那么 {B} 的三个坐标轴的单位向量在 {A} 下的表示，就构成了旋转矩阵 R_A_B 的三列。

公式看起来有点抽象，我直接给个例子。绕 Z 轴旋转 θ 角度的旋转矩阵是：

R_z(θ) = | cosθ  -sinθ  0 |
         | sinθ   cosθ  0 |
         | 0      0     1 |

绕 X 轴和 Y 轴的类似，大家自己推导一下，很直观。

旋转矩阵有个重要的性质：正交性。也就是说，R^T * R = I，并且 R^{-1} = R^T。这个性质在求逆变换时特别有用，省去了求逆矩阵的麻烦。

我的小技巧：在代码里，我一般不会手动去算旋转矩阵的逆，直接用转置。又快又不容易出错。但前提是你得确保这个矩阵确实是旋转矩阵，不是随便凑出来的。

旋转矩阵虽然数学上很完美，但9个参数，不够直观。你想想看，你跟同事说“末端姿态的旋转矩阵是 [[0.707, -0.707, 0], [0.707, 0.707, 0], [0, 0, 1]]”，他肯定一脸懵。但如果你说“绕Z轴转45度”，他立刻就懂了。

欧拉角就是干这个的。它用三个角度来表示旋转，最常见的是 ZYX 欧拉角（也叫RPY角，Roll、Pitch、Yaw）。

从欧拉角到旋转矩阵的转换，就是三个基本旋转矩阵的乘积：R = R_z(yaw) * R_y(pitch) * R_x(roll)。注意顺序，先转的放右边。

我曾经踩过的坑：欧拉角有个著名的“万向锁”问题。当 pitch 接近 ±90° 时，yaw 和 roll 的旋转轴会重合，丢失一个自由度。这时候姿态就“锁死”了，没法平滑控制。所以，在做轨迹规划时，如果涉及大范围姿态变化，我建议用四元数，别用欧拉角。这个我们后面会讲到。

前面我们分别讲了旋转和平移。但在实际计算中，我们经常需要同时处理旋转和平移。比如，从基座坐标系到末端执行器坐标系的变换，既有旋转又有平移。

齐次变换矩阵就是干这个的。它是一个 4x4 的矩阵，把旋转矩阵和平移向量整合在一起：

T = | R   P |
    | 0   1 |

其中，R 是 3x3 的旋转矩阵，P 是 3x1 的平移向量，最后一行是 [0, 0, 0, 1]。

使用齐次变换矩阵，我们可以把坐标变换写成非常简洁的形式：

P_A = T_A_B * P_B

这里的 P_A 和 P_B 都是齐次坐标（4x1 向量，最后一位是1）。

这样做的好处是，多个变换可以连乘：T_A_C = T_A_B * T_B_C。这就把复杂的坐标链串起来了。

实战经验：在写机器人运动学代码时，我几乎所有的位姿都用齐次变换矩阵来表示。它统一了旋转和平移，让代码逻辑非常清晰。而且，求逆也很方便：T^{-1} = | R^T -R^T*P |，记住这个公式，手算和编程都能省不少事。

为了让大家更直观地理解这一章的知识结构，我画了一张图。你可以看到，刚体运动描述是起点，它分出了位置和姿态。姿态可以用旋转矩阵或欧拉角表示，而位置就是平移向量。最后，齐次变换矩阵把它们俩统一起来，成为我们进行坐标变换的终极武器。

这张图把这一章的核心逻辑串起来了。从刚体运动描述出发，分支出位置和姿态，姿态又可以用旋转矩阵或欧拉角表示，最后齐次变换矩阵把它们统一，用于坐标变换。你把这个图记在脑子里，这一章就算学透了。

好了，这一章的内容就到这里。运动学是机器人学的基石，这些概念会贯穿整个课程。大家课后可以自己动手，用 Python 或者 MATLAB 写几个小函数，验证一下旋转矩阵的正交性，或者试试欧拉角到旋转矩阵的转换。动手做一遍，比看十遍都管用。