Part I · 第 6 章优化 · 6.10

流形上的优化

旋转矩阵和位姿不是向量：把梯度步长直接加上去，就会离开 $\SO$ 和 $\SE$。流形上的优化只有一个想法：在当前点的切空间里（那里是普通向量空间）迈一步，再用指数映射把结果"放回"流形。它让位姿图、旋转平均、扫描配准在不用欧拉角、不用约束的情况下，都能用上 Gauss-Newton 和梯度下降。

约 30 分钟前置：5.6 指数映射与对数映射 · 5.7 扰动与流形上的求导 · 6.3 梯度下降 · 6.6 Gauss-Newton 4 个交互

1为什么机器人需要它

机器人估计的东西，一大半不住在 $\R^n$ 里：相机姿态是 $\SO$，刚体位姿是 $\SE$，四元数是单位球面 $S^3$，法向量是 $S^2$。6.3、6.6 节的优化器都假定 $\x\leftarrow\x+\boldsymbol\delta$ 有意义。对旋转矩阵 $R$，$R+\Delta$ 通常不再是旋转矩阵（5.4 节）；对单位向量，$\x+\boldsymbol\delta$ 的长度不再是 1。

常见的"土办法"有三种：用欧拉角当变量（遇到万向节锁就失效）；把 $R$ 的 9 个元素当自由变量再加正交约束（变量多、约束难）；每步之后用 SVD 把矩阵"拉回"最近的旋转（能用，但丢掉了几何结构，也不知道该怎么求导）。流形优化给出第四种、也是最干净的办法：只在切空间里线性化和更新，再用 $\Exp$ 回到流形。三个自由度的旋转就用三个数的增量，没有冗余，没有奇异，也没有约束。

这一招在机器人里无处不在：SLAM 里每个位姿的更新、视觉惯性里程计（VIO）里每个姿态状态的更新、扫描匹配 ICP 里每次迭代的刚体变换、旋转平均与外参标定，背后都是它。

2直觉：在圆上取平均

最小的"弯曲空间"是圆 $S^1$，也就是平面旋转 $\mathrm{SO}(2)$。设圆上有几个角度 $\theta_1,\dots,\theta_n$，求它们的"平均"。不能直接用算术平均（想想 $359^\circ$ 与 $1^\circ$），要把"平均"理解成最小化到各点的距离平方和 $\sum_i\tfrac12 d(\theta,\theta_i)^2$，这里 $d$ 是沿圆弧的测地距离。

流形优化的一次迭代分三步：①在当前估计 $\theta$ 处画出切线（切空间）；②用对数映射把每个数据点"展开"到切线上，得到一个普通的实数 $s_i=\operatorname{Log}_\theta(\theta_i)$（沿圆弧走过的有向弧长）；③在切线上做普通平均 $\bar s=\tfrac1n\sum s_i$，再用指数映射沿圆弧走 $\bar s$，回到圆上。它就是梯度下降（$-\nabla f=\sum s_i$），只是"加法"换成了"沿测地线走"。

对比另一种做法：先把各点当作平面向量求平均（圆内的质心），再把结果投影回圆上。两者回答的是两个不同的问题（第二个最小化弦长，而不是弧长），一般给出不同的答案。

FIG 1圆上的平均：切线、对数、指数

紫色点是数据 $\theta_i$，橙色点是当前估计 $\theta$，三者都可拖动。青色直线是 $\theta$ 处的切线，紫色小点是各数据点的 $\operatorname{Log}_\theta$（沿圆弧的有向弧长，被"展开"到切线上）。橙色箭头是它们的平均；沿圆弧走过去（Exp）得到橙色实心点，沿切线走再投影（归一化）得到空心点。灰色 $\times$ 是平面质心，绿色是它投影回圆的"弦平均"。按"迭代一步"看估计如何走向测地平均（在圆上 $\operatorname{Log}$ 保持弧长，所以只要没有数据点越过 $\theta$ 的对点，一步就正好落在测地平均上）。注意圆上的测地代价可以有好几个局部极小，走到哪个取决于估计的起点。

按"迭代一步"：一般一步就落到测地平均上（若有数据点越过对点，则要多走几步）；再按一次，确认箭头已缩到零、估计不再移动。
拖动数据点，让"测地平均"和"弦平均"相差超过 $8^\circ$。数据点越不均匀，差别越大。
把两个数据点拖到几乎相对的位置（相差接近 $180^\circ$），再拖动估计：测地平均变得"不稳定"，这是流形上平均的固有限制。

核心洞见

切空间是流形在一点附近的"平直近似"，在这里加法、平均、线性方程组都可以放心使用。流形上的优化 = 在切空间里算，用 Exp 回到流形。这个"Exp"叫作回缩 (retraction)：它不必是严格的测地线，只要一阶上与测地线一致、且结果落在流形上即可。归一化（FIG 1 的空心点）也是合法的回缩。

3黎曼梯度：把梯度投到切面上

升到二维曲面：单位球面 $S^2=\{\x:\|\x\|=1\}$。在球面上最小化 $f(\x)=\tfrac12\x^\top A\x$（$A$ 是对称正定矩阵）。答案是 $A$ 的最小特征向量（1.11 节），也是 PCA 里"最不重要"的方向。

函数在 $\R^3$ 里的梯度 $\nabla f=A\x$ 一般不在切平面内：它的一部分指向球外或球内，这部分只会让你离开球面，对"沿球面怎样下降最快"毫无贡献。留下来的才是有用的方向：切平面上的投影 $\operatorname{grad}f=(I-\x\x^\top)\nabla f$，叫黎曼梯度。沿 $-\operatorname{grad}f$ 迈一步（切平面内的向量），再用 $\Exp$（沿大圆走）或归一化回到球面。

FIG 2球面上的黎曼梯度下降

拖动画面旋转视角

灰色细线是 $A$ 的三条特征轴（特征值 3、1.2、0.3：最小的是极小点，最大的是极大点，中间那个是鞍点）。橙色点是当前 $\x$；橙色箭头是欧氏梯度 $A\x$，紫色箭头是它在切平面上的投影，即黎曼梯度；半透明圆片是切平面。红点是"不管约束，直接 $\x-\eta\nabla f$"落到的位置，它掉出了球面；蓝点是回缩之后的新迭代点。

按"运行"，看迭代点沿球面滑向最小特征向量。
按"从鞍点出发"再运行：黎曼梯度在鞍点处为零，迭代点要先"犹豫"一阵（起点故意偏离鞍点 $10^{-3}$，这点偏差每步放大，直到把它推开），然后才滑向最小点。

4旋转平均：Lie 代数增量对欧拉角

现在换成机器人真正关心的 $\SO$。旋转平均：对一组带噪声的旋转 $R_1,\dots,R_n$（多个陀螺仪、多次手眼标定、多路姿态估计的输出），求使测地距离平方和最小的 $R$：

$$ f(R)=\tfrac12\sum_{i=1}^n \big\|\Log(R^\top R_i)\big\|^2 $$

$\Log(R^\top R_i)$ 正是"从 $R$ 转到 $R_i$ 的旋转向量"，也就是 FIG 1 里的 $s_i$ 在 3D 里的版本。代价对 $R$ 的黎曼梯度恰好是 $-\sum_i\Log(R^\top R_i)$（$\SO$ 上的度量是双不变的，所以没有额外系数）。于是整个迭代就是：

$$ \hl{lie}{\boldsymbol\omega} = \frac{\eta}{n}\sum_i \Log(R_k^\top R_i), \qquad R_{k+1}=R_k\,\hl{exp}{\Exp(\boldsymbol\omega)} $$

对比欧拉角：把 $R=R_z(\psi)R_y(\vartheta)R_x(\phi)$ 的三个角当作变量，对 $f$ 做普通梯度下降。它三个变量、没有约束，看上去很完美，缺点是参数化本身有奇点：当俯仰角 $\vartheta\to\pm90^\circ$（万向节锁），偏航和滚转绕同一根轴转，参数到旋转的映射的雅可比降秩，于是代价对其中一个方向几乎"没有梯度"，下降极慢甚至停滞。下面两栏用同一组数据、同一个起点，左边用欧拉角，右边用 Lie 代数增量。数据的真值俯仰角默认是 $86^\circ$，接近万向节锁。

FIG 3旋转平均：欧拉角 vs. 流形

欧拉角 + 梯度下降

流形：R ← R·Exp(ω)

单位球面上的小点是 5 个数据旋转的三个坐标轴 $R_i\hat{\imath},R_i\hat{\jmath},R_i\hat{k}$ 的端点（红、绿、蓝）；粗箭头是当前估计的坐标系；金色轨迹是 $\hat k$ 轴端点走过的路。下图：两种方法的代价与最优代价之差（对数坐标）。欧拉角一侧用中心差分求梯度、固定步长 $\gamma$；流形一侧用 $\eta=1$，即直接取对数的平均。

保持默认的真值俯仰角 $86^\circ$，按"运行"：欧拉角一侧停在一个平台上，流形一侧几步就收敛。
把真值俯仰角拖到 $0^\circ$（远离万向节锁）再运行：欧拉角一侧也能收敛，只是更慢。奇点才是问题所在。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮（增量 $\boldsymbol\omega$：FIG 1 的"平均 Log"箭头；指数回缩：FIG 3 的估计坐标系；欧氏梯度与黎曼梯度：FIG 2 的两支箭头）。

流形、切空间与回缩

光滑流形 $\mathcal M$ 在每点 $x$ 有一个切空间 $T_x\mathcal M$，它是与 $\mathcal M$ 的维数相同的向量空间。$\SO$ 的维数是 3，$S^2$ 是 2，$\SE$ 是 6。回缩 $\mathcal R_x:T_x\mathcal M\to\mathcal M$ 满足 $\mathcal R_x(\mathbf 0)=x$，并且在 0 处的导数是恒等映射（一阶上与"沿切向走"一致）。李群上最自然的回缩是指数映射（5.6 节）：

$$ R\ \leftarrow\ R\,\Exp(\boldsymbol\omega)\ \ (\text{right/body perturbation}),\qquad T\leftarrow \Exp(\boldsymbol\xi)\,T\ \ (\text{left/world perturbation}),\qquad \x\leftarrow\frac{\x+\mathbf v}{\|\x+\mathbf v\|}\ \ (S^2) $$

用哪一侧的扰动是自由的，但求导和更新必须用同一侧（5.7 节）。FIG 3 用右扰动，FIG 4 用左扰动。

黎曼梯度

对嵌在 $\R^m$ 中的流形（如球面），黎曼梯度是欧氏梯度在切空间上的正交投影；对李群，则把"扰动后的函数"在 0 处求导：

$$ \hl{riem}{\operatorname{grad}f(\x)}=\big(I-\x\x^\top\big)\hl{euc}{\nabla f(\x)}\ \ (S^2),\qquad \operatorname{grad}f(R)=\frac{\partial f\big(R\,\Exp(\boldsymbol\omega)\big)}{\partial\boldsymbol\omega}\Big|_{\boldsymbol\omega=\mathbf 0}\in\R^3\ \ (\SO) $$

于是梯度下降变成 $R_{k+1}=R_k\Exp(-\eta\operatorname{grad}f)$。Gauss-Newton 也一样：残差对扰动线性化 $\mathbf r(R\Exp(\boldsymbol\omega))\approx\mathbf r+J\boldsymbol\omega$，解 $J^\top J\boldsymbol\omega=-J^\top\mathbf r$ 得到 3 维（$\SE$ 则是 6 维）增量，再回缩。法方程的大小等于流形的维数，而不是矩阵元素的个数：100 个 $\SE$ 位姿的位姿图，固定第一个之后，正好是 $6\times99$ 个未知数。

旋转平均的推导

取 $R$ 附近的一点 $R\Exp(\boldsymbol\omega)$，则 $\Log\big((R\Exp(\boldsymbol\omega))^\top R_i\big)=\Log\big(\Exp(-\boldsymbol\omega)R^\top R_i\big)$。把 $\mathbf e_i=\Log(R^\top R_i)$ 记作误差，一阶近似为 $\mathbf e_i-J_l^{-1}(\mathbf e_i)\boldsymbol\omega$（$J_l$ 是 5.7 节的左雅可比），而 $J_l^{-1}(\mathbf e)^\top\mathbf e=\mathbf e$（因为 $J_l(\mathbf e)\mathbf e=\mathbf e$），所以代价对 $\boldsymbol\omega$ 的梯度是 $-\sum_i\mathbf e_i$，不必显式算雅可比。令它为零，得 Karcher 平均的条件 $\sum_i\Log(R^\top R_i)=\mathbf 0$。

SE(2) 上的刚体对齐

设位姿 $T=(R(\theta),\mathbf t)$，点 $\mathbf p_i$ 变换后 $\mathbf a_i=T\mathbf p_i$，对应目标点 $\mathbf q_i$，残差 $\mathbf r_i=\mathbf a_i-\mathbf q_i$。对左扰动 $\boldsymbol\delta=(\boldsymbol\rho,\varphi)$：$\Exp(\boldsymbol\delta)\,\mathbf a\approx\mathbf a+\boldsymbol\rho+\varphi\,\mathbf a^{\perp}$，其中 $\mathbf a^\perp=(-a_y,a_x)$。所以每个点的雅可比是 $J_i=[\,I_2\ \ \mathbf a_i^\perp\,]$（$2\times3$），解 $\sum_iJ_i^\top J_i\,\boldsymbol\delta=-\sum_iJ_i^\top\mathbf r_i$，再更新 $T\leftarrow\Exp(\boldsymbol\delta)T$，其中

$$ \Exp(\boldsymbol\rho,\varphi)=\Big(R(\varphi),\ V(\varphi)\boldsymbol\rho\Big),\qquad V(\varphi)=\frac1\varphi\begin{bmatrix}\sin\varphi & -(1-\cos\varphi)\\ 1-\cos\varphi & \sin\varphi\end{bmatrix} $$

这就是 FIG 4 里每一步迭代做的事。点云配准（ICP）还要在每步重新找最近邻对应，这让问题只在局部是"最小二乘"，所以对初值敏感。

6机器人中的它：激光扫描匹配

移动机器人每扫一帧激光，都要把新扫描 $\{\mathbf p_i\}$（在机器人坐标系里）对齐到已有的地图 $\{\mathbf q_j\}$，求出机器人在地图里的位姿。这就是 ICP：重复"找最近点对应 → 解一次 $\SE$ 上的 Gauss-Newton → 用 $\Exp$ 更新位姿"。下图的地图是一个带墙角的房间，扫描是同一个房间换了位姿（旋转 $17^\circ$、平移约 1 m）。拖动橙色手柄设定初始位姿猜测（圆点是平移，小圈是朝向）。

FIG 4SE(2) 上的扫描匹配 (ICP)

蓝点是地图，橙点是用当前位姿变换后的扫描，淡线是当前对应。"最近邻"是真正的 ICP；"已知对应"假定我们知道每个扫描点对应哪个地图点（一个更容易的、近似凸的问题）。"位姿误差"是用当前位姿与真实位姿变换同一批点后的均方根距离。每一步的更新都是 $T\leftarrow\Exp(\boldsymbol\delta)T$。

用默认的"最近邻"，从离真值不远的起点出发，按"运行"让扫描贴合地图（位姿误差小于 0.05）。
把初始朝向转到偏离真值 $80^\circ$ 以上再运行：ICP 最后停在一个错误的位姿（陷入局部极小，或越漂越远）。
在同一个糟糕的起点上切到"已知对应"：几步就收敛到正确位姿。问题难在对应，而不是 $\SE$ 本身。

真实系统里

3D 里同样的流程在 $\SE$ 上进行（6 维增量）。VIO 的 IMU 预积分、位姿图里的每个位姿、相机外参标定，也是"在切空间里解法方程，用 $\Exp$ 更新"。成熟库已经把这套封装好了：GTSAM、Sophus + Ceres（用 local parameterization / manifold）、Pinocchio 等都提供流形上的变量类型，让优化器本身不必知道变量是矩阵、四元数还是位姿。

7常见误区

误区 1：求导用左扰动，更新却用右乘

$R\Exp(\boldsymbol\omega)$ 与 $\Exp(\boldsymbol\omega)R$ 相差一个伴随 $\mathrm{Ad}_R$（5.8 节）。雅可比按左扰动推导、更新却写成 $R\leftarrow R\Exp(\boldsymbol\omega)$，程序不会报错，只是迭代变慢、不收敛，或收敛到不是最小值的点。推导时把"扰动在哪一侧"写在纸上，代码里也用同一侧；用数值微分 $\big(f(R\Exp(h\mathbf e_k))-f(R)\big)/h$ 检查解析雅可比。

误区 2：把旋转向量的增量直接"加"到旋转向量上

以 $\boldsymbol\phi=\Log(R)$ 为变量，更新 $\boldsymbol\phi\leftarrow\boldsymbol\phi+\boldsymbol\delta$，看起来无约束且无冗余。问题有两个：一是 $\|\boldsymbol\phi\|$ 在 $\pi$ 处有奇点和卷绕，二是 $\Exp(\boldsymbol\phi+\boldsymbol\delta)\ne\Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)$，增量的物理意义随当前 $\boldsymbol\phi$ 而变（雅可比 $J_l(\boldsymbol\phi)$ 介入）。流形优化的做法是每次迭代都在当前点重新建一个以 0 为中心的局部坐标，所以增量永远很小，永远远离奇点。

误区 3：忽视流形上的"最远点"和多个极小

FIG 1 里，取两个靠得很近的数据点（如 $170^\circ$ 与 $-170^\circ$），把估计放在它们中点的对面（$0^\circ$）：两边的 $\operatorname{Log}$ 恰好抵消，梯度为零，但那里是最大值而不是最小值（见第 3 题）。若两个数据点正好相对，测地平均则不唯一（两个极小点，各距两点 $90^\circ$）。$\SO$ 上若数据散布超过大约 $90^\circ$，Karcher 平均可能不唯一、迭代依赖初值。旋转平均实践中先求一个好的初值（沿生成树链式相乘，或为了鲁棒用谱方法 / 弦距松弛），再做局部流形优化。

误区 4：忘了固定规范，或让 $R$ 慢慢漂离正交

只含相对测量的问题有整体刚体变换的规范自由度（6.9 节）：要么固定一个变量，要么给法方程加一点阻尼。另外，若把 $R$ 存成 3×3 矩阵并反复相乘，浮点误差会让它慢慢偏离正交。每隔若干步做一次重新正交化（SVD 或四元数归一化），否则 $\Log$ 会返回错误的角度。

8小练习

1. 在单位球面上，当前点 $\x=(0,0,1)$，函数的欧氏梯度 $\nabla f=(1,0,2)$。黎曼梯度是？

$(I-\x\x^\top)\nabla f=\nabla f-(\x^\top\nabla f)\x=(1,0,2)-2\cdot(0,0,1)=(1,0,0)$。去掉的 $(0,0,2)$ 是沿球面法向的分量，对沿球面下降没有作用。

2. 用梯度下降优化旋转矩阵 $R$，为什么不能写 $R\leftarrow R-\eta\nabla f$，而要写 $R\leftarrow R\,\Exp(-\eta\,\operatorname{grad}f)$？

$R-\eta\nabla f$ 一般不满足 $R^\top R=I$，离开了流形；$R\Exp(\boldsymbol\omega)$ 永远是旋转，而且 $\boldsymbol\omega$ 只有 3 个分量，没有冗余。

3. 圆上有两个数据角 $170^\circ$ 和 $-170^\circ$。估计 $\theta=0^\circ$ 处两个 $\operatorname{Log}$ 的平均是 0，梯度为零。这个点是？

在 $0^\circ$，代价是 $\tfrac12(170^2+170^2)$（度²），在 $180^\circ$ 则只有 $\tfrac12(10^2+10^2)$。$0^\circ$ 是平稳点但是最大值，所以"梯度为零"并不保证是最小值，流形上的初值很重要。

4. 用欧拉角做变量的梯度下降，在俯仰角接近 $90^\circ$ 时为什么变慢甚至停滞？

这是参数化的奇点（万向节锁），不是旋转本身的问题：在 $\SO$ 上用 $R\Exp(\boldsymbol\omega)$ 的局部坐标，每次都以当前点为原点，永远不会碰到奇点。

5. 一个含 100 个 $\SE$ 位姿的位姿图，固定第 0 号位姿后用 Gauss-Newton 优化。每次迭代的法方程有多少个未知数？

$\SE$ 是 6 维流形，每个位姿的切空间增量有 6 个分量，所以是 $6\times99=594$。若把 $4\times4$ 矩阵的 12（或 16）个元素当变量，就会多出大量冗余变量和约束。

9延伸阅读

N. Boumal, An Introduction to Optimization on Smooth Manifolds, Cambridge University Press, 2023 — 流形优化的现代入门，免费在线
P.-A. Absil, R. Mahony & R. Sepulchre, Optimization Algorithms on Matrix Manifolds, Princeton University Press, 2008 — 回缩、黎曼梯度与 Newton 法的经典专著
J. Solà, J. Deray & D. Atchuthan, A micro Lie theory for state estimation in robotics, arXiv:1812.01537, 2018 — 机器人学里 $\oplus,\ominus$、左右扰动与雅可比的实用手册
T. D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017, Ch. 7–8 — 李群上的扰动与批量非线性估计
R. Hartley, J. Trumpf, Y. Dai & H. Li, Rotation Averaging, International Journal of Computer Vision, 2013 — 旋转平均的算法与理论综述
P. J. Besl & N. D. McKay, A Method for Registration of 3-D Shapes, IEEE TPAMI, 1992 — ICP 原始论文