2.7 齐次变换

位姿 = 位置 + 指向，而「先加平移、再乘旋转」的写法在链式变换中会越写越长。本节用齐次坐标把两者打包进一个 4×4 矩阵，为 DH 法的连乘打好地基。录播 0:30:54–0:39:43。

2.7.1 动机：平移 + 旋转的写法太长

设点 $P$ 在参考系 $0$ 与动系 $1$ 中的坐标分别为 $\boldsymbol p^0,\boldsymbol p^1$ ，动系原点在系 $0$ 中的位置为 $\boldsymbol o_1^0$ 、姿态为 $R_1^0$ （原书图 2.11）。由三角关系可得（录播 0:32:08 老师：「看这个三角关系就对了」）：

$\boldsymbol p^0=\boldsymbol o_1^0+R_1^0\boldsymbol p^1\qquad(\text{原书式 }2.38)$

课堂幻灯片 · 约30:54：p^0 = o_1^0 + R_1^0p^1（平移 + 旋转）

这里有两个要点。其一， $\boldsymbol o_1^0$ 与 $R_1^0\boldsymbol p^1$ 必须先表示在同一坐标系里，才能相加（录播 0:32:27：「把所有的量都描述在同一个坐标系」）；其二，逆变换只要左乘 $(R_1^0)^T$ ，即得 $\boldsymbol p^1=-R_0^1\boldsymbol o_1^0+R_0^1\boldsymbol p^0$ （式 2.39–2.40）。

链式场景把问题暴露得更清楚：引入坐标系 2、消去中间量之后（录播 0:34:03 板书），

$\boldsymbol p^0=\boldsymbol o_1^0+R_1^0\boldsymbol o_2^1+R_1^0R_2^1\boldsymbol p^2$

每多加一个坐标系，式子就长一层：旋转部分要连乘、平移部分要累加，两种运算形式并不统一，没法直接合并——这正是要把它们打包的原因。

2.7.2 齐次坐标与齐次变换矩阵

定义 2.7.1 · 齐次表示与齐次变换矩阵

给一般向量 $\boldsymbol p$ 添上第 4 个单位元素，就构成齐次坐标（homogeneous representation，原书式 2.41）：

$\tilde{\boldsymbol p}=\begin{bmatrix}\boldsymbol p\\ 1\end{bmatrix},$

则式 2.38 可写成（4×4）矩阵乘法（原书式 2.42–2.43）

$\tilde{\boldsymbol p}^0=A_1^0\,\tilde{\boldsymbol p}^1,\qquad A_1^0=\begin{bmatrix}R_1^0 & \boldsymbol o_1^0\\ \boldsymbol 0^T & 1\end{bmatrix},$

称 $A_1^0$ 为齐次变换矩阵。因 $\boldsymbol o_1^0\in\mathbb{R}^3$ 、 $R_1^0\in SO(3)$ ， $A$ 属于特殊欧几里得群 $SE(3)=\mathbb{R}^3\times SO(3)$ 。

课堂幻灯片 · 约35:52：齐次变换矩阵的分块结构

把矩阵乘开验算一下：前 3 行恰好是 $R_1^0\boldsymbol p^1+\boldsymbol o_1^0$ ，第 4 行是 $1$ （录播 0:34:53 老师提醒这东西「实际上很简单」，0:36:17 又现场带着算了一遍）。各块的含义也就一目了然（录播 0:39:19）：左上 $3\times3$ 管指向，右上 $3\times1$ 管位置，底行占位；当两系原点重合时， $A$ 退化为纯旋转。

2.7.3 逆变换与非正交性

齐次变换矩阵不再保持正交性（原书式 2.46）：一般地 $A^{-1}\neq A^T$ 。逆变换其实不必另起炉灶——式 2.39–2.40 已经把它算出来了：

$\boldsymbol p^1=\big(R_1^0\big)^{\mathrm T}\boldsymbol p^0-\big(R_1^0\big)^{\mathrm T}\boldsymbol o_1^0 ,$

这是一次「旋转 $R^{\mathrm T}$ + 平移 $-R^{\mathrm T}\boldsymbol o$ 」的仿射映射，按定义 2.7.1 的打包规则，立刻就能写成矩阵形式（原书式 2.44–2.45）：

$A_0^1=(A_1^0)^{-1}=\begin{bmatrix}R_1^{0\mathrm T} & -R_1^{0\mathrm T}\boldsymbol o_1^0\ \boldsymbol 0^T & 1\end{bmatrix} =\begin{bmatrix}R_0^1 & -R_0^1,\boldsymbol o_1^0\ \boldsymbol 0^T & 1\end{bmatrix}$

验证 · 分块乘一次即知（本编补充）

$\begin{bmatrix}R^{\mathrm T}&-R^{\mathrm T}\boldsymbol o\\ \boldsymbol 0^{\mathrm T}&1\end{bmatrix} \begin{bmatrix}R&\boldsymbol o\\ \boldsymbol 0^{\mathrm T}&1\end{bmatrix} =\begin{bmatrix}R^{\mathrm T}R&\ R^{\mathrm T}\boldsymbol o-R^{\mathrm T}\boldsymbol o\\ \boldsymbol 0^{\mathrm T}&1\end{bmatrix} =\begin{bmatrix}I_3&\boldsymbol 0\\ \boldsymbol 0^{\mathrm T}&1\end{bmatrix} .$

（左上块用正交性 $R^{\mathrm T}R=I$ ；右上块两项恰好相消。）既左逆又右逆（同法右乘一遍），故确为 $A^{-1}$ 。□

□

这与式 2.39–2.40 的结论恰好一致：旋转块取转置，平移块先反向、再左乘转置。要注意旋转部分仍是正交的——求逆之所以「不便宜」，只在于不能整体转置了事，而必须按分块公式重新装配。

2.7.4 连乘：为什么机器人学离不开它

原理 2.7.A · 链式变换的连乘（原书式 2.47）

一串坐标变换可以合成单一矩阵：

$\tilde{\boldsymbol p}^0=A_1^0A_2^1\cdots A_n^{n-1}\,\tilde{\boldsymbol p}^n,$

其中 $A_i^{i-1}$ 是「点在系 $i$ 的表示 → 系 $i-1$ 的表示」的齐次变换。

公式只有一行，但含义值得看清：下图把「连乘」画了出来。左图是一条机械链，每根连杆上都固连一个坐标系，点 $P$ 的坐标从系 2 传回系 0，恰好是两次矩阵乘法；右图是单个 $A$ 的分块职责——一个矩阵同时管指向与位置。

齐次变换的链式合成（本教程绘）：左图——点 P 的坐标沿运动链「逐系外推」，每跨一系乘一个

A

，这就是原理 2.7.A 的连乘；右图——

A

的分块职责，底行占位使「平移 + 旋转」统一为矩阵乘法。

这正是把式 2.38 的链条「写简单点」得到的答案。录播 0:38:53 老师还留了一道验证作业：自己展开，验证 $\tilde{\boldsymbol p}^0=A_1^0A_2^1\tilde{\boldsymbol p}^2$ 与冗长的写法一致。连乘的工程意义（录播 0:39:01–0:39:15）在于：机器人要处理连续的多坐标系变换，而「式子太长不方便，先写成矩阵的形式，去矩阵重上重去（做矩阵乘法）」——2.8 节的正运动学方程 $T_n^0(q)=A_1^0(q_1)A_2^1(q_2)\cdots A_n^{n-1}(q_n)$ 就是本原理的直接套用。

se(3)：SE(3) 的李代数（本编补充，与 2.5.3 对照）

$so(3)$ 的故事在 $SE(3)$ 上原样重演一遍。 $SE(3)$ 在单位元（ $I_4$ ）处的「无穷小版本」是

$se(3)=\Big\{\begin{bmatrix}S(\omega)&\boldsymbol v\\ \boldsymbol 0^{\mathrm T}&0\end{bmatrix}:\ \omega,\boldsymbol v\in\mathbb R^3\Big\},$

它由「角速度 $\omega$ + 线速度 $\boldsymbol v$ 」共 6 个参数描述，装帽 / 摘帽的做法与 $so(3)$ 完全相同。它的元素称为空间速度 / 旋量（twist）。指数映射 $\exp\big(\begin{bmatrix}S(\omega)&\boldsymbol v\\\boldsymbol 0^{\mathrm T}&0\end{bmatrix}\big)$ 给出螺旋运动——绕一根不过原点的轴「边转边走」：当 $\omega=0$ 时退化为纯平移，当 $\boldsymbol v$ 平行于 $\omega$ 时则是真正的螺钉运动（第 4 章以后的螺旋理论与空间速度即源于此）。本课程按原书路线，用分块的 $(\dot{\boldsymbol p},\omega)$ 处理（第 3 章 3.1.2）； $se(3)$ 这套语言，则留给对照现代教材（如 Lynch & Park《Modern Robotics》）时使用。

为什么值得知道： $SE(3)$ 和 $so(3)$ 一样「不可交换、不能整体求逆转置」，但它的李代数 $se(3)$ 只是一个普通的 6 维线性空间。于是做插值、求导、优化时，可以先到李代数里去算，算完再通过指数映射回到群上（「角速度不是角度向量的导数」这一注意点，在 $se(3)$ 上同样成立，见 3.6.3）。

默写点 · 合上书再写（主动回忆）

不要翻正文与速查卡，先把下面几条默写出来，再展开核对。写不出来，就说明这一节还没有真正「拿走」。

写出齐次变换矩阵 $A$ 的分块结构与它的逆。
为什么 $A^{-1}\neq A^{\mathrm T}$ ？
方向向量（自由向量）的变换用 $A$ 的哪一块？

核对

$A=\begin{bmatrix}R&\boldsymbol o\\ \boldsymbol 0^{\mathrm T}&1\end{bmatrix}$ ， $A^{-1}=\begin{bmatrix}R^{\mathrm T}&-R^{\mathrm T}\boldsymbol o\\ \boldsymbol 0^{\mathrm T}&1\end{bmatrix}$ 。

正交性只在旋转块内成立；平移块使整个 $4\times4$ 矩阵不再正交。

只用左上块 $R$ ——自由向量不吃平移。

2.7 小结与常见错误

一个打包： $\tilde{\boldsymbol p}=[\boldsymbol p;1]$ 把「平移 + 旋转」压缩成一个 $SE(3)$ 矩阵，加法项随之消失，链条变成连乘；求逆则按分块公式 $A^{-1}=\begin{bmatrix}R^T & -R^T\boldsymbol o\ \boldsymbol 0^T & 1\end{bmatrix}$ （最快的推导：设出逆的分块形式，再解方程）。
一条边界：方向向量（自由向量）的变换只用左上块 $R$ ，不吃平移。

常见错误清单 · 自查三条

把 $A^T$ 当成 $A$ 的逆——正交性只在旋转块内成立（原书式 2.46）；
齐次坐标乘完后忘记「除以第 4 分量归一化」——本课程恒取第 4 分量为 1，所以这一步可以省略，但读其他教材（透视 / 缩放变换，原书脚注）时会遇到一般情形；
把 $\boldsymbol o_1^0$ 读成「系 0 原点在系 1 中的坐标」——上下标读反了；记住这句顺口令：右上块 = 「动系原点用固定系的话说」。