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

插值与流形上的不确定性

有了指数和对数映射，在流形上就能像在向量空间里一样做三件事：沿最短路径插值，对一堆旋转求平均，以及用高斯分布描述位姿的不确定性。这一节把三件事都做一遍，并看清为什么里程计走一段路以后，位置的不确定性是一根弯香蕉，而不是椭圆。

约 25 分钟前置：4.4 高斯分布 · 5.6 指数映射 · 5.7 扰动 · 5.8 伴随 4 个交互

1为什么机器人需要它

机器人要做的三件事，在向量空间里都很简单，换到姿态和位姿上就都出问题：

插值。机械臂在两个路点之间要走一条平滑的路径；相机的曝光时刻落在两个 IMU 位姿之间，要插出此刻的位姿；两帧视觉里程计之间要补出中间姿态。向量里用 $(1-s)\mathbf a + s\mathbf b$，旋转矩阵这样插，中间结果就不是旋转了（5.6 节）；对欧拉角逐个插，路线会绕远，还会撞上万向节锁。
求平均。几个传感器给出了同一个姿态的几个估计；一个粒子滤波器里的粒子云要给出一个"最可能的姿态"。向量里是算术平均，旋转矩阵的算术平均不是旋转，欧拉角在 $\pm180^\circ$ 附近平均会得出完全错误的答案。
不确定性。卡尔曼滤波要为姿态维护一个协方差，告诉后端"我对自己的朝向有多大把握"。可是姿态不在向量空间里，"协方差"的 3×3 矩阵究竟是定义在哪个空间上的，需要有一个说法。

这三件事共用一个思路：把问题搬到切空间（李代数）里做，做完再用 $\Exp$ 回到群上。$\Log$ 把"两个姿态之间的差"变成向量，$\Exp$ 把向量变回姿态。这一节分别看三种用法。

2直觉：走最短的路，匀速

在地球仪上从北京飞往纽约，最短的航线是大圆弧，不是地图上看起来"直"的那条线。旋转群也是一样的：两个姿态 $R_0, R_1$ 之间最短的路，叫测地线。它有一个非常简单的写法。先算出从 $R_0$ 看过去的相对旋转 $R_0\T R_1$，取对数得到一个旋转向量 $\mathbf w = \Log(R_0\T R_1)$，再沿它匀速走：

$$ R(s) = R_0\,\Exp\big(s\,\Log(R_0\T R_1)\big), \qquad s\in[0,1]. $$

转轴始终不变，每一刻转过的角度和 $s$ 成正比，所以角速度恒定。这就是球面线性插值 SLERP；用四元数写出来的公式和它是同一条曲线（见"再看数学"）。常见的两个替代做法都有毛病：欧拉角逐个线性插值，路线与欧拉角的取值范围有关，可能绕一大圈；四元数线性插值再归一化 (NLERP) 路径对，但角速度不匀。下面的实验把三种做法放在一起：球面上的三条线是机体 x 轴尖走过的路径，右图是每种做法在 $s$ 处的角速度（以测地线的匀速为 1）。

FIG 1旋转插值：测地线 vs. 欧拉角 vs. NLERP

拖动画面旋转视角

淡色的两个坐标系是起点 $R_0$ 和终点 $R_1$。黄色是测地线，当前时刻的坐标系用粗的红绿蓝三轴画出；青色是欧拉角逐个线性插值（直接对滑块里的偏航、俯仰、横滚数值插值）；紫色是 NLERP。球面上的曲线是各方法下 x 轴尖的轨迹。右图横轴是 $s$，纵轴是瞬时角速度除以测地线的恒定角速度。

试一试

把起点偏航设成 $+150^\circ$，终点偏航设成 $-150^\circ$（短的一边只差 $60^\circ$）。欧拉角插值"扫过"的总角度是测地线的几倍？它走的是哪一边？
让起点和终点相差很大（大致超过 $100^\circ$），直到 NLERP 的角速度在 $s$ 的范围内变化超过 25%（右图紫线明显弯曲）。它在哪儿最慢？

3动手玩：位姿插值，螺旋 vs. 平移旋转分开

对位姿 $T\in\SE$，同样的思路是 $T(s) = T_0\,\Exp\big(s\,\Log(T_0^{-1}T_1)\big)$：沿一个恒定的 twist 走，也就是一次螺旋运动（5.6 节的 Chasles 定理）。另一种更常见的做法是分开插值：旋转用 SLERP，位置用直线 $(1-s)\mathbf t_0 + s\mathbf t_1$。后者看起来更"直观"：机体原点走直线。

两者的区别在于：你关心的点不一定是机体原点。比如机械臂末端装了一根长 $b$ 的工具，真正在乎的是工具尖。下面左边是螺旋插值，右边是分开插值，各画出机体原点（细线）和工具尖（粗线）的路径，读数里是工具尖的速度范围。

FIG 2位姿插值：螺旋 vs. 分开插值

螺旋：T₀ Exp(s Log(T₀⁻¹T₁))

分开：SLERP + 直线

灰色是起点 $T_0$ 和终点 $T_1$（带盒子和坐标轴）；彩色盒子是当前的 $T(s)$，从机体原点伸出的线是长 $b$ 的工具（沿机体 z 轴）。青色细线：机体原点的路径；黄色粗线：工具尖的路径。两边共用一个相机。

试一试

把工具长度 $b$ 调到 0.8 以上。右边（分开插值）的工具尖速度最大 / 最小之比明显大于 1，左边（螺旋）却始终是 1.00。
让终点的姿态和起点相同（旋转相差 < $2^\circ$），位置相距 1 以上。这时螺旋插值退化成什么？

规律是：螺旋插值是双向不变的。换一个世界系，或者换一个机体系上的参考点（比如把工具装在法兰盘的不同位置），插值出来的运动是同一个刚体运动，每个机体上的点都以恒定的速率走过一段螺旋线。分开插值只对世界系的选择不变；一旦你换了"哪个点是机体原点"，它的轨迹就变了，只有原点那个点保证走直线、匀速。

4求平均与切空间里的高斯

向量 $\mathbf x_1,\dots,\mathbf x_N$ 的平均值，是使 $\sum_i\|\mathbf x_i-\boldsymbol\mu\|^2$ 最小的 $\boldsymbol\mu$。把"距离"换成测地线距离，就得到旋转的平均：Fréchet 均值（也叫 Karcher 均值）

$$ \hl{karcher}{\bar R} = \arg\min_{R\in\SO}\sum_{i=1}^{N}\big\|\Log(R\T R_i)\big\|^2. $$

它没有闭式解，但有一个很自然的迭代：站在当前的猜测 $\mu$，把每个 $R_i$ 用 $\Log$ 搬到 $\mu$ 的切空间里，在那里取向量的算术平均，沿着这个平均方向走一步，如此反复。每一步都用 5.7 节的"切空间里求增量，回缩"：

$$ \boldsymbol\delta = \frac1N\sum_{i=1}^{N}\Log\big(\mu\T R_i\big), \qquad \mu\leftarrow \mu\,\Exp(\boldsymbol\delta). $$

FIG 3 里有 $N$ 个绕中心旋转 $R_c$ 随机抖动的姿态：$R_i = R_c\,\Exp(\sigma\,\mathbf n_i)$，$\mathbf n_i\sim\mathcal N(0,I)$。比较三种"平均"：Karcher 均值（上面的迭代）、对欧拉角取算术平均（偏航、俯仰、横滚各自平均）、以及对矩阵取算术平均后投影回 $\SO$（SVD，也叫弦距均值）。球面上的小点是各个样本的 x 轴尖（红）和 z 轴尖（蓝）。

FIG 3旋转的平均：Karcher、欧拉角、矩阵平均再投影

拖动画面旋转视角

粗的红绿蓝实线坐标系：Karcher 均值；青色虚线箭头：欧拉角平均；紫色点：矩阵平均再投影。读数里的"距离"是各均值与 Karcher 均值之间的测地线角度，"代价"是 $\frac1N\sum\|\Log(R\T R_i)\|^2$，Karcher 均值使它最小。把中心偏航设到接近 $\pm180^\circ$ 再看欧拉角平均。

试一试

让中心偏航的绝对值超过 $160^\circ$，扩散 $\sigma > 0.2$，直到欧拉角平均偏离 Karcher 均值超过 $30^\circ$。样本明明很集中，为什么平均会错？
把扩散 $\sigma$ 拉大，直到矩阵平均再投影与 Karcher 均值差超过 $1^\circ$。

流形上的高斯

有了均值，不确定性的写法就顺理成章：在均值处的切空间里放一个普通的高斯，再用 $\Exp$ 搬到群上。

$$ T = \bar T\,\Exp(\hl{lie}{\boldsymbol\varepsilon}), \qquad \boldsymbol\varepsilon\sim\mathcal N(\mathbf 0,\Sigma),\quad \Sigma\in\R^{6\times6}\ \ (\R^{3\times3}\text{ on }\SO). $$

协方差 $\Sigma$ 的每个元素有清楚的物理意义：它描述的是"机体系里的小转动和小平移"的不确定性（右扰动，5.7 节）。对 $\SO$，$\Sigma$ 的对角线是绕机体三轴的方差，单位 rad²。因为 $\Sigma$ 活在切空间里（一个真正的向量空间），4.4 节里高斯的所有性质都可以直接用，而不必担心角度回绕。

FIG 4 里的位置分布会告诉你，这样做和"直接对 $(x,y,\theta)$ 取高斯"有什么区别。

5再看数学

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

测地线与 SLERP

$$ d(R_0,R_1) = \|\Log(R_0\T R_1)\|\in[0,\pi], $$ $$ \hl{geo}{R(s) = R_0\,\Exp\big(s\,\Log(R_0\T R_1)\big)} = \Exp\big(s\,\Log(R_1R_0\T)\big)R_0. $$

用单位四元数 $q_0,q_1$（先取符号使 $q_0\cdot q_1\ge0$，即走短的一边），记 $\cos\Omega = q_0\cdot q_1$（$\Omega$ 是转角的一半）：

$$ q(s) = \frac{\sin((1-s)\Omega)}{\sin\Omega}\,q_0 + \frac{\sin(s\Omega)}{\sin\Omega}\,q_1 \quad(\text{SLERP}), $$ $$ \hl{nlerp}{q(s) = \frac{(1-s)q_0 + s\,q_1}{\|(1-s)q_0 + s\,q_1\|}}\quad(\text{NLERP}). $$

SLERP 的四元数曲线与 $R_0\Exp(s\Log(R_0\T R_1))$ 完全一致（FIG 1 的读数给出两者之差，约 $10^{-16}$）。NLERP 的路径相同，因为都在 $q_0,q_1$ 张成的大圆上，但角速度不匀：中间快、两头慢，夹角越大越明显。

位姿插值的不变性

令 $T(s) = T_0\Exp(s\,\Log(T_0^{-1}T_1))$。对任意 $G,B\in\SE$：左乘 $G$（换世界系）得到 $GT(s)$，右乘 $B$（换机体系参考点）得到 $T(s)B$，因为 $\Exp(s\Log(B^{-1}XB)) = B^{-1}\Exp(s\Log X)B$。由于机体 twist $\boldsymbol\xi_b = \Log(T_0^{-1}T_1)$ 不变，机体上每个点的速度 $\mathbf v_b + \boldsymbol\omega_b\times\mathbf b$ 都是常数。分开插值满足左不变，不满足右不变。

均值的条件与性质

Karcher 均值满足一阶最优性条件 $\sum_i\Log(\bar R\T R_i) = \mathbf 0$：所有样本在均值处的切空间里"合力为零"。如果所有样本落在以某点为中心、半径小于 $\pi/2$（$90^\circ$）的测地球里，均值唯一，上面的迭代收敛。矩阵平均再投影 $\bar R_{\text{chord}} = U\operatorname{diag}(1,1,\det UV\T)V\T$（$U\Sigma V\T$ 是 $\tfrac1N\sum R_i$ 的 SVD，1.12 节）是 $\sum\|R_i - R\|_F^2$ 的最小化者，样本集中时与 Karcher 均值很接近。

不确定性的传播

设 $T = \bar T\Exp(\boldsymbol\varepsilon)$，$\boldsymbol\varepsilon\sim\mathcal N(0,\Sigma)$（右扰动）。由 5.8 节的 $T\Exp(\boldsymbol\delta) = \Exp(\operatorname{Ad}_T\boldsymbol\delta)T$，取一阶近似：

运算	均值	协方差（右扰动）
换成左扰动	$\bar T$	$\Sigma_l = \operatorname{Ad}_{\bar T}\,\Sigma\,\operatorname{Ad}_{\bar T}\T$
求逆 $T^{-1}$	$\bar T^{-1}$	$\operatorname{Ad}_{\bar T}\,\Sigma\,\operatorname{Ad}_{\bar T}\T$
复合 $T_1T_2$（独立）	$\bar T_1\bar T_2$	$\operatorname{Ad}_{\bar T_2^{-1}}\,\Sigma_1\operatorname{Ad}_{\bar T_2^{-1}}\T + \Sigma_2$
作用在点上 $T\mathbf p$	$\bar T\mathbf p$	$J\,\Sigma\,J\T,\ J = \begin{bmatrix}-\bar R\skew{\mathbf p} & \bar R\end{bmatrix}$

复合那一行的来源：$T_1\Exp(\boldsymbol\varepsilon_1)\,T_2\Exp(\boldsymbol\varepsilon_2) = T_1T_2\,\Exp(\operatorname{Ad}_{T_2^{-1}}\boldsymbol\varepsilon_1)\Exp(\boldsymbol\varepsilon_2)\approx \bar T\,\Exp(\operatorname{Ad}_{T_2^{-1}}\boldsymbol\varepsilon_1+\boldsymbol\varepsilon_2)$。这就是 4.5 节"不确定性传播"在流形上的版本；一阶线性化有效，要求 $\Sigma$ 不太大。里程计每走一步，就做一次复合。

6机器人中的它：里程计的"香蕉"

一台差速轮式机器人只靠轮速和陀螺仪积分（航位推算 dead reckoning），每一步的转角和前进距离都带一点噪声。走得越远，位置的不确定性越大，而且形状很有特点：沿着路径方向不确定性小，垂直于路径方向的不确定性大得多，并且随路径弯曲，像一根香蕉。原因是，转角的小误差累积下来，会让后面每一步都朝偏一点的方向走，位置误差因此是转角误差的 $\sin/\cos$ 函数，不是线性的。

FIG 4 蒙特卡洛地走 300 次，每次做 $K$ 步 $T\leftarrow T\,\Exp\big((\omega\,dt+\text{噪声},\ v\,dt+\text{噪声},\ 0)\big)$，青色点是走到终点的位置。然后拟合两个高斯：欧氏高斯直接对位置 $(x,y)$ 算均值和协方差（紫色椭圆是它的 $2\sigma$ 轮廓）；切空间高斯先求 Karcher 均值 $\bar T$，再把 $\Log(\bar T^{-1}T_i)$ 的协方差 $\Sigma$ 拟合出来，然后从 $\bar T\,\Exp(\boldsymbol\varepsilon)$ 采样。用"各个拟合样本到最近的真实样本的平均距离"衡量拟合得好不好。

FIG 4航位推算的位置分布：香蕉与高斯拟合

青色点：蒙特卡洛的真实终点位置；黄色曲线：无噪声的名义路径；紫色点和椭圆：欧氏高斯拟合的样本和 $2\sigma$ 轮廓；绿色点：切空间高斯 $\bar T\Exp(\boldsymbol\varepsilon)$ 的样本。选"两者都不显示"可以单独看真实分布。

试一试

加长路径（$K \ge 40$），航向噪声 $\sigma_\omega\ge0.2$。看欧氏椭圆：它包进了哪些根本没有真实样本的区域？切空间高斯的距离指标为什么更小？
把路径缩短到 $K \le 8$。两种拟合的差别变成什么样？这说明高斯近似在什么条件下才安全？

真实系统里

EKF 与位姿图 SLAM 都在切空间里维护协方差：每一步按"复合"一行传播，观测更新再把 $\boldsymbol\delta$ 回缩。里程计的协方差（比如 ROS 的 nav_msgs/Odometry 里的 6×6 位姿协方差）是同一类局部协方差；使用时要确认它是机体系（右）还是世界系（左）扰动，以及 $(x,y,z,\text{roll},\text{pitch},\text{yaw})$ 的排列，不要直接拿来当作欧氏坐标上的高斯。轨迹插值（相机与 IMU 的时间对齐，滚动快门校正，连续时间 SLAM）用的就是 FIG 1 和 FIG 2 的 SLERP 和螺旋插值。多传感器融合和粒子滤波里的"平均姿态"就是 FIG 3 的 Karcher 均值。

7常见误区

误区 1：四元数 q 和 −q 是同一个旋转

做 SLERP 或 NLERP 之前，要先检查 $q_0\cdot q_1$。如果是负的，就把 $q_1$ 换成 $-q_1$，否则插值会走长的那一边（绕过大于 $180^\circ$ 的弯）。这是自制轨迹插值器里最常见的 bug，症状是"相机偶尔在两帧之间突然猛转一圈"。同理，$\Log$ 的主值只给最短路径，对角度序列要先解缠绕。

误区 2：对欧拉角、四元数分量或矩阵元素直接取平均

欧拉角在 $\pm180^\circ$ 处回绕（FIG 3）；四元数分量平均时要先统一符号；矩阵平均要再投影回 $\SO$。三者在样本集中时勉强可用，不集中时结果会离 Karcher 均值越来越远。要一个"最小化角度距离"的答案，就用 Karcher 均值。

误区 3：把位姿协方差当成欧氏坐标的协方差

切空间里的 $\Sigma$ 描述的是机体系里的小扰动 $\boldsymbol\varepsilon$，不是世界系坐标 $(x,y,\theta)$ 的协方差，两者的分量含义不同，不能直接互换。而且只要航向的散布一大，世界系 $(x,y)$ 上的任何高斯都会出问题：不论是像 FIG 4 那样直接对位置拟合，还是把 $\Sigma$ 经过 $\Exp$ 的雅可比线性化过去，得到的都是一个椭圆，都会"包进空白区域"、丢掉香蕉的弯曲。需要保留弯曲时，就留在切空间形式 $\bar T\Exp(\boldsymbol\varepsilon)$ 里，或者经过 $\Exp$ 采样（sigma points、蒙特卡洛）；线性化只在散布小时才可靠。

误区 4：左右协方差混用，twist 顺序不一致

$\Sigma_l = \operatorname{Ad}_{\bar T}\Sigma_r\operatorname{Ad}_{\bar T}\T$。$\bar T$ 离单位元越远（转得越多、平移越大），两者差得越多。把一个库输出的 6×6 协方差喂给另一个库之前，要同时确认：左还是右，以及 $(\boldsymbol\omega,\mathbf v)$ 还是 $(\mathbf v,\boldsymbol\omega)$（5.6 节误区 5）。

8小练习

1. 两个姿态的偏航分别为 $170^\circ$ 和 $-170^\circ$（俯仰、横滚均为 0）。测地线插值的中点，偏航是多少？

相对旋转 $R_0\T R_1$ 是绕 z 转 $-170^\circ-170^\circ = -340^\circ$，等价于 $+20^\circ$，主值的 $\Log$ 给出 $+20^\circ$，一半是 $+10^\circ$，所以中点偏航 $=170^\circ+10^\circ=180^\circ$。直接对偏航数值取平均得到 $0^\circ$，会走相反的长路。

2. 对于 $\SO$ 上的高斯 $R=\bar R\,\Exp(\boldsymbol\varepsilon)$，右扰动协方差为 $\Sigma_r = \operatorname{diag}(0.01,\,0.04,\,0.09)$。若 $\bar R$ 是绕 z 轴转 $90^\circ$，左扰动协方差 $\Sigma_l = \bar R\,\Sigma_r\bar R\T$ 是？

绕 z 转 $90^\circ$ 把 $\hat x\to\hat y$，$\hat y\to-\hat x$。机体 x 轴的方差 0.01 在世界里落到 y 轴上，机体 y 轴的方差 0.04 落到 x 轴上，z 不变。所以 $\Sigma_l=\operatorname{diag}(0.04,0.01,0.09)$。

3. 螺旋插值 $T_0\Exp(s\Log(T_0^{-1}T_1))$ 与"SLERP + 位置线性插值"相比，前者独有的性质是？

螺旋插值的机体 twist 恒定，所以每个机体点的速度恒定；它对左、右乘都不变。两种做法的旋转部分其实相同（都是 SLERP）；直线属于分开插值，不是螺旋插值的性质。

4. 里程计每一步的转角都有小噪声，走很远以后，位置分布呈"香蕉形"，主要原因是？

即使每一步的噪声都是严格的高斯，位置 $=\sum$（步长 × $(\cos\theta_k,\sin\theta_k)$），$\theta_k$ 是累积的航向。把高斯的 $\theta$ 送过 $\sin/\cos$，分布就弯了。切空间高斯 $\bar T\Exp(\boldsymbol\varepsilon)$ 再经过 $\Exp$，保留了这种弯曲。

5. Karcher 均值迭代 $\mu\leftarrow\mu\Exp(\boldsymbol\delta)$ 中，$\boldsymbol\delta$ 是什么？

先把每个样本相对 $\mu$ 取 $\Log$（搬到 $\mu$ 的切空间），再对向量取平均。选项 A 对矩阵平均后才取对数，B 是在单位元的切空间里平均（等于旋转向量平均，会遇到回绕和大角度误差）。

9延伸阅读

K. Shoemake, "Animating rotation with quaternion curves", SIGGRAPH 1985 — SLERP 的出处
J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537 — 流形上的高斯、协方差传播、插值与均值（平移在前）
T. Barfoot, State Estimation for Robotics, Ch. 7 — SE(3) 上的不确定性，位姿复合与"香蕉分布"
K. Lynch & F. Park, Modern Robotics, Sec. 9.2 — 螺旋运动轨迹（时间标度的 Exp(s Log) 插值）
M. Moakher, "Means and averaging in the group of rotations", SIAM J. Matrix Anal. Appl., 2002 — 旋转群上的 Fréchet 均值与弦距均值

运算	均值	协方差（右扰动）
换成左扰动	\(\bar T\)	\(\Sigma_l = \operatorname{Ad}_{\bar T}\,\Sigma\,\operatorname{Ad}_{\bar T}\T\)
求逆 \(T^{-1}\)	\(\bar T^{-1}\)	\(\operatorname{Ad}_{\bar T}\,\Sigma\,\operatorname{Ad}_{\bar T}\T\)
复合 \(T_1T_2\)（独立）	\(\bar T_1\bar T_2\)	\(\operatorname{Ad}_{\bar T_2^{-1}}\,\Sigma_1\operatorname{Ad}_{\bar T_2^{-1}}\T + \Sigma_2\)
作用在点上 \(T\mathbf p\)	\(\bar T\mathbf p\)	\(J\,\Sigma\,J\T,\ J = \begin{bmatrix}-\bar R\skew{\mathbf p} & \bar R\end{bmatrix}\)