02 刚体变换Rigid-Body Transformations — 旋转 · 平移 · 齐次坐标

1 坐标不是点的属性

同一个杯子，在桌上说“往东 30 厘米”，站在门口说“往北 2 米”，指的是同一个杯子。三个数全变了，杯子没动。坐标是“相对谁量”的读数，不是物体自带的标签。机器人学里绝大多数“算出来的结果和画面不一致”的困惑，追到底都是这一句没站稳。

所以本章的第一件事不是记公式，而是把“相对谁”这件事本身变成一个可以参与计算的对象——也就是坐标系。

2 质点三个数，刚体六个

描述空间中的一个质点，坐标 $(x,y,z)$ 三个数就够：位置本身就是全部信息。

机器人的连杆不是质点，是刚体——它上面任意两点的距离永远不变。于是“它在哪”不再够用，还必须回答“它朝哪”。位置 3 个自由度，姿态 3 个自由度，一共 6 个。

这里要分清两件事：自由刚体在空间中有 6 个自由度；一条六关节机械臂的位形空间也是 6 维，但那 6 个是关节角，不是位置坐标。本章只解决前者的描述，第 04 课才把关节角和刚体位形接起来。

3 描述姿态的最小装置：两个坐标系

一个固结在参考空间，记作 $A$（空间系）；一个固结在刚体上，记作 $B$（物体系），随刚体一起运动。

为什么必须是两个？一个坐标系只能给点标坐标，说不出“一根轴朝哪”——“朝哪”永远是相对另一根轴讲的。这也解释了后面所有记号为什么都带两个下标：$R_{AB}$、$g_{AB}$，第一个字母是参考系，第二个是被描述的那个。

于是“$B$ 相对 $A$ 的关系”这一个数学对象，就完整携带了刚体的位置与姿态。本章全部内容都长在这个对象上。

4 本章要造的三件东西

只转不移：旋转矩阵 $R_{AB}$（02.1）。又转又移：齐次变换 $g_{AB}$（02.2）。反过来问“$A$ 相对 $B$ 是什么”：$g$ 的逆（02.2 内）。

这三件东西是整门课的地基：速度是 $g$ 对时间求导，力用同一套矩阵搬运，第 04 课的正运动学是 $g$ 的连乘，第 05 课的雅可比是它的微分。

5 前置知识，以及一个 30 秒自检

需要四样：矩阵乘法、转置、逆、行列式；向量点积与叉积；$\sin$、$\cos$ 的和角公式；特征值（03 课才用）。微积分只在 03.1 解一个最简单的常微分方程时出现。

自检：能不能在纸上推出 $\cos(\theta+90^\circ) = -\sin\theta$？推不出来先补三角恒等式——02.1 的第一步就卡在这里。

6 这个页面怎么用

每一节都是同一套节奏：读一段 → 拖一次画布 → 对一眼右侧面板 → 做小测。文字和面板是同一个对象的两面：文字说“应该是什么”，面板给“现在是多少”。

两者对不上，说明理解有偏差，回去重读那一段，而不是继续往下翻。段落带编号，就是为了让你能回头精确地找到它。

1 把两根箭头写成数

设 $B$ 相对 $A$ 只转了一个角度 $\theta$，原点重合。“$B$ 的姿态”这句话翻译成数学，就是：$B$ 的两根轴各自指向 $A$ 里的哪个方向。

$x_B$ 与 $x_A$ 的夹角是 $\theta$，它的 $A$ 坐标就是 $(\cos\theta,\ \sin\theta)$，放进第 1 列；$y_B$ 比 $x_B$ 再转 $90^\circ$，坐标 $(\cos(\theta+90^\circ),\ \sin(\theta+90^\circ)) = (-\sin\theta,\ \cos\theta)$，放进第 2 列：

矩阵的列就是坐标轴。拖动红色手柄，右侧矩阵逐项联动——它不是抽象表格，就是那两根箭头的坐标。这句话是本节全部内容的钥匙。

2 为什么一定是“列”，不是“行”

点 $q$ 固结在刚体上。它在 $B$ 中的坐标 $q^{B}$ 不随运动改变，改变的是它在 $A$ 中的坐标。同一个箭头，用两套基展开：

把 $x_B = (\cos\theta,\sin\theta)$、$y_B = (-\sin\theta,\cos\theta)$ 代进去，按 $q_1^{B}$、$q_2^{B}$ 合并同类项：

系数正好是那两根轴的坐标。“列”不是约定，是被这个乘法逼出来的：第 1 列如果写成别的东西，$q^{A} = R\,q^{B}$ 立刻不成立。

3 同一个矩阵的两个身份（最容易混的地方）

身份一，描述：$R_{AB}$ 记录 $B$ 相对 $A$ 转了多少。身份二，换算：$R_{AB}$ 把 $B$ 系的读数换成 $A$ 系的读数。两个用途的数值完全相同、方向也一致，不是互为逆的两个操作。

互为逆的是 $R_{BA} = R_{AB}^{\mathsf{T}}$——它才是“$A$ 相对 $B$ 的姿态”，也是“把 $A$ 系读数换成 $B$ 系”的那个变换。

一个可靠的记忆钩子：列 = $B$ 的轴在 $A$ 中；行 = $A$ 的轴在 $B$ 中。用 $\theta = 90^\circ$ 核对：$R$ 的第 1 行是 $(0,-1)$，而 $A$ 的 $x$ 轴在转了 $90^\circ$ 的 $B$ 系里确实指向 $-y_B$。

4 三条性质，来源都是“列是单位正交向量”

两列长度都是 1，且互相垂直，于是 $R^{\mathsf{T}}R = I$。行列式只能是 $\pm 1$，而刚体运动不允许镜像（那会把左右手互换），所以取 $+1$：

于是逆等于转置：$R^{-1} = R^{\mathsf{T}}$。二维三维都是零成本，而且数值上最稳——不用做除法，不会因为接近奇异而炸掉。这条在 03 课反解旋转时还会再用一次。

反过来也要记住：任何同时满足这两条的矩阵都是合法旋转。它们因此可以当验算工具，考试写完 $R$ 花十秒查一遍，能挡住大部分符号错。

5 复合：下标接龙

$R_{AC} = R_{AB}R_{BC}$。中间字母相同才能相乘：先把 $C$ 系的读数换成 $B$ 系，再把 $B$ 系换成 $A$ 系，两步连着做。顺序不能交换——一换，中间的 $B$ 就接不上了。

三维里这不是空话。取 $R_x(90^\circ)$、$R_y(90^\circ)$，乘积 $R_xR_y$ 的第 1 列是 $(0,1,0)$，交换成 $R_yR_x$ 后变成 $(0,0,-1)$。两次旋转的顺序，改变了结果。

二维是特例：所有平面旋转互相可交换（同轴）。这个巧合不要往外推——03.1 会给出一般结论：矩阵指数只有在可交换时才允许把指数相加。

6 手算一遍：$\theta = 30^\circ$

取 $q^{B} = (1,0)$：这是 $B$ 的 $x$ 轴上一单位长的点，转 $30^\circ$ 后应落在 $(\cos 30^\circ,\ \sin 30^\circ) = (0.866,\ 0.500)$。用矩阵算：第一行 $0.866 \times 1 + (-0.5)\times 0 = 0.866$，第二行 $0.5 \times 1 + 0.866 \times 0 = 0.5$。对上了。

再取 $q^{B} = (0,1)$：结果是 $(-0.5,\ 0.866)$，正好是第 2 列。把两个基向量各算一次，等于把矩阵的两列各读了一次——这是检查自己有没有算错的最快办法。

反查：$q^{A} = (0,1)$（$A$ 的 $y$ 轴）在 $B$ 里是多少？用 $R^{\mathsf{T}}$ 乘，得 $(0.5,\ 0.866)$。这同时验证了上面的“行”规则：它就是 $R$ 的第 2 行。

7 面板操作清单

拖红色手柄：$\theta$ 变，两列一起转，第 1 列始终指向 $x_B$。拖黑点 $q$：$q^{B}$ 变而 $R$ 不动——刚体上的点在哪，和刚体朝哪，是两件互不干扰的事。

点“转到 90°”：第 1 列变 $(0,1)$、第 2 列变 $(-1,0)$，$R^{\mathsf{T}}R$ 仍是 $I$。看算术流水：面板把 $R\,q^{B}$ 拆成逐项乘加，两个分量各占一行，手算卡住时对照这里找差异。

1 $R$ 缺了半句话

$q^{A} = R\,q^{B}$ 成立的前提是：$B$ 的原点与 $A$ 的原点重合。真实机械臂上这个前提几乎从不成立——第二根连杆的坐标系原点，离第一根有好几十厘米。

拿数看一眼：$\theta = 90^\circ$，$B$ 原点在 $A$ 的 $(2,1)$，$q^{B} = (1,0)$。$R\,q^{B} = (0,1)$，可真实的点在 $(2,2)$——差了整整一个原点偏移。旋转没错，缺的是平移。

2 先转再移：仿射形式

补上平移，最直接的形式是

$p_{AB}$ 的定义要咬死：$B$ 的原点在 $A$ 中的坐标。它描述一个随刚体运动的点，不是“每次平移了多少”，也和 $\theta$ 无关——$B$ 原地转身，$p$ 一动不动。

顺序同样不能倒：先 $R$ 后加 $p$。写成 $R(q^{B} + p)$ 就变成了“先在 $B$ 系里挪一段，再整体转”，是另一个变换，结果也不同。

3 为什么必须升一维

矩阵乘法是线性的：$M(0)=0$，$M(ax+by)=aMx+bMy$。而“加上 $p$”把原点挪走了，它不是线性映射，$2\times2$ 的矩阵装不下它。

齐次坐标的办法：给点补一个 $1$，把 $2\times2$ 扩成 $3\times3$：

把乘法展开：前两行给出 $R\,q^{B} + p\cdot 1$，正是要的仿射式；底行给出 $0\cdot q^{B} + 1\cdot 1 = 1$，恒成立，意思是“补的那个 $1$ 保持为 $1$”。加法的位置，就是这样被 $1$ 腾出来的。

三维把 $2\times2$ 换成 $3\times3$、$3\times3$ 换成 $4\times4$，结构一模一样。本页画布是二维，所以面板显示 $3\times3$。

4 齐次坐标顺手解决了另一件事

方向向量（速度、力、轴）不该被平移影响。把补的那个数写成 $0$：

只转不移。同一套 $4\times4$ 矩阵因此能同时搬运“点”和“方向”，区别只在最后一个数是 $1$ 还是 $0$。这个区分在后面会变成 twist 与 wrench 的区别——现在只要记住：点是 $1$，方向是 $0$。

5 复合时 $p$ 的公式是易错点

把两个 $g$ 按块乘开：

所以

位置块不是 $p_{AB} + p_{BC}$。几何上很清楚：$p_{BC}$ 是 $C$ 原点在 $B$ 系里的坐标，得先用 $B$ 的朝向把它转到 $A$ 的方向（乘 $R_{AB}$），才能和 $B$ 原点在 $A$ 的位置相加。

漏掉那个 $R_{AB}$ 是最高频的复合错误，而且只在 $R_{AB}=I$ 时才碰巧正确——所以对着一个“转过的”例子验一遍，就会暴露。

6 求逆：2024 Q1(a) 原题

求 $g^{-1}$ 不需要背公式，解方程就行。设 $g^{-1}$ 的左上块为 $A$、右上块为 $b$，由 $g\,g^{-1}=I$ 得到两个块方程：$R\,A = I$ 与 $R\,b + p = 0$。于是 $A = R^{\mathsf{T}}$、$b = -R^{\mathsf{T}}p$：

几何读法：$-R^{\mathsf{T}}p$ 就是 $A$ 的原点在 $B$ 中的坐标。先算 $R^{\mathsf{T}}p$（把 $p$ 转进 $B$ 的方向），再取反（从 $B$ 原点指回 $A$ 原点）。

考场写法，三步：左上写 $R^{\mathsf{T}}$；右上写 $-R^{\mathsf{T}}p$（转置、乘 $p$、添负号）；底行照抄零行加 1。不需要思考，但一定要用 $g^{-1}g = I$ 或代一个点复核，符号错一眼就能看出来。

7 数值走一遍

取 $\theta = 90^\circ$、$p_{AB} = (2,1)$、$q^{B} = (1,0)$。

正向：$R\,q^{B} = (0,1)$，加 $p$ 得 $q^{A} = (2,2)$。

逆向：$R^{\mathsf{T}}$ 的两行分别是 $(0,1)$ 与 $(-1,0)$，所以 $R^{\mathsf{T}}p = (1,-2)$，取反得 $(-1,2)$。核对：$R^{\mathsf{T}}q^{A} + (-1,2) = (2,-2) + (-1,2) = (1,0)$，回到 $q^{B}$。

8 面板怎么用，以及和考场的关系

拖 $B$ 原点（黑手柄）：只有第 3 列在变，左上 $2\times2$ 一动不动。拖红色手柄：只有左上块在变。$g$ 的两块各管各的，这就是“打包”的含义。

下方流水实时算 $-R^{\mathsf{T}}p$，并核对 $g^{-1}g = I$。把它和自己手算的一行对照，就知道错在转置、乘法还是符号。

规则。三道题，做完即时反馈。答错不算失败——错题会告诉我们明天从哪里开始。做题时可以用上方画布：把 02.1 转到对应角度，右侧矩阵就是答案的对照。