Part I · 第 5 章 刚体运动与李群 · 5.3

齐次变换与 SE(3)

旋转加平移不是线性变换，没法直接写成一个矩阵乘法。给坐标多添一个 1，升高一维，"旋转加平移"就变成了一个 4×4 矩阵，位姿的连乘、求逆都变成了普通的矩阵运算。这样的矩阵构成的集合就是 SE(3)，它是李群的第一个主角。

约 25 分钟 前置：1.3 矩阵即变换 · 1.6 逆 · 5.1 坐标系与位姿 · 5.2 旋转的表示 5 个交互

    1为什么机器人需要它

    机械臂有六七个关节，每个关节处的坐标系相对前一个都是"转一下再平移一段"。末端在底座里的位姿，是这一长串"旋转 + 平移"的连乘。用 5.1 节的写法 \((R, \mathbf t)\)，每乘一次要算 \(R_1R_2\) 和 \(R_1\mathbf t_2 + \mathbf t_1\) 两个式子，求逆还要记 \(-R\T\mathbf t\)。关节多了，推导和代码都很容易写错。

    齐次坐标的好处是：把"旋转 + 平移"装进同一个矩阵，连乘就是矩阵乘法，求逆有固定的分块公式。相机投影 \(P = K\,[R\,|\,\mathbf t]\)、机械臂的正运动学、SLAM 里的位姿图，都靠这一个小技巧。更重要的是，这样得到的矩阵集合 SE(3) 本身是一个群，下一节会看到，它也是一个弯曲的光滑流形，这正是李群理论登场的地方。

    2直觉：升一维，把平移变成"剪切"

    线性变换必须把原点留在原点（1.3 节），所以平移不是线性的。但看一个小把戏：把平面上的点 \((x, y)\) 放到三维空间的高度 \(w = 1\) 的那张平面上，变成 \((x, y, 1)\)。现在对三维空间做一个剪切：每个点沿 \(xy\) 方向平移的距离，和它的高度 \(w\) 成正比，\((x, y, w) \mapsto (x + t_x w,\ y + t_y w,\ w)\)。高度为 1 的那张平面上，每个点恰好平移了 \((t_x, t_y)\)；这个剪切是三维里的线性变换，因此可以写成矩阵乘法。

    还有一个意外的好处：高度 \(w = 0\) 的点，剪切对它们没有任何影响。所以 \(w=0\) 的"点"可以用来表示方向向量，它们只会被旋转，不会被平移。下面的实验里，拖动 \(t_x, t_y, \theta\)：琥珀色的点（\(w=1\)）会被平移，紫色的箭头（\(w=0\)）不会。

    FIG 1升一维：平面平移 = 三维剪切
    拖动画面旋转视角
    灰色是 \(w=0\) 的地面和 \(w=1\) 的"平面世界"。灰色的 F 是原来的图形，琥珀色的 F 是变换后的图形，它仍然停在 \(w=1\) 的平面上，只是平移（并旋转）了。三色箭头是矩阵的三列，也就是 \(\hat x\)、\(\hat y\)、\(\hat w\) 的像：蓝色的 \(\hat w\) 会歪向 \((t_x, t_y, 1)\)，这正是平移信息所在的那一列。
    试一试
    • 令 \(\theta = 0\)，把 \((t_x, t_y)\) 拉到长度超过 2。琥珀色的 F 平移了，蓝色箭头歪向哪里？紫色箭头动了吗？
    • 令 \(t = 0\)，把 \(\theta\) 转到 90° 左右。整个"平面世界"绕蓝色的 \(w\) 轴转动，紫色箭头也跟着转。
    核心洞见

    矩阵的最后一列是平移，因为 \(\hat w\)（即"原点"）被送到了 \((\mathbf t, 1)\)。矩阵的左上角是旋转，右下角的 1 保证平面 \(w=1\) 被送回 \(w=1\)。对点 \((\mathbf p, 1)\) 做乘法，得到 \((R\mathbf p + \mathbf t,\ 1)\)，恰好就是"先转后移"。

    3动手玩：SE(2) 与求逆

    平面里的刚体运动构成 SE(2)，位姿是 \((x, y, \theta)\)，3×3 齐次矩阵是

    $$ T = \begin{bmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}, \qquad T^{-1} = \begin{bmatrix} R\T & -R\T\mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}. $$

    \(T\) 把机器人系的读数变成世界系的读数，\(T^{-1}\) 反过来。直观上，\(T^{-1}\) 就是"站在机器人身上看世界"：机器人变成原点，朝向变成 \(+x\)，世界系被放到了 \(T^{-1}\) 所描述的位置。左边是世界视角，右边是机器人视角。拖动任何一边的点，另一边同步更新。

    FIG 2SE(2)：T 与 T⁻¹，世界视角与机器人视角
    世界视角：拖动机器人和点
    机器人视角（用 T⁻¹）
    左：世界系 W（灰）和带着机器人系 B 的三角形。右：同一个场景，但以机器人为中心；灰色的 W 系画在 \(T^{-1}\) 的位置。琥珀点在两边是同一个点，读数分别是 \(\mathbf p^W = T\mathbf p^B\) 和 \(\mathbf p^B = T^{-1}\mathbf p^W\)。下面的矩阵里，琥珀色是平移那一列。
    试一试
    • 把机器人放到 \((3, 1)\)、朝向 \(90°\)。预测 \(T^{-1}\) 的平移列，再核对读数。（答案：\((-1, 3)\)，而不是 \((-3,-1)\)。）
    • 在右图里把点拖到机器人正前方 2 米，也就是 \(\mathbf p^B = (2, 0)\)。再看左图：它在世界里的哪里？

    三维：SE(3) 编辑器

    三维位姿是 \(T = \begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1\end{bmatrix} \in \R^{4\times4}\)，其中 \(R\) 的三列依次是机体 \(x, y, z\) 轴在世界里的方向（红、绿、蓝），\(\mathbf t\) 是机体原点的位置（琥珀）。用六个滑块摆一个位姿（\(R = R_z(\text{yaw})R_y(\text{pitch})R_x(\text{roll})\)，5.2 节），观察 4×4 矩阵每一块怎样对应到图上。

    FIG 3SE(3)：4×4 矩阵的每一块
    拖动画面旋转视角
    淡色是世界系，亮色是机体系 \(\{B\}\)（带箱体）。琥珀小点 \(\mathbf p\) 固定在机体上，坐标是 \(\mathbf p^B = (0.8, 0, 0.5)\)；紫色小点 \(\mathbf q\) 固定在世界里，\(\mathbf q^W = (1.6, 1.0, 0.6)\)。前者用 \(T\) 变到世界系，后者用 \(T^{-1}\) 变到机体系。把鼠标放到下方"再看数学"一节公式里带底色的 \(R\)、\(\mathbf t\) 上，这里的箭头会加粗。
    试一试
    • 让机体的 \(x\) 轴（红）指向正上方，即 \(R\) 的第一列变成 \((0,0,1)\)。需要什么 pitch？（顺便想想：这是哪一种欧拉角奇异？）
    • 用平移滑块把机体原点放到紫色点 \(\mathbf q\) 上（距离小于 0.12）。此时 \(T^{-1}\mathbf q\) 是什么？为什么？

    连乘：平面三连杆

    每个关节处的坐标系相对前一个是"先沿前一段连杆走 \(L\)，再转 \(q\)"：\(T_{i-1,i} = \mathrm{Trans}(L_{i-1})\,\mathrm{Rot}(q_i)\)。末端相对底座的位姿就是把它们依次乘起来：\(T_{0E} = T_{01}\,T_{12}\,T_{23}\,T_{3E}\)。拖动三个关节（圆圈）。

    FIG 4连乘 T₀ₑ = T₀₁T₁₂T₂₃T₃ₑ
    拖动三个圆圈（关节 / 末端）
    连杆长度 \(L_1, L_2, L_3 = 1.0, 0.8, 0.5\)。每个关节画出它的坐标系（红 \(x\)、绿 \(y\)）。把鼠标放到公式里的 \(T_{01}\)、\(T_{12}\)、\(T_{23}\) 上，会加粗对应关节的坐标系。绿色圆圈是目标点。
    试一试
    • 让末端落到绿色目标点上（距离小于 0.08）。读一读 \(T_{0E}\) 的最后一列。
    • 把三个关节全拉直（\(q_1=q_2=q_3=0\)）。这时每个 \(T_{i-1,i}\) 是什么样子？

    4再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 3、FIG 4 或 FIG 5 中对应的元素会被高亮。

    齐次坐标：点和方向

    给 \(\mathbf p \in \R^3\) 添一个分量 1，得到齐次坐标 \(\tilde{\mathbf p} = (\mathbf p, 1) \in \R^4\)。方向（自由向量）则用 \((\mathbf d, 0)\)。位姿 \(T\) 作用在它们上：

    $$ T\tilde{\mathbf p} = \begin{bmatrix} \hl{R}{R} & \hl{t}{\mathbf t} \\ \mathbf 0\T & 1 \end{bmatrix}\begin{bmatrix} \mathbf p \\ 1 \end{bmatrix} = \begin{bmatrix} R\mathbf p + \mathbf t \\ 1 \end{bmatrix}, \qquad T\begin{bmatrix} \mathbf d \\ 0 \end{bmatrix} = \begin{bmatrix} R\mathbf d \\ 0 \end{bmatrix}. $$

    点 − 点 = 方向（\(w\) 分量 \(1 - 1 = 0\)），点 + 方向 = 点（\(1 + 0 = 1\)），点 + 点没有意义（\(w = 2\)）。这和直觉一致：两个位置相减得到位移，两个位置相加没有物理意义。

    SE(3) 的定义

    $$ \SE = \left\{ T = \begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix} \;\middle|\; R \in \SO,\ \mathbf t \in \R^3 \right\}, \qquad \mathrm{SE}(2) \text{ is the same with } R \in \mathrm{SO}(2),\ \mathbf t\in\R^2. $$

    "S"是 special（\(\det R = +1\)，没有镜像），"E"是 Euclidean（保持距离）。它有 \(3 + 3 = 6\) 个自由度（SE(2) 有 \(1 + 2 = 3\) 个）。注意 \(T\) 本身不是正交矩阵：\(T\T T \ne I\)，所以 \(T^{-1} \ne T\T\)。

    连乘与求逆

    分块矩阵相乘，正好重现了 5.1 节的两个公式：

    $$ \begin{bmatrix} R_1 & \mathbf t_1 \\ \mathbf 0\T & 1 \end{bmatrix}\begin{bmatrix} R_2 & \mathbf t_2 \\ \mathbf 0\T & 1 \end{bmatrix} = \begin{bmatrix} R_1R_2 & R_1\mathbf t_2 + \mathbf t_1 \\ \mathbf 0\T & 1 \end{bmatrix}, \qquad T^{-1} = \begin{bmatrix} R\T & -R\T\mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}. $$

    验证逆：\(\begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}\begin{bmatrix} R\T & -R\T\mathbf t \\ \mathbf 0\T & 1 \end{bmatrix} = \begin{bmatrix} RR\T & -RR\T\mathbf t + \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix} = I\)。求逆不需要解线性方程组，只要一次转置和一次矩阵乘向量。而且 \(\det T = \det R = 1\)，\(T^{-1}\) 一定存在。

    它是一个群

    上面的公式说明 SE(3) 满足群的四条要求：封闭（两个 \(T\) 相乘还是 \(T\)）、结合律（矩阵乘法天然满足）、有单位元 \(I\)、每个元素有逆。但它不可交换：\(T_1T_2 \ne T_2T_1\)（5.1 节的左乘右乘之别，以及 FIG 4 里"先转后移"和"先移后转"的区别）。群的定义是下一节（5.4）的主题。

    平面三连杆的正运动学

    $$ \hl{k1}{T_{01}} = \mathrm{Rot}(q_1),\quad \hl{k2}{T_{12}} = \mathrm{Trans}(L_1)\,\mathrm{Rot}(q_2),\quad \hl{k3}{T_{23}} = \mathrm{Trans}(L_2)\,\mathrm{Rot}(q_3),\quad T_{3E} = \mathrm{Trans}(L_3), $$ $$ T_{0E} = T_{01}\,T_{12}\,T_{23}\,T_{3E}, \qquad \mathrm{Trans}(L) = \begin{bmatrix} 1 & 0 & L \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}. $$

    读最后一列就得到末端位置，左上角的 \(2\times2\) 块给出末端朝向 \(\theta = q_1 + q_2 + q_3\)（平面里的角度直接相加）。三维关节用的是同样的思路，5.10 节会把它改写成指数积公式。

    5机器人中的它：手眼标定误差

    相机装在机械臂的手上（eye-in-hand）。相机看到杯子，给出 \(\mathbf p^C\)；机器人要把杯子变到底座系才能规划抓取：

    $$ \tilde{\mathbf p}^{B} = T_{BE}\,\hl{cal}{T_{EC}}\,\tilde{\mathbf p}^{C}. $$

    \(T_{BE}\) 来自关节角和正运动学（FIG 4），非常准；\(T_{EC}\) 是相机相对手的安装位姿，由手眼标定得到，一定有误差。下面让 \(T_{EC}\) 的真值比标定值多转了 \(\varepsilon\)。注意这个小角度误差会被距离放大：相机离杯子越远，估计出来的杯子位置偏得越多。

    FIG 5eye-in-hand：标定误差被距离放大
    拖动杯子
    两连杆机械臂（底座在原点），手上装着相机。实线相机是真值（物理上相机的真实安装），虚线相机是机器人以为的安装。琥珀点是杯子的真值，红点是用标定值算出来的估计。红线的长度就是抓取偏差。把鼠标放到上面公式里的 \(T_{EC}\) 上，标定的安装（手到虚线相机）会以琥珀色加粗显示。
    试一试
    • 把标定误差 \(\varepsilon\) 调到 0：估计和真值重合，红线消失。
    • 设 \(\varepsilon = 5°\)，把杯子拖到离相机 1.2 m 以外，让偏差超过 10 cm。偏差 ≈ 距离 × \(\varepsilon\)（弧度）。
    真实系统里

    机械臂控制器内部把每个连杆的位姿存成 4×4 矩阵（或旋转 + 平移的对），正运动学就是一连串乘法。相机的投影矩阵 \(P = K[R\,|\,\mathbf t]\) 里的 \([R\,|\,\mathbf t]\) 就是 \(T_{CW}\) 去掉最后一行。点云配准（ICP）输出的是一个 4×4 的 \(T\)。SLAM 的位姿图每条边都是一个相对位姿 \(T_{ij}\)。数值上，连乘几千次后 \(R\) 会慢慢漂离旋转矩阵，需要定期重新正交化（1.10 节）。

    6常见误区

    误区 1：用转置求 4×4 位姿的逆

    只有左上角的 \(R\) 满足 \(R^{-1} = R\T\)。整个 \(T\) 的逆是 \(\begin{bmatrix}R\T & -R\T\mathbf t\\ \mathbf 0\T & 1\end{bmatrix}\)，而 \(T\T = \begin{bmatrix}R\T & \mathbf 0\\ \mathbf t\T & 1\end{bmatrix}\) 完全是另一个东西。如果平移为零，两者碰巧一样，所以这个 bug 在原点附近测试时不会暴露。

    误区 2：把方向向量当成点来变换

    法向量、速度、力的方向、坐标轴方向都是方向，只应被旋转，不应被平移：用 \(R\mathbf d\)，或者用 \(w = 0\) 的齐次坐标。把它们当点 \((\mathbf d, 1)\) 变换，就会凭空加上 \(\mathbf t\)。反过来，把点当方向变换，位置就不会被平移到正确的地方。（注意：速度作为 twist 的一部分时，变换规则更复杂，见 5.8 节的伴随。）

    误区 3：把 SE(3) 里的位姿当向量相加、平均

    两个 \(T\) 相加得到的 4×4 矩阵不是位姿，\((T_1 + T_2)/2\) 的左上角不是旋转矩阵（5.4 节会看到它怎样"缩水"）。求平均、插值、做优化都不能直接对矩阵元素做加法，需要李群的工具（5.6–5.9 节）。

    误区 4：忘了左上角的 R 要保持正交

    矩阵乘法在浮点数里会累积误差。把一个位姿连乘上万次（比如积分里程计）之后，左上角不再是严格的旋转矩阵，长度开始缩放、直角开始倾斜。对策是定期把 \(R\) 投影回 \(\SO\)（SVD 或 Gram–Schmidt），或者把旋转存成单位四元数，随时归一化。

    7小练习

    1. 平面里把点平移 \((2, 3)\) 的 3×3 齐次矩阵是？
    平移放在最后一列（列向量约定）：\((x, y, 1)\mapsto(x + 2, y + 3, 1)\)。选项 A 是缩放，选项 C 是行向量约定下的写法（与本课程的列向量约定不同），选项 D 是剪切。
    2. 机器人位姿 \(T\) 的平移是 \(\mathbf t = (4, 0, 0)\)。机器人的前进方向 \(\mathbf d = (1, 0, 0)\)（\(w = 0\)）在世界里经 \(T\) 变换后，当 \(R = I\) 时是？
    \(w = 0\) 的方向不受平移影响：\(R\mathbf d + \mathbf t\cdot 0 = (1,0,0)\)。选项 B 是把它当成点 \((\mathbf d, 1)\) 来变换的结果；选项 A 加上了平移却保留 \(w = 0\)，自相矛盾。
    3. 平面位姿 \(T\)：\(R = R(90°)\)，\(\mathbf t = (3, 1)\)。\(T^{-1}\) 的平移是？
    \(-R\T\mathbf t\)，\(R\T = \begin{bmatrix}0&1\\-1&0\end{bmatrix}\)，\(R\T(3,1)^\top = (1,-3)\)，取负得 \((-1, 3)\)。\((1,-3)\) 忘了负号，\((-3,-1)\) 忘了旋转。
    4. 平面两连杆：\(L_1 = 1\)，\(q_1 = 90°\)，\(q_2 = 0\)，末端在第二根杆的末端，\(L_2 = 1\)。\(T_{0E} = \mathrm{Rot}(q_1)\mathrm{Trans}(L_1)\mathrm{Rot}(q_2)\mathrm{Trans}(L_2)\) 的末端位置是？
    \(\mathrm{Trans}(1)\mathrm{Rot}(0)\mathrm{Trans}(1)\) 把原点送到 \((2, 0)\)，再经 \(\mathrm{Rot}(90°)\) 变成 \((0, 2)\)。两根杆都沿着第一个关节转后的 \(+y\) 方向伸出去。
    5. 下列关于 SE(3) 的说法，哪一个是对的？
    封闭、结合、有单位元、有逆，加起来就是群；位姿的乘法不可交换。\(T\) 只有 \(R\) 块是正交的，\(T\T T \ne I\)。

    8延伸阅读

    • K. Lynch & F. Park, Modern Robotics, Ch. 3.3 — 齐次变换矩阵与 SE(3) 的性质
    • R. Hartley & A. Zisserman, Multiple View Geometry in Computer Vision, Ch. 2 — 齐次坐标与射影几何
    • R. Murray, Z. Li, S. Sastry, A Mathematical Introduction to Robotic Manipulation, Ch. 2 — 刚体运动的齐次表示
    • B. Siciliano et al., Robotics: Modelling, Planning and Control, Ch. 2.6 — 齐次变换