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

插值与流形上的不确定性Interpolation & Uncertainty on Manifolds

有了指数和对数映射，在流形上就能像在向量空间里一样做三件事：沿最短路径插值，对一堆旋转求平均，以及用高斯分布描述位姿的不确定性。这一节把三件事都做一遍，并看清为什么里程计走一段路以后，位置的不确定性是一根弯香蕉，而不是椭圆。 With the exponential and logarithm maps you can do three things on a manifold that you would do in a vector space: interpolate along the shortest path, average a set of rotations, and describe pose uncertainty with a Gaussian. This lesson does all three, and shows why after some dead reckoning a robot's position uncertainty is a bent banana rather than an ellipse.

约 25 分钟~25 min 前置：4.4 高斯分布 · 5.6 指数映射 · 5.7 扰动 · 5.8 伴随Prereqs: 4.4 Gaussian · 5.6 Exp & Log · 5.7 Perturbations · 5.8 Adjoint 4 个交互4 interactives

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

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

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

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

Three things a robot must do are easy in a vector space and break on orientations and poses:

Interpolation. An arm needs a smooth path between two waypoints; a camera exposure falls between two IMU poses and you need the pose at that instant; visual odometry needs in-between orientations. For vectors you use $(1-s)\mathbf a + s\mathbf b$; interpolate rotation matrices that way and the in-betweens are no longer rotations (Lesson 5.6); interpolate Euler angles one by one and the route detours and runs into gimbal lock.
Averaging. Several sensors give several estimates of one orientation; a particle filter's cloud must be summarized by a "most likely attitude". For vectors you take the arithmetic mean, but the arithmetic mean of rotation matrices is not a rotation, and averaging Euler angles near $\pm180^\circ$ gives a completely wrong answer.
Uncertainty. A Kalman filter keeps a covariance for the attitude, telling the back end "how sure I am about my heading". But attitudes do not live in a vector space, so we have to say which space that 3×3 covariance matrix is defined on.

All three share one idea: do the work in the tangent space (the Lie algebra), then return to the group with $\Exp$. $\Log$ turns "the difference between two orientations" into a vector, and $\Exp$ turns a vector back into an orientation. This lesson looks at the three uses in turn.

2直觉：走最短的路，匀速Intuition: take the shortest path, at constant speed

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

Flying from Beijing to New York on a globe, the shortest route is a great-circle arc, not the line that looks "straight" on a flat map. The rotation group is the same: the shortest path between two orientations $R_0, R_1$ is called a geodesic, and it has a very simple form. Compute the relative rotation seen from $R_0$, $R_0\T R_1$, take its logarithm to get a rotation vector $\mathbf w = \Log(R_0\T R_1)$, and walk along it at constant speed:

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

The axis never changes and the angle turned at each moment is proportional to $s$, so the angular speed is constant. This is spherical linear interpolation, SLERP; the quaternion formula traces the very same curve (see "The math"). The two common alternatives both have flaws: Euler angles interpolated one by one take a route that depends on the range of the angle values and can detour wildly; quaternion lerp plus normalization (NLERP) follows the right path but not at constant speed. The lab below puts the three side by side: the three curves on the sphere are the paths of the body x-axis tip, and the plot on the right shows each method's angular speed at $s$ (geodesic constant speed = 1).

FIG 1旋转插值：测地线 vs. 欧拉角 vs. NLERPRotation interpolation: geodesic vs. Euler angles vs. NLERP

拖动画面旋转视角drag to orbit

淡色的两个坐标系是起点 $R_0$ 和终点 $R_1$。黄色是测地线，当前时刻的坐标系用粗的红绿蓝三轴画出；青色是欧拉角逐个线性插值（直接对滑块里的偏航、俯仰、横滚数值插值）；紫色是 NLERP。球面上的曲线是各方法下 x 轴尖的轨迹。右图横轴是 $s$，纵轴是瞬时角速度除以测地线的恒定角速度。 The two faint frames are the start $R_0$ and the end $R_1$. Amber is the geodesic, with its current frame drawn as the thick red/green/blue axes; cyan is Euler-angle interpolation (linearly interpolating the yaw, pitch and roll numbers in the sliders); purple is NLERP. The curves on the sphere are the paths of the x-axis tip under each method. In the plot the horizontal axis is $s$ and the vertical axis is the instantaneous angular speed divided by the geodesic's constant speed.

试一试Try this

把起点偏航设成 $+150^\circ$，终点偏航设成 $-150^\circ$（短的一边只差 $60^\circ$）。欧拉角插值"扫过"的总角度是测地线的几倍？它走的是哪一边？Set the start yaw to $+150^\circ$ and the end yaw to $-150^\circ$ (the short way is only $60^\circ$ apart). How many times the geodesic's total angle does the Euler interpolation sweep, and which way does it go?
让起点和终点相差很大（大致超过 $100^\circ$），直到 NLERP 的角速度在 $s$ 的范围内变化超过 25%（右图紫线明显弯曲）。它在哪儿最慢？Make start and end differ by a lot (roughly over $100^\circ$) until NLERP's angular speed varies by more than 25% over $s$ (the purple line visibly bends). Where is it slowest?

3动手玩：位姿插值，螺旋 vs. 平移旋转分开Play: pose interpolation, screw vs. decoupled

对位姿 $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$ 的工具，真正在乎的是工具尖。下面左边是螺旋插值，右边是分开插值，各画出机体原点（细线）和工具尖（粗线）的路径，读数里是工具尖的速度范围。

For poses $T\in\SE$ the same idea gives $T(s) = T_0\,\Exp\big(s\,\Log(T_0^{-1}T_1)\big)$: follow a constant twist, i.e. one screw motion (Chasles' theorem from Lesson 5.6). The more common alternative is decoupled interpolation: SLERP for the rotation and a straight line $(1-s)\mathbf t_0 + s\mathbf t_1$ for the position. The latter looks more "natural": the body origin moves along a straight line.

The difference is that the point you care about is not necessarily the body origin. If the arm carries a tool of length $b$, what matters is the tool tip. Below, the left panel is screw interpolation and the right is decoupled; each draws the path of the body origin (thin) and of the tool tip (thick), and the readout gives the tool tip's speed range.

FIG 2位姿插值：螺旋 vs. 分开插值Pose interpolation: screw vs. decoupled

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

分开：SLERP + 直线DECOUPLED · SLERP + straight line

灰色是起点 $T_0$ 和终点 $T_1$（带盒子和坐标轴）；彩色盒子是当前的 $T(s)$，从机体原点伸出的线是长 $b$ 的工具（沿机体 z 轴）。青色细线：机体原点的路径；黄色粗线：工具尖的路径。两边共用一个相机。 Grey shows the start $T_0$ and the end $T_1$ (a box with axes); the colored box is the current $T(s)$, and the line out of the body origin is a tool of length $b$ (along the body z-axis). Thin teal line: the path of the body origin; thick amber line: the path of the tool tip. The two panels share one camera.

试一试Try this

把工具长度 $b$ 调到 0.8 以上。右边（分开插值）的工具尖速度最大 / 最小之比明显大于 1，左边（螺旋）却始终是 1.00。Raise the tool length $b$ above 0.8. On the right (decoupled) the ratio of maximum to minimum tool-tip speed is clearly above 1, while on the left (screw) it stays 1.00.
让终点的姿态和起点相同（旋转相差 < $2^\circ$），位置相距 1 以上。这时螺旋插值退化成什么？Give the end the same orientation as the start (rotation within $2^\circ$) with the positions more than 1 apart. What does screw interpolation degenerate to?

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

The rule: screw interpolation is bi-invariant. Change the world frame, or the reference point on the body (say mount the tool at a different place on the flange), and the interpolated motion is the same rigid motion, with every body point travelling a helix at constant speed. Decoupled interpolation is invariant only to the choice of world frame; change "which point is the body origin" and its trajectory changes, and only the origin itself is guaranteed to move on a straight line at constant speed.

4求平均与切空间里的高斯Averaging, and Gaussians in the tangent space

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

The average of vectors $\mathbf x_1,\dots,\mathbf x_N$ is the $\boldsymbol\mu$ that minimizes $\sum_i\|\mathbf x_i-\boldsymbol\mu\|^2$. Replace "distance" with geodesic distance and you get the average of rotations, the Fréchet mean (also called the Karcher mean):

$$ \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 节的"切空间里求增量，回缩"：

There is no closed form, but there is a very natural iteration: stand at the current guess $\mu$, bring every $R_i$ into $\mu$'s tangent space with $\Log$, take the ordinary arithmetic mean of the vectors there, step along that mean direction, and repeat. Each step is the "solve for an increment in the tangent space, then retract" of Lesson 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 has $N$ orientations jittered around a centre rotation $R_c$: $R_i = R_c\,\Exp(\sigma\,\mathbf n_i)$ with $\mathbf n_i\sim\mathcal N(0,I)$. Three "averages" are compared: the Karcher mean (the iteration above), the arithmetic mean of the Euler angles (yaw, pitch and roll averaged separately), and the arithmetic mean of the matrices projected back onto $\SO$ (via SVD, also called the chordal mean). The small dots on the sphere are the samples' x-axis tips (red) and z-axis tips (blue).

FIG 3旋转的平均：Karcher、欧拉角、矩阵平均再投影Averaging rotations: Karcher, Euler angles, projected matrix mean

拖动画面旋转视角drag to orbit

粗的红绿蓝实线坐标系：Karcher 均值；青色虚线箭头：欧拉角平均；紫色点：矩阵平均再投影。读数里的"距离"是各均值与 Karcher 均值之间的测地线角度，"代价"是 $\frac1N\sum\|\Log(R\T R_i)\|^2$，Karcher 均值使它最小。把中心偏航设到接近 $\pm180^\circ$ 再看欧拉角平均。 Thick solid red/green/blue frame: the Karcher mean; cyan dashed arrows: the Euler-angle average; purple dots: the projected matrix mean. The "distance" in the readout is the geodesic angle between each mean and the Karcher mean, and the "cost" is $\frac1N\sum\|\Log(R\T R_i)\|^2$, which the Karcher mean minimizes. Set the centre yaw close to $\pm180^\circ$ and watch the Euler-angle average.

试一试Try this

让中心偏航的绝对值超过 $160^\circ$，扩散 $\sigma > 0.2$，直到欧拉角平均偏离 Karcher 均值超过 $30^\circ$。样本明明很集中，为什么平均会错？Set the centre yaw above $160^\circ$ in magnitude with spread $\sigma > 0.2$, until the Euler-angle average is more than $30^\circ$ from the Karcher mean. The samples are tightly clustered, so why is the average wrong?
把扩散 $\sigma$ 拉大，直到矩阵平均再投影与 Karcher 均值差超过 $1^\circ$。Increase the spread $\sigma$ until the projected matrix mean differs from the Karcher mean by more than $1^\circ$.

流形上的高斯Gaussians on a manifold

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

With a mean in hand, writing down uncertainty is straightforward: put an ordinary Gaussian in the tangent space at the mean, then carry it to the group with $\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)$ 取高斯"有什么区别。

Every entry of the covariance $\Sigma$ has a clear physical meaning: it describes the uncertainty of "small rotations and translations expressed in the body frame" (a right perturbation, Lesson 5.7). On $\SO$ the diagonal of $\Sigma$ holds the variances about the three body axes, in rad². Because $\Sigma$ lives in the tangent space (a true vector space), every property of the Gaussian from Lesson 4.4 applies directly, with no angle wrapping to worry about.

The position distribution in FIG 4 will show how this differs from "just take a Gaussian on $(x,y,\theta)$".

5再看数学The math

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

测地线与 SLERPGeodesics and 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$ 是转角的一半）：

With unit quaternions $q_0,q_1$ (first pick signs so that $q_0\cdot q_1\ge0$, i.e. take the short way), and $\cos\Omega = q_0\cdot q_1$ ($\Omega$ is half the rotation angle):

$$ 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$ 张成的大圆上，但角速度不匀：中间快、两头慢，夹角越大越明显。

The SLERP quaternion curve coincides with $R_0\Exp(s\Log(R_0\T R_1))$ (the FIG 1 readout gives the difference, about $10^{-16}$). NLERP follows the same path, since both lie on the great circle spanned by $q_0,q_1$, but at non-constant speed: fast in the middle and slow at the ends, the more so the larger the angle.

位姿插值的不变性Invariance of pose interpolation

令 $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$ 都是常数。分开插值满足左不变，不满足右不变。

Let $T(s) = T_0\Exp(s\,\Log(T_0^{-1}T_1))$. For any $G,B\in\SE$: left-multiplying by $G$ (changing the world frame) gives $GT(s)$, and right-multiplying by $B$ (changing the reference point on the body) gives $T(s)B$, because $\Exp(s\Log(B^{-1}XB)) = B^{-1}\Exp(s\Log X)B$. Since the body twist $\boldsymbol\xi_b = \Log(T_0^{-1}T_1)$ is constant, every body point's velocity $\mathbf v_b + \boldsymbol\omega_b\times\mathbf b$ is constant. Decoupled interpolation is left-invariant but not right-invariant.

均值的条件与性质Conditions and properties of the mean

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 均值很接近。

The Karcher mean satisfies the first-order condition $\sum_i\Log(\bar R\T R_i) = \mathbf 0$: in the tangent space at the mean, the samples "balance". If all samples lie in a geodesic ball of radius below $\pi/2$ ($90^\circ$), the mean is unique and the iteration converges. The projected matrix mean $\bar R_{\text{chord}} = U\operatorname{diag}(1,1,\det UV\T)V\T$ ($U\Sigma V\T$ is the SVD of $\tfrac1N\sum R_i$, Lesson 1.12) minimizes $\sum\|R_i - R\|_F^2$ and is close to the Karcher mean when the samples are concentrated.

不确定性的传播Propagating uncertainty

设 $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$，取一阶近似：

Let $T = \bar T\Exp(\boldsymbol\varepsilon)$, $\boldsymbol\varepsilon\sim\mathcal N(0,\Sigma)$ (right perturbation). From $T\Exp(\boldsymbol\delta) = \Exp(\operatorname{Ad}_T\boldsymbol\delta)T$ in Lesson 5.8, to first order:

运算Operation	均值Mean	协方差（右扰动）Covariance (right perturbation)
换成左扰动to left perturbation	$\bar T$	$\Sigma_l = \operatorname{Ad}_{\bar T}\,\Sigma\,\operatorname{Ad}_{\bar T}\T$
求逆 $T^{-1}$inverse $T^{-1}$	$\bar T^{-1}$	$\operatorname{Ad}_{\bar T}\,\Sigma\,\operatorname{Ad}_{\bar T}\T$
复合 $T_1T_2$（独立）composition $T_1T_2$ (independent)	$\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$acting on a point $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$ 不太大。里程计每走一步，就做一次复合。

The composition row comes from $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)$. This is Lesson 4.5's "propagating uncertainty" on a manifold; the first-order linearization is valid only when $\Sigma$ is not too large. Odometry does one composition per step.

6机器人中的它：里程计的"香蕉"In robotics: the odometry "banana"

一台差速轮式机器人只靠轮速和陀螺仪积分（航位推算 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)$ 采样。用"各个拟合样本到最近的真实样本的平均距离"衡量拟合得好不好。

A differential-drive robot that integrates wheel speeds and a gyro (dead reckoning) gets a little noise in each step's turn and advance. The farther it goes, the larger its position uncertainty, and the shape is distinctive: small along the path, much larger across it, and curved along with the path like a banana. The reason is that small heading errors accumulate, so every later step heads off slightly rotated; the position error is therefore a $\sin/\cos$ function of the heading error, not a linear one.

FIG 4 runs a Monte Carlo of 300 trials, each doing $K$ steps of $T\leftarrow T\,\Exp\big((\omega\,dt+\text{noise},\ v\,dt+\text{noise},\ 0)\big)$; the teal dots are the final positions. Then two Gaussians are fitted: the Euclidean Gaussian takes the mean and covariance of the position $(x,y)$ directly (the purple ellipse is its $2\sigma$ contour); the tangent-space Gaussian first finds the Karcher mean $\bar T$, fits the covariance $\Sigma$ of $\Log(\bar T^{-1}T_i)$, and then samples $\bar T\,\Exp(\boldsymbol\varepsilon)$. The mean distance from each fitted sample to its nearest true sample measures how good the fit is.

FIG 4航位推算的位置分布：香蕉与高斯拟合Dead-reckoning position distribution: the banana and Gaussian fits

青色点：蒙特卡洛的真实终点位置；黄色曲线：无噪声的名义路径；紫色点和椭圆：欧氏高斯拟合的样本和 $2\sigma$ 轮廓；绿色点：切空间高斯 $\bar T\Exp(\boldsymbol\varepsilon)$ 的样本。选"两者都不显示"可以单独看真实分布。 Teal dots: the Monte Carlo final positions; amber curve: the noise-free nominal path; purple dots and ellipse: samples and $2\sigma$ contour of the Euclidean Gaussian fit; green dots: samples of the tangent-space Gaussian $\bar T\Exp(\boldsymbol\varepsilon)$. Choose "neither" to see the true distribution alone.

试一试Try this

加长路径（$K \ge 40$），航向噪声 $\sigma_\omega\ge0.2$。看欧氏椭圆：它包进了哪些根本没有真实样本的区域？切空间高斯的距离指标为什么更小？Lengthen the path ($K \ge 40$) with heading noise $\sigma_\omega\ge0.2$. Look at the Euclidean ellipse: which regions does it swallow that contain no true samples? Why is the tangent-space Gaussian's distance metric smaller?
把路径缩短到 $K \le 8$。两种拟合的差别变成什么样？这说明高斯近似在什么条件下才安全？Shorten the path to $K \le 8$. How do the two fits compare now? What does that say about when a Gaussian approximation is safe?

真实系统里In real systems

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 均值。

EKFs and pose-graph SLAM keep their covariances in the tangent space: propagate with the "composition" row at every step, and retract $\boldsymbol\delta$ after each measurement update. An odometry covariance (for example the 6×6 pose covariance in ROS's nav_msgs/Odometry) is a local covariance of this kind; check whether it is a body-frame (right) or world-frame (left) perturbation and how $(x,y,z,\text{roll},\text{pitch},\text{yaw})$ are ordered, and do not treat it as a Gaussian on Euclidean coordinates. Trajectory interpolation (camera-IMU time alignment, rolling-shutter correction, continuous-time SLAM) is the SLERP and screw interpolation of FIG 1 and FIG 2. The "average attitude" in multi-sensor fusion and particle filters is the Karcher mean of FIG 3.

7常见误区Common pitfalls

误区 1：四元数 q 和 −q 是同一个旋转Pitfall 1: q and −q are the same rotation

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

Before SLERP or NLERP, check $q_0\cdot q_1$. If it is negative, replace $q_1$ with $-q_1$, otherwise the interpolation takes the long way (a detour of more than $180^\circ$). This is the most common bug in home-made trajectory interpolators; the symptom is "the camera occasionally spins a full turn between two frames". Likewise, the principal value of $\Log$ gives only the shortest path, so unwrap angle sequences first.

误区 2：对欧拉角、四元数分量或矩阵元素直接取平均Pitfall 2: averaging Euler angles, quaternion components or matrix entries directly

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

Euler angles wrap at $\pm180^\circ$ (FIG 3); averaging quaternion components needs signs unified first; the matrix mean must be projected back onto $\SO$. All three are barely usable when the samples are tight and drift away from the Karcher mean when they are not. For the answer that minimizes angular distance, use the Karcher mean.

误区 3：把位姿协方差当成欧氏坐标的协方差Pitfall 3: treating a pose covariance as a covariance of Euclidean coordinates

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

The tangent-space $\Sigma$ describes small body-frame perturbations $\boldsymbol\varepsilon$, not the covariance of the world coordinates $(x,y,\theta)$; the entries mean different things and cannot be swapped directly. Moreover, once the heading spread is large, any Gaussian on the world $(x,y)$ breaks down: whether fitted directly to the positions as in FIG 4 or obtained by linearizing $\Sigma$ through the Jacobian of $\Exp$, it is an ellipse that swallows empty regions and loses the banana's curvature. To keep the curvature, stay in the tangent-space form $\bar T\Exp(\boldsymbol\varepsilon)$ or sample through $\Exp$ (sigma points, Monte Carlo); linearization is reliable only when the spread is small.

误区 4：左右协方差混用，twist 顺序不一致Pitfall 4: mixing left and right covariances, inconsistent twist ordering

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

$\Sigma_l = \operatorname{Ad}_{\bar T}\Sigma_r\operatorname{Ad}_{\bar T}\T$, and the farther $\bar T$ is from the identity (more rotation, larger translation), the more they differ. Before feeding one library's 6×6 covariance to another, confirm both: left or right, and $(\boldsymbol\omega,\mathbf v)$ or $(\mathbf v,\boldsymbol\omega)$ (Pitfall 5 of Lesson 5.6).

8小练习Check yourself

1. 两个姿态的偏航分别为 $170^\circ$ 和 $-170^\circ$（俯仰、横滚均为 0）。测地线插值的中点，偏航是多少？1. Two orientations have yaw $170^\circ$ and $-170^\circ$ (pitch and roll both 0). What is the yaw at the midpoint of the geodesic?

相对旋转 $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$，会走相反的长路。The relative rotation $R_0\T R_1$ is a turn of $-170^\circ-170^\circ=-340^\circ$ about z, equivalent to $+20^\circ$; the principal $\Log$ gives $+20^\circ$, half of which is $+10^\circ$, so the midpoint yaw is $170^\circ+10^\circ=180^\circ$. Averaging the yaw numbers directly gives $0^\circ$, the long way round.

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$ 是？2. For a Gaussian $R=\bar R\,\Exp(\boldsymbol\varepsilon)$ on $\SO$ the right-perturbation covariance is $\Sigma_r = \operatorname{diag}(0.01,\,0.04,\,0.09)$. If $\bar R$ is a $90^\circ$ rotation about z, what is the left-perturbation covariance $\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)$。A $90^\circ$ turn about z sends $\hat x\to\hat y$ and $\hat y\to-\hat x$. The body x-axis variance 0.01 lands on the world y-axis, the body y-axis variance 0.04 lands on the world x-axis, and z is unchanged. So $\Sigma_l=\operatorname{diag}(0.04,0.01,0.09)$.

3. 螺旋插值 $T_0\Exp(s\Log(T_0^{-1}T_1))$ 与"SLERP + 位置线性插值"相比，前者独有的性质是？3. Compared with "SLERP plus linear interpolation of position", which property is unique to screw interpolation $T_0\Exp(s\Log(T_0^{-1}T_1))$?

螺旋插值的机体 twist 恒定，所以每个机体点的速度恒定；它对左、右乘都不变。两种做法的旋转部分其实相同（都是 SLERP）；直线属于分开插值，不是螺旋插值的性质。Screw interpolation has a constant body twist, so every body point's velocity is constant, and it is invariant under both left and right multiplication. The rotation parts are in fact identical (both SLERP); the straight line belongs to decoupled interpolation, not to screw interpolation.

4. 里程计每一步的转角都有小噪声，走很远以后，位置分布呈"香蕉形"，主要原因是？4. Each odometry step has a small heading noise. After a long drive the position distribution is banana-shaped. The main reason is

即使每一步的噪声都是严格的高斯，位置 $=\sum$（步长 × $(\cos\theta_k,\sin\theta_k)$），$\theta_k$ 是累积的航向。把高斯的 $\theta$ 送过 $\sin/\cos$，分布就弯了。切空间高斯 $\bar T\Exp(\boldsymbol\varepsilon)$ 再经过 $\Exp$，保留了这种弯曲。Even if every step's noise is exactly Gaussian, the position is $\sum$ (step length × $(\cos\theta_k,\sin\theta_k)$) with $\theta_k$ the accumulated heading. Pushing a Gaussian $\theta$ through $\sin/\cos$ bends the distribution. The tangent-space Gaussian $\bar T\Exp(\boldsymbol\varepsilon)$, carried by $\Exp$, keeps that curvature.

5. Karcher 均值迭代 $\mu\leftarrow\mu\Exp(\boldsymbol\delta)$ 中，$\boldsymbol\delta$ 是什么？5. In the Karcher-mean iteration $\mu\leftarrow\mu\Exp(\boldsymbol\delta)$, what is $\boldsymbol\delta$?

先把每个样本相对 $\mu$ 取 $\Log$（搬到 $\mu$ 的切空间），再对向量取平均。选项 A 对矩阵平均后才取对数，B 是在单位元的切空间里平均（等于旋转向量平均，会遇到回绕和大角度误差）。Take the $\Log$ of each sample relative to $\mu$ (moving it into $\mu$'s tangent space), then average the vectors. Option A averages the matrices before taking the log; option B averages in the tangent space at the identity (an average of rotation vectors, which suffers from wrapping and large-angle errors).

9延伸阅读Further reading

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

运算Operation	均值Mean	协方差（右扰动）Covariance (right perturbation)
换成左扰动to left perturbation	\(\bar T\)	\(\Sigma_l = \operatorname{Ad}_{\bar T}\,\Sigma\,\operatorname{Ad}_{\bar T}\T\)
求逆 \(T^{-1}\)inverse \(T^{-1}\)	\(\bar T^{-1}\)	\(\operatorname{Ad}_{\bar T}\,\Sigma\,\operatorname{Ad}_{\bar T}\T\)
复合 \(T_1T_2\)（独立）composition \(T_1T_2\) (independent)	\(\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\)acting on a point \(T\mathbf p\)	\(\bar T\mathbf p\)	\(J\,\Sigma\,J\T,\ J = \begin{bmatrix}-\bar R\skew{\mathbf p} & \bar R\end{bmatrix}\)