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

指数映射与对数映射

旋转和位姿不能相加，速度却可以。指数映射把"一个恒定的速度保持 1 秒"变成一次旋转或一次刚体运动；对数映射再把运动还原成速度。理解了这一对映射，陀螺仪积分、位姿插值、流形上的优化就都有了统一的写法。

约 35 分钟前置：1.8 叉积 · 5.3 齐次变换 SE(3) · 5.4 为什么需要李群 · 5.5 李代数 5 个交互

1为什么机器人需要它

机器人状态里的量，有的能相加，有的不能。位置可以相加：先往前走 1 米，再走 2 米，一共 3 米。姿态和位姿就不行：两个旋转矩阵相加，结果根本不是旋转矩阵。5.4 节已经看到，硬加的结果会把坐标系压扁、拉歪。$\SO$ 和 $\SE$ 是弯曲的流形，不是向量空间。

速度却可以相加。陀螺仪报告的角速度 $\boldsymbol\omega$、机械臂末端的速度旋量 (twist)，都是普通的向量，可以相加、乘以时间、求平均。指数映射 exp 连接这两个世界，它回答一个问题：把一个恒定速度保持 1 秒，你会到哪里？对数映射 log 正好反过来：给定终点，问用什么恒定速度、1 秒恰好到达。

机器人软件里有很多操作都要用到这一对映射：把陀螺仪读数积分成姿态；在两个位姿之间平滑插值；优化器在 $\SO$、$\SE$ 上迈出一小步（5.7、6.10 节）；机械臂的每个关节本身就是一个指数映射（5.10 节）。

2直觉：把切线缠到圆上

先看最简单的李群：平面旋转 SO(2)。每个平面旋转对应单位圆上的一个点 $(\cos\theta, \sin\theta)$。圆本身就是这个群，点 $(1, 0)$ 是"什么也不转"的单位元。

在单位元处画一条切线。切线上的点代表角速度：它们可以相加、数乘，组成一个普通的一维向量空间，也就是 SO(2) 的李代数（5.5 节）。

指数映射做的事很朴素：把切线像绳子一样缠到圆上，就像把线绕上轮子，不拉长，也不压缩。在切线上走了长度 $\theta$，在圆上就走过弧长 $\theta$。换句话说，以角速度 $\theta$ 匀速转 1 秒，停下的位置就是 $\exp(\theta)$。

FIG 1SO(2)：缠绳子与幂级数

缠绕模式：黄色绳子从单位元出发，长度为 $\theta$，绳上的小点每隔 1 个单位一个。缠到圆上之后，点与点的间距不变，这就是"不拉伸"。级数模式：紫色小箭头依次是 $I, A, A^2/2!, \dots$，每一项比上一项转 90°，长度乘 $\theta/n$。注意：只取前两项 $I + A$，得到的正好是切线上的那个点（一阶近似）。

把 $\theta$ 调到恰好缠满一整圈。绳子的终点回到了哪里？这说明 $\exp$ 有什么性质？

用矩阵写，切线上的 $\theta$ 对应李代数元素 $A = \begin{bmatrix}0 & -\theta\\ \theta & 0\end{bmatrix}$（5.5 节的 hat）。矩阵指数的定义和 $e^x$ 完全一样，是一个无穷级数：

$$ \exp(A) = I + A + \frac{A^2}{2!} + \frac{A^3}{3!} + \cdots = \sum_{n=0}^{\infty}\frac{A^n}{n!}. $$

对这个 $A$，有 $A^2 = -\theta^2 I$：每乘一次 $A$，就是转 90° 再乘 $\theta$。把偶数项和奇数项分开收集，正好是 $\cos\theta$ 和 $\sin\theta$ 的泰勒级数：

$$ \exp\begin{bmatrix}0 & -\theta\\ \theta & 0\end{bmatrix} = \Big(1 - \tfrac{\theta^2}{2!} + \cdots\Big) I + \Big(\theta - \tfrac{\theta^3}{3!} + \cdots\Big)\begin{bmatrix}0 & -1\\ 1 & 0\end{bmatrix} = \begin{bmatrix}\cos\theta & -\sin\theta\\ \sin\theta & \cos\theta\end{bmatrix}. $$

从圆到球：旋转向量

三维旋转多了两个自由度：除了转多少，还要说绕哪根轴转。$\SO$ 的李代数是三维的，其中的元素可以写成一个旋转向量 $\boldsymbol\omega = \theta\,\hat{\mathbf u}$：方向 $\hat{\mathbf u}$ 是转轴，长度 $\theta$ 是转角。$\Exp(\boldsymbol\omega)$ 的意思是以角速度 $\boldsymbol\omega$ 匀速转 1 秒，也就是绕 $\hat{\mathbf u}$ 转 $\theta$。

刚体上的每个点都沿着一个绕转轴的圆周运动。这和 FIG 1 的圆是同一回事，只是现在每个点都有自己的一个圆。拖动下面的时间滑块 $t$，看坐标系的三个轴尖各自画出一段圆弧。

FIG 2旋转向量 ω = θû 与 Exp(tω)

拖动画面旋转视角

预设

淡色坐标系是单位元 $I$，亮色坐标系是 $\Exp(t\boldsymbol\omega)$。紫色虚线是转轴，紫色箭头是旋转向量 $\boldsymbol\omega$（为了放得下，按 0.4 倍长度画）。彩色细圆是各轴尖绕转轴的完整圆周，粗线是 $0 \to t$ 已经走过的部分。

只用滑块（不用预设），在 $t = 1$ 时得到绕 z 轴转 180° 的旋转。你能找到两种不同的 $\boldsymbol\omega$ 吗？

核心洞见

指数映射把"平"的速度空间（李代数）卷到"弯"的群上。$\exp(t\boldsymbol\omega)$ 随 $t$ 走出的是一条匀速路径：转轴始终不变，每段相等的时间转过相等的角度。所以 $\exp(s\boldsymbol\omega)\exp(t\boldsymbol\omega) = \exp((s+t)\boldsymbol\omega)$：先转 $s$ 秒再转 $t$ 秒，等于一口气转 $s+t$ 秒。不过这只对同一个 $\boldsymbol\omega$ 成立（见误区 3）。

3动手玩：速度旋量与螺旋运动

位姿 $\SE$ 的速度有 6 个数：角速度 $\cw{\boldsymbol\omega}$（3 个）和线速度 $\cv{\mathbf v}$（3 个），合起来叫速度旋量 (twist) $\boldsymbol\xi = (\cw{\boldsymbol\omega}, \cv{\mathbf v}) \in \R^6$。

本课的顺序约定：先 ω，后 v

本课的 twist 一律把旋转放在前面：$\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)$，与 Lynch & Park 的 Modern Robotics 以及课程代码 R101.M.expSE3 / logSE3 一致。也有不少教材和库把平移放在前面，例如 Barfoot 的书、Solà 等人的论文和 Sophus 库，写作 $(\boldsymbol\rho, \boldsymbol\phi)$ 或 $(\mathbf v, \boldsymbol\omega)$。照抄公式之前一定先确认顺序，否则 6×6 矩阵的各个分块会全部错位。

让一个恒定的 twist 作用 1 秒，物体会怎样运动？答案出奇地简单，这就是 Chasles 定理：任何刚体运动都是螺旋运动。物体绕某根固定的轴转动，同时沿这根轴滑动，就像拧螺丝。这根轴叫螺旋轴；每转 1 弧度沿轴前进的距离叫螺距 $h$。有两个特例：$h = 0$ 是纯转动，而且转轴不一定过原点，比如门绕铰链转；$\boldsymbol\omega = 0$ 是纯平移，可以看成螺旋轴在无穷远处。

下面的实验室里，拖动 6 个滑块改变 twist，或者点一个预设。紫色虚线是螺旋轴，黄色曲线是物体原点走过的路径。

FIG 3Twist 实验室：exp(sξ) 是一次螺旋运动

拖动画面旋转视角

预设

灰色线框是起点（单位位姿），实心盒子是 $\exp(s\boldsymbol\xi)$。紫色点 $\mathbf q$ 是螺旋轴上离原点最近的点，青色细线是盒子一个角走过的路径。注意 $\mathbf v$ 是"此刻恰好位于世界原点的那个物体点"的速度。起点（$s = 0$）时这个点就是物体原点，所以琥珀色路径一开始沿 $\mathbf v$ 的方向出发；盒子离开原点以后（$s > 0$），两者一般就不同了，除非 $\boldsymbol\omega = 0$。

试一试

用滑块做出一个纯平移。这时螺旋轴去哪了？
让 $\boldsymbol\omega \ne 0$，同时螺距恰好为 0，并且转轴不过原点（$\|\mathbf q\| > 0.3$）。提示：$\mathbf v$ 和 $\boldsymbol\omega$ 应该是什么关系？
让盒子在 $s = 1$ 时回到起点，但中途确实在动（$\boldsymbol\xi \ne 0$）。于是 $\exp(\boldsymbol\xi) = I$ 却 $\boldsymbol\xi \ne 0$，这对 log 意味着什么？

4反过来：对数映射与插值

对数映射回答反问题：给定一个目标位姿 $T$，用哪个恒定的 twist 恰好 1 秒到达？答案是 $\boldsymbol\xi = \Log(T)$。由 Chasles 定理，这样的螺旋运动一定存在。

有了它，插值就很自然：$T(s) = \Exp(s\,\Log T)$，$s$ 从 0 走到 1。这是"以恒定速度从起点走到终点"，路上的每一刻都是合法位姿。拿它和 5.4 节的天真做法比一比：把矩阵元素直接线性插值，$(1-s)I + sT$。中间那些"位姿"会缩小、剪切，旋转部分已经不是旋转矩阵了。

FIG 4Log 与插值：沿螺旋走 vs. 逐元素插值

Exp(s · Log T)：沿螺旋走

(1−s) I + s T：逐元素插值

先用滑块设定目标位姿 $T$（淡色坐标系），再拖动 $s$ 或点播放。两边的视角是联动的。左边的坐标系始终保持单位长度、互相垂直；右边的坐标系在中途变短、变斜，行列式小于 1。

找一个目标姿态，让右边的逐元素插值在中途塌成一个平面（$\det \approx 0$）。这需要转多少度？

在两个任意位姿 $T_a, T_b$ 之间插值也是同一个思路：先算出从 $T_a$ 看过去的相对运动，再沿它匀速走，即 $T(s) = T_a\,\Exp\big(s\,\Log(T_a^{-1}T_b)\big)$。5.9 节会用它来平滑轨迹、对位姿求平均。

5再看数学

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

矩阵指数 = 恒定速度

对任意方阵 $A$，级数 $\exp(A) = \sum_n A^n/n!$ 总是收敛的。逐项求导，可以得到它最重要的性质：

$$ \frac{d}{dt}\exp(tA) = A\,\exp(tA) = \exp(tA)\,A, \qquad \exp(0) = I. $$

也就是说，$X(t) = \exp(tA)$ 是微分方程 $\dot X = XA$ 从 $I$ 出发的解，而 $X^{-1}\dot X = A$ 恒定不变：这正是"以恒定速度 $A$ 运动"。集合 $\{\exp(tA)\}_{t\in\R}$ 叫一参数子群，可以看成群里的"直线"。

大写 Exp 与小写 exp

小写 $\exp$ 作用在李代数的矩阵上；大写 $\Exp$ 直接吃一个向量，先用 hat 变成矩阵（5.5 节），再取 $\exp$。$\Log$ 则是先取 $\log$，再用 vee 读出向量：

$$ \R^3 \xrightarrow{\;(\cdot)^\wedge\;} \so \xrightarrow{\;\exp\;} \SO, \qquad \Exp(\boldsymbol\omega) = \exp(\skew{\boldsymbol\omega}), \qquad \Log(R) = \log(R)^\vee. $$

SO(3)：Rodrigues 公式

设 $\boldsymbol\omega = \theta\hat{\mathbf u}$，记 $K = \skew{\hat{\mathbf u}}$。由叉积的性质（1.8 节）可以验证 $K^3 = -K$，于是 $K$ 的高次幂只剩 $K$ 和 $K^2$ 两种，无穷级数也就合并成了两项：

$$ \begin{aligned} \Exp(\hl{theta}{\theta}\,\hl{u}{\cw{\hat{\mathbf u}}}) &= I + \Big(\theta - \tfrac{\theta^3}{3!} + \cdots\Big)K + \Big(\tfrac{\theta^2}{2!} - \tfrac{\theta^4}{4!} + \cdots\Big)K^2 \\ &= I + \sin\theta\,\skew{\hat{\mathbf u}} + (1-\cos\theta)\,\skew{\hat{\mathbf u}}^2. \end{aligned} $$

反过来，由 $\operatorname{tr} R = 1 + 2\cos\theta$ 和 $R - R\T = 2\sin\theta\,\skew{\hat{\mathbf u}}$ 可以读出转角和转轴（取主值 $\theta \in [0, \pi]$）：

$$ \theta = \arccos\frac{\operatorname{tr} R - 1}{2}, \qquad \skew{\hat{\mathbf u}} = \frac{R - R\T}{2\sin\theta}, \qquad \Log(R) = \theta\,\hat{\mathbf u}. $$

SE(3)：带平移的指数映射

twist $\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)$ 的 hat 是一个 4×4 矩阵。它的指数里，旋转部分就是 Rodrigues；平移部分是 $\mathbf v$ 乘上一个矩阵 $V$，$V$ 把"边转边走"累积下来的位移算了进去：

$$ \boldsymbol\xi^\wedge = \begin{bmatrix} \skew{\boldsymbol\omega} & \mathbf v \\ \mathbf 0\T & 0 \end{bmatrix}, \qquad \Exp(\boldsymbol\xi) = \begin{bmatrix} \Exp(\boldsymbol\omega) & V\mathbf v \\ \mathbf 0\T & 1 \end{bmatrix}, $$ $$ V = I + \frac{1-\cos\theta}{\theta^2}\skew{\boldsymbol\omega} + \frac{\theta - \sin\theta}{\theta^3}\skew{\boldsymbol\omega}^2. $$

求 $\Log(T)$ 时，先 $\boldsymbol\omega = \Log(R)$，再解 $\mathbf v = V^{-1}\mathbf t$。$V^{-1}$ 也有闭式解，课程代码 R101.M.logSE3 里就是这么写的。

螺旋参数（Chasles 定理）

当 $\boldsymbol\omega \ne 0$ 时，把 $\mathbf v$ 拆成垂直于 $\boldsymbol\omega$ 和平行于 $\boldsymbol\omega$ 的两部分：

$$ \hl{v}{\cv{\mathbf v}} = \underbrace{-\hl{w}{\cw{\boldsymbol\omega}}\times\hl{q}{\mathbf q}}_{\perp\,\boldsymbol\omega} + \underbrace{\hl{h}{h}\,\boldsymbol\omega}_{\parallel\,\boldsymbol\omega}, \qquad \hl{q}{\mathbf q} = \frac{\boldsymbol\omega\times\mathbf v}{\|\boldsymbol\omega\|^2}, \qquad \hl{h}{h} = \frac{\boldsymbol\omega\cdot\mathbf v}{\|\boldsymbol\omega\|^2}. $$

读法：绕过点 $\mathbf q$、方向为 $\hat{\boldsymbol\omega}$ 的轴转 $\theta = \|\boldsymbol\omega\|$，同时沿轴前进 $h\theta$。$\mathbf v \perp \boldsymbol\omega$ 时 $h = 0$，是纯转动；$\boldsymbol\omega = 0$ 时是纯平移。任何 $T \in \SE$ 都有一个 $\Log$，所以任何刚体运动都是某个螺旋运动。这就是 Chasles 定理。

6机器人中的它：陀螺仪积分

IMU 的陀螺仪每隔 $dt$ 报告一次机体系角速度 $\boldsymbol\omega$。在这一小段时间里把 $\boldsymbol\omega$ 看作常数，姿态更新就是"以 $\boldsymbol\omega$ 匀速转 $dt$ 秒"，再右乘到当前姿态上（右乘表示在机体系里运动）。另一种写法是对微分方程 $\dot R = R\skew{\boldsymbol\omega}$ 直接用欧拉法：

$$ \underbrace{R_{k+1} = R_k\,\Exp(\boldsymbol\omega\,dt)}_{\Exp} \qquad\text{vs.}\qquad \underbrace{R_{k+1} = R_k + R_k\skew{\boldsymbol\omega}\,dt = R_k\big(I + \skew{\boldsymbol\omega}dt\big)}_{\text{Euler}} $$

欧拉法只取了 $\Exp$ 级数的前两项，也就是 FIG 1 里切线上的那个点。每一步都把 $R$ 往 $\SO$ 外面推一点，误差还会逐步累积。下面两种方法用的是同样的 $dt$ 和同样的陀螺读数。

FIG 5陀螺仪积分：Exp vs. 欧拉法

拖动画面旋转视角

左：两个立方体以同一个机体角速度旋转，绿色用 $\Exp$ 更新，红色用欧拉法更新；红色旁边的淡色坐标系是真实姿态。右：上图是正交性误差 $\|R\T R - I\|_F$（对数坐标），下图是 $\det R$。在图上拖动可以调整时间。$\Exp$ 的误差一直停在机器精度（约 $10^{-15}$）附近。

真实系统里

IMU 预积分（视觉惯性里程计的标准做法）把两帧图像之间几百个陀螺读数连乘成 $\Delta R = \prod_k \Exp(\boldsymbol\omega_k\,dt)$。流形上的优化器（Ceres 的 Manifold、GTSAM 的 retract）在切空间里求出一个小增量 $\boldsymbol\delta$，再用 $T \leftarrow T\,\Exp(\boldsymbol\delta)$ 更新，这样每一步都留在群上（5.7、6.10 节）。机械臂的转动关节是螺距为 0 的 twist，移动关节是纯平移 twist，正运动学就是把各关节的 $\Exp(\boldsymbol\xi_i\theta_i)$ 连乘起来（指数积公式，5.10 节）。

7常见误区

误区 1：以为 log 是唯一的

exp 会"绕圈"：$\theta$ 和 $\theta + 2\pi$ 给出同一个旋转（FIG 1 的任务），$\theta = 2\pi$ 的螺旋运动甚至可以回到原点（FIG 3 的任务）。$\Log$ 只返回主值 $\theta \in [0, \pi]$，转 270° 会被报告成反方向转 90°。在 $\theta = \pi$ 处，$\hat{\mathbf u}$ 和 $-\hat{\mathbf u}$ 都对。对角度序列做插值或求平均时，要先"解缠绕 (unwrap)"。

误区 2：直接照公式算，不管 θ ≈ 0 和 θ ≈ π

$\sin\theta/\theta$、$(1-\cos\theta)/\theta^2$ 在 $\theta \to 0$ 时是 0/0，要改用泰勒展开（例如 $1 - \theta^2/6$）。$\Log$ 里的 $1/(2\sin\theta)$ 在 $\theta \to \pi$ 时会爆掉，这时要改从 $(R + I)/2 \approx \hat{\mathbf u}\hat{\mathbf u}\T$ 的最大对角元读出转轴，再用 $R - R\T$ 确定正负号。$\arccos$ 的参数也要先截断到 $[-1, 1]$。

误区 3：以为 Exp(a) Exp(b) = Exp(a + b)

只有 $\mathbf a$ 与 $\mathbf b$ 平行（同一根轴）时才成立。三维旋转不交换，先绕 x 转再绕 y 转，和反过来不一样。一般情况要用 BCH 公式：$\Log(\Exp(\mathbf a)\Exp(\mathbf b)) = \mathbf a + \mathbf b + \tfrac12\,\mathbf a \times \mathbf b + \cdots$，5.7 节会用到它的一阶近似。

误区 4：左乘和右乘随便用

$T\,\Exp(\boldsymbol\xi)$ 表示在机体系里按 $\boldsymbol\xi$ 运动，$\Exp(\boldsymbol\xi)\,T$ 表示在世界系里按 $\boldsymbol\xi$ 运动，两者一般不相等。陀螺仪测的是机体系角速度，所以要右乘。如果写成左乘，转轴就错了，而且这种 bug 只在机器人倾斜时才会暴露出来。

误区 5：twist 分量的顺序

本课和 Modern Robotics 用 $(\boldsymbol\omega, \mathbf v)$，Barfoot、Solà 和 Sophus 用 $(\mathbf v, \boldsymbol\omega)$。把一个库的 6 维向量或 6×6 协方差、雅可比矩阵 (Jacobian) 交给另一个库之前，必须先重排。

8小练习

1. $\Exp\big((0, 0, \pi/2)\big)$ 是什么旋转？

旋转向量的方向是转轴 $(0,0,1)$，长度是转角 $\pi/2$。由 Rodrigues 公式，$\Exp$ 的结果一定是旋转矩阵。

2. twist $\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)$，其中 $\boldsymbol\omega = (0,0,1)$，$\mathbf v = (0,0,0.5)$。$\exp(t\boldsymbol\xi)$ 是什么运动？

$h = \boldsymbol\omega\cdot\mathbf v/\|\boldsymbol\omega\|^2 = 0.5$，$\mathbf q = \boldsymbol\omega\times\mathbf v/\|\boldsymbol\omega\|^2 = \mathbf 0$：螺旋轴就是 z 轴，螺距 0.5，像拧一颗螺丝。

3. $R$ 是绕 z 轴转 270°。标准的 $\Log(R)$（主值）返回什么？

主值要求 $\theta \in [0, \pi]$。绕 z 转 270° 和绕 z 转 −90°（等价于绕 −z 转 90°）是同一个旋转，所以 $\Log$ 给出 $(0,0,-\pi/2)$。$(0,0,3\pi/2)$ 也满足 $\Exp = R$，但它不是主值。

4. 用 $R_{k+1} = R_k\,\Exp(\boldsymbol\omega\,dt)$ 积分陀螺仪，而不用 $R_k + R_k\skew{\boldsymbol\omega}dt$，主要好处是什么？

$\Exp$ 的输出一定是旋转矩阵，乘积也一定是旋转矩阵；欧拉法每一步都会让 $\det R$ 增大、各列不再正交。零偏需要另外估计（例如放进 EKF 的状态里），$\Exp$ 也比加法稍贵一点。

9延伸阅读

K. Lynch & F. Park, Modern Robotics, Ch. 3 — 指数坐标、twist、螺旋运动；本课的 (ω, v) 顺序与它一致
J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537 — Exp/Log、⊕/⊖ 和雅可比矩阵的简明手册（平移在前）
T. Barfoot, State Estimation for Robotics, Ch. 7 (Matrix Lie Groups) — SO(3)/SE(3) 的指数映射、雅可比与不确定性
E. Eade, Lie Groups for 2D and 3D Transformations — 简短的技术笔记，含所有闭式公式及其数值稳定写法