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

为什么需要李群

估计、滤波、优化、学习，几乎都离不开三个动作：加一个小修正、求平均、沿梯度走一步。对向量这都是家常便饭，对旋转和位姿却统统失效：旋转矩阵相加不再是旋转，平均会"缩水"，梯度一步就走出群外。李群就是为此准备的工具：群负责乘法和逆，流形负责局部像平地。

约 25 分钟 前置：1.10 正交矩阵与旋转 · 5.2 旋转的表示 · 5.3 齐次变换与 SE(3) 5 个交互

    1为什么机器人需要它

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

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

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

    2直觉：地球表面与"加不起来"的旋转

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

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

    FIG 1SO(2) 是一个圆：相加会掉进圆里
    拖动圆上两个点 R₁、R₂
    单位圆 = SO(2)。琥珀点是 \(\tfrac12(R_1+R_2)\)：在弦的中点，落在圆内。绿点是把它"贴回"圆上（沿半径方向放大到长度 1）得到的旋转，它恰好是转角的平均 \(\tfrac12(\alpha+\beta)\)（只要两角相差小于 180°）。左下角的青色小方块是单位正方形在平均矩阵作用下的像：随着两点分开，它不断缩小，直至塌成一个点。
    试一试
    • 把两个点拉得很近（相差小于 10°）。琥珀点几乎贴着圆。局部看，圆几乎是平的，这是后面所有"局部加法"的依据。
    • 把两个点放到相对的两端（相差约 180°）。平均矩阵变成了什么？它还是一个旋转吗？
    核心洞见

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

    3动手玩：三个小实验

    实验一：三维旋转不可交换

    平面旋转"先转 \(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)\)），看它们之间的夹角。

    FIG 2两种顺序，两个朝向
    拖动画面旋转视角
    灰色是起点。两个机体朝向的差异用"夹角"（\(R_1\T R_2\) 的转角）衡量。小角度时，夹角约等于 \(a\,b\)（弧度）：差距是二阶小量。小旋转"几乎"可交换，大旋转则差得很远。
    试一试
    • 找一对 \((a, b)\)，让两种顺序的夹角超过 60°。
    • 取 \(a, b\) 都在 5° 到 10° 之间，看夹角是否近似 \(a\,b\)。（\(a=b=8°\) 时 \(ab\approx 1.1°\)。）

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

    实验二：放大看，弯曲的圆就像一条直线

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

    FIG 3放大单位元：圆变成切线
    拖动圆上的点
    青色圆是 SO(2)，紫色虚线是在单位元处的切线（带刻度，每格 0.1 弧度）。琥珀点在圆上（\(\theta\) 的旋转），紫色点是切线上对应的 \((1, \theta)\)。红线是两者的距离：它大约是 \(\theta^2/2\)，所以 \(\theta\) 越小，相对误差 \(g/\theta \approx \theta/2\) 越小。
    试一试
    • 先让 \(\theta\) 约 0.15 弧度，再把视野缩小到 0.3 以内。圆和切线几乎重合。
    • 用"角度 \(\theta\)"滑块找出：相对误差 \(g/\theta\) 刚好超过 5% 时，\(\theta\) 约是多少度？这给"小角度近似"划了一条经验线。

    实验三：沿梯度走一步，会走出群外

    想估计一个旋转 \(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\) 的那个旋转。

    FIG 4朴素梯度 vs. 切线上的梯度
    拖动三个测量点（青色）
    青色点 \(z_i\) 是测量，琥珀点是它们的平均 \(\bar z\)（在圆内）。紫色轨迹：把圆当平面做普通梯度下降，每一步都离开了圆。绿色轨迹：在切线方向走步再贴回，始终在圆上，最后停在 \(\bar z\) 的方向。两条轨迹从同一个起点（圆上 \((1, 0)\) 处的实心小点，转角 0）出发。
    试一试
    • 点"运行"，看紫色迭代的长度 \(\|z\|\) 最终停在小于 1 的地方。
    • 把三个测量点散开到三个方向（互相约 120°），平均 \(\|\bar z\|\) 小于 0.25 后再运行。紫色解缩成了一个几乎是零的点，绿色解却几乎没有方向可言（切线上的梯度也很小）：测量太分散时，"平均姿态"本来就没有定义。

    4再看数学

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

    群

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

    • 封闭：\(a, b \in G \Rightarrow ab \in G\)。
    • 结合：\((ab)c = a(bc)\)。
    • 单位元：存在 \(e\)，使 \(ea = ae = a\)。
    • 逆：每个 \(a\) 有 \(a^{-1}\)，使 \(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）。

    为什么"加起来"不行

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

    $$ \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\) 接近奇异时会失效。

    流形与维数

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

    一个有用的拓扑事实

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

    李群 = 群 + 光滑流形

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

    群形状维数可交换？
    \((\R^n, +)\)平坦空间\(n\)是
    SO(2)圆1是
    \(\SO\)半径 \(\pi\) 的球，表面对径点粘合3否
    SE(2)圆 × 平面（乘法是"拧"在一起的）3否
    \(\SE\)\(\SO \times \R^3\)（乘法是"拧"在一起的）6否

    单位元处的切空间：反对称矩阵

    取一条经过单位元的光滑曲线 \(R(t)\)，\(R(0) = I\)，对恒等式 \(R(t)\T R(t) = I\) 求导：\(\dot R\T R + R\T\dot R = 0\)，代入 \(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\)。

    $$ \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 节给出贴回去的指数映射。

    5机器人中的它：融合多个姿态估计

    一块标定板被 \(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)\)，每次都贴回群上。
    FIG 5三种平均姿态的方法
    拖动画面旋转视角
    青色小点是各个测量的 \(x\) 轴尖端。淡色大坐标系是真值。虚线（细）是逐元素平均的三根轴：它们比真值短，噪声越大越短。实心粗坐标系是流形平均，与真值叠在一起时几乎看不出区别。右边给出各方法的行列式和对真值的角度误差。
    试一试
    • 把噪声 \(\sigma\) 调到 5° 以下：三种方法几乎一样。小噪声时，局部近似足够好。
    • 把 \(\sigma\) 加大到 40° 以上，让逐元素平均的行列式小于 0.3：它已经严重变形（甚至行列式变负），而流形平均仍是一个合法的旋转。
    真实系统里

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

    6常见误区

    误区 1：对旋转矩阵 / 位姿矩阵逐元素加、减、平均

    \(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)\|\)。）

    误区 2：对欧拉角或轴角向量做加法和平均

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

    误区 3：以为"小角度近似"永远成立

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

    误区 4：一投影就万事大吉

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

    7小练习

    1. 为什么两个旋转矩阵相加通常不是旋转矩阵？
    矩阵加法当然有定义，但 \(\SO\) 是一个弯曲的集合，两个点的和（弦）不在集合里。例如 \(I + (-I) = 0\)。选项 C 也是错的：三维旋转一般不可交换。
    2. SO(2) 中 \(R(30°)\) 和 \(R(90°)\) 的逐元素平均 \(\frac12(R_1+R_2)\) 的行列式是？
    平均矩阵 = \(\cos\frac{\alpha-\beta}{2}\,R(\frac{\alpha+\beta}{2})\)，\(\alpha-\beta = 60°\)，所以缩放因子是 \(\cos30°\)，行列式是它的平方 \(0.75\)。\(0.5 = \cos 60°\) 把半角忘了。
    3. \(\SO\) 的维数是 3，因为？
    选项 A 是定义性的算法；选项 C 虽然数字对（4 − 1 = 3），但它解释的是"单位四元数集合 \(S^3\) 的维数"，不是 \(R\T R = I\) 的计数，而且 \(S^3\) 只是 \(\SO\) 的双覆盖。
    4. 曲线 \(R(t)\) 过单位元，\(R(0) = I\)。\(\dot R(0)\) 是什么矩阵？
    对 \(R\T R = I\) 求导并令 \(t = 0\)，得 \(\dot R(0)\T + \dot R(0) = 0\)，即反对称。这个反对称矩阵就是 \(\skew{\boldsymbol\omega}\)，\(\boldsymbol\omega\) 是角速度。
    5. 在流形上做梯度下降，"投影到切空间再贴回"的主要好处是？
    贴回群上保证了约束 \(R\T R = I\) 始终成立，目标函数始终有明确的几何意义。速度不一定更快，梯度仍然需要。

    8延伸阅读

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