📚 课程目录第2章机器人运动学基础

2. 机器人运动学基础

各位同学，欢迎来到第二章。说实话，运动学基础这部分，是后面所有抓取规划算法的「地基」。我见过不少同学，一上来就猛啃路径规划算法，结果代码跑起来，机械臂要么撞到桌子，要么抓了个寂寞——问题往往就出在坐标变换没搞对。

这一章，我会带你把这些「地基」打牢。咱们不讲虚的，直接上干货。

先问个问题：你让机械臂去抓桌上的杯子，你告诉它「杯子在桌子中心前方10厘米」。但机械臂的「大脑」里，它的基座坐标系原点在肩膀位置。它怎么知道「桌子中心前方10厘米」对应自己肩膀的什么位置？

这就是刚体变换要解决的问题。说白了，就是描述一个物体在空间中的位置和姿态。位置好理解，就是三个坐标 (x, y, z)。姿态呢？就是物体是正着、歪着、还是倒着。

我个人习惯把刚体变换拆成两部分看：旋转（姿态） + 平移（位置）。咱们先啃最核心的——旋转。

旋转矩阵，就是一个 3x3 的矩阵，用来描述一个坐标系相对于另一个坐标系的旋转关系。

它的每一列，其实就是被旋转坐标系的基向量在参考坐标系下的投影。举个例子，坐标系 {B} 绕坐标系 {A} 的 Z 轴旋转 θ 角度，旋转矩阵长这样：

R = | cosθ  -sinθ  0 |
    | sinθ   cosθ  0 |
    | 0      0     1 |

嗯，看着挺规整。但我在项目里用旋转矩阵时，最头疼的就是它太「冗余」了。9个元素，但真正独立的只有3个自由度。而且，你想想看，如果你要连续旋转好几次，矩阵乘法一乘，很容易积累数值误差。更别提它还不直观——你盯着一个 3x3 矩阵，能一眼看出它代表什么姿态吗？反正我不能。

  核心性质：旋转矩阵是正交矩阵，满足 RT = R-1，且 det(R) = 1。这个性质在后续求逆变换时非常有用。

欧拉角就直观多了。它用三个角度来描述旋转：绕 X 轴转（滚转 Roll）、绕 Y 轴转（俯仰 Pitch）、绕 Z 轴转（偏航 Yaw）。也就是我们常说的 RPY 角。

比如，一个常见的旋转顺序是 Z-Y-X：先绕 Z 转 γ，再绕 Y 转 β，最后绕 X 转 α。对应的旋转矩阵就是三个基本旋转矩阵的乘积：

R = Rz(γ) * Ry(β) * Rx(α)

听起来很美好，对吧？但这里有个大坑——万向锁。

避坑指南：我曾经在一个机械臂的逆解模块里用了欧拉角，结果当俯仰角 β 接近 ±90° 时，滚转和偏航变得无法区分，系统直接失去了一个自由度。当时排查了好久，最后发现是万向锁在作怪。从那以后，我在需要插值和连续旋转的场景里，再也不敢只用欧拉角了。

为什么会这样？因为欧拉角的表示不是唯一的，而且存在奇异点。说白了，就是当第二个旋转角达到 ±90° 时，第一个和第三个旋转轴会重合，导致自由度丢失。

既然旋转矩阵太冗余，欧拉角有奇点，那有没有更好的方案？有，就是四元数。

四元数可以看作是复数的推广，由一个实部和三个虚部组成：

q = w + xi + yj + zk

其中 w 是实部，(x, y, z) 是虚部，且满足 i² = j² = k² = ijk = -1。

用四元数表示绕单位向量 (u_x, u_y, u_z) 旋转 θ 角度：

q = cos(θ/2) + sin(θ/2) * (u<sub>x</sub>i + u<sub>y</sub>j + u<sub>z</sub>k)

我个人觉得，四元数最大的好处是：没有奇点，插值平滑，计算效率高。在机器人路径规划里，我们经常要对姿态做平滑插值，四元数的球面线性插值（SLERP）简直是神器。

我的经验：在写抓取规划代码时，我习惯在内部计算全部用四元数，只在最后给用户展示或调试时，才转成欧拉角。这样既避免了万向锁，又保留了直观性。

好了，旋转搞定了，平移怎么加进来？总不能旋转用矩阵，平移用向量，分开算吧？太麻烦了。

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

T = | R   t |
    | 0   1 |

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

这样，一个点 p 在坐标系 {B} 下的坐标，要变换到坐标系 {A} 下，只需要一次矩阵乘法：

p<sub>A</sub> = T<sub>AB</sub> * p<sub>B</sub>

注意，这里的 p 是齐次坐标，也就是在三维坐标 (x, y, z) 后面加一个 1，变成 (x, y, z, 1)。

连续变换就更方便了。比如从基座到工具末端的变换，可以写成：

T<sub>base_to_tool</sub> = T<sub>base_to_shoulder</sub> * T<sub>shoulder_to_elbow</sub> * T<sub>elbow_to_wrist</sub> * T<sub>wrist_to_tool</sub>

嗯，这就是机器人运动学里最核心的「链式法则」。你想想看，如果没有齐次变换矩阵，这一串变换写出来得多麻烦。

下面这张图，把这一章的核心概念和它们之间的关系梳理了一下。你可以把它当作一个「地图」，学完这一章后，对照着检查自己是否都掌握了。

为了方便你选择，我把这几种方法的核心特性整理成了表格。你在实际项目中，可以根据场景来选。

我的建议：如果你刚开始学，先吃透旋转矩阵和齐次变换矩阵，这是基础中的基础。等你要做轨迹规划、姿态插值时，再深入四元数。欧拉角嘛，用来做调试显示和人工输入就够了，别在核心算法里用它。

好了，这一章的内容就是这些。记住，运动学变换是机器人抓取的「语言」，你后面写的每一行路径规划代码，都离不开它。多动手推导几次，把代码写出来跑一跑，比光看强一百倍。