Modern Robotics 3.3.1 齐次变换矩阵详解：SE(3)、齐次坐标与坐标系变换

在 3.2 中，旋转矩阵 $R\in SO(3)$ 用来描述刚体参考系的姿态，也就是“朝向”。进入 3.3 后，我们要描述完整的刚体位形：既包括姿态，也包括位置。

如果 { b } \{b\} {b} 是物体参考系， { s } \{s\} {s} 是固定参考系，那么一个刚体位形可以由两部分组成：

$$R_{sb}\in SO(3),p_{sb}\in {\mathbb{R}}^3$$

其中 $R_{sb}$ 表示 { b } \{b\} {b} 相对 { s } \{s\} {s} 的姿态，$p_{sb}$ 表示 { b } \{b\} {b} 原点在 { s } \{s\} {s} 中的位置。3.3.1 的核心，就是把这两个量合并成一个矩阵：

$$T_{sb}=\left[\begin{array}{cc}{R}_{sb}& p_{sb}\\ 0& 1\end{array}\right]$$

这个矩阵就是齐次变换矩阵。

一、从 $R,p$ 到 $T\in SE(3)$

三维特殊欧氏群 $SE(3)$ 定义：

$$SE(3)=\left\{T=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]|R\in SO(3),p\in {\mathbb{R}}^3\right\}$$

写成完整矩阵是：

$$T=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_1\\ r_{21}& r_{22}& r_{23}& p_2\\ r_{31}& r_{32}& r_{33}& p_3\\ 0& 0& 0& 1\end{array}\right]$$

可以把 $SE(3)$ 先理解为所有合法三维刚体位形的集合。合法的意思是：左上角必须是旋转矩阵 $R\in SO(3)$，右上角是位置向量 $p$，最后一行固定为 $[0001]$。

书中也给出二维版本 $SE(2)$：

$$T=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & p_1\\ \sin \theta & \cos \theta & p_2\\ 0& 0& 1\end{array}\right]$$

二维和三维的思想相同：用一个矩阵统一表示旋转和平移。

二、为什么引入齐次坐标

对三维点 $x$ 做刚体变换时，结果是：

$$Rx+p$$

这里同时出现了矩阵乘法和向量加法。齐次坐标把点 $x$ 写成四维形式：

$$\left[\begin{array}{c}x\\ 1\end{array}\right]$$

于是旋转和平移可以合并为一次矩阵乘法：

$$T\left[\begin{array}{c}x\\ 1\end{array}\right]=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\left[\begin{array}{c}x\\ 1\end{array}\right]=\left[\begin{array}{c}Rx+p\\ 1\end{array}\right]$$

这个式子也解释了为什么 $T$ 要写成左上角 $R$、右上角 $p$、最后一行 $[0001]$ 的形式：最后一行保证点的齐次坐标乘完以后仍然是点，结果最后一维仍然是 1，可以继续参与下一次变换。

更重要的是，它让连续刚体变换可以直接写成矩阵连乘。若

$$T_1=\left[\begin{array}{cc}{R}_1& p_1\\ 0& 1\end{array}\right],T_2=\left[\begin{array}{cc}{R}_2& p_2\\ 0& 1\end{array}\right]$$

则：

$$T_1{T}_2=\left[\begin{array}{cc}{R}_1{R}_2& R_1{p}_2+p_1\\ 0& 1\end{array}\right]$$

乘积仍然是同样结构的齐次变换矩阵。也就是说，多个旋转和平移可以按顺序连续复合，而不需要每一步都拆成 $Rx+p$ 再手动整理。

三、齐次变换矩阵的基本性质

单位矩阵。 $I$ 是一个齐次变换矩阵，对应“没有旋转、没有平移”。

逆矩阵。 齐次变换矩阵的逆仍然是齐次变换矩阵。若

$$T=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]$$

则：

$$\begin{array}{rl}y& =Rx+p\\ x& =R^{T}y-R^{T}p\end{array}$$

$$T^{-1}\left[\begin{array}{c}y\\ 1\end{array}\right]=\left[\begin{array}{c}{R}^{T}y-R^{T}p\\ 1\end{array}\right]$$

$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]$$

封闭性。 两个齐次变换矩阵相乘，结果仍然在 $SE(3)$ 中：

$$T_1,T_2\in SE(3)\Rightarrow T_1{T}_2\in SE(3)$$

结合律与非交换性。 矩阵乘法满足结合律，但通常不满足交换律：

$$(T_1{T}_2)T_3=T_1(T_2{T}_3)$$

$$T_1{T}_2\ne T_2{T}_1$$

这意味着多个刚体变换可以连续复合，但顺序不能随意交换。

保持距离和角度。 若 $T=(R,p)$，则对点 $x,y,z\in {\mathbb{R}}^3$：

$$\Vert Tx-Ty\Vert =\Vert x-y\Vert$$

$$⟨Tx-Tz,Ty-Tz⟩=⟨x-z,y-z⟩$$

因此 $T$ 描述的是刚体运动，不会拉伸、压缩或剪切物体。

四、$T$ 的三种用法

和旋转矩阵一样，齐次变换矩阵也有三种主要用法：

1. 表示一个刚体或参考系的位形；
2. 改变点或参考系的坐标表达；
3. 主动移动点或参考系。

图 1：原书 Figure 3.14。三个参考系 { a } , { b } , { c } \{a\},\{b\},\{c\} {a},{b},{c} 和一个点 $v$。

1. 表示位形

若

$$T_{sb}=\left[\begin{array}{cc}{R}_{sb}& p_{sb}\\ 0& 1\end{array}\right]$$

则 $T_{sb}$ 表示 { b } \{b\} {b} 相对 { s } \{s\} {s} 的位形。其中：

• $R_{sb}$ 的三列，是 { b } \{b\} {b} 的三根坐标轴在 { s } \{s\} {s} 中的表达；
• $p_{sb}$ 是 { b } \{b\} {b} 原点在 { s } \{s\} {s} 中的位置。

下标可以这样读：

$$T_{\text{表达参考系},\text{被描述参考系}}$$

例如 $T_{bc}$ 表示 { c } \{c\} {c} 相对 { b } \{b\} {b} 的位形。图 1 中：

$$R_{bc}=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& -1\\ -1& 0& 0\end{array}\right],p_{bc}=\left[\begin{array}{c}0\\ -3\\ -1\end{array}\right]$$

也就是 { c } \{c\} {c} 的姿态和原点位置都用 { b } \{b\} {b} 来表达。

2. 改变坐标表达

齐次变换矩阵遵循下标消去规则：

$$T_{ab}{T}_{bc}=T_{ac}$$

含义是：先把 { c } \{c\} {c} 放在 { b } \{b\} {b} 中描述，再把 { b } \{b\} {b} 放在 { a } \{a\} {a} 中描述，最终得到 { c } \{c\} {c} 在 { a } \{a\} {a} 中的描述。

对点也类似。若点 $v$ 在 { b } \{b\} {b} 中的坐标为 $v_b$，则：

$$T_{ab}\left[\begin{array}{c}{v}_b\\ 1\end{array}\right]=\left[\begin{array}{c}{v}_a\\ 1\end{array}\right]$$

这个关系说明，$T_{ab}$ 可以把同一个几何点的坐标从 { b } \{b\} {b} 表达改写成 { a } \{a\} {a} 表达。

3. 主动移动参考系

同一个 $T$ 也可以表示一个实际施加的刚体运动。此时要特别区分左乘和右乘。

设当前位形为 $T_{sb}$，另有一个变换

$$T=(R,p)=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]$$

左乘表示固定系变换：

$$T_{s{b}^{\prime }}=T{T}_{sb}=\left[\begin{array}{cc}R{R}_{sb}& R{p}_{sb}+p\\ 0& 1\end{array}\right]$$

这里 $R$ 的旋转轴和 $p$ 的平移方向，都用固定参考系 { s } \{s\} {s} 解释。

右乘表示体系变换：

$$T_{s{b}^{\prime \prime }}=T_{sb}T=\left[\begin{array}{cc}{R}_{sb}R& R_{sb}p+p_{sb}\\ 0& 1\end{array}\right]$$

这里 $R$ 的旋转轴和 $p$ 的平移方向，都用当前物体参考系 { b } \{b\} {b} 解释。

图 2：原书 Figure 3.15。同一个变换 $T$，左乘和右乘对应不同的物理解释。

图 2 中取：

$$\hat{\omega }=(0,0,1),\theta =90^{\circ },p=(0,2,0)$$

对应的齐次变换为：

$$T=\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

左乘 $T{T}_{sb}$ 表示先绕固定系的 ${\hat{z}}_s$ 轴旋转，再沿固定系的 ${\hat{y}}_s$ 平移。右乘 $T_{sb}T$ 表示沿物体系自己的 ${\hat{y}}_b$ 平移，再绕新的物体系 $\hat{z}$ 轴旋转。因为解释参考系不同，最终结果一般不同。

五、Example 3.19：用下标组织变换链

图 3：原书 Figure 3.16。移动平台、机械臂末端、相机、固定系和物体参考系之间的关系。

参考系含义如下：

• { a } \{a\} {a}：房间固定参考系；
• { b } \{b\} {b}：移动平台参考系；
• { c } \{c\} {c}：机械臂末端参考系；
• { d } \{d\} {d}：相机参考系；
• { e } \{e\} {e}：待抓取物体参考系。

已知量包括：

• 相机测得 $T_{db}$ 和 $T_{de}$；
• 机械臂关节角给出 $T_{bc}$；
• 相机相对房间的标定 $T_{ad}$ 已知。

要求的是物体相对手爪的位形：

$$T_{ce}$$

关键是先写对参考系链条。经过机器人一侧，从 { a } \{a\} {a} 到 { e } \{e\} {e} 有：

$$T_{ab}{T}_{bc}{T}_{ce}$$

经过相机一侧，从 { a } \{a\} {a} 到 { e } \{e\} {e} 有：

$$T_{ad}{T}_{de}$$

二者描述同一个 $T_{ae}$，因此：

$$T_{ab}{T}_{bc}{T}_{ce}=T_{ad}{T}_{de}$$

又因为：

$$T_{ab}=T_{ad}{T}_{db}$$

所以：

$$T_{ce}=(T_{ad}{T}_{db}{T}_{bc}{)}^{-1}{T}_{ad}{T}_{de}$$

原书代入数值后得到：

$$T_{ce}=\left[\begin{array}{cccc}0& 0& 1& -75\\ -1/\sqrt{2}& 1/\sqrt{2}& 0& -260/\sqrt{2}\\ -1/\sqrt{2}& -1/\sqrt{2}& 0& 160/\sqrt{2}\\ 0& 0& 0& 1\end{array}\right]$$

复杂机器人系统中的位形关系，应该先用下标关系建模，再进行矩阵运算。

小结

3.3.1 建立了描述三维刚体位形的基本对象：

$$T=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\in SE(3)$$

它把姿态 $R$ 和位置 $p$ 放进同一个矩阵中，使得刚体变换可以写成：

$$T\left[\begin{array}{c}x\\ 1\end{array}\right]=\left[\begin{array}{c}Rx+p\\ 1\end{array}\right]$$

这一节需要掌握四件事：

1. $T$ 的结构和 $SE(3)$ 的含义；
2. 逆矩阵公式 $T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]$；
3. 下标消去规则 $T_{ab}{T}_{bc}=T_{ac}$；
4. 左乘是固定系变换，右乘是体系变换。

后续的 twist、刚体运动指数坐标、机械臂正运动学，都会以 $T\in SE(3)$ 为基础。

参考资料

1. Kevin M. Lynch and Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Section 3.3.1 Homogeneous Transformation Matrices.
2. Lynch and Park, Modern Robotics 2019 preprint PDF. 文中图片为该 PDF 中 3.3.1 节相关原书图的局部截图。