论文Review A micro Lie theory for state estimation in robotics

元让_vincent

已于 2025-04-22 15:17:40 修改

阅读量752

点赞数 24

分类专栏：论文review SLAM 文章标签：学习几何学 slam 自动驾驶 c++ 拓扑学矩阵

于 2025-04-22 15:17:26 首次发布

本文链接：https://blog.csdn.net/weixin_42148238/article/details/147192156

版权

SLAM 同时被 2 个专栏收录

8 篇文章

订阅专栏

论文review

5 篇文章

订阅专栏

鉴于李群李代数对于SLAM学习的重要性，决定翻译下这篇论文，在以后的学习中当作字典供查阅。

我们的方法旨在让本论文的目标读者——即精通状态估计（如卡尔曼滤波、基于图的优化等）但尚未熟悉李理论完整理论体系的工程师和研究者——感到亲切易懂。

摘要

李群是一个古老的数学抽象概念，可以追溯到19世纪，当时数学家索菲斯·李（Sophus Lie）奠定了连续变换群理论的基础。许多年后，它的影响已经遍及科技的各个领域。在机器人学中，我们最近正在经历一个重要的趋势，至少在估计领域，特别是在用于导航的运动估计中。然而，对于绝大多数的机器人学家来说，李群是高度抽象的结构，因此很难理解和使用。

在对机器人学进行评估时，往往不需要利用理论的全部能力，因此我们要筛选需要的理论材料。在这篇论文中，我们将浏览李理论中最基本的定理，目的是传达清晰有用的思想，并留下李理论的重要主体。即便是非完整的李理论，这里所包含的材料已经被证明在机器人的现代估计算法中非常有用，特别是在SLAM、视觉里程计等领域。

除了微李理论（Micro Lie Theory），我们提供了包含有几个应用例子的章节，并为机器人中使用的主要李群提供了大量的公式参考，包括大多数的雅克比矩阵和易于操作的方法。我们还提出了一个新的C + +模板库来实现这里描述的所有功能。

I. INTRODUCTION

图 1. 李群与李代数关系示意。李代数 TₑM（红色平面）是李群流形 M（此处以蓝色球面示意）在单位元 E 处的切空间。通过指数映射，李代数中每条经过原点的直线轨迹 vₜ 都会在流形上生成一条沿相应测地线延伸的曲线 exp(vₜ)。反之，李群中的每个元素在李代数中都有对应的等价表示。由于这一关联极为深刻，群上的几乎所有弯曲且非线性的运算都能在作为线性向量空间的李代数中找到精确等价。尽管 ℝ³ 中的球面并非李群（这里仅用作可图示的示意），但 ℝ⁴ 中的球面确实构成单位四元数群 — 参见图 4 和示例 5。

近年来，机器人学界在正确地构造状态估计问题方面投入了大量努力。这一趋势源自对解的精度、一致性和稳定性日益增长的需求。实际上，对状态与观测、以及两者之间映射函数及其不确定性进行恰当建模，对实现上述目标至关重要。这就催生了所谓“流形”（manifold）设计——在此语境中，流形即李群（Lie group）上一类光滑拓扑曲面，状态表征就在其上演化。借助李理论（Lie Theory, LT），我们能够建立严谨的微积分体系，以高精度、易操作地处理不确定性、导数和积分。

通常，这些工作主要聚焦于人们熟知的旋转群 SO(3) 和刚体运动群 SE(3) 两种流形。初次接触李群时，应尝试从不同视角来理解它们。拓扑视角（参见图 1）关注流形的形状，并对其与切空间（tangent space）及指数映射（exponential map）之间的关系提供直观认识。代数视角则研究群作用及其具体实现方式，借助其代数性质来推导或简化闭式表达式。几何视角在机器人学中尤为有用，将群元素与物体或参考系的位置、速度、姿态等变换对应起来。我们可以将原点坐标系视作群的单位元，而流形上的任意一点则表示某个“局部”坐标系。通过这些类比，李理论中的许多抽象概念能够贴近向量空间、几何、运动学等更为经典的领域，从而获得更直观、易理解的解释。

李理论绝非易事。要对李理论（LT）有最基本的认识，我们可以参考以下三部著作。首先，Abbaspour 的《基本李理论》（Basic Lie Theory）[1]篇幅逾四百页；Howe 的《非常基础李理论》（Very Basic Lie Theory）[2]仅二十四页，内容浓缩，被视为必读导论；最后，现代且备受推崇的 Stillwell 的《朴素李理论》（Naive Lie Theory）[3]逾二百页。既有“基本”、“非常基础”和“朴素”之名，而本文仅十七页，却旨在进一步精简李理论——因此冠以“微观”之谓。我们通过两方面实现这一目标：

精选材料：仅取李理论中极少部分内容，探讨其核心潜力。尽管范围有限，这些内容对于机器人估计中的不确定性管理（如惯性预积分、里程计与 SLAM、视觉伺服等）却极为有用，从而能优雅且严谨地设计最优优化器。
教学式阐释：以大量冗余说明来降低入门门槛——这一需求尚未得到充分满足。承接 Stillwell 等人的简化思路，本文在此基础上进一步精简，使得主体内容保持通用性的同时，将抽象层次压到最低。

文中插入的示例为概念落地于常见李群（如旋转矩阵、刚体运动矩阵、四元数等）提供了直观演示；大量图示及详尽图注再次复述相同概念，帮助读者理解。我们特别关注雅可比矩阵的推导与计算——这是 Stillwell [3]中未触及的关键主题，对于绝大多数最优估计器至关重要，也是新算法设计中的难点所在。此外，本文包含一章机器人定位与建图的应用示例，基于李理论实现扩展卡尔曼滤波（EKF）和非线性优化算法。若干附录则汇编了机器人学中最常用李群的详细参考：单位复数群、四元数群、二维/三维旋转矩阵群、二维/三维刚体运动群以及平移群等。

然而，我们对李理论最重要的简化在于其适用范围。如 Howe[2] 所言，我们在此将摒弃以下内容：“李理论的核心阐释在于，可以自然而然地将李群 G 关联到其李代数 𝔤。李代数 𝔤 首先是一个向量空间，其次配备一种称为李括号的双线性非结合积[...]。令人惊讶的是，李群 G 几乎完全由 𝔤 及其李括号所决定。因此，在许多场合，人们可以用 𝔤 来替代 G。由于 G 是一个复杂的非线性对象，而 𝔤 仅是一个向量空间，因此通常使用 𝔤[...] 要简单得多。这正是李理论强大之处。”在 Stillwell[3] 中，更将其称为“李理论的奇迹”。在本文中，我们实际上将李代数置于次要地位，而直接采用其等价的向量空间 ℝⁿ 进行描述，且完全不引入李括号。因此，这里不会像经典教材那样深入阐释李群与李代数之间的深层联系。我们的立场是，针对本文所面向的应用领域，这部分内容往往并非必需。相反，若囊括此类抽象而微妙的数学概念，反而会削弱文本的清晰性与实用性，使读者陷入不必要的复杂性。

我们的工作与其他近期研究[4]、[5]、[6]保持一致，这些研究同样强调需要将李理论更加贴近机器人领域的实践者。我们的方法旨在让本论文的目标读者——即精通状态估计（如卡尔曼滤波、基于图的优化等）但尚未熟悉李理论完整理论体系的工程师和研究者——感到亲切易懂。为此，我们在符号表示上做出了一些创新，尤其是在导数的定义上，将其与向量空间中的对应概念对齐，从而使链式法则清晰可见。如前所述，我们实际上几乎摒弃了李代数的专有材料，而是直接在其同构的正切向量空间 ℝⁿ 上进行计算，这也是我们最终表示不确定性或（微小）状态增量的场所。所有这些简化步骤均不损失任何精确性或严谨性，反而使读者更容易理解李理论并灵活运用其工具。

本文还附带了一个新的开源 C++ 头文件库 manif [7]（地址：https://github.com/artivis/manif）。manif 实现了机器人学中广泛使用的群 SO(2)、SO(3)、SE(2) 和 SE(3)，并支持解析雅可比矩阵的自动生成。该库在易用性、灵活性和运行性能方面均进行了优化。

II. A MICRO LIE THEORY

A. The Lie group

图2. 流形 M 及其在点 X 处的切空间 TₓM （在本例中同构于 ℝ²），以及一个便于说明的截面。
X˙=∂t/∂X
速度元素\dot X并不属于流形 M 本身，而是位于切空间 TₓM 中。

李群将“群”与“光滑流形”这两大概念有机地结合在一起：李群 G 即是同时满足群公理的光滑流形。我们先简要介绍这两者，再将它们融为一体。

一方面，可微（或光滑）流形是一种在局部看起来与线性空间相似的拓扑空间。读者可借助图 2 想象流形的概念：它犹如一条弯曲且光滑的（超）曲面，无棱有角地嵌入在更高维度的空间中。在机器人学中，我们常说状态向量在此类曲面上演化——换言之，流形即由状态所受约束所刻画或定义。例如，满足单位范数约束的向量就构成了半径为 1 的球面流形。流形的“光滑性”保证了在每一点上都存在唯一的切空间；该切空间本质上是一个线性（或向量）空间，使我们能够在其上进行微积分运算。

另一方面，群 $(G, \circ)$ 是一个集合 G 及其上的二元运算 $\circ$ ，对任意 $X, Y, Z \in G$ 满足以下公理：

封闭性 Closure under " $\circ$ "： $X \circ Y \in G$ 。
单位元 Identity $\varepsilon$ ： $\varepsilon \circ X = X \circ \varepsilon = X$ 。
逆元 Inverse $X^{-1}$ ： $X^{-1} \circ X = X \circ X^{-1} = E$ 。
结合律 Associativity： $(X \circ Y) \circ Z = X \circ (Y \circ Z)$ 。

在李群中，流形在每一点处局部形状相同（如球面上的任意两点），因此所有切空间也相互同构。群结构保证元素在流形上的封闭性（公理 1）以及每个元素在流形内存在逆元（公理 3）。其中某个满足单位元性质的元素 $\varepsilon$ （公理 2）对应的切空间即称为该李群的李代数。李群将光滑流形可进行微积分运算的局部性质，与群所具备的对远处元素进行非线性组合的全局性质有机结合，使我们既能在切空间中做微分、积分等运算，又能在流形上以群作用的方式组合状态。

B. The group actions

重要的是，李群具有对其他集合元素进行变换的能力，例如实现旋转、平移、缩放及其组合。这些变换在机器人学中（无论二维还是三维）都被广泛应用。

设李群 $\mathcal{M}$ 与集合 $V$ ，记

其中 $X\cdot v$ 表示 $X\in\mathcal{M}$ 对 $v\in V$ 的一种运算action。为了使“ $\cdot$ ”成为合法的群作用action，必须满足以下公理：

常见的群包括旋转矩阵群 $\mathrm{SO}(n)$ 、单位四元数群 $S^3$ 以及刚体运动群 $\mathrm{SE}(n)$ 。它们在向量（或坐标）x 上的action分别为：

详见表 I 及附录，以获取更完整的说明。群的合成运算（公式 (1)）也可视为群对自身的作用： $\circ : \mathcal{M} \times \mathcal{M} \;\to\; \mathcal{M}$ 。另一个重要且有趣的群作用是伴随运算（adjoint action），将在第 II‑F 节中详细讨论。

示例 1：单位复数群 $S^1$

图3 S¹ 流形是在复平面 ℂ 中由满足 z∗z=1 的单位复数组成的单位圆（蓝色）。其李代数
\mathfrak{s}^1 = T_\varepsilon S^1
对应虚数轴 iℝ （红色），而任意一点的切空间 T_x S^1 亦与实数轴 ℝ （红色）同构。切向量（红色线段）经指数映射包裹于流形上，生成相应的圆弧（蓝色弧）。指数映射 exp 与对数映射 log （箭头）在 iℝ 与 S¹（蓝色圆弧）之间完成“warp”和“unwarp”。单位复数之间的增量可借助 群加法（composition）与指数映射在切空间中表达（后文将定义专用增量运算符 ⊕），详见解释说明图 4。

我们第一个李群示例，也是最易于可视化的，是复数乘法下的单位复数群（见图 3）。单位复数可表示为 $z = \cos\theta + i\,\sin\theta\,.$

运算 Action:
对于复平面中的向量 $x = x + i\,y\,$ ，与单位复数 $z$ 相乘 $x' = z\,x$ ，即可在平面内绕原点旋转角度 θ。

群的性质 Group facts：

封闭性：单位复数的乘积仍为单位复数；
单位元：1；
逆元：共轭复数 z∗z^*；
结合律：与一般乘法相同。

流形性质 Manifold facts：
单位范数约束 $|z|=1$ ，在复平面中定义了一条单位圆，等同于一维球面 $S^1$ ，是一条嵌入二维空间中的一自由度曲线。单位复数随时间沿此圆演化。局部来看，该圆（李群）与其切空间（此处即一条切线）同构，可在线性空间上做微分运算；但全局而言，圆形流形却不同于平直的线性空间。

示例 2：单位四元数群 $S^3$

图 4. S^3 流形是在四元数空间 \mathbb{H} 中的单位三维球面（蓝色），其中单位四元数满足 q∗q=1。其李代数s3=TES3是纯虚四元数空间 Hp={ix+jy+kz∣x,y,z∈R},
同构于三维超平面 \mathbb{R}^3（红色网格）；任意点处的切空间 T_X S^3 亦同构于 \mathbb{R}^3。切向量（红色线段）经指数映射沿大圆（虚线）在流形上“包裹”生成测地线。中图和右图为沿该测地线的侧面截面（可参见图 3 中的 S^1）。指数映射 ⁡\exp 与对数映射 ⁡\log（箭头）在纯虚四元数空间 \mathbb{H}_p 与流形 S^3（蓝色圆弧）之间完成“warp”与“unwarp”。四元数之间的增量可通过切空间中的运算符 ⊕ 表示（详见正文）。

第二个相对易于可视化的李群示例是单位四元数群，其群作用为四元数乘法（见图 4）。单位四元数可表示为 $q = \cos\frac{\theta}{2} + \mathbf{u}\,\sin\frac{\theta}{2}$ ，其中 $\mathbf{u}=i\,u_x + j\,u_y + k\,u_z$ 为单位旋转轴，θ为旋转角。

运算 Action:
在三维空间中，任意纯四元数表示的向量 $x = i\,x + j\,y + k\,z$ 。可通过双侧四元数乘积 $x' = q\,x\,q^*$ 绕单位轴 $\mathbf{u}$ 旋转角度θ。

群的性质 Group facts：

封闭性：单位四元数乘积仍为单位四元数；
单位元：1；
逆元：四元数共轭 $q^*$ ；
结合律：与一般乘法相同。

流形性质 Manifold facts：
单位范数约束 $\|q\| = 1$ 。在四维空间中定义了三维球面 $S^3$ ，即一个嵌入在 $\mathbb{R}^4$ （4维空间）中的三维光滑流形。单位四元数随时间沿此三维球面演化。局部来看，该球面（李群）与其切平面（ $\mathbb{R}^3 \subset \mathbb{R}^4$ ）同构，可在线性空间上做微积分运算；但从全局角度，它仍然是一个非线性流形。

C. The tangent spaces and the Lie algebra

给定在李群流形 $\mathcal{M}$ 上随时间运动的点 $X(t)$ ，其速度 $\dot X = \frac{\partial X}{\partial t}$ 属于该点的切空间 $T_X\mathcal{M}$ （见图 2）。流形的“光滑性”（无棱角、尖点）保证了在每一点都存在且仅存在一个切空间，且所有切空间在结构上同构。

1）李代数 $\mathfrak{m}$

图 5. 令点 z∈S1以恒定角速率 ω运动，其参数方程为
z(t)=cos⁡(ωt)+isin⁡(ωt) .
当它经过 1 和 z 时，其速度分别位于相应的切空间 T_{1}S^1 和 T_{z}S^1 。 **在 T_{z}S^1 中**，若采用全局坐标表示，速度为z˙=z (iω)=−ωsin⁡(ωt)+i ωcos⁡(ωt) ;
若采用局部（李代数）坐标，则表示为zv∧=i ω .
二者关系由{}^z v^\wedge = z^{-1}\dot z = z^*\,\dot z给出。
**在 T_{1}S^1 中**，此关系退化为恒等映射：{}^1 v^\wedge = \dot z = i\,\omega\,.
由此可见，所有切空间的结构均同构于 i\mathbb{R}，这正是该李群的李代数 \mathfrak{s}^1。 **由于在单位元处的切空间就已具备这一线性结构，故李代数常被定义为单位元处的切空间。**

将单位元 $\varepsilon$ 处的切空间 $T_\varepsilon \mathcal{M}$ 称为李代数，记作

每个李群都对应一个李代数。我们通过以下几点将李群 $\mathcal{M}$ 与其李代数 $\mathfrak{m}$ 联系起来[5]（参见图 1 与图 6）：

向量空间结构：李代数 $\mathfrak{m}$ 本质上是一个向量空间1，因此其任一元素可视为 $\mathbb{R}^m$ 中的向量，其中 $m$ 等于李群 $M$ 的自由度数。

1 在任何李代数中，向量空间都被赋予一个称为李括号（Lie bracket）的非结合积（non-associative product）。在这项工作中，我们将不加以利用。

指数与对数映射： $\exp: \mathfrak{m} \to \mathcal{M}\quad$ 将李代数元素映射到李群元素； $\log: \mathcal{M} \to \mathfrak{m}$ 为其逆运算。
伴随变换（Adjoint）：切空间在X处的向量 $T_X\mathcal{M}$ 可通过一个linear transform到单位元处的切空间 $T_\varepsilon \mathcal{M}$ ，该transform即称为Adjoint。

李代数也可局部定义在任一点 $X$ 处，以此建立 $T_X\mathcal{M}$ 的局部坐标系（见图 5）。我们常用 $\wedge$ 符号标注李代数元素，例如将速度向量 $v$ 写作 $v^\wedge$ ,或将一般元素 $\tau = v\,t$ 写作 $\tau^\wedge = (v\,t)^\wedge = v^\wedge t$ 。若需指明所属切空间，可加左上标，如 ${}^X\!v^\wedge \in T_X\mathcal{M},\quad {}^E\!v^\wedge \in T_E\mathcal{M}\,.$

李代数的具体结构可通过对群元素的约束式（式 (3)）做时间导数来推导，详见示例 3 和示例 5。对于乘法李群（multiplicative groups），这会产生如下新的约束： $X^{-1}\dot X \;+\; X\,\dot X^{-1} \;=\; 0$ ，该约束作用于位于 X 处的切向量（其中 $\dot X^{-1}$ 表示逆元 $X^{-1}$ 的导数）。因此，李代数中的元素可表示为2：

2 对于加法李群（additive Lie groups），约束 $X - X = 0$ 微分之后可得 $\dot X = \dot X$ ，即切空间没有约束的影响。这意味着切空间与群空间相同，更多细节详见附录E。

2）笛卡尔向量空间 $\mathbb{R}^m$

李代数 $\mathfrak{m}$ 中的元素 $\tau^\wedge$ 自身具有特殊的结构（如反对称矩阵、虚数、纯四元数等，见表 I），但对我们而言，关键在于它们都可以表示为一组基元（base elements） $E_i$ 的线性组合，其中 $E_i$ 称为李代数的生成元（generators）（即 X 在原点沿第 i 个方向的导数）。此时，仅操纵其坐标向量 $\tau = [\tau_1, \tau_2, \dots, \tau_m]^\top \in \mathbb{R}^m$ ，便足以代替直接操作 $\tau^\wedge\in\mathfrak{m}$ 。我们可通过两种互逆的线性映射（同构，isomorphisms）在 $\mathfrak{m}$ 与 $\mathbb{R}^m$ 之间转换，通常称为 hat 与 vee（见图 6）：

图 6. 流形 M 与其在单位元 E 处切空间两种表示（李代数 \mathfrak{m}=T_E M 和笛卡尔向量空间 \mathbb{R}^m）之间的映射关系。hat映射(\cdot)^\wedge 与(\cdot)^\vee 为线性可逆同构(linear invertible maps or isomorphisms)（式 (10)–(11)）；\exp(\cdot) 与 \log(\cdot) 在 **李代数与流形之间**互为映射；而大写的 \mathrm{Exp}(\cdot) 与 \mathrm{Log}(\cdot) 则是直接在 **向量空间 \mathbb{R}^m 与流形 M** 之间的快捷映射。

其中 $e_i$ 为 $\mathbb{R}^m$ 的标准基（vectors of the base），且有 $e_i^\wedge=E_i$ 。由此可见，李代数 $\mathfrak{m}$ 与向量空间 $\mathbb{R}^m$ 同构，可记作 $\mathfrak{m} \cong \mathbb{R}^m,\quad \tau^\wedge \cong \tau.$ 。对于我们的应用，将元素表示为坐标向量 $\tau\in\mathbb{R}^m$ 要比直接操作 $\tau^\wedge\in\mathfrak{m}$ 更为方便：它们不仅可直接堆叠至更大的状态向量中，更能借助矩阵运算轻松进行线性代数处理。在本文中，我们始终优先使用 $\mathbb{R}^m$ 的表示方式，以致本文大多数定义的算子与对象（特别是伴随算子、雅可比矩阵、状态扰动及其协方差矩阵等）均作用于 $\mathbb{R}^m$ 上。

示例 3：旋转群 SO(3)、其李代数so(3)与向量空间 ℝ³

        在旋转群 SO(3) 中，群元素为 3×3 的旋转矩阵 R，满足正交约束 $R^T R = I.$ 。对该约束两边取时间导数，得：

$R^T\dot R + \dot R^T R = 0 \;\;\Longrightarrow\;\; R^T\dot R = -\bigl(R^T\dot R\bigr)^T,$

        表明矩阵 $R^T\dot R$ 是反对称矩阵，常记作：

$[\boldsymbol\omega]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}.$

        因此有 $R^T\dot R = [\boldsymbol\omega]_\times.$ ，当 $R=I$ 时：

$\dot R=[\boldsymbol\omega]_\times$

        说明 $[\boldsymbol\omega]_\times$ 即属于 so(3)，我们称之为 SO(3) 的李代数。由于 $[\boldsymbol\omega]_\times$ 含 3 个自由度，SO(3) 的维度为 m=3。李代数 so(3) 是一个向量空间，其任一元素均可分解为：

$[\boldsymbol\omega]_\times = \omega_x E_x + \omega_y E_y + \omega_z E_z$

        其中生成元矩阵 $E_x, E_y, E_z$ 为：

$E_x = \begin{bmatrix} 0 & 0 & 0\\ 0 & 0 & -1\\ 0 & 1 & 0 \end{bmatrix},\quad E_y = \begin{bmatrix} 0 & 0 & 1\\ 0 & 0 & 0\\ -1 & 0 & 0 \end{bmatrix},\quad E_z = \begin{bmatrix} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 0 \end{bmatrix}$

        而 $\boldsymbol\omega = (\omega_x, \omega_y, \omega_z)^\top \in \mathbb{R}^3$ 即为角速度向量。上述一一线性对应关系使得 $\text{so(3)} \;\cong\; \mathbb{R}^3.$ 我们通过以下两种线性算符在二者间互相转换：

D. The exponential map

指数映射 $\exp(\cdot)$ 能够将李代数中的元素精确地“折回”到李群上（见图 1），这一操作通常称为收缩映射（retraction）。直观地，⁡ $\exp$ 将正切空间上的元素，沿着大圆弧（great arc）或测地线（geodesic）将切向量“缠绕”到流形上（如同将绳子绕在球面上一样，参见图 1、3 和 4）；其逆运算 $\log(\cdot)$ 则完成“解缠绕”。指数映射自然产生于流形上元素 $X(t)\in \mathcal{M}$ 对时间的导数。由式 (9)

若 v 为常量，上式即为常微分方程，其解析解为

因为 X(0) 与 X(t) 均属于群 $\mathcal{M}$ ，故 $\exp\bigl(v^\wedge\,t\bigr)\;=\;X(0)^{-1}X(t)$ 也必定属于 $\mathcal{M}$ ，这就定义了指数映射将李代数元素 $v^\wedge t\in\mathfrak m$ 映射到李群。

为给出更通用的形式，令切向量增量为每时刻的速度 $\tau \triangleq v\,t\in\mathbb{R}^m$ ,则 $\tau^\wedge=v^\wedge\,t\in\mathfrak m$ 为该点处的李代数。指数映射及其逆运算对数映射，可由上写出

在乘法群中，闭式的指数映射 $\exp(\tau^\wedge)$ ，可由绝对收敛的泰勒级数（absolutely convergent Taylor series,）写出：

并利用 $\tau^\wedge$ 的幂的代数性质（见示例 4、示例5中 SO(3) 与 $S^3$ 的对数映射的发展）推导出对数映射的公式。指数映射的若干重要性质有：

其中式 (20) 是一个奇特但重要的公式，可通过将泰勒级数展开进行证明并利用 $X^{-1}X=E$ 的形式多次简化证明过程。

1）Exp/Log 映射（capitalized exponential map）

为了简化表述，我们引入直接从 $\tau\in\mathbb{R}^m (\cong T_\varepsilon \mathcal{M})$ 到 $\mathcal{M}$ 的快捷映射 Exp/Log：

结合图 6，可见如下公式。详见附录中各流形的 Exp/Log 具体实现。

示例 4：SO(3) 的指数映射

        在示例 3 中我们已看到 $\dot R = R\,[\omega]_\times \;\in\; T_RSO(3)$ 。当角速度向量 ω 保持常量时，上式构成一个常微分方程（ordinary differential equation，ODE），其通解为 $R(t) \;=\; R_0\,\exp\bigl([\omega]_\times\,t\bigr)$ 。在原点处令 $R_0 = I$ ，则指数映射定义为

$R(t) = \exp\bigl([\omega]_\times\,t\bigr)\;\in\;SO(3)$

        接下来，定义向量 $\theta \triangleq u\,\theta \triangleq \omega\,t \;\in\;\mathbb{R}^3$ 为以角–轴形式积分得到的旋转，其中旋转角为 θ，单位旋转轴为 u。于是 $[\theta]_\times\in \mathfrak{so}(3)$ 表示李代数中的总体旋转。将其代入并展开指数的幂级数，得

$R = \exp\bigl([\theta]_\times\bigr) = \sum_{k=0}^{\infty} \frac{1}{k!}\,\bigl([\theta]_\times\bigr)^k = \sum_{k=0}^{\infty} \frac{\theta^k}{k!}\,\bigl([u]_\times\bigr)^k.$

        观察单位轴的反对称矩阵幂次：

$[u]^0_\times = I,\quad [u]^1_\times = [u]_\times,\quad [u]^2_\times = u\,u^T - I,\quad [u]^3_\times = -[u]_\times,\quad [u]^4_\times = -[u]^2_\times,\;\dots$

        可见所有高次幂均可表示为 $I$ 、 $[u]_\times$ 或 $[u]^2_\times$ 的线性组合。因此，将级数按这两类基元分组：

$R = I \;+\; [u]_\times\Bigl(\theta - \tfrac1{3!}\theta^3 + \tfrac1{5!}\theta^5 - \cdots\Bigr) \;+\; [u]^2_\times\Bigl(\tfrac1{2!}\theta^2 - \tfrac1{4!}\theta^4 + \tfrac1{6!}\theta^6 - \cdots\Bigr).$

        识别出正弦和余弦的泰勒展开后，得到著名的罗德里格斯旋转公式：

$\boxed{\,R = \exp\bigl([u\,\theta]_\times\bigr) = I + [u]_\times\,\sin\theta + [u]^2_\times\,(1 - \cos\theta)\,}.$

        若将其作为大写指数映射（见式 (23)），只需令 $R = \mathrm{Exp}(u\,\theta) = \exp\bigl([u\,\theta]_\times\bigr).$

示例 5：单位四元数群 $S^3$ （续）

        在四元数群 $S^3$ （参见示例 2 与文献 [8]）中，单位范数约束 $q^*q = 1$ ，对时间求导，得

$q^*\,\dot q = -\bigl(q^*\,\dot q\bigr)^*$

        说明 $q^*\,\dot q$ 为纯四元数（其实部为零）。纯四元数任意纯四元数可写作

$u\,v = \bigl(i\,u_x + j\,u_y + k\,u_z\bigr)\,v = i\,v_x + j\,v_y + k\,v_z,$

        其中 $u \triangleq i\,u_x + j\,u_y + k\,u_z$ 为纯单位四元数， $v\in\mathbb{R}$ 为标量幅值， $i,j,k$ 为李代数 $\mathfrak{s}^3 = \mathbb{H}_p$ 的生成元（generators）。将上述结果改写，可得流形上速度表示：

$\dot q \;=\; q\,(u\,v)\;\in\;T_qS^3,$

        对其积分，设初始值 $q(0)=q_0$ ，有 $q(t) = q_0\,\exp\bigl(u\,v\,t\bigr)$ 。令 $q_0=1$ ，并定义角量 $\phi \triangleq u\,\phi \triangleq u\, v\,t$ ，则对应的指数映射为:

$q = \exp\bigl(u\,\phi\bigr) = \sum_{k=0}^\infty \frac{(u\,\phi)^k}{k!} \;\in\;S^3.$

        由于纯四元数 $u$ 的幂次循环满足 $u^0 = 1,\quad u^1 = u,\quad u^2 = -1,\quad u^3 = -u,\quad u^4 = 1,\;\dots$ 将级数按基元 1 与 $u$ 分组，并识别出余弦与正弦的泰勒展开，得到四元数版的 Euler 公式推广：

$\boxed{\,q = \exp\bigl(u\,\phi\bigr) = \cos\phi + u\,\sin\phi\,.}$

        李代数元素 $\phi = u\,\phi\in\mathfrak{s}^3$ 可通过hat与vee与旋转向量 $\theta\in\mathbb{R}^3$ 互相对应：

$\text{Hat:}\quad \mathbb{R}^3\;\xrightarrow{\;\widehat{\ }\;}\;\mathfrak{s}^3,\quad \theta\;\mapsto\;\theta^\wedge = \tfrac12\,\theta,$

$\text{Vee:} \quad \mathfrak{s}^3\;\xrightarrow{\;\vee\;}\;\mathbb{R}^3,\quad \phi\;\mapsto\;\phi^\vee = 2\,\phi,$

        其中因子 2 考虑了四元数在旋转作用 $x' = q\,x\,q^*\;$ 中“双侧相乘”的效果。因此，对于同一旋转向量 θ， $q = \mathrm{Exp}\bigl(u\,\theta\bigr) = \cos\frac{\theta}{2} + u\,\sin\frac{\theta}{2}$ 的四元数表达，与通过相同向量生成的旋转矩阵 $R = \mathrm{Exp}(u\,\theta)\;$ 完全等价。

E. Plus and minus operators

加法（⊕）与减法（⊖）算子使我们能够在（曲面）流形的元素之间引入增量，并将这些增量表达在其（平坦）切向量空间中。它们各自将一次 Exp/Log 映射与一次群合成运算结合起来。由于群合成在一般情况下不遵循交换律( non-commutativity)，这些算子又分为右、左两种形式，分别对应操作数的先后顺序。

右加／右减算子（见图 4 右图）

由于式 (25) 中的 $\mathrm{Exp}({}^X\tau)$ 出现在合成的右侧， ${}^X\tau$ 属于点 X 处的切空间 $T_X \mathcal{M}$ 。我们约定3 将此类增量称为在 X 点的局部参考系下表示（在符号上可加左上标以指明参考系）。

3 该约定遵循坐标系变换的惯例，例如 ${}^Gx = R\,{}^Lx,$ ，其中矩阵 $R\in\mathrm{SO}(3)$ 将局部（local）向量变换到全局（global）参考系。请注意，并非所有作者都采用此约定，例如文献 [9] 中则使用相反的表示： ${}^Lx = R\,{}^Gx.$

左加／左减算子

由于式 (27) 中的 $\mathrm{Exp}({}^\varepsilon \tau)$ 在合成的左侧，故 ${}^\varepsilon \tau$ 属于单位元 $\varepsilon$ 处的切空间 $T_\varepsilon M$ ，也称全局参考系下的表示

注意：虽然左右⊕／⊖的定义仅由操作数顺序决定，但式 (26) 与 (28) 的符号看似相同。
本文中，我们默认将扰动表示为局部坐标，因此在不特别说明时，均采用右加／右减形式。

F. 伴随作用与伴随矩阵 The adjoint, and the adjoint matrix

图 7. 两条路径.X \circ {}^\delta X 与 {}^\varepsilon \delta \circ X分别将单位元 \varepsilon 与点 Y 连接起来。前者将增量 {}^X \delta （局部参考系下的扰动）右乘在 X 之后，后者则将增量 {}^\varepsilon \delta （全局参考系下的扰动）左乘在 X 之前。由于李群的运算一般不满足交换律，故Xδ≠Eδ.它们在各自切空间中的对应切向量Xτ=\Log(Xδ)∈TXM, Eτ=\Log(Eδ)∈TEM自然也不相同。二者通过李群在点 X 处的 **伴随算子**\Ad_X 线性关联：Eτ=\AdX(Xτ),即先将局部切向量 Xτ 映射至全局切空间，得到 Eτ。

当我们在式 (25) 与 (27) 中令同一 $\mathcal{Y}$ 同时由两种 ⊕ 方式得到，就得到 ${}^\varepsilon \tau\,\oplus\,\mathcal{X} \,=\, \mathcal{X}\,\oplus\,{}^\mathcal{X}\tau$ ,该式在图 7 中示意了全局（Global）与局部（Local）切向量之间的对应关系。结合式 (20)、(25) 与 (27) 可推导出：

1) 伴随作用（Adjoint）

据此我们定义李群 $\mathcal{M}$ 在 $\mathcal{X}$ 处对其李代数 $\mathfrak m$ 的伴随映射 $\mathrm{Ad}_\mathcal{X}$ ：

从而有 ${}^\varepsilon \tau^\wedge \;=\;\mathrm{Ad}_\mathcal{X}\bigl({}^\mathcal{X}\tau^\wedge\bigr).$ 这就是群对自身李代数的伴随作用(adjoint action)。它满足两条重要性质,线性和同态性：

2）伴随矩阵（Adjoint Matrix）

由于 $\mathrm{Ad}_\mathcal{X}()$ 为线性映射，我们可以引入等价的矩阵算子 $\mathbf{Ad}_\mathcal{X}$ ，用于映射笛卡尔坐标系的正切坐标向量 ${}^\varepsilon \tau \cong {}^\varepsilon \tau^\wedge$ 和 ${}^\mathcal{X} \tau \cong {}^\mathcal{X} \tau^\wedge$ ：

$\mathbf{Ad}_\mathcal{X}$ 被称为 adjoint matrix. 它可通过对式 (29) 做∨映射得到：

再在右手展开并提取系数，即可求出 $\mathbf{Ad}_\mathcal{X}$ 的具体矩阵形式（详见示例 6 及附录）。伴随矩阵还具备以下性质：

注意式 (33) 与 (34) 的左边通常比右边计算更高效。本文中，经常使用伴随矩阵将 $T_\mathcal{X} \mathcal{M}$ 上的切向量线性地映射到 $T_\varepsilon M$ 上： ${}^\varepsilon \tau = \mathbf{Ad}_\mathcal{X}\,{}^\mathcal{X}\tau$ (见式 (30))。在后文中，若无特别说明，“伴随”一词即指伴随矩阵。

示例 6：SE(3) 的伴随矩阵（Adjoint Matrix）

        刚体运动群 SE(3)（详见附录 D）由下列形式的群元素、李代数元素及其向量表示组成：

群元素（Group element）：

$\mathbf{M} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}$

李代数元素（Lie algebra element）：

$\boldsymbol{\tau}^\wedge = \begin{bmatrix} [\boldsymbol{\theta}]_\times & \boldsymbol{\rho} \\ \mathbf{0} & 0 \end{bmatrix}$

切向量表示（Tangent vector element）：

$\boldsymbol{\tau} = \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\theta} \end{bmatrix}$

        我们通过展开公式 (31) 来推导其伴随矩阵：

$\text{Ad}_{\mathbf{M}}\, \boldsymbol{\tau} = \big(\mathbf{M} \boldsymbol{\tau}^\wedge \mathbf{M}^{-1}\big)^\vee = \cdots = \left( \begin{bmatrix} \mathbf{R} [\boldsymbol{\theta}]_\times \mathbf{R}^\top & -\mathbf{R} [\boldsymbol{\theta}]_\times \mathbf{R}^\top \mathbf{t} + \mathbf{R} \boldsymbol{\rho} \\ \mathbf{0} & 0 \end{bmatrix} \right)^\vee$

        我们接着利用公式性质进行化简：

        利用 $[ \mathbf{R} \boldsymbol{\theta} ]_\times = \mathbf{R} [\boldsymbol{\theta}]_\times \mathbf{R}^\top$ ,以及 $[ \mathbf{a} ]_\times \mathbf{b} = -[ \mathbf{b} ]_\times \mathbf{a}$ 得到:

$\begin{bmatrix} [\mathbf{t}]_\times \mathbf{R} \boldsymbol{\theta} + \mathbf{R} \boldsymbol{\rho} \\ \mathbf{R} \boldsymbol{\theta} \end{bmatrix} = \begin{bmatrix} \mathbf{R} & [\mathbf{t}]_\times \mathbf{R} \\ \mathbf{0} & \mathbf{R} \end{bmatrix} \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\theta} \end{bmatrix}$

        因此，SE(3) 的伴随矩阵为：

$\boxed{ \text{Ad}_{\mathbf{M}} = \begin{bmatrix} \mathbf{R} & [\mathbf{t}]_\times \mathbf{R} \\ \mathbf{0} & \mathbf{R} \end{bmatrix} \in \mathbb{R}^{6 \times 6} }$

        这是刚体运动群 SE(3) 在其李代数上的线性作用表达，将局部扰动向量从一个切空间变换到另一个参考点的切空间。

G. 在李群上的导数 Derivatives on Lie groups

在李群背景下定义导数的方式多种多样，这里我们侧重于以雅可比矩阵（Jacobian matrix）的形式，映射切向量空间。由于不确定性和增量均可在这些平坦的切空间中方便地定义，使用此类雅可比矩阵即可让李群中的不确定性管理公式与向量空间中的形式高度相似。

下文所述的雅可比矩阵均满足链式法则，因此可以通过inversion求逆, composition, exponentiation and action等基本块的局部雅可比，轻松拼装任意复杂映射的整体雅可比。详见第 III‑A 节中的推导与证明。

1) 提示：向量空间上的雅可比 Jacobians on vector spaces

对于多元函数 $f: \mathbb{R}^m \to \mathbb{R}^n,$ 其雅可比矩阵定义为将所有偏导数（partial derivatives）按列堆叠的 n×m 矩阵：

更方便的写法是将 J 划分为列向量：

$J = [\,j_1\ \cdots\ j_m\,], \quad j_i = \begin{bmatrix} \frac{\partial f_1}{\partial x_i}\\ \vdots\\ \frac{\partial f_n}{\partial x_i} \end{bmatrix},$

则第 i 列可由差分定义：

其中 $e_i$ 为 $\mathbb{R}^m$ 的第 i 个标准基向量。考虑分子（numerator）

(37)表示在方向 $e_i$ 上对 x 做微小扰动时，fx函数值的变化量；雅可比列 $j_i$ 即为 $v_i(h)$ 对 h 在零点处的导数： $j_i = \left.\frac{\partial v_i(h)}{\partial h}\right|_{h=0} = \lim_{h\rightarrow 0} v_i(h)/h$ 为进一步简化表述，可将所有列同时写成对向量 $h\in\mathbb{R}^m$ 的整体差分形式：

尽管严格来说“除以向量 h”无定义，此记号仅为将各列 (36) 聚合的便捷之选。然而，这种形式可能是通过将分子展开成关于h的线性形式并将左手侧识别为雅可比，即计算雅可比只需将分子在 h 上展成线性形式，然后对应识别分母前的矩阵即为 $J$ ：。

最后，对于足够小的 h，可用线性近似：

2）李群上的右雅可比矩阵 Right Jacobians on Lie goups:

图 8. 函数 f:M→N的 **右雅可比**示意图。在点 X 处切空间 TxM 的规范方向上，扰动向量τi=hei∈TxM
经由“加法 ⊕ → f() → 减法 ⊖”（绿色箭头）被传播到像空间 Tf(X)N 上，得到切空间扰动向量
σi(h)=f(X⊕h ei) ⊖ f(X) ∈ Tf(X)N. 当 h 变化时，流形 M 上的扰动轨迹
τi(h)=h ei（粗红线）沿测地线（蓝色）生成路径；而由于 f(⋅)的非线性，其像空间中的路径（实线蓝色）通常并不沿测地线（虚线蓝色）。这些像空间路径被“提升”到切空间 Tf(X)N 中，形成光滑弯曲的路径（细红线）。雅可比矩阵J_f 的第 i 列向量 ji=lim⁡h→0σi(h)/h ,即为提升后路径在 f(X) 点的方向导数。每个扰动方向 hei∈TXM都对应一个雅可比列向量 ji∈Tf(X)N，因此J=[ j1 ⋯ jm ]∈Rn×m
将切空间 TXM≅Rm中的向量线性映射到 Tf(X)N≅Rn。

受上述标准导数定义（式 (38)）的启发，我们现可借助右加／右减算子 ⊕、⊖ 在流形之间定义映射 $f: \mathcal M \to \mathcal N$ 的雅可比矩阵（参见图 8）。将右{⊕,⊖}替换向量空间中的{+，-}，得到类似标准导数的形式：

其中展开为

我们称此矩阵为 $f$ 的右雅可比。注意式 (41c) 其实是对复合函数 $g(\tau) =Log\bigl(f(\mathcal{X})^{-1} \circ f(\mathcal{X} \circ Exp(\tau))\bigr)$ 应用标准向量导数（式 (38)）而得；而以式 (41a) 的形式书写，则更能直观地理解它——它即是 $f(\mathcal{X})$ 关于 $\mathcal{X}$ 的导数，只不过微小增量已被表示在切空间中！得益于右加⊕／右减算子⊖的构造， $\mathcal{X}$ 及 $f(\mathcal{X})$ 的微小变动均可用切空间中的向量来表达，即分别属于 $T_\mathcal X \mathcal M$ 与 $T_{f(\mathcal X)}\mathcal N$ 。因此，右雅可比矩阵是真正将局部切空间线性映射 $T_\mathcal X\mathcal M \;\longrightarrow\; T_{f(\mathcal X)}\mathcal N$ 的 $\mathbb R^{n\times m}$ 矩阵，我们习惯在符号上加局部参考系“ $^ \mathcal X$ ”上标以示区分。正如在向量空间中一样，此矩阵的各列对应该方向导数：

该向量为沿着 $e_i$ 方向当 $\mathcal{X}$ 变化时 $f(\mathcal{X})$ 的变化量（再看图8, 比较(42)中的 $\mathbb{\sigma}_i$ 和(37)中的 $v_i$ ），该向量雅可比的第 i 列为 . $j_i \;=\;\left.\frac{\partial\sigma_i(h)}{\partial h}\right|_{h=0}.$

按往常一样，我们仍按式 (41a) 并借助差分展开（类似式 (39)）来计算雅可比。示例：令

$f: \mathrm{SO}(3)\to\mathbb R^3,\quad f(R)=R\,p$ ，此时 $\mathcal M=\mathrm{SO}(3)$ ， $\mathcal N=\mathbb R^3$ 。参见附录 B‑C5，可得

这一机制在第 III 节及各附录中有大量示例。

注意：当函数f从一个流形到另一个流形时，式 (41a) 中的加减算子须针对域（domain） $\mathcal M$ 与像空间（codomain or image） $\mathcal N$ 分别选用正确的 ⊕ 与 ⊖。

对于极小增量 τ，以下近似成立：

3）李群上的左雅可比 Left Jacobians on Lie groups

图9 函数 Y = f(X) 在从流形 M 到 N 的映射中，所涉及的所有切空间之间的线性映射如图所示。
全局到局部的映射： Eτ=AdX Xτ，
局部到全局的映射： Eσ=AdY Yσ，
通过左雅可比再结合伴随： Eσ=EDy/Dx Eτ,
通过右雅可比再结合伴随： Yσ=XDy/DxXτ。
这些实线映射构成一个闭合环路，对应公式 (46)；而虚线所示的“交叉”雅可比映射则构成其他环路，分别导出公式 (47) 与 (48)。

同样地，我们也可以利用左加／左减算子来定义映射 $f:\mathcal M\to\mathcal N$ 的雅可比矩阵。左雅可比定义为：

此时， $\tau \in T_\varepsilon \mathcal M$ ，分子属于 $T_\varepsilon \mathcal N$ ， $\quad E\tau\oplus X=Exp(E\tau)\circ X$ 因此，左Jacobi矩阵是一个n × m的矩阵，映射为整体切空间 $T_\varepsilon \mathcal M \rightarrow T_\varepsilon \mathcal N$ ，它们分别是 $\mathcal M$ 和 $\mathcal N$ 的李代数(并且我们用全局或原点的' $\varepsilon$ '上标来标记导数).当 τ足够小时，有近似关系:

进一步可从式 (32)、(43) 和 (45)（见图 9）推导出左、右雅可比间的关联公式：

该式表明，通过李群 $\mathcal M$ 和 $\mathcal N$ 的伴随算子，左雅可比与右雅可比可相互转换。

4）右–左交叉雅可比 Crossed right–left Jacobians

除了纯右算子或纯左算子外，还可以定义“右加 ⊕”但“左减 ⊖”的雅可比，或反之。虽然用得不多，但它们有时很有用，因为可以在局部切空间和全局切空间之间直接映射。为简洁起见，我们仅通过伴随算子将这两种交叉雅可比与前述左右雅可比联系起来：

其中 $\mathcal Y=f(\mathcal X)$ 。上下标分别指明了导数所处的参考系。对微小增量 τ 的近似为

H. 流形上的不确定性与协方差传播 Uncertainty in manifolds, covariance propagation

图10 流形上某点 Xˉ∈M 周围的不确定性，应当通过该点处切向量空间上的协方差来恰当地表达（红色）。利用加法算子 ⊕式 (51)），切空间中的概率椭圆被“包裹”到流形上（蓝色），从而在李群上直观地展现出概率集中区域。

我们在位于流形 $\mathcal M$ 上的某一点 $\bar {\mathcal X}$ 周围定义局部扰动 τ，其属于切空间 $T_{\bar {\mathcal X}} \mathcal M$ ，并使用右加⊕/右减⊖算子表示：

在该切空间上，可通过期望算子 $\mathbb E[\cdot]$ 定义协方差矩阵：

从而在流形上定义高斯分布 $\mathcal X\sim\mathcal N(\bar {\mathcal X},\Sigma_{\mathcal X})$ （见图 10）。注意，虽然用 $\Sigma_{\mathcal X}$ 来记，但它描述的是切向量扰动 τ 的协方差。由于切空间维度 m 与李群自由度一致，该协方差矩阵的定义是良好的（well defined）5。

5 当变量 $\mathcal X$ 的维度 $\mathrm{size}(\mathcal X)$ 大于流形 $\mathcal M$ 的自由度 $\dim(\mathcal M)$ 时，直接使用
$\Sigma_{\mathcal X} = \mathbb{E}\bigl[(\mathcal X - \bar {\mathcal X})(\mathcal X - \bar {\mathcal X})^\top\bigr]$ 来定义协方差总是无意义的；而一般非特殊流形都满足 $\mathrm{size}(\mathcal X)>\dim(\mathcal M)$ 。

同样地，也可在全局参考系（即原点切空间 $T_{ \varepsilon} \mathcal M$ ）中，用左加⊕/左减⊖算子表示扰动：

这就允许在( 52 )式中使用左减法对协方差矩阵进行全局坐标系表示。例如，某个三维姿态在水平面上的旋转不确定，可设置全局协方差 ${}^\varepsilon\!\Sigma=\mathrm{diag}(\sigma_\phi^2,\sigma_\theta^2,\infty)$ 。因为“水平”是全局坐标系上的标定，故协方差必须在全局参考系中给出。

由于全局扰动与局部扰动通过伴随矩阵 (30) 相互关联，其协方差也可用在切空间间转换。

协方差传播通过函数 $f: \mathcal M \to \mathcal N$ ， $\mathcal X \mapsto \mathcal Y = f(\mathcal X)$ ，则协方差的传播仅需用线性化公式 (43) 及右雅可比 (41a)，即可得:

I. 在流形上的离散积分 Discrete integration on manifolds

图11 流形上的运动积分。每次运动数据产生一个切向量步长τk∈TXk−1M，先通过指数映射转化为局部运动增量缠绕在流形上，δk=\Exp(τk)∈M。
再与上一状态 Xk−1 做群作用，得到当前时刻状态Xk = Xk−1∘δk =Xk−1∘\Exp(τk) = Xk−1⊕τk ∈ M..

指数映射 $\mathcal X(t)=\mathcal X_{0}\,\circ\,Exp(v\,t)$ 完成了常量切向量 $v\in T_{\mathcal X_{0}}\mathcal M$ 在流形上的连续时间积分。对于随时间变化的非恒定速度 $v(t)$ ，通常将其分段为一系列在各自短时间区间 $\delta t_{k}$ 内保持常量的切向量 $v_{k}\in T_{\mathcal X_{k-1}}\mathcal M$ 的离散积分：

等价地（见图 11），可以定义 $\tau_{k}=v_{k}\,\delta t_{k}\in T_{\mathcal X_{k-1}}\mathcal M$ ，则可将积分视作一系列小切步长 $\tau_{k}$ 在切空间中的“累加”： $\mathcal X_{k} \triangleq \mathcal X_{0}\;\oplus\;\tau_{1}\;\oplus\;\tau_{2}\;\oplus\cdots\oplus\;\tau_{k}$ .上述过程亦可递归地写作

常见示例包括将三维角速度 ω 在旋转矩阵上积分： $R_{k}=R_{k-1}\,Exp(\omega_{k}\,\delta t)$ ,或在单位四元数上积分： $q_{k}=q_{k-1}\,Exp(\omega_{k}\,\delta t)$ .

III. DIFFERENTIATION RULES ON MANIFOLDS

对于我们常用的典型流形 $\mathcal M$ ，可以为以下基本运算——求逆、合成、指数映射和群作用（inversion, composition, exponentiation and action）——推导出闭式的初等雅可比。其中部分模块可与伴随算子 $\mathrm{Ad}_{\mathcal X}$ 关联，使其成为整个微分流程的核心构件。由此，Log、⊕ 与 ⊖ 的雅可比也可轻松派生。一旦得到了这些模块，其余所有雅可比均可通过链式法则拼接而成。

注意：除下面将给出的左雅可比之外，本文所述的所有雅可比均为右雅可比（即按式 (41a) 定义）。欲自行推导左雅可比的读者，可参照此处提示，或直接运用公式

即利用伴随算子在左右雅可比之间相互转化。

我们在符号上使用

$J^{f(\mathcal X)}_{\mathcal X} \triangleq \frac{{}^\mathcal X Df(\mathcal X)}{D \mathcal X}$ 表示右雅可比，
$\frac{{}^\varepsilon Df(\mathcal X)}{D \mathcal X}$ 表示左雅可比，

另外，注意伴随的逆算子应实现为 $\mathrm{Ad}_{\mathcal X^{-1}}$ （见式 (33)、(34) 及其注释），而非简单取矩阵逆。

A. 链式法则 The chain rule

设 $\mathcal Y=f(\mathcal X)$ , $\mathcal Z = g(\mathcal Y)$ ,则复合映射为 $\mathcal Z = g\bigl(f(\mathcal X)\bigr)$ . 链式法则表明，对于右雅可比有

其证明可借助近似公式 (43) 连续使用3次：

箭头表示极限 τ→0 ，此时 $J_{\mathcal X}^{\mathcal Z}=J_{\mathcal Y}^{\mathcal Z}\;J_{\mathcal X}^{\mathcal Y}.$ ，上述三箭头分别对应公式 (43) 的第一步、第二步和第三步.对于左雅可比和交叉雅可比，其证明思路类似，分别使用公式 (45)、(49) 与 (50)。

注意：当在复合运算中混用右、左及交叉雅可比时，还必须将参考系的转换一并串联，例如

例如式 (59) 的第一条，可通过以下两种途径分别套用公式 (50) 与 (43)：

比较上述两种展开可得式 (59)。

B. 基本雅可比模块 Elementary Jacobian blocks

1) 求逆（Inverse）

我们根据右雅可比的定义（式 (41a)）引入：

利用伴随性质（式 (20)）和伴随矩阵表达（式 (31)）可得：

2) 合成运算（Composition）

令复合映射 $X\circ Y$ ，其右雅可比对“左”与“右”两操作数分别定义为：

同样借助伴随运算与伴随矩阵（式 (20, 31)）以及伴随逆（式 (33)），可推导：

3) 流形 $\mathcal M$ 的雅可比（Jacobian of $\mathcal M$ ）

右雅可比（局部雅可比）：定义为大写指数映射 $\mathcal X = Exp(\tau)$ 的右雅可比，对任意 $\tau\in\mathbb R^m$ ，

它将自变量 τ 的微小变动映射到点 \Exp(τ) 处的局部切空间。由定义可得对小增量 δτ 有近似：

左雅可比（全局雅可比）：定义为大写指数映射的左雅可比，

对应的近似为：

左雅可比将 τ 的变动映射到全局切空间（李代数）。由式 (68) 与 (72) 可得伴随与左右雅可比的关系：

亦可利用链式法则将两者互相关联：

对常用流形， $J_r$ 、 $J_r^{-1}$ 、 $J_\ell$ 及 $J_\ell^{-1}$ 的闭式表达见附录。

4) 群作用（Group Action）

对于群作用 $\mathcal X\cdot v$ （ $\mathcal X\in \mathcal M$ ， $v \in \mathcal V$ ），可定义右雅可比：

由于群作用依赖于目标集合 $\mathcal V$ 的结构，上述雅可比表达式无法一概而论，详见各附录中具体群作用的推导。

C. Useful, but deduced, Jacobian blocks

1) 对数映射（Log）

令 τ = \Log(X)，结合式 (70) 可得其右雅可比为：

2) 加法⊕ 与减法⊖

右加法和右加法对增量的雅可比

右减法的雅可比

对于给定 $\mathcal Z = \mathcal X^{-1}\!\circ \mathcal Y$ 和 $\quad \tau = \mathcal Y\ominus \mathcal X = Log(\mathcal Z)$ 则

式 (82) 的推导示例如下：

IV. COMPOSITE MANIFOLDS

以牺牲部分李群理论的一致性为代价，但在符号表示和操作上获得一定便利，我们可以将大型异构状态视为复合流形（或称为流形束，manifold bundles）的组合。

一个复合流形 $\mathcal{M} = \langle \mathcal{M}_1, \cdots, \mathcal{M}_M \rangle$ 本质上是由 $M$ 个互不作用（non-interacting）的子流形拼接而成。这种组合方式源于对恒等元identity, 逆元inverse和复合操作composition acting在每一个子流形上分别定义：

这确保了该结构满足群的公理。同时，对于复合流形也定义了非耦合的收缩映射（retraction map），我们也记作“指数映射”以统一记号（注意使用尖括号表示复合结构）：

从而保证整体的光滑性（smoothness）。这进一步引出了复合流形的右侧加法与减法操作（此处使用菱形符号表示）：

这些推导的关键结果（参见示例7）是，可以基于这些加法和减法操作定义新的导数 6：

6 这里我们假设右导数，但左导数同样适用

使用上述导数定义，任何定义在复合流形上的函数 $f: \mathcal{M} \to \mathcal{N}$ 的雅可比矩阵可以按照“按块分解”（per-block basis）计算。这将大大简化表达式，仅需了解各个子流形上的雅可比结构即可：

其中每一项 $\frac{\mathrm{D}f_i}{\mathrm{D}\mathcal X_j}$ 可使用标准右雅可比定义（式 41a）进行计算。当 τ 足够小时，有如下近似成立：

在使用上述导数定义的情况下，协方差与不确定性传播也必须遵循同样的操作约定。特别地，协方差矩阵（对应式 52）定义为：

对于该定义，线性化传播（即式 55）可直接使用上述导数公式（式 88）进行近似传播分析。

V. LANDMARK-BASED LOCALIZATION AND MAPPING

我们提供了三个与机器人定位与建图相关的应用示例，用于展示本理论的实用性。

第一个示例是基于地标的定位的卡尔曼滤波器（Kalman Filter）；
第二个是用于同时定位与建图（SLAM）的图优化平滑方法（graph-based smoothing）；
第三个示例则引入了传感器自标定（sensor self-calibration）功能。

这三个例子都建立在一个统一的场景假设之上，具体如下所述：

我们考虑一个移动机器人在二维平面中运动（关于三维情形请参考第 V-D 节），周围分布着少量点状地标（punctual landmarks）或信标（beacons）。机器人能够接收控制指令，其形式为线速度和角速度，并且能够测量地标在其自身坐标系下的位置。

机器人的位姿（pose）定义在特殊欧氏群 SE(2)（见附录 C），地标位置则位于欧氏空间 R2\mathbb{R}^2（见附录 E）中：

控制信号 u 是一个定义在李代数 $\mathfrak{se}(2)$ 上的twist，由纵向前进速度 v 和角速度 ω 构成（不含侧向速度分量），并在采样时间 δt 内积分。其中控制信号包含高斯噪声 $w \sim \mathcal{N}(0, W)$ ，该噪声模型也能反映例如侧滑等非理想运动误差，通过非零的 $\sigma_s$ 来建模：

当在时间 j 接收到控制输入 $u_j$ 时，机器人的位姿更新方式如下（参见公式 56），即通过指数映射将 twist 积分后应用于当前状态。

地标观测为距离-角度（range and bearing）类型，但为了简洁处理，我们将其表示为笛卡尔坐标形式。测量噪声 $n \sim \mathcal{N}(0, N)$ 是零均值高斯分布，噪声协方差矩阵为：

其中 $\mathcal X^{-1} \cdot b_k$ 表示刚体运动的作用（见附录 C），即将地标从世界坐标系变换到机器人坐标系下的位置。

A. 基于流形的误差状态卡尔曼滤波器（ESKF）定位方法

我们首先考虑地标 $b_k$ 的位置是已知的，且固定在环境中。我们定义待估计的机器人位姿为：

$\hat{\mathcal X} \in \mathrm{SE}(2)$ .在李群上，估计误差 $\delta x$ 及其协方差 $P$ 被表示在参考位姿 $\hat{X}$ 处的切空间（tangent space）中，其定义如下（参见公式 51、52）：

1. 预测步骤（Prediction）

在每一次机器人运动之后，采用 ESKF 的预测更新公式如下：

其中各个雅可比矩阵来自附录 C 中的模块计算：状态转移雅可比矩阵（F）,控制输入的扰动传播矩阵（G）分别为：

2. 修正步骤（Correction）

当接收到地标观测 $y_k$ 时，执行 ESKF 的观测更新步骤：

观测模型的雅可比矩阵 HH 的推导如下（也来自附录 C 的李群工具）：

其中：

R 是 $\hat{X}$ 的旋转矩阵部分
t 是平移部分
$[1]_\times$ 表示二维叉乘矩阵： $[1]_\times a = [-a_2, a_1]^\top$

注意事项：

与常规的扩展卡尔曼滤波器（EKF）相比，这里的唯一区别在于：

状态更新和预测公式中使用了李群上的“⊕”运算（见公式 99 和 101）
其余公式（例如卡尔曼增益）形式保持不变

不过，所有的雅可比矩阵均采用了李群理论进行计算（详见附录 C）。这使得方法在非线性状态空间上具有更好的一致性与几何合理性。

B. 基于图优化的平滑与绘图（Smoothing and Mapping）

图12 具有 3 个姿势和 3 个信标的 SAM 因子图。每个测量都会在图表中贡献一个因子。有 2 个运动因子（黑色）和 5 个信标因子（灰色）。X1 上的先验因素提供全局可观测性。

我们现在讨论平滑与建图（smoothing and mapping，SAM）问题，此时需同时估计机器人轨迹与地标位置。首选的求解器为基于因子图的迭代最小二乘优化器。为简化说明，假设轨迹由三个位姿 $\{X_1, X_2, X_3\}$ 组成，环境中有三个点状地标 $\{b_4, b_5, b_6\}$ 。则整体状态可视为复合流形

所得因子图如图 12 所示。每一个先验或观测都会在图中引入一个因子。运动因子和观测因子如下：

$u_{ij}$ 运动因子（Motion Factor）(104)从位姿 i 到 j 的运动测量源自式 (94)。其中 $w_{ij}\sim\mathcal{N}(0,W_{ij})$ 。地标观测因子（Beacon Factor）(105)位姿 i 对地标 k 的测量遵照式 (95).其中 $n_{ik}\sim\mathcal{N}(0,N_{ik})$ 。

每个因子还配有相应的信息矩阵, $\Omega_1 \triangleq W_1^{-1},\;\Omega_{ij} \triangleq W_{ij}^{-1},\;\Omega_{ik} \triangleq N_{ik}^{-1}$ ,定义加权残差为：

最优增量 δx 由最小化所有残差平方和得到：

其中测量集合 $\mathcal P=\{(1),(1\,2),(2\,3),(1\,4),(1\,5),(2\,5),(2\,6),(3\,6)\}$ 见图12。迭代求解过程如下：1.线性化残差。按式 (90) 将各残差在当前估计处展开

其中 $J_{r_p}^{\mathcal X}$ 是稀疏雅可比，非零块包括 $J^{r_1}_{\mathcal X_1},\,J^{r_{ij}}_{\mathcal X_i},\,J^{r_{ij}}_{\mathcal X_j},\,J^{r_{ik}}_{\mathcal X_i},\,J^{r_{ik}}_{b_k}$ ，可按第 V‑A 节方法计算，且由于定义 $J^{f(\mathcal X\oplus\delta x)}_{\delta x}|_{\delta x=0} =J^{f(\mathcal X \oplus \delta x)}_{\mathcal X}|_{\delta x=0} = J^{f(\mathcal X)}_{\mathcal X}$ 。构建总雅可比与残差向量:

求解线性化最小二乘

这是通过使用 J 的伪逆的最小二乘法来解决的（对于大型问题，需要 QR [12]、[13] 或 Cholesky [14]、[15] 因式分解），

状态更新

重复上述步骤直至收敛。

要点提示

我们在这里强调（103）中复合表示法的使用，它允许对雅可比矩阵（107）和更新（110）进行块定义。
在运动模型和观测模型中一律使用 SE(2) 流形及其指数／对数映射，确保几何一致性。

C. 带自标定的平滑与建图 Smoothing and mapping with self-calibration

我们在上述平滑与建图问题基础上，考虑运动传感器存在未知标定bias $\mathbf{c} = \begin{bmatrix}c_v\\c_\omega\end{bmatrix}$ 。此时控制量变为 $\tilde u = \begin{bmatrix}v\,\delta t + c_v\\0\\\omega\,\delta t + c_\omega\end{bmatrix} + w.$ 。定义偏置校正函数

将复合状态向量扩展，引入偏置变量 $\mathbf{c}$ ：

运动残差更新为

其余流程与第 V‑B 节完全一致，仅需在总雅可比矩阵左侧多加一列对应偏置的分量：

其中 $J_{c}^{r_{ij}} = \Omega_{ij}^{T/2}\;J_{c}^{ c (u_{ij}, \mathbf{c})} \in\;\mathbb{R}^{3\times2}$ 是式 (111) 对应的 $3\times2$ 雅可比。如前所述，利用式 (109) 和 (110) 可迭代求解出最优增量 δx，并完成状态更新,实现了在线自标定。

D. 三维情形的推广 3D implementations

惊人地，以上所有示例均可无缝推广至三维。只需将所有变量定义在相应的三维空间与李群上：

机器人位姿： $\mathcal X\in\mathrm{SE}(3)$ ，其李代数可表示为 $\mathbb{R}^6\cong\mathfrak{se}(3)$ （见附录 D）；
地标位置与观测量： $b_k,\,y\in\mathbb{R}^3$ （见附录 E）。

各雅可比矩阵与协方差矩阵的维度相应扩展即可。核心思想在于，自式 (97) 以来的所有数学推导，与二维场景完全相同——这正是得益于李群理论提供的一致的几何抽象。

结论 TODO

todo

附录 TODO

todo

REFERENCES

H. Abbaspour and M. Moskowitz, Basic Lie Theory. WORLD SCIENTIFIC, 2007. [Online]. Available: https://worldscientific.com/ doi/abs/10.1142/6462
R. Howe, “Very basic Lie theory,” The American Mathematical Monthly, vol. 90, pp. 600–623, 1983.
J. Stillwell, Naive Lie Theory. Springer-Verlag New York, 2008.
T. D. Barfoot, State Estimation for Robotics. Cambridge University Press, 2017.
E. Eade, “Lie groups for 2d and 3d transformations,” Tech. Rep.
C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual–inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1–21, 2017.
J. Deray and J. Sola`, “Manif: A micro lie theory library for state estimation in robotics applications,” Journal of Open Source Software, vol. 5, no. 46, p. 1371, 2020. [Online]. Available: https://doi.org/10.21105/joss.01371
J. Sol`a, “Quaternion kinematics for the error-state Kalman filter,” CoRR, vol. abs/1711.02508, 2017. [Online]. Available: http://arxiv.org/ abs/1711.02508
G. Gallego and A. Yezzi, “A compact formula for the derivative of a 3-D rotation in exponential coordinates,” Tech. Rep., 2013.
T. D. Barfoot and P. T. Furgale, “Associating uncertainty with threedimensional poses for use in estimation problems,” IEEE Transactions on Robotics, vol. 30, no. 3, pp. 679–693, June 2014.
G. Chirikjian, Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications, ser. Applied and Numerical Harmonic Analysis. Birkha ̈user Boston, 2011. [Online]. Available: https://books.google.ch/books?id=hZ1CAAAAQBAJ
F. Dellaert and M. Kaess, “Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing,” vol. 25, no. 12, pp. 1181–1203, 2006.
M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on, May 2011, pp. 32813288.
R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “G2o: A general framework for graph optimization,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on, May 2011, pp. 3607–3613.
V. Ila, L. Polok, M. Solony, and P. Svoboda, “SLAM++ - a highly efficient and temporally scalable incremental SLAM framework,” The International Journal of Robotics Research, vol. 36, no. 2, pp. 210–230, 2017. [Online]. Available: https://doi.org/10.1177/0278364917691110