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

为什么需要李群Why Lie Groups?

估计、滤波、优化、学习，几乎都离不开三个动作：加一个小修正、求平均、沿梯度走一步。对向量这都是家常便饭，对旋转和位姿却统统失效：旋转矩阵相加不再是旋转，平均会"缩水"，梯度一步就走出群外。李群就是为此准备的工具：群负责乘法和逆，流形负责局部像平地。 Estimation, filtering, optimisation and learning all boil down to three moves: add a small correction, take an average, step along a gradient. These are routine for vectors and break down for rotations and poses: the sum of rotation matrices is not a rotation, averages "shrink", and one gradient step walks off the group. Lie groups are the toolbox made for this: the group supplies multiplication and inverses, the manifold makes the space locally flat.

约 25 分钟~25 min 前置：1.10 正交矩阵与旋转 · 5.2 旋转的表示 · 5.3 齐次变换与 SE(3)Prereqs: 1.10 Orthogonal matrices & rotations · 5.2 Representing rotations · 5.3 SE(3) 5 个交互5 interactives

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

想象你有三个相机分别估出了同一个物体的姿态，每个都带着噪声，你想把它们融合成一个更准的估计。向量的世界里答案是：取平均。把三个旋转矩阵逐个元素相加再除以 3 呢？结果通常不是一个旋转矩阵：它会让坐标系变短、变斜，行列式小于 1。

再想想卡尔曼滤波：状态加上一个小的修正量 $\mathbf x \leftarrow \mathbf x + \delta$；非线性优化：每次迭代 $\mathbf x \leftarrow \mathbf x - \alpha\nabla f$；神经网络：反向传播之后更新参数。这些全都默认"状态可以相加"。当状态是姿态 $R$ 或位姿 $T$ 时，$R + \delta$ 就不再是旋转了。

解决办法不是放弃这些算法，而是给姿态和位姿换一套"局部的加法"：在当前点附近建一个平坦的小坐标系，在里面做加法、求平均、迈梯度步，再把结果"贴回"曲面上。支撑这套做法的数学结构就是李群。这一节先说明它的动机；后面几节（5.5 起）给出具体工具。

Suppose three cameras each estimate the orientation of the same object, each with noise, and you want to fuse them into a better estimate. In a vector space the answer is: average them. What if you add the three rotation matrices entry by entry and divide by 3? The result is usually not a rotation matrix: the frame gets shorter and skewed, with determinant below 1.

Think of a Kalman filter: add a small correction to the state, $\mathbf x \leftarrow \mathbf x + \delta$. Nonlinear optimisation: each iteration does $\mathbf x \leftarrow \mathbf x - \alpha\nabla f$. Neural networks: update the parameters after backprop. All of these assume "states can be added". When the state is an orientation $R$ or a pose $T$, $R + \delta$ is no longer a rotation.

The fix is not to give up these algorithms but to give orientations and poses a different, "local addition": build a small flat coordinate system around the current point, add, average and take gradient steps in there, then "paste" the result back onto the curved space. The mathematical structure behind it is a Lie group. This lesson explains the motivation; Lesson 5.5 onward supplies the concrete tools.

2直觉：地球表面与"加不起来"的旋转Intuition: the Earth's surface and rotations that will not add

地球表面是弯的，但你在城市里用平面地图就够了：小范围里，弯曲可以忽略。但如果你把"北京"和"旧金山"的三维坐标 $(x, y, z)$（以地心为原点）直接取平均去找中点，得到的位置会落在地球内部：两点的连线（弦）穿过了地下。正确的中点要沿着球面走（测地线）。旋转的情形完全一样：旋转构成的空间是弯的，直接相加走的是"弦"，会穿出这个空间。

从最简单的平面旋转说起。每个平面旋转对应单位圆上的一个点 $(\cos\alpha, \sin\alpha)$，旋转矩阵 $R(\alpha)$ 的第一列就是这个点。下面拖动两个旋转 $R_1$、$R_2$，看看它们的"平均" $\tfrac12(R_1 + R_2)$ 在哪里。

The Earth's surface is curved, yet a flat map is fine inside a city: over a small region, the curvature can be ignored. But if you average the 3D $(x, y, z)$ positions of Beijing and San Francisco (measured from the Earth's centre) directly to find a midpoint you land inside the Earth: the straight line (chord) between them goes underground. The proper midpoint follows the surface (a geodesic). Rotations are exactly alike: the space of rotations is curved, and adding takes the "chord", which exits the space.

Begin with planar rotations. Each planar rotation is a point $(\cos\alpha, \sin\alpha)$ on the unit circle, the first column of the matrix $R(\alpha)$. Drag two rotations $R_1$, $R_2$ below and see where their "average" $\tfrac12(R_1 + R_2)$ lands.

FIG 1SO(2) 是一个圆：相加会掉进圆里SO(2) is a circle: adding drops you inside

拖动圆上两个点 R₁、R₂drag the two points R₁, R₂ on the circle

单位圆 = SO(2)。琥珀点是 $\tfrac12(R_1+R_2)$：在弦的中点，落在圆内。绿点是把它"贴回"圆上（沿半径方向放大到长度 1）得到的旋转，它恰好是转角的平均 $\tfrac12(\alpha+\beta)$（只要两角相差小于 180°）。左下角的青色小方块是单位正方形在平均矩阵作用下的像：随着两点分开，它不断缩小，直至塌成一个点。 The unit circle = SO(2). The amber dot is $\tfrac12(R_1+R_2)$: the midpoint of the chord, inside the circle. The green dot is that point "pasted back" onto the circle (scaled radially to length 1); it is exactly the mean angle $\tfrac12(\alpha+\beta)$ as long as the two angles differ by less than 180°. The cyan square at the lower left is the unit square's image under the averaged matrix: as the two points separate it shrinks, ending as a single point.

试一试Try this

把两个点拉得很近（相差小于 10°）。琥珀点几乎贴着圆。局部看，圆几乎是平的，这是后面所有"局部加法"的依据。Bring the two points close (under 10° apart). The amber dot nearly touches the circle: locally the circle looks flat, which is the basis of every "local addition" to come.
把两个点放到相对的两端（相差约 180°）。平均矩阵变成了什么？它还是一个旋转吗？Put the two points on opposite ends (about 180° apart). What does the averaged matrix become? Is it still a rotation?

核心洞见Key insight

旋转的集合不是向量空间：对加法和数乘不封闭。它是一个弯曲的空间（流形），但只要停留在一个很小的邻域里，它几乎是平的。李群的全部想法，就是利用这"整体弯、局部平"的性质。

The set of rotations is not a vector space: it is not closed under addition and scaling. It is a curved space (a manifold), but inside a small neighbourhood it is almost flat. The whole idea of Lie groups is to exploit this "globally curved, locally flat" property.

3动手玩：三个小实验Play: three small experiments

实验一：三维旋转不可交换Experiment 1: 3D rotations do not commute

平面旋转"先转 $a$ 再转 $b$"和"先转 $b$ 再转 $a$"结果一样（1.10 节），所以可以用一个数 $a+b$ 来记。三维就不行了：先绕 $x$ 转 $a$、再绕 $y$ 转 $b$，和反过来，得到不同的朝向。这意味着连"加法"这个词都不够用：$a + b = b + a$ 对旋转不成立。

下面把两种顺序的结果并排画出来（琥珀色：$R_x(a)R_y(b)$，紫色：$R_y(b)R_x(a)$），看它们之间的夹角。

In the plane, "turn by $a$ then $b$" equals "turn by $b$ then $a$" (Lesson 1.10), so a single number $a+b$ records it. Not in 3D: turning about $x$ by $a$ then about $y$ by $b$ gives a different orientation than the reverse. Even the word "addition" fails: $a + b = b + a$ does not hold for rotations.

Below the two orders are drawn side by side (amber: $R_x(a)R_y(b)$; purple: $R_y(b)R_x(a)$), and we read the angle between them.

FIG 2两种顺序，两个朝向Two orders, two orientations

拖动画面旋转视角drag to orbit

灰色是起点。两个机体朝向的差异用"夹角"（$R_1\T R_2$ 的转角）衡量。小角度时，夹角约等于 $a\,b$（弧度）：差距是二阶小量。小旋转"几乎"可交换，大旋转则差得很远。 Grey is the start. The difference between the two orientations is measured by the angle (the rotation angle of $R_1\T R_2$). For small angles it is about $a\,b$ (radians): the discrepancy is second order. Small rotations "almost" commute; large ones differ a lot.

试一试Try this

找一对 $(a, b)$，让两种顺序的夹角超过 60°。Find an $(a, b)$ for which the two orders differ by more than 60°.
取 $a, b$ 都在 5° 到 10° 之间，看夹角是否近似 $a\,b$。（$a=b=8°$ 时 $ab\approx 1.1°$。）Take $a, b$ both between 5° and 10° and check that the gap is about $a\,b$. (For $a=b=8°$, $ab\approx 1.1°$.)

这个"二阶小量"不是巧合：它正是下一节要见到的李括号（叉积）。我们先记住结论：SO(3) 是不可交换的群，连乘的顺序不能随便换。

This "second-order quantity" is no coincidence: it is the Lie bracket (the cross product) of the next lesson. For now remember the verdict: SO(3) is a non-commutative group, and the order of the product cannot be swapped.

实验二：放大看，弯曲的圆就像一条直线Experiment 2: zoom in and the curve becomes a line

前面看到整体是弯的，局部是平的。把这件事看得更具体：在圆的单位元 $(1, 0)$ 处画一条切线。切线上的点 $(1, \theta)$ 和圆上的点 $(\cos\theta, \sin\theta)$ 对应同一个转角 $\theta$，二者的距离随 $\theta$ 变小而飞快缩小。拖动缩放滑块，放大到单位元附近，圆和切线会重合；这条切线就是 SO(2) 的李代数（5.5 节）。

We saw "globally curved, locally flat". Make this concrete: at the identity $(1, 0)$ of the circle draw the tangent line. The point $(1, \theta)$ on the line and the point $(\cos\theta, \sin\theta)$ on the circle correspond to the same angle $\theta$, and their distance shrinks very fast as $\theta$ gets small. Drag the zoom slider to zoom in on the identity: the circle and the line merge. This tangent line is the Lie algebra of SO(2) (Lesson 5.5).

FIG 3放大单位元：圆变成切线Zoom in on the identity: the circle becomes its tangent

拖动圆上的点drag the point on the circle

青色圆是 SO(2)，紫色虚线是在单位元处的切线（带刻度，每格 0.1 弧度）。琥珀点在圆上（$\theta$ 的旋转），紫色点是切线上对应的 $(1, \theta)$。红线是两者的距离：它大约是 $\theta^2/2$，所以 $\theta$ 越小，相对误差 $g/\theta \approx \theta/2$ 越小。 The cyan circle is SO(2) and the dashed purple line its tangent at the identity (ticks every 0.1 rad). The amber dot is on the circle (the rotation by $\theta$); the purple dot is the matching $(1, \theta)$ on the tangent. The red line is their distance, about $\theta^2/2$, so the relative error $g/\theta \approx \theta/2$ shrinks with $\theta$.

试一试Try this

先让 $\theta$ 约 0.15 弧度，再把视野缩小到 0.3 以内。圆和切线几乎重合。With $\theta$ about 0.15 rad, shrink the view span below 0.3. The circle and the tangent nearly coincide.
用"角度 $\theta$"滑块找出：相对误差 $g/\theta$ 刚好超过 5% 时，$\theta$ 约是多少度？这给"小角度近似"划了一条经验线。With the "angle $\theta$" slider find the $\theta$, in degrees, where the relative error $g/\theta$ just passes 5%. This draws a rule-of-thumb boundary for the "small-angle approximation".

实验三：沿梯度走一步，会走出群外Experiment 3: a gradient step walks off the group

想估计一个旋转 $z$（还是在圆上），让它离三个测量 $z_1, z_2, z_3$ 尽量近：最小化 $f(z) = \tfrac13\sum_i \|z - z_i\|^2$。梯度是 $2(z - \bar z)$，$\bar z$ 是三个点的平均。如果把圆当成平面，按普通梯度下降 $z \leftarrow z - \eta\nabla f$ 迭代，它会收敛到 $\bar z$，一个在圆里面的点，根本不是旋转。如果把梯度投影到切线，沿切线方向走一小步再贴回圆上，迭代始终在圆上，收敛到最接近 $\bar z$ 的那个旋转。

Estimate a rotation $z$ (still on the circle) as close as possible to three measurements $z_1, z_2, z_3$: minimise $f(z) = \tfrac13\sum_i \|z - z_i\|^2$. The gradient is $2(z - \bar z)$ with $\bar z$ the mean of the three points. Treat the circle as a plane and iterate plain gradient descent $z \leftarrow z - \eta\nabla f$: it converges to $\bar z$, a point inside the circle that is no rotation at all. Project the gradient onto the tangent, take a small step along it and paste back onto the circle: the iterates stay on the circle and converge to the rotation nearest to $\bar z$.

FIG 4朴素梯度 vs. 切线上的梯度Naive gradient vs. tangent-space gradient

拖动三个测量点（青色）drag the three measurements (cyan)

青色点 $z_i$ 是测量，琥珀点是它们的平均 $\bar z$（在圆内）。紫色轨迹：把圆当平面做普通梯度下降，每一步都离开了圆。绿色轨迹：在切线方向走步再贴回，始终在圆上，最后停在 $\bar z$ 的方向。两条轨迹从同一个起点（圆上 $(1, 0)$ 处的实心小点，转角 0）出发。 The cyan dots $z_i$ are the measurements and the amber dot their mean $\bar z$ (inside the circle). Purple trail: plain gradient descent treating the circle as a plane; every step leaves the circle. Green trail: step along the tangent, paste back; it stays on the circle and stops in the direction of $\bar z$. Both start at the same point (the solid dot at $(1, 0)$ on the circle, angle 0).

试一试Try this

点"运行"，看紫色迭代的长度 $\|z\|$ 最终停在小于 1 的地方。Press "Run" and see the purple iterate's length $\|z\|$ end up below 1.
把三个测量点散开到三个方向（互相约 120°），平均 $\|\bar z\|$ 小于 0.25 后再运行。紫色解缩成了一个几乎是零的点，绿色解却几乎没有方向可言（切线上的梯度也很小）：测量太分散时，"平均姿态"本来就没有定义。Spread the three measurements in three directions (about 120° apart) so $\|\bar z\|$ drops below 0.25, then run. The purple solution shrinks to almost zero, and the green one has almost no preferred direction (the tangent gradient is tiny too): when measurements are too scattered, the "mean orientation" is simply ill-defined.

4再看数学The math

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

群Groups

一个群 $(G, \cdot)$ 是一个集合加一种乘法，满足四条：

A group $(G, \cdot)$ is a set with a multiplication satisfying four rules:

封闭：$a, b \in G \Rightarrow ab \in G$。
Closure: $a, b \in G \Rightarrow ab \in G$.
结合：$(ab)c = a(bc)$。
Associativity: $(ab)c = a(bc)$.
单位元：存在 $e$，使 $ea = ae = a$。
Identity: there is an $e$ with $ea = ae = a$.
逆：每个 $a$ 有 $a^{-1}$，使 $aa^{-1} = a^{-1}a = e$。
Inverses: every $a$ has an $a^{-1}$ with $aa^{-1} = a^{-1}a = e$.

验证 $\SO$：$(R_1R_2)\T(R_1R_2) = R_2\T R_1\T R_1R_2 = I$，$\det(R_1R_2) = 1$（封闭）；矩阵乘法满足结合律；单位元是 $I$；逆是 $R\T$。这些在 1.10 节已经出现过。$\SE$（5.3 节）同理。群里没有加法，只有乘法。是否交换是另一回事：SO(2) 可交换，$\SO$ 和 $\SE$ 不可交换（FIG 2）。

Check $\SO$: $(R_1R_2)\T(R_1R_2) = R_2\T R_1\T R_1R_2 = I$ and $\det(R_1R_2) = 1$ (closure); matrix multiplication is associative; the identity is $I$; the inverse is $R\T$. All of this appeared in Lesson 1.10, and $\SE$ (Lesson 5.3) is the same. A group has no addition, only multiplication. Commutativity is a separate matter: SO(2) commutes, $\SO$ and $\SE$ do not (FIG 2).

为什么"加起来"不行Why "adding" fails

以 SO(2) 为例，$R_1 + R_2$ 的第一列是 $(\cos\alpha + \cos\beta,\ \sin\alpha + \sin\beta)$，长度是 $2\cos\frac{\alpha-\beta}{2}$。所以

In SO(2), the first column of $R_1 + R_2$ is $(\cos\alpha + \cos\beta,\ \sin\alpha + \sin\beta)$, of length $2\cos\frac{\alpha-\beta}{2}$. Hence

$$ \hl{mid}{\tfrac12(R_1 + R_2)} = \cos\tfrac{\alpha-\beta}{2}\; R\!\Big(\tfrac{\alpha+\beta}{2}\Big), \qquad \det = \cos^2\tfrac{\alpha-\beta}{2} \le 1. $$

它是一个旋转（平均角度 $\frac{\alpha+\beta}{2}$）再乘一个缩放因子 $\cos\frac{\alpha-\beta}{2} \le 1$。两角越分开，缩得越厉害，相差 180° 时变成零矩阵。$\SO$ 里同样如此：平均出来的 $\frac1N\sum R_i$ 的奇异值总是不超过 1，通常小于 1，所以它既不正交，也没有单位行列式。"贴回"的做法（$\hl{proj}{\text{投影到最近的旋转}}$，用 SVD 或极分解）可以补救，但它只是一个事后的补丁，而且在 $\frac1N\sum R_i$ 接近奇异时会失效。

It is a rotation (the mean angle $\frac{\alpha+\beta}{2}$) times a scale factor $\cos\frac{\alpha-\beta}{2} \le 1$. The farther the angles are apart, the stronger the shrinking, ending in the zero matrix at 180°. In $\SO$ it is the same: the singular values of the average $\frac1N\sum R_i$ never exceed 1 and are usually below 1, so it is neither orthogonal nor of unit determinant. "Pasting back" ($\hl{proj}{\text{projecting onto the nearest rotation}}$ with an SVD or polar decomposition) can repair it, but that is a patch after the fact, and it fails when $\frac1N\sum R_i$ is near-singular.

流形与维数Manifolds and dimension

一个 $n$ 维光滑流形是这样的空间：每个点附近都像 $\R^n$ 的一小块（有局部坐标），不同局部坐标之间的转换是光滑的。球面、圆环、$\SO$ 都是流形。$\SO$ 在 $\R^{3\times3} = \R^9$ 里由 $R\T R = I$ 定义；$R\T R$ 是对称矩阵，所以这个等式只有 $6$ 个独立方程，自由度是 $9 - 6 = 3$。$\SE$ 再加 3 个平移分量，维数是 6。

An $n$-dimensional smooth manifold is a space that near every point looks like a piece of $\R^n$ (it has local coordinates), with smooth changes between overlapping local coordinates. Spheres, tori and $\SO$ are manifolds. $\SO$ sits in $\R^{3\times3} = \R^9$, cut out by $R\T R = I$; since $R\T R$ is symmetric, this holds six independent equations, so the dimension is $9 - 6 = 3$. $\SE$ adds three translation components for dimension 6.

一个有用的拓扑事实A useful topological fact

$\SO$ 在整体上不是一个平坦的区域：它可以看成一个半径为 $\pi$ 的实心球，表面上相对的两点被粘成同一个点（转 $\pi$ 绕 $\hat{\mathbf u}$ 和绕 $-\hat{\mathbf u}$ 是同一个旋转）。这也是为什么没有任何 3 参数的描述能不出奇点（5.2 节）。

$\SO$ is globally not a flat region: it can be pictured as a solid ball of radius $\pi$ whose opposite surface points are glued together (turning by $\pi$ about $\hat{\mathbf u}$ and about $-\hat{\mathbf u}$ is the same rotation). This is why no 3-parameter description can avoid singularities (Lesson 5.2).

李群 = 群 + 光滑流形Lie group = group + smooth manifold

一个李群是同时又是光滑流形的群，而且乘法 $(a, b)\mapsto ab$ 与取逆 $a\mapsto a^{-1}$ 都是光滑映射。$\SO$、$\SE$、SO(2)、SE(2)，以及向量空间 $(\R^n, +)$（平坦的、可交换的李群）都是李群。机器人学里最常遇到的就是这几个。

A Lie group is a group that is also a smooth manifold, with multiplication $(a, b)\mapsto ab$ and inversion $a\mapsto a^{-1}$ both smooth maps. $\SO$, $\SE$, SO(2), SE(2) and the vector space $(\R^n, +)$ (a flat, commutative Lie group) are Lie groups. Those are the ones robotics meets most.

群Group	形状Shape	维数Dim.	可交换？Commutative?
$(\R^n, +)$	平坦空间flat space	$n$	是yes
SO(2)	圆circle	1	是yes
$\SO$	半径 $\pi$ 的球，表面对径点粘合ball of radius $\pi$, antipodal surface points glued	3	否no
SE(2)	圆 × 平面（乘法是"拧"在一起的）circle × plane (twisted product)	3	否no
$\SE$	$\SO \times \R^3$（乘法是"拧"在一起的） (twisted product)	6	否no

单位元处的切空间：反对称矩阵The tangent space at the identity: skew-symmetric matrices

取一条经过单位元的光滑曲线 $R(t)$，$R(0) = I$，对恒等式 $R(t)\T R(t) = I$ 求导：$\dot R\T R + R\T\dot R = 0$，代入 $t = 0$：

Take a smooth curve $R(t)$ through the identity, $R(0) = I$, and differentiate the identity $R(t)\T R(t) = I$: $\dot R\T R + R\T\dot R = 0$. At $t = 0$,

$$ \dot R(0)\T + \dot R(0) = 0 \quad\Longleftrightarrow\quad \dot R(0) \text{ is skew-symmetric}. $$

所以 $\SO$ 在单位元处的所有"速度"恰好是 $3\times3$ 反对称矩阵，它们构成 3 维向量空间，也就是一个"平地"。反对称矩阵有 3 个自由度，可以用向量 $\boldsymbol\omega\in\R^3$ 记录（$\skew{\boldsymbol\omega}$，1.8 节）。这个切空间 $\so$ 就是 $\SO$ 的李代数，下一节的主角。在别处的切空间可以由群乘法搬运：$T_R\SO = R\,\so$。同样的推导对 $\SE$ 得到 6 维的 $\se$。

So every "velocity" of $\SO$ at the identity is a $3\times3$ skew-symmetric matrix, and these form a 3-dimensional vector space: a flat "ground". A skew-symmetric matrix has three degrees of freedom and can be recorded by a vector $\boldsymbol\omega\in\R^3$ ($\skew{\boldsymbol\omega}$, Lesson 1.8). This tangent space $\so$ is the Lie algebra of $\SO$, the star of the next lesson. Tangent spaces elsewhere are carried over by group multiplication: $T_R\SO = R\,\so$. The same computation gives the 6-dimensional $\se$ for $\SE$.

$$ \underbrace{\SO}_{\text{curved group}} \;\xleftarrow{\ \exp\ }\; \underbrace{\so}_{\text{flat tangent space}\;\cong\;\R^3}, \qquad \text{local addition: } R \oplus \boldsymbol\delta = R\,\Exp(\boldsymbol\delta). $$

这就是李群的"配方"：在平坦的切空间（李代数）里做加法、平均、迈梯度步，再用指数映射贴回群上。5.5 节把切空间写成具体的向量和矩阵，5.6 节给出贴回去的指数映射。

That is the Lie-group recipe: do the adding, averaging and gradient stepping in the flat tangent space (the Lie algebra), then paste back onto the group with the exponential map. Lesson 5.5 writes the tangent space in concrete vectors and matrices; Lesson 5.6 supplies the exponential that pastes back.

5机器人中的它：融合多个姿态估计In robotics: fusing several orientation estimates

一块标定板被 $N$ 个视角看到，或者几个 IMU 各自给出一个姿态，每个估计 $R_i$ 都带着噪声。融合的第一步就是求平均。下面的实验里，真值是一个固定姿态，每个测量是 $R_i = R_{\text{true}}\Exp(\sigma\,\mathbf n_i)$（$\mathbf n_i$ 是高斯噪声向量，$\sigma$ 是噪声大小）。比较三种平均：

逐元素平均 $\frac1N\sum R_i$：不是旋转，看它的行列式。
投影平均：把逐元素平均投影到最近的旋转（极分解）。
流形平均（测地线平均、Karcher 平均）：反复在切空间里取平均 $\bar R \leftarrow \bar R\,\Exp\big(\tfrac1N\sum_i\Log(\bar R\T R_i)\big)$，每次都贴回群上。

A calibration board is seen from $N$ viewpoints, or several IMUs each report an orientation, and every estimate $R_i$ carries noise. The first step of fusion is to average. In the lab below the truth is a fixed orientation and each measurement is $R_i = R_{\text{true}}\Exp(\sigma\,\mathbf n_i)$ ($\mathbf n_i$ a Gaussian noise vector and $\sigma$ the noise level). Compare three averages:

Entrywise mean $\frac1N\sum R_i$: not a rotation; look at its determinant.
Projected mean: project the entrywise mean onto the nearest rotation (polar decomposition).
Manifold mean (geodesic or Karcher mean): iterate the tangent-space average $\bar R \leftarrow \bar R\,\Exp\big(\tfrac1N\sum_i\Log(\bar R\T R_i)\big)$, pasting back onto the group each time.

FIG 5三种平均姿态的方法Three ways to average orientations

拖动画面旋转视角drag to orbit

青色小点是各个测量的 $x$ 轴尖端。淡色大坐标系是真值。虚线（细）是逐元素平均的三根轴：它们比真值短，噪声越大越短。实心粗坐标系是流形平均，与真值叠在一起时几乎看不出区别。右边给出各方法的行列式和对真值的角度误差。 The small cyan dots are the $x$-axis tips of the individual measurements. The large faint frame is the truth. The thin dashed axes are the entrywise mean: shorter than the truth, and shorter the more noise there is. The solid bold frame is the manifold mean, nearly indistinguishable from the truth when they overlap. The right panel lists each method's determinant and angular error against the truth.

试一试Try this

把噪声 $\sigma$ 调到 5° 以下：三种方法几乎一样。小噪声时，局部近似足够好。Bring the noise $\sigma$ below 5°: the three methods nearly agree. With small noise the local approximation is good enough.
把 $\sigma$ 加大到 40° 以上，让逐元素平均的行列式小于 0.3：它已经严重变形（甚至行列式变负），而流形平均仍是一个合法的旋转。Raise $\sigma$ above 40° until the entrywise mean's determinant falls below 0.3: it is badly deformed (the determinant can even turn negative) while the manifold mean is still a legitimate rotation.

真实系统里In real systems

姿态估计和 VIO（视觉惯性里程计）里的 EKF / 因子图，状态里有 $R$ 或 $T$：更新时 $\hat R \leftarrow \hat R\,\Exp(\delta\boldsymbol\theta)$，协方差也在切空间里（5.7、5.9 节）。GTSAM、Ceres、g2o 等优化库有"流形"或"retraction"接口，就是为了在切空间里迈步、贴回群上。深度学习里让网络输出旋转时，回归 6D 表示再正交化，也是在回避"直接回归 9 个元素会出群"的问题。SLAM 回环里把累积漂移拆分到每个位姿上，需要的是群上的"除法"和"分数次幂"，这些也只有在切空间里才好定义。

In attitude estimation and VIO (visual-inertial odometry) EKFs and factor graphs hold $R$ or $T$ in the state: the update is $\hat R \leftarrow \hat R\,\Exp(\delta\boldsymbol\theta)$ and the covariance lives in the tangent space (Lessons 5.7 and 5.9). Optimisation libraries such as GTSAM, Ceres and g2o expose "manifold" or "retraction" interfaces precisely so that you step in the tangent space and paste back onto the group. In deep learning, networks that output rotations regress a 6D representation and orthonormalise it, which dodges the problem that regressing nine entries directly leaves the group. In SLAM loop closure, spreading accumulated drift over all poses needs "division" and "fractional powers" on the group, and those are only well defined in the tangent space.

6常见误区Common pitfalls

误区 1：对旋转矩阵 / 位姿矩阵逐元素加、减、平均Pitfall 1: adding, subtracting or averaging rotation / pose matrices entrywise

$R_1 + R_2$、$R_1 - R_2$、$\frac12(R_1+R_2)$ 都不是旋转。能做的是：用乘法 $R_1R_2$、$R_1\T R_2$（相对旋转）做组合与比较；平均、插值、加扰动一律走切空间。（偶尔 $\|R_1 - R_2\|_F$ 被用作"距离"，它是弦距离，对小角度有效，但不是真正的测地距离 $\|\Log(R_1\T R_2)\|$。）

$R_1 + R_2$, $R_1 - R_2$ and $\frac12(R_1+R_2)$ are not rotations. What you can do: combine and compare with products $R_1R_2$ and $R_1\T R_2$ (the relative rotation), and route averaging, interpolation and perturbation through the tangent space. (Occasionally $\|R_1 - R_2\|_F$ is used as a "distance"; it is the chordal distance, valid for small angles but not the true geodesic distance $\|\Log(R_1\T R_2)\|$.)

误区 2：对欧拉角或轴角向量做加法和平均Pitfall 2: adding and averaging Euler angles or rotation vectors

欧拉角有周期（$179°$ 和 $-179°$ 的平均不是 $0°$）、有奇点，又依赖顺序。把两个旋转向量相加 $\boldsymbol\omega_1 + \boldsymbol\omega_2$ 只在两者平行时才等于合成旋转（5.6 节的误区 3）。在小角度、同一个线性化点附近，这些做法"差不多对"，一旦角度大了，误差就是一阶的。

Euler angles are periodic (the mean of $179°$ and $-179°$ is not $0°$), singular, and order-dependent. Adding two rotation vectors $\boldsymbol\omega_1 + \boldsymbol\omega_2$ equals the composed rotation only when they are parallel (Pitfall 3 of Lesson 5.6). For small angles around one linearisation point these shortcuts are "roughly right"; for large angles the error is first order.

误区 3：以为"小角度近似"永远成立Pitfall 3: assuming the small-angle approximation always holds

$\sin\theta\approx\theta$、"旋转可交换"、"切线上的点就是圆上的点"，这些都只在 $\theta$ 很小时成立；相对误差大约是 $\theta/2$（FIG 3）。线性化点离真实状态很远（例如滤波器刚启动、姿态未知）时，EKF 会发散。需要时要重新线性化，或者用对大误差更稳健的方法。

$\sin\theta\approx\theta$, "rotations commute" and "a point on the tangent is a point on the circle" hold only for tiny $\theta$; the relative error is about $\theta/2$ (FIG 3). When the linearisation point is far from the true state (a filter just started with unknown attitude, say) an EKF diverges. Re-linearise as needed, or use methods that tolerate large errors.

误区 4：一投影就万事大吉Pitfall 4: thinking a projection fixes everything

"先当作向量算，最后再投影回 $\SO$"在噪声小时能用，也是很多代码的做法。但是在优化迭代中间放任变量离开群，会让目标函数的梯度和 Hessian 不再对应真实的几何问题；当数据分散、平均接近奇异（FIG 4 的第二个任务）时，投影的结果不稳定甚至不唯一。把优化直接写在流形上（6.10 节）更干净。

"Compute as if in a vector space and project back onto $\SO$ at the end" works for small noise and is common in code. But letting variables leave the group in the middle of an optimisation makes the gradient and Hessian of the objective stop matching the true geometric problem; when the data are scattered and the mean is near-singular (the second task of FIG 4) the projection becomes unstable or even non-unique. Writing the optimisation directly on the manifold (Lesson 6.10) is cleaner.

7小练习Check yourself

1. 为什么两个旋转矩阵相加通常不是旋转矩阵？1. Why is the sum of two rotation matrices usually not a rotation matrix?

矩阵加法当然有定义，但 $\SO$ 是一个弯曲的集合，两个点的和（弦）不在集合里。例如 $I + (-I) = 0$。选项 C 也是错的：三维旋转一般不可交换。Matrix addition is certainly defined, but $\SO$ is a curved set and the sum of two points (the chord) is not in it; for instance $I + (-I) = 0$. Option C is wrong too: 3D rotations generally do not commute.

2. SO(2) 中 $R(30°)$ 和 $R(90°)$ 的逐元素平均 $\frac12(R_1+R_2)$ 的行列式是？2. What is the determinant of the entrywise mean $\frac12(R_1+R_2)$ of $R(30°)$ and $R(90°)$ in SO(2)?

平均矩阵 = $\cos\frac{\alpha-\beta}{2}\,R(\frac{\alpha+\beta}{2})$，$\alpha-\beta = 60°$，所以缩放因子是 $\cos30°$，行列式是它的平方 $0.75$。$0.5 = \cos 60°$ 把半角忘了。The mean matrix is $\cos\frac{\alpha-\beta}{2}\,R(\frac{\alpha+\beta}{2})$, and $\alpha-\beta = 60°$, so the scale factor is $\cos30°$ and the determinant is its square, $0.75$. $0.5 = \cos 60°$ forgets the half angle.

3. $\SO$ 的维数是 3，因为？3. Why is the dimension of $\SO$ equal to 3?

选项 A 是定义性的算法；选项 C 虽然数字对（4 − 1 = 3），但它解释的是"单位四元数集合 $S^3$ 的维数"，不是 $R\T R = I$ 的计数，而且 $S^3$ 只是 $\SO$ 的双覆盖。Option A is the defining count. Option C gets the right number (4 − 1 = 3) but explains the dimension of the unit-quaternion set $S^3$, not a count from $R\T R = I$; and $S^3$ is only a double cover of $\SO$.

4. 曲线 $R(t)$ 过单位元，$R(0) = I$。$\dot R(0)$ 是什么矩阵？4. A curve $R(t)$ passes through the identity, $R(0) = I$. What kind of matrix is $\dot R(0)$?

对 $R\T R = I$ 求导并令 $t = 0$，得 $\dot R(0)\T + \dot R(0) = 0$，即反对称。这个反对称矩阵就是 $\skew{\boldsymbol\omega}$，$\boldsymbol\omega$ 是角速度。Differentiating $R\T R = I$ at $t = 0$ gives $\dot R(0)\T + \dot R(0) = 0$: skew-symmetric. That skew-symmetric matrix is $\skew{\boldsymbol\omega}$ with $\boldsymbol\omega$ the angular velocity.

5. 在流形上做梯度下降，"投影到切空间再贴回"的主要好处是？5. What is the main benefit of "project to the tangent space, then paste back" in manifold gradient descent?

贴回群上保证了约束 $R\T R = I$ 始终成立，目标函数始终有明确的几何意义。速度不一定更快，梯度仍然需要。Pasting back onto the group keeps the constraint $R\T R = I$ true throughout, so the objective always has a clear geometric meaning. It is not necessarily faster, and gradients are still needed.

8延伸阅读Further reading

J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537, §1–2 — 为什么机器人学需要李群，以及 ⊕ / ⊖ 记号why robotics needs Lie groups, and the ⊕ / ⊖ notation
T. Barfoot, State Estimation for Robotics, Ch. 7 (Matrix Lie Groups) — 矩阵李群的详细讲解，附大量推导a detailed treatment of matrix Lie groups with many derivations
K. Lynch & F. Park, Modern Robotics, Ch. 3 — SO(3)、SE(3) 作为群的性质SO(3) and SE(3) as groups
R. Hartley, J. Trumpf, Y. Dai, H. Li, "Rotation averaging", International Journal of Computer Vision, 2013 — 旋转平均的几种定义与算法the definitions and algorithms of rotation averaging
3Blue1Brown, "Group theory, abstraction, and the 196,883-dimensional monster" — 以直观方式介绍"群"是什么an intuitive introduction to what a "group" is

群Group	形状Shape	维数Dim.	可交换？Commutative?
\((\R^n, +)\)	平坦空间flat space	\(n\)	是yes
SO(2)	圆circle	1	是yes
\(\SO\)	半径 \(\pi\) 的球，表面对径点粘合ball of radius \(\pi\), antipodal surface points glued	3	否no
SE(2)	圆 × 平面（乘法是"拧"在一起的）circle × plane (twisted product)	3	否no
\(\SE\)	\(\SO \times \R^3\)（乘法是"拧"在一起的） (twisted product)	6	否no