齐次变换矩阵与 SE(3)

本文最后更新于：2026年8月5日中午

齐次变换矩阵把旋转 $R$ 和平移 $\mathbf p$ 嵌进一个 $4\times4$ 矩阵，让位姿的描述与复合都化为矩阵乘法。它的难点不在定义，而在含义：同一个 ${}^{A}T_B$ 既是坐标换算、又是位姿描述、又是相对运动，由此牵出分解顺序、作用对象、旋转轴位置、内外旋、左右乘一连串相互关联的问题。全文沿"立定义（§1）→ 记号的三层含义（§2，概念核心）→ 数值算例（§3）→ 五个易混概念（§4–§8）→ 进阶工具（§9–§10）→ 工程实践（§11）"展开，把这些逐个理清，结论均配数值算例与示意图。本文与《万向节死锁》同属一套 Robotics 笔记。

1. 齐次变换矩阵与 SE(3)

把旋转 $R$（$3\times3$）和平移 $\mathbf p$（$3\times1$）嵌入一个 $4\times4$ 矩阵，得到齐次变换矩阵：

$$ T = \begin{bmatrix} R & \mathbf p \\ \mathbf 0^\top & 1 \end{bmatrix} $$

所有这样的矩阵构成特殊欧氏群 SE(3)。它把位置与姿态统一进一个矩阵，而且复合变换只是矩阵乘法：${}^{a}T_c={}^{a}T_b\cdot{}^{b}T_c$（任意三个系，记号见第二节）。这正是手眼标定与机器人运动学推导里反复出现的工具。

为什么要升到四维。 三维旋转是线性的，写成矩阵乘 $R\mathbf x$；但平移是仿射，只能写成加法 $\mathbf x+\mathbf p$，二者合不进同一个 $3\times3$ 矩阵相乘。给坐标末尾补 $1$（齐次坐标），平移就归化成矩阵乘法：

$$ \begin{bmatrix} R & \mathbf p \\ \mathbf 0^\top & 1 \end{bmatrix} \begin{bmatrix} \mathbf x \\ 1 \end{bmatrix} = \begin{bmatrix} R\mathbf x+\mathbf p \\ 1 \end{bmatrix} $$

"旋转 + 平移"这一仿射变换由此在四维里成为单个线性变换，复合也化为矩阵相乘——这正是上文 ${}^{A}T_C={}^{A}T_B\cdot{}^{B}T_C$ 能成立的前提。末行 $\mathbf 0^\top$ 与 $1$ 还保证两个这样的矩阵相乘后仍同形，SE(3) 因此对矩阵乘法封闭。

齐次变换矩阵的结构：旋转块 R、平移列 p、齐次行 0 1

2. ${}^{A}T_B$ 的三层含义

全文用下标记法。${}^{A}T_B$ 这个 $4\times4$ 矩阵同时承载三层等价含义：

坐标变换（被动）：同一个点 $P$ 不动，把它在 $B$ 系的坐标换算成 $A$ 系的坐标（可理解为坐标系移动），变换矩阵左乘 $B$ 系的点——$\mathbf x^{(A)}={}^{A}T_B\,\mathbf x^{(B)}$，得到该点在 $A$ 系下的表示（$B\to A$ 方向用 $R={}^{A}R_B$；反向 $A\to B$ 用其逆 ${}^{B}T_A=R^\top$）。
位姿描述（被动）：$B$ 系在 $A$ 系下的位姿——站在 $A$ 看，$B$ 的原点在哪、三根轴朝哪。这是一个静态的几何事实，没有东西在动。
相对运动（主动）：当 $A,B$ 是同一刚体的两个构型，${}^{A}T_B$ 是把刚体从 $A$ 构型搬运到 $B$ 构型的那个刚体运动（把一个原本与 $A$ 重合的刚体搬到 $B$ 的位置）——刚体真的在动；反方向 $B\to A$ 的运动是其逆 ${}^{B}T_A=({}^{A}T_B)^{-1}$。

同一个齐次变换的三种等价读法：坐标换算、位姿描述、相对运动

三张图同一几何——黑系 $A$、蓝系 $B$。左：同一个点 $P$ 不动，分别读 $A$、$B$ 两系坐标（被动换算）；中：$B$ 原点 $\mathbf p$ 与三轴朝向（$R$ 的列）静态刻画 $B$ 在 $A$ 的位姿（被动描述）；右：刚体由 $A$ 构型被搬到 $B$ 构型（主动运动）。三件事共享同一个 ${}^{A}T_B$。

主动还是被动，只看一件事——谁在动。①②里点和刚体都不动（①是换个坐标系读同一个点，②是描述相对位姿），都属被动（alias）；③里刚体真的从 $A$ 被搬到 $B$，属主动（alibi）。①的坐标换算（$B\to A$）与③的刚体搬运（$A\to B$）方向相反，却共用同一个 ${}^{A}T_B$——“B 相对 A 在哪”，既是从 $A$ 到 $B$ 的动作，又是两系坐标的换算规则（主动与被动的完整对偶见第五节）。

为什么这三层是同一个矩阵。 设 ${}^{A}T_B=\begin{bmatrix}R&\mathbf p\\\mathbf 0^\top&1\end{bmatrix}$。从坐标变换 $\mathbf x^{(A)}=R\,\mathbf x^{(B)}+\mathbf p$ 取两个特殊点：令 $\mathbf x^{(B)}=\mathbf 0$（$B$ 原点），得 $\mathbf p=\mathbf x^{(A)}$，即平移列 $\mathbf p$ 正是 $B$ 原点在 $A$ 系的坐标；令 $\mathbf x^{(B)}=(1,0,0)$（$B$ 的 $x$ 轴上单位点），它相对原点的位移正是 $R$ 第一列，即 $B$ 的 $x$ 轴在 $A$ 系的方向。所以"把 $B$ 变到 $A$ 的变换"与"$B$ 在 $A$ 下的位姿"由同一组 $(R,\mathbf p)$ 决定。

左上角那个系（A）是起点，右下角那个系（B）是终点——${}^{A}T_B$ 永远把刚体从 A 搬到 B，把坐标从 B 算回 A。

两条派生规则贯穿全文：

逆（对调观察者）：$({}^{A}T_B)^{-1}={}^{B}T_A$；
复合（右乘链）：${}^{A}T_C={}^{A}T_B\cdot{}^{B}T_C$，要求中间系匹配（前项尾 $B$ = 后项头 $B$）。

右乘复合：从 A 经 B 到 C，前一项的尾系与后一项的头系必须匹配

为什么复合落在右乘。 把每个变换看成一个函数——${}^{A}T_B$ 吃进 $B$ 系坐标、吐出 $A$ 系坐标。同一个点要从 $C$ 系一路读到 $A$ 系，中间必经 $B$：先把 $C$ 系坐标换成 $B$ 系，$\mathbf x^{(B)}={}^{B}T_C\,\mathbf x^{(C)}$；再换成 $A$ 系：

$$ \mathbf x^{(A)}={}^{A}T_B\,\mathbf x^{(B)}={}^{A}T_B\,{}^{B}T_C\,\mathbf x^{(C)} $$

于是把 $C$ 直接变到 $A$ 的总变换就是 ${}^{A}T_C={}^{A}T_B\cdot{}^{B}T_C$——新接进来、离向量最近的那个变换自然落在右边。这正是函数嵌套 $f(g(x))$ 写成矩阵 $FG\,\mathbf x$ 的样子：最右的先执行。

换个角度看同一条规则——下标咬合：${}^{A}T_B\cdot{}^{B}T_C$ 里前项的尾 $B$ 等于后项的头 $B$，像分数约分一样消去，剩下 $A$ 和 $C$。若反过来写成 ${}^{B}T_C\cdot{}^{A}T_B$，相邻处是 $C$ 接 $A$，对不上、消不掉——等于把"$A$ 系下的坐标"喂给一个"只认 $B$ 系输入"的算子，类型不匹配，没有几何意义。所以接链只能朝右延伸。

右乘 = 函数复合 = 离向量最近的先作用 = 下标首尾相接。注意这说的是"换系接链"（被动）；给同一个刚体连续施加旋转时新旋转该左乘还是右乘，是另一件事，见第七、八节。

3. 坐标换算的数值算例

${}^{A}T_B$ 把同一个点 $P$ 的 $B$ 系坐标换算成 $A$ 系坐标（被动用法；主动读法见第五节）：$\mathbf x^{(A)}=R\,\mathbf x^{(B)}+\mathbf p$。用一个具体数字把它算到底（对应下图）：

已知：$B$ 原点在 $A$ 系 $\mathbf p=(2,1)$，转角 $\theta=30°$；
点 $P$ 在 $B$ 系坐标 $P^{(B)}=(1.2,\,0.8)$。

对应的旋转矩阵为：

$$ R(30°)=\begin{bmatrix}0.866&-0.500\\0.500&0.866\end{bmatrix} $$

两步算：

旋转：$R\,P^{(B)}=(0.866{\times}1.2-0.500{\times}0.8,\;0.500{\times}1.2+0.866{\times}0.8)=(0.64,\,1.29)$；
平移：$P^{(A)}=R\,P^{(B)}+\mathbf p=(0.64+2,\,1.29+1)=(2.64,\,2.29)$。

齐次变换坐标换算的完整数值算例

红色两段是 $P$ 沿 $B$ 轴的分解（读出 $B$ 系坐标 $1.2,0.8$）；橙色空心点是旋转中间结果 $R\,P^{(B)}=(0.64,1.29)$，橙色箭头是再加 $\mathbf p$ 的平移；灰色虚线是 $P$ 到 $A$ 轴的投影（读出 $A$ 系坐标 $2.64,2.29$）。一个点，两个坐标，${}^{A}T_B$ 负责在两者间换算。

4. 先转后移与先移后转

§4–§8 围绕齐次变换最易混淆的几件事展开：分解顺序、作用对象、旋转轴位置、内外旋、左右乘。本节先看分解顺序。

矩阵乘法作用于列向量时，最靠近向量（最右边）的矩阵先执行。把 $T$ 拆成"一次纯旋转 + 一次纯平移"有两种等价写法，顺序恰好相反，差别只在平移量用哪个系：

参考系（$A$）视角——先转后移：

$$ T=\begin{bmatrix}I&\mathbf p\\\mathbf 0^\top&1\end{bmatrix}\begin{bmatrix}R&\mathbf 0\\\mathbf 0^\top&1\end{bmatrix}=\mathrm{Trans}(\mathbf p)\,\mathrm{Rot}(R) $$

靠右的 $\mathrm{Rot}®$ 先执行（绕 $A$ 原点转），靠左的 $\mathrm{Trans}(\mathbf p)$ 后执行（沿 $A$ 系平移）。平移量 $\mathbf p$ 在 $A$ 系。

自身系（$B$）视角——先移后转：

$$ T=\begin{bmatrix}R&\mathbf 0\\\mathbf 0^\top&1\end{bmatrix}\begin{bmatrix}I&R^{-1}\mathbf p\\\mathbf 0^\top&1\end{bmatrix}=\mathrm{Rot}(R)\,\mathrm{Trans}(R^{-1}\mathbf p) $$

靠右的 $\mathrm{Trans}(R^{-1}\mathbf p)$ 先执行（沿 $B$ 系平移），靠左的 $\mathrm{Rot}®$ 后执行（绕 $A$ 原点转）。平移量 $R^{-1}\mathbf p$ 在 $B$ 系。

两种分解展开都是 $R\mathbf x+\mathbf p$，所以"先转后移"和"先移后转"都对，只是平移分别用目标系 $A$、源系 $B$ 的量。到底先转还是先移，取决于平移量写在哪一侧：写在左（目标系 $\mathbf p$）则先转后移，写在右（源系 $R^{-1}\mathbf p$）则先移后转。

坐标系变换的两种等价分解

图中把两种分解逐步画出：左为先转后移——先绕 $A$ 原点转（金色中间态，原点不动、朝向变 $30°$），再沿 $A$ 系平移 $\mathbf p$；右为先移后转——先沿 $B$ 系量 $R^{-1}\mathbf p=(2.23,-0.13)$ 平移（金色中间态，朝向不变），再旋转。两条路径中间态不同，但终点 $B$（原点 $(2,1)$、朝向 $30°$）完全相同。

5. 作用于点与作用于坐标系

同一个 ${}^{A}T_B$，作用在点上（主动，对应第二节③搬刚体）与作用在坐标系上（被动，对应第二节①换坐标）是方向相反的两件事。这里的"对偶"指同一个公式两种互为镜像的读法，而非两个不同的公式。镜像的来源是交换固定参考系：主动是物体动、坐标系固定；被动是坐标系动、物体固定。于是 ${}^{A}T_B$ 读出两个相反的箭头——作为主动算子，把刚体由 $A$ 构型搬到 $B$ 构型（世界里 $A\to B$）；作为被动换系规则，把同一个点的 $B$ 系读数换成 $A$ 系读数（标签上 $B\to A$）。一个符号、两个反向箭头，正是第二节所述"方向相反却共用同一个矩阵"的展开。

作用于点（主动 / alibi）：点动、坐标系不动。旋转改变点的位置（$P$ 绕 $A$ 原点转一段弧）。
作用于坐标系（被动 / alias）：点不动、坐标系动。把一个初始与 $A$ 重合的坐标系挪到位姿 $B$，旋转改变坐标系的朝向（三轴指向变、原点暂不动），平移再挪原点。

作用于点 vs 作用于坐标系：点动用 R，坐标系动用 Rᵀ，二者互为转置

作用于点的代数形式 $R\,\mathbf x$（矩阵乘列向量）对主动与被动都成立，区分二者的不是代数，而是输入、输出这对向量的含义。若它们是同一个点在两个系里的读数（$\mathbf x^{(B)}\to\mathbf x^{(A)}$），点没有移动，是被动；若它们是同一个系里两个不同的点（$\mathbf x\to\mathbf x'=R\,\mathbf x$），点确实移动，是主动。因此判据只有一条：输入与输出是同一个点的两个读数，还是两个不同的点。这与"算的是不是点"无关，也与左乘、右乘无关（对列向量左乘是固定写法，见第八节）。

两者互为逆（转置）。在同一帧内主动转动一个点用 $R$：$\mathbf x'=R\,\mathbf x$；把坐标系被动转 $R$，同一个固定点在新系里的坐标变为：

$$ \mathbf x_{\text{新}}=R^{-1}\mathbf x_{\text{旧}}=R^{\top}\mathbf x_{\text{旧}} $$

坐标系沿某方向前进时，固定点在该系里的坐标反而后退，正是这个转置关系的体现。所以 ${}^{A}T_B$ 既是"$B$ 在 $A$ 中的位姿描述"、又是"$A\to B$ 的变换算子"——一个矩阵、两个视角。作用于点转位置，作用于坐标系转朝向；主动用 $R$，被动用 $R^{\top}$。

主动与被动回答"谁在动"，与"转轴是否随机体一起动"是两个独立的维度。"作用在点上左乘 $R\mathbf x$"只是矩阵乘列向量的固定写法，对单次旋转谈不上内外旋；内外旋只在一串旋转的复合顺序里才有意义，取决于转轴固定在世界系还是机体（见第七节）。本节与第七节分属两个维度；左乘、右乘本身的含义统一在第八节澄清。

6. 旋转的轴：过不过原点

由欧拉定理，任何旋转矩阵 $R$ 等价于绕单一轴 $\hat{\mathbf u}$ 转 $\theta$ 的一次旋转（Rodrigues 公式）。$\hat{\mathbf u}$（$R$ 特征值 1 的特征向量）只给轴的方向，不给位置。轴是否过原点要看对象：

纯旋转 $R$（即分解里的 $\mathrm{Rot}®$ 这一步）：轴穿过原点。因为纯旋转是线性变换，必然固定原点 $R\,\mathbf 0=\mathbf 0$。
完整变换 $T=(R,\mathbf p)$（整体刚体运动）：等效旋转轴（更准确说是螺旋轴 screw axis）一般不过原点——除非 $\mathbf p$ 平行于转轴（即没有垂直于轴的分量，含 $\mathbf p=\mathbf 0$ 的纯旋转）。一般情形下 $T$ 等价于绕某条不过原点的轴转 $\theta$、并沿该轴滑移一段（Chasles 定理）。

纯旋转的轴穿过原点；完整变换的螺旋轴一般不过原点

左：纯旋转，轴穿原点，点绕原点画弧，螺距 $h=0$。右：完整变换 $T=(R,\mathbf p)$，等效螺旋轴不过原点（绿色偏置），点绕这条偏置轴转 $\theta$、同时沿轴滑移 $d$，螺距 $h=d/\theta$。把"轴的位置"从三维螺旋轴拉回平面对照，更为直观。

螺旋运动与 Chasles 定理：绕轴转 θ 的同时沿轴平移 d，螺距 h=d/θ

所以"旋转绕原点"指的是分解里的那一步纯旋转，不是 $T$ 整体的等效轴。

Chasles 定理。 任意刚体运动 $T=(R,\mathbf p)$（$R\ne I$）都唯一等价于绕空间中某条固定轴 $\hat{\boldsymbol\ell}$ 转 $\theta$、同时沿该轴平移距离 $d$ 的螺旋运动（screw motion）：轴方向为 $R$ 的旋转轴 $\hat{\boldsymbol\omega}$，沿轴平移分量 $d=\hat{\boldsymbol\omega}^\top\mathbf p$，垂直于轴的平移分量则体现为"轴不过原点"的偏置。比值 $h=d/\theta$ 称为螺距（pitch）：$h=0$ 为纯转动（转动副），$h\to\infty$ 为纯平动（移动副），有限非零为螺旋副。这正是"完整 $T$ 的螺旋轴一般不过原点"的根据。

7. 内旋与外旋

单个 $R$ 只是一次旋转、无先后，谈不上内外旋。只有需要连续追加旋转时，才要回答：每一次新旋转，绕的是世界固定轴还是物体当前轴？这个选择决定了矩阵该左乘还是右乘。

7.1 外旋左乘，内旋右乘

记当前已累积的旋转为 $R$，新追加一次绕轴 $\hat{\mathbf u}$ 转 $\theta$ 的旋转 $\mathrm{Rot}(\hat{\mathbf u},\theta)$：

外旋（extrinsic，定轴）——轴固定在世界系，左乘：

$$ R \;\leftarrow\; \mathrm{Rot}(\hat{\mathbf u},\theta)\,R $$

内旋（intrinsic，动轴）——轴固定在物体（随机体一起转），右乘：

$$ R \;\leftarrow\; R\,\mathrm{Rot}(\hat{\mathbf u},\theta) $$

内旋（机体轴，右乘）vs 外旋（世界轴，左乘）：同一姿态，两种累积顺序

为什么右乘。 内旋绕机体当前轴 $\hat{\mathbf u}$——该轴此刻在世界系里的方向是 $R\hat{\mathbf u}$。绕这条世界轴 $R\hat{\mathbf u}$ 转 $\theta$，由共轭（见 7.3）等于 $R\,\mathrm{Rot}(\hat{\mathbf u},\theta)\,R^{-1}$；把它接在当前姿态之后：$\bigl(R\,\mathrm{Rot}(\hat{\mathbf u},\theta)\,R^{-1}\bigr)\,R=R\,\mathrm{Rot}(\hat{\mathbf u},\theta)$——右乘。外旋的轴本就固定在世界系，直接左乘即可。

7.2 同一个矩阵，两种读法

一串旋转的总矩阵只取决于"写成什么乘积"，不取决于怎么解释它。以航空常用的 $R_z(\psi)R_y(\theta)R_x(\varphi)$ 为例，它有两种等价的物理读法：

内旋（机体轴，顺序 $z\to y\to x$）：每次绕当前机体轴转，复合是右乘链——

$$ I \,\xrightarrow{\,\cdot R_z(\psi)\,}\, R_z(\psi) \,\xrightarrow{\,\cdot R_y(\theta)\,}\, R_z(\psi)R_y(\theta) \,\xrightarrow{\,\cdot R_x(\varphi)\,}\, R_z(\psi)R_y(\theta)R_x(\varphi) $$

外旋（世界轴，顺序 $x\to y\to z$，反序）：每次绕固定世界轴转，复合是左乘链——

$$ I \,\xrightarrow{\,R_x(\varphi)\cdot\,}\, R_x(\varphi) \,\xrightarrow{\,R_y(\theta)\cdot\,}\, R_y(\theta)R_x(\varphi) \,\xrightarrow{\,R_z(\psi)\cdot\,}\, R_z(\psi)R_y(\theta)R_x(\varphi) $$

两条链终点完全相同。一般定理：内旋按某轴序 = 外旋按反序（内旋 ZYX = 外旋 XYZ）。

7.3 共轭证明

设机体当前姿态为 $R$，机体上某轴在体系里是 $\hat{\mathbf u}$，则它在世界系里的方向是 $R\hat{\mathbf u}$。绕这条世界系中的 $R\hat{\mathbf u}$ 轴转 $\alpha$，等价于共轭：

$$ \mathrm{Rot}(R\hat{\mathbf u},\alpha)=R\,\mathrm{Rot}(\hat{\mathbf u},\alpha)\,R^{-1} $$

几何上是把机体转回世界、绕世界轴转、再转回去。据此可证："先绕体轴 $e_1$ 转 $\alpha_1$、再绕（新）体轴 $e_2$ 转 $\alpha_2$"的内旋，等于"先绕世界 $e_2$ 转 $\alpha_2$、再绕世界 $e_1$ 转 $\alpha_1$“的外旋，二者都得 $R_{e_1}(\alpha_1)R_{e_2}(\alpha_2)$。三轴同理，即得"内旋 = 外旋反序”，这也是 7.1 里内旋落到右乘的根源。

7.4 与作用对象的关系

常有说法"作用在点上左乘就是外旋、作用在坐标系上右乘就是内旋"——方向对，但省略了前提（第五节已把主动、被动分开）：

一个孤立的点或向量没有自己的轴，连续转它时转轴只能取世界轴，自然落到外旋、左乘复合。所以"点 = 外旋 = 左乘"成立。
一个坐标系或刚体有自身机体轴；若让它一步步绕自己的轴转，就是内旋、右乘复合。所以"坐标系按自身轴 = 内旋 = 右乘"成立。
但坐标系也能绕世界轴转（那仍是外旋、仍左乘）；点也能绑在刚体上随内旋一起动。所以"点↔外旋、坐标系↔内旋"只是最自然的搭配，不是规定。判定标准始终只有一条：这一步的转轴，定在世界里还是定在机体上？与作用在点上还是坐标系上无关。

7.5 内旋与外旋的选用

轴在世界 → 外旋 → 左乘；轴在机体 → 内旋 → 右乘。按物理转轴长在哪来选：

机械臂 / DH 建模：关节绕的是连杆自己的轴，属内旋、右乘，$T=T_1T_2\cdots T_n$。机器人学默认。
图形学 / 点云在世界里操作物体（先绕世界 $X$ 再绕世界 $Y$）：世界轴，属外旋、左乘。
航空航天 yaw-pitch-roll：yaw 绕地垂线（世界），pitch/roll 绕机体轴，整体通常按内旋 ZYX 描述。

内旋与外旋能表达的姿态集合完全相同（内旋 XYZ ≡ 外旋 ZYX），选哪个不影响结果，只影响哪种写法最顺、最不容易错。原则是：物理转轴属于哪个系，就按那个系选内外。

7.6 万向节死锁

三角度欧拉表示在内旋（或等价的外旋反序）下，当中间角达到 $\pm90°$ 时雅可比降秩、丢失一个自由度，即万向节死锁。它是三角度参数化的固有奇点，与内/外旋无关（同一个矩阵、同一个奇点）；换四元数或旋转矩阵表示即可消除。完整推导（几何、雅可比 $\det M=\cos\theta$、矩阵三层证明，以及外旋死锁的几何图像）见《万向节死锁》。

8. 左乘、右乘与执行顺序

“左乘”"右乘"是相对被变换的量而言的：变换矩阵是主语，被变换的量是参照物。

$T$ 左乘 $\mathbf x$：$T$ 写在 $\mathbf x$ 左边，得 $T\mathbf x$；
$T$ 右乘 $\mathbf x$：$T$ 写在 $\mathbf x$ 右边，得 $\mathbf xT$。

即"左/右"描述的是变换矩阵落在被变换量的哪一侧，主语是新引入的矩阵。同一术语在不同语境作用的对象不同，需要分开理解：

作用于向量：变换在向量左边（$T\mathbf x$），即搬点。列向量约定下变换对点恒为左乘，反向是左乘其逆 $T^{-1}\mathbf x$，而非右乘。
复合变换（接链）：${}^{W}T_A\cdot{}^{A}T_B$，新变换追加在链右侧，即接一个更局部的系。
连续旋转（见第七节）：新旋转对已有姿态 $R$ 左乘（外旋，$\mathrm{Rot}\,R$）或右乘（内旋，$R\,\mathrm{Rot}$）。
执行顺序：由"谁更靠近被作用的向量"决定（最右的先执行），与上述三种"左/右乘"含义无关。

一句话："左/右乘"说的是位置（变换落在被变换量的哪一侧），不是执行先后。

左乘、右乘是位置；执行顺序由谁更靠近向量决定

上半画出乘积链 $T_1T_2T_3\,x$ 的执行顺序：自右向左，最靠近向量 $x$ 的 $T_3$ 先执行。下半区分"左乘"（$T$ 在 $x$ 左边，得 $T\,x$）与"右乘"（$T$ 在 $x$ 右边，得 $x\,T$）——说的是位置，不是先后。

9. 指数坐标：SE(3) 的矩阵指数与对数

前面用 $(R,\mathbf p)$ 拼 $T$。反过来，任意 $T\in SE(3)$ 也可写成某旋量（twist）的矩阵指数，这是螺旋理论与李群视角的核心。一个旋量 $\mathcal S=(\boldsymbol\omega,\mathbf v)$（$\boldsymbol\omega$ 角速度、$\mathbf v$ 线速度）对应 $4\times4$ 矩阵：

$$ [\mathcal S]=\begin{bmatrix}[\boldsymbol\omega]_\times&\mathbf v\\\mathbf 0^\top&0\end{bmatrix} $$

这里 $\boldsymbol\omega$ 已含转角（$\theta=\|\boldsymbol\omega\|$），故运动就是 $[\mathcal S]$ 自身的指数（不再另乘 $\theta$）：

$$ \exp([\mathcal S])=\begin{bmatrix}R&V\mathbf v\\\mathbf 0^\top&1\end{bmatrix} $$

旋量经矩阵指数给出刚体运动：绕轴转的同时沿轴平移

红色螺旋轨迹即 $\exp([\mathcal S])$ 描述的运动：点既绕偏置轴转 $\theta$、又沿轴滑移 $d$，角速度方向 $\omega$ 沿螺旋轴。一次矩阵指数，把"绕一根轴边转边移"一次写完。

其中 $R$ 由 Rodrigues 公式给出：

$$ R=I+\frac{\sin\theta}{\theta}[\boldsymbol\omega]_\times+\frac{1-\cos\theta}{\theta^2}[\boldsymbol\omega]_\times^2 $$

（$\theta=\|\boldsymbol\omega\|$），而

$$ V=I+\frac{1-\cos\theta}{\theta^2}[\boldsymbol\omega]_\times+\frac{\theta-\sin\theta}{\theta^3}[\boldsymbol\omega]_\times^2 $$

这两条公式从哪来。 矩阵指数按定义展开为幂级数 $\exp([\boldsymbol\omega]_\times)=I+[\boldsymbol\omega]_\times+\tfrac{1}{2!}[\boldsymbol\omega]_\times^2+\cdots$。关键是反对称矩阵满足 $[\boldsymbol\omega]_\times^3=-\theta^2[\boldsymbol\omega]_\times$（$\theta=\|\boldsymbol\omega\|$），三次及以上的幂都能降回一、二次：$[\boldsymbol\omega]_\times^3=-\theta^2[\boldsymbol\omega]_\times$、$[\boldsymbol\omega]_\times^4=-\theta^2[\boldsymbol\omega]_\times^2$，循环往复。把级数按奇、偶次分组，分别套 $\sin\theta$ 与 $\cos\theta$ 的泰勒展开，即得 Rodrigues；同样的展开用于平移项，得 $V$。这解释了为何系数里尽是 $\sin\theta/\theta$、$(1-\cos\theta)/\theta^2$ 这类角度之比。

逆运算矩阵对数可由 $T$ 反解螺旋参数：

$$ \log T=\begin{bmatrix}\log R&V^{-1}\mathbf p\\\mathbf 0^\top&0\end{bmatrix} $$

由 $\mathrm{tr}®$ 取 $\theta=\cos^{-1}\frac{\mathrm{tr}R-1}{2}$，由 $R-R^\top$ 取 $\boldsymbol\omega$，再算：

$$ V^{-1}=I-\tfrac12[\boldsymbol\omega]_\times+\frac{1}{\theta^2}\bigl(1-\frac{\theta\sin\theta}{2(1-\cos\theta)}\bigr)[\boldsymbol\omega]_\times^2 $$

$R=I$（纯平移）时退化为 $\theta=\|\mathbf p\|,\mathbf v=\mathbf p/\theta$；小 $\theta$ 时须用泰勒展开避免除零。指数坐标把"任意 $T$ = 一个螺旋运动"这一 Chasles 结论变成可计算的工具，也是机器人学指数积（PoE）正运动学的语言。

10. 伴随矩阵：旋量与力旋量的换系

$T=(R,\mathbf p)$ 的 $6\times6$ 伴随矩阵为：

$$ [\mathrm{Ad}_T]=\begin{bmatrix}R&\mathbf 0\\{}[\mathbf p]_\times R&R\end{bmatrix} $$

它有两类用途：

旋量（速度）换系：体速度 $\mathcal V_b$ 与空间速度 $\mathcal V_s$ 由伴随矩阵联系，$\mathcal V_s=[\mathrm{Ad}_T]\mathcal V_b$（体 → 空间）。
力旋量换系（与速度反向）：由"功率与坐标系无关"$\mathcal F_s^\top\mathcal V_s=\mathcal F_b^\top\mathcal V_b$，代入 $\mathcal V_s=[\mathrm{Ad}_T]\mathcal V_b$ 得 $\mathcal F_b=[\mathrm{Ad}_T]^\top\mathcal F_s$（空间 → 体）。速度从体到空间用 $[\mathrm{Ad}_T]$，力从空间到体用 $[\mathrm{Ad}_T]^\top$——方向恰好相反。

伴随矩阵：速度用本身换系，力旋量用转置换系，方向相反

左：体速度 $\mathcal V_b$ 经 $[\mathrm{Ad}_T]$ 变为空间速度 $\mathcal V_s$（体→空间）。右：空间力旋量 $\mathcal F_s$ 经 $[\mathrm{Ad}_T]^\top$ 变为体力旋量 $\mathcal F_b$（空间→体）。正用与转置方向相反，并排对照一目了然；下方的功率等式 $F_s^\top V_s=F_b^\top V_b$ 正是这一相反方向的根源。

力旋量换系用的是 $[\mathrm{Ad}_T]^\top$（转置）而非 $[\mathrm{Ad}_T]^{-1}=[\mathrm{Ad}_{T^{-1}}]$，这是"功率不变"所强制的（用错了功率就会变）。这条对偶正是雅可比"正用映射速度、转置映射力"的群论根据。

11. 数值实践：正交化、快速求逆、误差累积

工程实现中 $T$ 有几个常用技巧：

SE(3) 快速求逆：无需 $4\times4$ 通用求逆，利用 $R^{-1}=R^\top$，直接：

$$ T^{-1}=\begin{bmatrix}R^\top&-R^\top\mathbf p\\\mathbf 0^\top&1\end{bmatrix} $$

仅需 $3\times3$ 转置加一次矩阵-向量乘。

正交性漂移：多次连乘 $R$ 后浮点误差使结果漂离 SO(3)（$R^\top R\ne I$）。常用 SVD 正交化：对漂移矩阵 $M=U\Sigma V^\top$，取：

$$ R=U\,\mathrm{diag}(1,1,\det(UV^\top))\,V^\top $$

内层 $\mathrm{diag}$ 纠正反射、保证 $\det R=+1$。SVD 正交化精度约是 Gram-Schmidt 的两倍。长时间仿真应周期性对 $R$ 块做正交化以抑制累积误差。

分量顺序：不同教材对旋量/伴随的分量顺序约定不同——有的 $[\boldsymbol\omega;\mathbf v]$、有的 $[\mathbf v;\boldsymbol\omega]$，引用公式前须先确认。

12. 小结

${}^{A}T_B$ 把 $\mathbf x^{(B)}$ 变为 $R\mathbf x^{(B)}+\mathbf p$；先转还是先移取决于平移量用哪个系（目标系 $\mathbf p$ → 先转后移，源系 $R^{-1}\mathbf p$ → 先移后转），两种分解等价；作用于点（主动，$R$）与作用于坐标系（被动，$R^{\top}$）互为转置；纯旋转的轴过原点、完整 $T$ 的螺旋轴一般不过（Chasles 定理，螺距 $h=d/\theta$）；任意 $T$ 可经矩阵对数反解为单一螺旋运动；伴随矩阵 $[\mathrm{Ad}_T]$ 换系（旋量用本身、力旋量用转置）；连续旋转中外旋（世界轴）左乘、内旋（机体轴）右乘，内旋 ZYX = 外旋 XYZ，二者可表姿态相同、死锁也同处（详见《万向节死锁》）。