Part I · 第 5 章刚体运动与李群 · 5.6Part I · Ch. 5 Rigid-Body Motion & Lie Groups · 5.6

指数映射与对数映射The Exponential & Logarithm Maps

旋转和位姿不能相加，速度却可以。指数映射把"一个恒定的速度保持 1 秒"变成一次旋转或一次刚体运动；对数映射再把运动还原成速度。理解了这一对映射，陀螺仪积分、位姿插值、流形上的优化就都有了统一的写法。 You cannot add rotations or poses, but you can add velocities. The exponential map turns "hold a constant velocity for one second" into a rotation or a rigid motion; the logarithm turns the motion back into a velocity. With this pair of maps, gyro integration, pose interpolation and optimization on manifolds all become the same idea.

约 35 分钟~35 min 前置：1.8 叉积 · 5.3 齐次变换 SE(3) · 5.4 为什么需要李群 · 5.5 李代数Prereqs: 1.8 Cross product · 5.3 SE(3) · 5.4 Why Lie groups · 5.5 Lie algebras 5 个交互5 interactives

1为什么机器人需要它Why robots need this

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

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

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

Some quantities in a robot's state can be added and some cannot. Positions add: walk 1 m forward, then 2 m, and you are 3 m out. Orientations and poses do not: the sum of two rotation matrices is not a rotation matrix at all. Lesson 5.4 showed how forcing it squashes and skews the frame. $\SO$ and $\SE$ are curved manifolds, not vector spaces.

Velocities, however, do add. The angular velocity $\boldsymbol\omega$ reported by a gyro and the twist of an end effector are ordinary vectors: you can add them, scale them by time and average them. The exponential map connects the two worlds by answering one question: hold a constant velocity for one second, and where do you end up? The logarithm goes the other way: given the destination, which constant velocity gets you there in exactly one second?

A lot of robot software relies on this pair: integrating gyro readings into an orientation, interpolating smoothly between two poses, letting an optimizer take a step on $\SO$ or $\SE$ (Lessons 5.7 and 6.10). Even each joint of a robot arm is an exponential map (Lesson 5.10).

2直觉：把切线缠到圆上Intuition: wrapping a line around a circle

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

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

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

Start with the simplest Lie group, planar rotations SO(2). Each planar rotation is a point $(\cos\theta, \sin\theta)$ on the unit circle. The circle itself is the group, and the point $(1, 0)$, "don't rotate at all", is the identity.

Draw the tangent line at the identity. Points on that line are angular velocities: they add and scale, forming an ordinary one-dimensional vector space. This is the Lie algebra of SO(2) (Lesson 5.5).

The exponential map does something very plain: it wraps the tangent line around the circle like a string, the way thread winds onto a spool, with no stretching and no squeezing. Walk a length $\theta$ along the line and you sweep an arc of length $\theta$ on the circle. Put differently: spin at angular velocity $\theta$ for one second, and where you stop is $\exp(\theta)$.

FIG 1SO(2)：缠绳子与幂级数SO(2): wrapping a string, summing a series

缠绕模式：黄色绳子从单位元出发，长度为 $\theta$，绳上的小点每隔 1 个单位一个。缠到圆上之后，点与点的间距不变，这就是"不拉伸"。级数模式：紫色小箭头依次是 $I, A, A^2/2!, \dots$，每一项比上一项转 90°，长度乘 $\theta/n$。注意：只取前两项 $I + A$，得到的正好是切线上的那个点（一阶近似）。 Wrap mode: the amber string starts at the identity and has length $\theta$; its dots are spaced 1 unit apart. Once wound onto the circle the spacing is unchanged: no stretching. Series mode: the purple arrows are $I, A, A^2/2!, \dots$; each is the previous one turned 90° and scaled by $\theta/n$. Note that the first two terms alone, $I + A$, land exactly on the tangent-line point: the first-order approximation.

把 $\theta$ 调到恰好缠满一整圈。绳子的终点回到了哪里？这说明 $\exp$ 有什么性质？Set $\theta$ so the string wraps exactly one full turn. Where does its end land? What does that say about $\exp$?

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

In matrix form, the point $\theta$ on the tangent line is the Lie-algebra element $A = \begin{bmatrix}0 & -\theta\\ \theta & 0\end{bmatrix}$ (the hat of Lesson 5.5). The matrix exponential is defined exactly like $e^x$, as an infinite series:

$$ \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$ 的泰勒级数：

For this $A$, $A^2 = -\theta^2 I$: each multiplication by $A$ turns by 90° and scales by $\theta$. Collect the even and the odd terms separately and you get exactly the Taylor series of $\cos\theta$ and $\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}. $$

从圆到球：旋转向量From circle to sphere: the rotation vector

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

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

Rotations in 3D have two more degrees of freedom: besides how much to turn, you must say about which axis. The Lie algebra of $\SO$ is three-dimensional, and its elements can be written as a rotation vector $\boldsymbol\omega = \theta\,\hat{\mathbf u}$: the direction $\hat{\mathbf u}$ is the axis and the length $\theta$ is the angle. $\Exp(\boldsymbol\omega)$ means spinning at angular velocity $\boldsymbol\omega$ for one second, which turns by $\theta$ about $\hat{\mathbf u}$.

Every point of the rigid body travels along a circle around the axis. It is the circle of FIG 1 again, except that now every point has a circle of its own. Drag the time slider $t$ below and watch each axis tip of the frame trace its own arc.

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

拖动画面旋转视角drag to orbit

预设Presets

淡色坐标系是单位元 $I$，亮色坐标系是 $\Exp(t\boldsymbol\omega)$。紫色虚线是转轴，紫色箭头是旋转向量 $\boldsymbol\omega$（为了放得下，按 0.4 倍长度画）。彩色细圆是各轴尖绕转轴的完整圆周，粗线是 $0 \to t$ 已经走过的部分。 The faint frame is the identity $I$; the bright frame is $\Exp(t\boldsymbol\omega)$. The dashed purple line is the axis and the purple arrow is the rotation vector $\boldsymbol\omega$ (drawn at 0.4× length so it fits). The thin colored circles are the full orbits of the axis tips; the thick arcs are the part covered from $0$ to $t$.

只用滑块（不用预设），在 $t = 1$ 时得到绕 z 轴转 180° 的旋转。你能找到两种不同的 $\boldsymbol\omega$ 吗？Using only the sliders (no presets), produce a 180° rotation about the z-axis at $t = 1$. Can you find two different $\boldsymbol\omega$ that do it?

核心洞见Key insight

指数映射把"平"的速度空间（李代数）卷到"弯"的群上。$\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）。

The exponential map rolls the "flat" space of velocities (the Lie algebra) onto the "curved" group. As $t$ grows, $\exp(t\boldsymbol\omega)$ traces a constant-speed path: the axis never changes and equal times give equal angles. Hence $\exp(s\boldsymbol\omega)\exp(t\boldsymbol\omega) = \exp((s+t)\boldsymbol\omega)$: spinning for $s$ seconds and then $t$ seconds is the same as spinning for $s+t$ seconds. This holds only for the same $\boldsymbol\omega$, though (see Pitfall 3).

3动手玩：速度旋量与螺旋运动Play: twists and screw motions

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

A velocity in $\SE$ has six numbers: an angular velocity $\cw{\boldsymbol\omega}$ (three) and a linear velocity $\cv{\mathbf v}$ (three). Together they form a twist $\boldsymbol\xi = (\cw{\boldsymbol\omega}, \cv{\mathbf v}) \in \R^6$.

本课的顺序约定：先 ω，后 vThis lesson's convention: ω first, then 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 矩阵的各个分块会全部错位。

Throughout this lesson a twist puts rotation first: $\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)$, matching Lynch & Park's Modern Robotics and the course code R101.M.expSE3 / logSE3. Many texts and libraries put translation first instead, for example Barfoot's book, the paper by Solà et al. and the Sophus library, writing $(\boldsymbol\rho, \boldsymbol\phi)$ or $(\mathbf v, \boldsymbol\omega)$. Check the ordering before copying any formula, or every block of a 6×6 matrix ends up in the wrong place.

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

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

Apply a constant twist for one second: how does the body move? The answer is surprisingly simple, and it is Chasles' theorem: every rigid motion is a screw motion. The body turns about a fixed axis while sliding along that same axis, like a screw being driven. The axis is the screw axis; the distance advanced along it per radian turned is the pitch $h$. There are two special cases. $h = 0$ is a pure rotation, and the axis need not pass through the origin (think of a door on its hinge). $\boldsymbol\omega = 0$ is a pure translation, which you can think of as a screw axis at infinity.

In the lab below, drag the six sliders to change the twist, or pick a preset. The dashed purple line is the screw axis and the amber curve is the path of the body's origin.

FIG 3Twist 实验室：exp(sξ) 是一次螺旋运动Twist lab: exp(sξ) is a screw motion

拖动画面旋转视角drag to orbit

预设Presets

灰色线框是起点（单位位姿），实心盒子是 $\exp(s\boldsymbol\xi)$。紫色点 $\mathbf q$ 是螺旋轴上离原点最近的点，青色细线是盒子一个角走过的路径。注意 $\mathbf v$ 是"此刻恰好位于世界原点的那个物体点"的速度。起点（$s = 0$）时这个点就是物体原点，所以琥珀色路径一开始沿 $\mathbf v$ 的方向出发；盒子离开原点以后（$s > 0$），两者一般就不同了，除非 $\boldsymbol\omega = 0$。 The grey wireframe is the start (identity pose); the solid box is $\exp(s\boldsymbol\xi)$. The purple point $\mathbf q$ is the point of the screw axis closest to the origin, and the thin cyan curve is the path of one corner of the box. Note that $\mathbf v$ is the velocity of whichever body point currently sits at the world origin. At the start ($s = 0$) that point is the body origin itself, so the amber path leaves along $\mathbf v$; once the box has moved away ($s > 0$) the two generally differ, unless $\boldsymbol\omega = 0$.

试一试Try this

用滑块做出一个纯平移。这时螺旋轴去哪了？With the sliders, make a pure translation. Where did the screw axis go?
让 $\boldsymbol\omega \ne 0$，同时螺距恰好为 0，并且转轴不过原点（$\|\mathbf q\| > 0.3$）。提示：$\mathbf v$ 和 $\boldsymbol\omega$ 应该是什么关系？Keep $\boldsymbol\omega \ne 0$ but make the pitch exactly 0, with an axis that misses the origin ($\|\mathbf q\| > 0.3$). Hint: how should $\mathbf v$ relate to $\boldsymbol\omega$?
让盒子在 $s = 1$ 时回到起点，但中途确实在动（$\boldsymbol\xi \ne 0$）。于是 $\exp(\boldsymbol\xi) = I$ 却 $\boldsymbol\xi \ne 0$，这对 log 意味着什么？Make the box end where it started at $s = 1$ while really moving in between ($\boldsymbol\xi \ne 0$). So $\exp(\boldsymbol\xi) = I$ with $\boldsymbol\xi \ne 0$: what does that mean for log?

4反过来：对数映射与插值The way back: the log map and interpolation

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

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

The logarithm answers the reverse question: given a target pose $T$, which constant twist gets you there in exactly one second? The answer is $\boldsymbol\xi = \Log(T)$. Chasles' theorem guarantees such a screw motion exists.

With it, interpolation becomes natural: $T(s) = \Exp(s\,\Log T)$ as $s$ runs from 0 to 1. This means "travel from start to goal at constant velocity", and every intermediate pose is a valid pose. Compare that with the naive approach from Lesson 5.4, linear interpolation of the matrix entries, $(1-s)I + sT$. The in-between "poses" shrink and shear, and their rotation part is no longer a rotation matrix.

FIG 4Log 与插值：沿螺旋走 vs. 逐元素插值Log and interpolation: along the screw vs. entry by entry

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

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

先用滑块设定目标位姿 $T$（淡色坐标系），再拖动 $s$ 或点播放。两边的视角是联动的。左边的坐标系始终保持单位长度、互相垂直；右边的坐标系在中途变短、变斜，行列式小于 1。 Set the target pose $T$ (faint frame) with the sliders, then drag $s$ or press play. The two views share one camera. On the left the frame keeps unit, perpendicular axes; on the right it shrinks and skews mid-way and its determinant drops below 1.

找一个目标姿态，让右边的逐元素插值在中途塌成一个平面（$\det \approx 0$）。这需要转多少度？Find a target orientation for which the entry-by-entry interpolation on the right collapses to a plane mid-way ($\det \approx 0$). How large a rotation does that take?

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

Interpolating between two arbitrary poses $T_a, T_b$ works the same way: take the relative motion as seen from $T_a$ and walk along it at constant speed, $T(s) = T_a\,\Exp\big(s\,\Log(T_a^{-1}T_b)\big)$. Lesson 5.9 uses this to smooth trajectories and to average poses.

5再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，FIG 2 或 FIG 3 中对应的元素会被高亮。Tip: hover the highlighted symbols in the formulas; the matching element in FIG 2 or FIG 3 lights up.

矩阵指数 = 恒定速度The matrix exponential = constant velocity

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

For any square matrix $A$ the series $\exp(A) = \sum_n A^n/n!$ always converges. Differentiating term by term gives its most important property:

$$ \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}$ 叫一参数子群，可以看成群里的"直线"。

So $X(t) = \exp(tA)$ solves $\dot X = XA$ starting from $I$, and $X^{-1}\dot X = A$ never changes: that is exactly "moving at constant velocity $A$". The set $\{\exp(tA)\}_{t\in\R}$ is called a one-parameter subgroup, the group's version of a straight line.

大写 Exp 与小写 expCapital Exp vs. lowercase exp

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

Lowercase $\exp$ acts on matrices in the Lie algebra; capital $\Exp$ takes a vector, applies hat (Lesson 5.5), then $\exp$. $\Log$ takes $\log$ and then reads off the vector with 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 公式SO(3): the Rodrigues formula

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

Let $\boldsymbol\omega = \theta\hat{\mathbf u}$ and $K = \skew{\hat{\mathbf u}}$. From the cross product (Lesson 1.8) one checks $K^3 = -K$, so every power of $K$ reduces to $K$ or $K^2$, and the infinite series collapses into two terms:

$$ \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]$）：

In reverse, $\operatorname{tr} R = 1 + 2\cos\theta$ and $R - R\T = 2\sin\theta\,\skew{\hat{\mathbf u}}$ give back the angle and the axis (principal value $\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)：带平移的指数映射SE(3): the exponential with translation

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

The hat of a twist $\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)$ is a 4×4 matrix. In its exponential the rotation block is Rodrigues again, and the translation is $\mathbf v$ multiplied by a matrix $V$, which accounts for the displacement accumulated while turning:

$$ \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 里就是这么写的。

To compute $\Log(T)$, first take $\boldsymbol\omega = \Log(R)$, then solve $\mathbf v = V^{-1}\mathbf t$. $V^{-1}$ has a closed form too; that is what the course code R101.M.logSE3 uses.

螺旋参数（Chasles 定理）Screw parameters (Chasles' theorem)

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

When $\boldsymbol\omega \ne 0$, split $\mathbf v$ into the parts perpendicular and parallel to $\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 定理。

Read it as: turn by $\theta = \|\boldsymbol\omega\|$ about the axis through $\mathbf q$ with direction $\hat{\boldsymbol\omega}$, while advancing $h\theta$ along it. If $\mathbf v \perp \boldsymbol\omega$ then $h = 0$ and it is a pure rotation; if $\boldsymbol\omega = 0$ it is a pure translation. Every $T \in \SE$ has a $\Log$, so every rigid motion is some screw motion. That is Chasles' theorem.

6机器人中的它：陀螺仪积分In robotics: integrating a gyro

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

An IMU's gyro reports the body-frame angular velocity $\boldsymbol\omega$ every $dt$ seconds. Treat $\boldsymbol\omega$ as constant over that short interval and the update is "spin at $\boldsymbol\omega$ for $dt$ seconds", multiplied onto the current orientation from the right (right multiplication means motion in the body frame). The alternative is plain Euler integration of the differential equation $\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$ 和同样的陀螺读数。

Euler keeps only the first two terms of the $\Exp$ series, the tangent-line point of FIG 1. Every step pushes $R$ a little off $\SO$, and the errors pile up. Below, both methods use the same $dt$ and the same gyro readings.

FIG 5陀螺仪积分：Exp vs. 欧拉法Gyro integration: Exp vs. naive Euler

拖动画面旋转视角drag to orbit

左：两个立方体以同一个机体角速度旋转，绿色用 $\Exp$ 更新，红色用欧拉法更新；红色旁边的淡色坐标系是真实姿态。右：上图是正交性误差 $\|R\T R - I\|_F$（对数坐标），下图是 $\det R$。在图上拖动可以调整时间。$\Exp$ 的误差一直停在机器精度（约 $10^{-15}$）附近。 Left: two cubes spin with the same body angular velocity; green is updated with $\Exp$, red with naive Euler, and the faint frame next to the red cube is the true orientation. Right: the top plot is the orthogonality error $\|R\T R - I\|_F$ (log scale) and the bottom one is $\det R$. Drag on the plot to scrub time. The $\Exp$ error stays at machine precision (about $10^{-15}$).

真实系统里In real systems

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 节）。

IMU preintegration, standard in visual-inertial odometry, chains the hundreds of gyro samples between two camera frames into $\Delta R = \prod_k \Exp(\boldsymbol\omega_k\,dt)$. Optimizers on manifolds (Ceres' Manifold, GTSAM's retract) solve for a small increment $\boldsymbol\delta$ in the tangent space and update with $T \leftarrow T\,\Exp(\boldsymbol\delta)$, so every step stays on the group (Lessons 5.7 and 6.10). In a robot arm, a revolute joint is a twist of pitch 0 and a prismatic joint is a pure-translation twist; forward kinematics is the product of the joints' $\Exp(\boldsymbol\xi_i\theta_i)$ (the product-of-exponentials formula, Lesson 5.10).

7常见误区Common pitfalls

误区 1：以为 log 是唯一的Pitfall 1: assuming log is unique

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)"。

exp wraps around: $\theta$ and $\theta + 2\pi$ give the same rotation (the FIG 1 task), and a $\theta = 2\pi$ screw motion can even return to the start (the FIG 3 task). $\Log$ returns only the principal value $\theta \in [0, \pi]$, so a 270° turn is reported as 90° the other way. At $\theta = \pi$, both $\hat{\mathbf u}$ and $-\hat{\mathbf u}$ are valid. Unwrap angle sequences before interpolating or averaging them.

误区 2：直接照公式算，不管 θ ≈ 0 和 θ ≈ πPitfall 2: coding the formulas verbatim near θ ≈ 0 and θ ≈ π

$\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]$。

$\sin\theta/\theta$ and $(1-\cos\theta)/\theta^2$ are 0/0 as $\theta \to 0$; switch to Taylor expansions (e.g. $1 - \theta^2/6$). The $1/(2\sin\theta)$ in $\Log$ blows up as $\theta \to \pi$; there, read the axis from the largest diagonal entry of $(R + I)/2 \approx \hat{\mathbf u}\hat{\mathbf u}\T$ and fix its sign with $R - R\T$. Also clamp the argument of $\arccos$ to $[-1, 1]$.

误区 3：以为 Exp(a) Exp(b) = Exp(a + b)Pitfall 3: assuming 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 节会用到它的一阶近似。

It holds only when $\mathbf a$ and $\mathbf b$ are parallel (same axis). 3D rotations do not commute: x then y differs from y then x. In general you need the BCH formula, $\Log(\Exp(\mathbf a)\Exp(\mathbf b)) = \mathbf a + \mathbf b + \tfrac12\,\mathbf a \times \mathbf b + \cdots$, whose first-order form Lesson 5.7 relies on.

误区 4：左乘和右乘随便用Pitfall 4: left and right multiplication used interchangeably

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

$T\,\Exp(\boldsymbol\xi)$ moves by $\boldsymbol\xi$ in the body frame; $\Exp(\boldsymbol\xi)\,T$ moves by $\boldsymbol\xi$ in the world frame. In general they differ. A gyro measures body-frame rates, so it multiplies on the right. Write it on the left and the rotation axis is wrong, a bug that only shows up once the robot tilts.

误区 5：twist 分量的顺序Pitfall 5: the ordering of twist components

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

This lesson and Modern Robotics use $(\boldsymbol\omega, \mathbf v)$; Barfoot, Solà and Sophus use $(\mathbf v, \boldsymbol\omega)$. Permute before you hand one library's 6-vectors, 6×6 covariances or Jacobians to another.

8小练习Check yourself

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

旋转向量的方向是转轴 $(0,0,1)$，长度是转角 $\pi/2$。由 Rodrigues 公式，$\Exp$ 的结果一定是旋转矩阵。The direction of the rotation vector is the axis $(0,0,1)$ and its length is the angle $\pi/2$. By Rodrigues, $\Exp$ always produces a rotation matrix.

2. twist $\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)$，其中 $\boldsymbol\omega = (0,0,1)$，$\mathbf v = (0,0,0.5)$。$\exp(t\boldsymbol\xi)$ 是什么运动？2. A twist $\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)$ has $\boldsymbol\omega = (0,0,1)$ and $\mathbf v = (0,0,0.5)$. What motion is $\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，像拧一颗螺丝。$h = \boldsymbol\omega\cdot\mathbf v/\|\boldsymbol\omega\|^2 = 0.5$ and $\mathbf q = \boldsymbol\omega\times\mathbf v/\|\boldsymbol\omega\|^2 = \mathbf 0$: the screw axis is the z-axis with pitch 0.5, just like driving a screw.

3. $R$ 是绕 z 轴转 270°。标准的 $\Log(R)$（主值）返回什么？3. $R$ is a 270° rotation about z. What does the standard (principal) $\Log(R)$ return?

主值要求 $\theta \in [0, \pi]$。绕 z 转 270° 和绕 z 转 −90°（等价于绕 −z 转 90°）是同一个旋转，所以 $\Log$ 给出 $(0,0,-\pi/2)$。$(0,0,3\pi/2)$ 也满足 $\Exp = R$，但它不是主值。The principal value has $\theta \in [0, \pi]$. A 270° turn about z is the same rotation as −90° about z (i.e. 90° about −z), so $\Log$ gives $(0,0,-\pi/2)$. $(0,0,3\pi/2)$ also satisfies $\Exp = R$, but it is not the principal value.

4. 用 $R_{k+1} = R_k\,\Exp(\boldsymbol\omega\,dt)$ 积分陀螺仪，而不用 $R_k + R_k\skew{\boldsymbol\omega}dt$，主要好处是什么？4. What is the main advantage of integrating a gyro with $R_{k+1} = R_k\,\Exp(\boldsymbol\omega\,dt)$ instead of $R_k + R_k\skew{\boldsymbol\omega}dt$?

$\Exp$ 的输出一定是旋转矩阵，乘积也一定是旋转矩阵；欧拉法每一步都会让 $\det R$ 增大、各列不再正交。零偏需要另外估计（例如放进 EKF 的状态里），$\Exp$ 也比加法稍贵一点。$\Exp$ always outputs a rotation matrix, and so does any product of them; Euler inflates $\det R$ and breaks the orthogonality of the columns at every step. Bias must be estimated separately (e.g. in an EKF state), and $\Exp$ is slightly more expensive than an addition.

9延伸阅读Further reading

K. Lynch & F. Park, Modern Robotics, Ch. 3 — 指数坐标、twist、螺旋运动；本课的 (ω, v) 顺序与它一致exponential coordinates, twists and screw motion; uses the same (ω, v) ordering as this lesson
J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537 — Exp/Log、⊕/⊖ 和雅可比矩阵的简明手册（平移在前）a compact handbook of Exp/Log, ⊕/⊖ and Jacobians (translation first)
T. Barfoot, State Estimation for Robotics, Ch. 7 (Matrix Lie Groups) — SO(3)/SE(3) 的指数映射、雅可比与不确定性exponential maps, Jacobians and uncertainty on SO(3)/SE(3)
E. Eade, Lie Groups for 2D and 3D Transformations — 简短的技术笔记，含所有闭式公式及其数值稳定写法a short technical note with every closed form and its numerically safe version