企业官网建设流程全解析

摘要

李群（Lie group）是一种历史悠久的数学抽象对象，其理论可追溯到19世纪，当时数学家 Sophus Lie 奠定了连续变换群理论的基础。此后多年，李群的影响逐渐扩展到科学与技术的诸多领域。近年来，在机器人领域，尤其是在状态估计问题中，并且在导航中的运动估计方面，我们正在见证李群使用上的一个重要趋势。然而，对于绝大多数机器人研究者而言，李群仍然是一种高度抽象的数学结构，因此难以理解和实际应用。

在机器人领域的估计问题中，往往并不需要充分动用李群理论的全部能力，因此有必要对相关内容进行取舍和筛选。本文将围绕李群理论中最基础的原理展开，旨在传达清晰且实用的思想，而将相当一部分更为完整的李群理论内容置于一旁。尽管如此，即便经过这样的“裁剪”，本文所涵盖的材料在现代机器人估计算法中仍然被证明极具价值，尤其是在 SLAM、视觉里程计等领域。

在这套“微型李群理论”的介绍之外，我们还提供了一章包含若干应用示例的内容，并汇总了机器人领域中常用的主要李群的公式参考，其中包括大部分雅可比矩阵及其便捷的操作方式。此外，本文还介绍了一个全新的、仅使用模板的 C++ 库，实现了文中所描述的全部功能。

I. 引言

近年来，机器人学界在对估计问题进行规范化建模方面投入了大量努力。这一趋势源于人们对解的精度、一致性以及稳定性不断增长的需求。事实上，只有对状态和观测、它们之间的函数关系以及相应的不确定性进行恰当建模，才能实现这些目标。由此，人们开始采用所谓的流形（manifolds）进行系统设计——在这里，流形并非抽象的概念，而是李群（Lie groups）所对应的光滑拓扑曲面，状态表示正是在这些曲面上演化的。

借助李理论（Lie Theory，LT），我们能够构建一套严格而系统的微积分工具，用以精确而方便地处理不确定性、导数以及积分问题。通常，这类研究主要集中在众所周知的旋转群S O ( 3 ) SO(3)SO(3)和刚体运动群S E ( 3 ) SE(3)SE(3)这两个流形上。

在初次接触李群时，从不同视角去理解它们是非常重要的：

• 拓扑视角（见图 1）关注流形本身的形状，并直观地揭示其与切空间以及指数映射之间的关系；
• 代数视角关注群运算及其具体实现方式，使我们可以利用代数性质来推导闭式公式或对其进行简化；
• 几何视角在机器人学中尤为重要，它将群元素与物体或参考坐标系的位置、速度、姿态及其他变换联系起来。

在几何视角下，原点坐标系可与群的单位元相对应，而流形上的任意一点则代表一个特定的“局部”坐标系。通过这些类比，李理论中的诸多数学抽象概念，便可以被拉近到向量空间、几何学、运动学以及其他更为经典的领域，从而更易理解。

图 1. 李群与李代数之间关系的表示。
李代数T E M \mathcal{T}_E\mathcal{M}TE​M（红色平面）是李群流形M \mathcal{M}M在单位元E EE处的切空间（此处将流形表示为一个蓝色球面）。通过指数映射，李代数中穿过原点的每一条直线路径v t v tvt，都会在流形上生成一条路径exp ⁡ ( v t ) \exp(v t)exp(vt)，该路径沿着相应的测地线运行。反过来，群中的每一个元素在李代数中都有一个对应的表示。

这种对应关系极其深刻：李群中几乎所有操作（该空间是弯曲且非线性的）在李代数中都存在完全等价的操作，而李代数是一个线性的向量空间。需要注意的是，R 3 \mathbb{R}^3R3中的球面并不是一个李群（这里仅作为一种便于在纸面上绘制的示意），而R 4 \mathbb{R}^4R4中的球面则是一个李群，它描述了单位四元数群——参见图 4 与示例 5。

李理论绝非简单。若要对其内容有一个最低限度的认识，可以参考以下三本著作：

1. Abbaspour 的《Basic Lie theory》[1]，全书超过 400 页；
2. Howe 的《Very basic Lie theory》[2]，虽仅有 24 页（但内容高度凝练），常被视为必读的入门文献；
3. Stillwell 的《Naive Lie theory》[3]，内容较新，且颇受推崇，全书超过 200 页。

在这些书名都冠以“基本（basic）”“非常基本（very basic）”“直观（naive）”的背景下，本文仅用17 页，旨在将李理论进一步简化（这也是标题中“micro”的由来）。我们主要通过以下两种方式来实现这一目标。

首先，我们从李理论中挑选了一个极小的子集。这个子集小到仅能初步展现李理论的潜力，但它在机器人领域中常见的估计问题（如惯性预积分、里程计、SLAM、视觉伺服等）中的不确定性管理方面，却显得极为实用，并能支持优雅而严格的最优估计器设计。

其次，我们采用一种高度教学化（didactical）的讲述方式，包含大量的重复说明，以进一步降低进入李理论的门槛——我们认为这一点至今仍然十分必要。可以说，我们延续并加强了 Stillwell [3] 这类工作的努力方向，提供了一个更加简化的版本。

正文主体保持通用性，同时尽量降低抽象层级；文中穿插的示例将一般性概念落实到熟知的群（如旋转矩阵、运动矩阵、四元数等）之上；此外，还包含大量配有详细说明的图示，对同一概念进行多次讲解。我们特别重视雅可比矩阵的计算——这一主题在 [3] 中并未涉及，但它却是绝大多数最优估计器的核心所在，也是设计新算法时常见问题的根源。

我们还提供了一章内容，展示若干机器人定位与建图的应用实例，基于李理论实现了 EKF 和非线性优化算法。最后，多个附录系统性地汇总了机器人领域中最常用李群的重要细节，包括：单位复数、四元数、二维与三维旋转矩阵、二维与三维刚体运动矩阵，以及平凡的平移群。

然而，本文在研究范围上的简化，或许才是对李理论最为重要的“裁剪”。以下来自 Howe [2] 的一段话可以很好地说明我们刻意省略的部分：

“李理论的核心现象在于：可以以一种自然的方式，将一个李群G GG关联到其李代数g \mathfrak{g}g。李代数g \mathfrak{g}g首先是一个向量空间，其次它配备了一种称为李括号的双线性、非结合乘法……令人惊讶的是，群G GG在很大程度上可以由g \mathfrak{g}g及其李括号完全刻画。因此，在许多情况下，可以用g \mathfrak{g}g来替代G GG。由于G GG是一个复杂的非线性对象，而g \mathfrak{g}g只是一个向量空间，后者通常要简单得多……这正是李理论威力的来源之一。”

Stillwell [3] 甚至将其称为“李理论的奇迹”。

在本文中，我们有意将李代数降到次要地位，转而使用与其等价的切向量空间R n \mathbb{R}^nRn，并且完全不引入李括号。因此，李群与其李代数之间的深层联系，并未得到充分展开。我们的观点是：在本文所面向的应用领域中，这些内容往往并非必需；一旦引入，反而会迫使读者深入一些因高度抽象或微妙性而显得不必要复杂的数学概念，从而违背“清晰且实用”的初衷。

我们的工作与近年来其他一些尝试将李理论拉近机器人研究者的工作是一致的，例如 [4]、[5]、[6]。本文的目标读者是：已经熟悉状态估计（如卡尔曼滤波、图优化等），但尚未系统接触李理论的机器人研究人员。

为此，我们在符号体系上也采取了一些新的约定，尤其是在导数的定义上，使其更接近向量空间中的对应形式，从而使链式法则更加直观清晰。正如前文所述，我们几乎完全避免了李代数的正式理论，而是优先在其同构的切向量空间R n \mathbb{R}^nRn上开展工作——毕竟，我们最终正是在这个空间中表示不确定性或（小的）状态增量。所有这些做法完全不会损失精度或严谨性，同时我们相信，它们显著降低了理解李理论及其工具的难度。

本文还配套提供了一个全新的开源 C++ 仅头文件（header-only）库，名为manif[7]，代码托管于：

https://github.com/artivis/manif

manif实现了机器人领域中广泛使用的群S O ( 2 ) SO(2)SO(2)、S O ( 3 ) SO(3)SO(3)、S E ( 2 ) SE(2)SE(2)和S E ( 3 ) SE(3)SE(3)，并支持解析形式的雅可比矩阵计算。该库在设计时充分考虑了易用性、灵活性以及运行效率。

II. 微型李理论（A Micro Lie Theory）

A. 李群（The Lie Group）

李群在一个统一的数学结构中同时涵盖了群（group）与光滑流形（smooth manifold）这两个概念：
一个李群G GG是一个光滑流形，其元素同时满足群公理。在将这两个概念结合起来之前，我们先分别简要介绍它们。

一方面，可微流形（differentiable manifold）或光滑流形（smooth manifold）是一个在局部上“看起来像线性空间”的拓扑空间。读者应当能够直观地想象流形的概念（见图 2）：它类似于一个嵌入在高维空间中的、平滑弯曲的（超）曲面，没有边缘或尖点。在机器人学中，我们常说系统的状态向量在该曲面上演化，也就是说，流形描述或由施加在状态上的约束所定义。

图 2.流形M \mathcal{M}M及其在点X XX处的切空间T X M \mathcal{T}_X\mathcal{M}TX​M（此处T X M ≃ R 2 \mathcal{T}_X\mathcal{M} \simeq \mathbb{R}^2TX​M≃R2），并给出了一个便于理解的侧向剖切示意。速度元素
X ˙ = ∂ X ∂ t \dot{X} = \frac{\partial X}{\partial t}X˙=∂t∂X​
并不属于流形M \mathcal{M}M本身，而是属于切空间T X M \mathcal{T}_X\mathcal{M}TX​M。

例如，具有单位范数约束的向量集合，构成了一个半径为 1 的球面流形。流形的光滑性意味着：在流形的每一个点处，都存在唯一的切空间（tangent space）。这个切空间是一个线性空间或向量空间，在其上我们可以进行微积分运算。

另一方面，一个群( G , ∘ ) (G, \circ)(G,∘)由一个集合G GG及其上的一个复合运算∘ \circ∘构成。对于任意X , Y , Z ∈ G X, Y, Z \in GX,Y,Z∈G，群运算需满足以下公理：

封闭性（Closure）：
X ∘ Y ∈ G . (1) X \circ Y \in G . \tag{1}X∘Y∈G.(1)

单位元（Identity）E EE：
E ∘ X = X ∘ E = X . (2) E \circ X = X \circ E = X . \tag{2}E∘X=X∘E=X.(2)

逆元（Inverse）X − 1 X^{-1}X−1：
X − 1 ∘ X = X ∘ X − 1 = E . (3) X^{-1} \circ X = X \circ X^{-1} = E . \tag{3}X−1∘X=X∘X−1=E.(3)

结合律（Associativity）：
( X ∘ Y ) ∘ Z = X ∘ ( Y ∘ Z ) . (4) (X \circ Y) \circ Z = X \circ (Y \circ Z) . \tag{4}(X∘Y)∘Z=X∘(Y∘Z).(4)

在一个李群中，流形在任意一点处的局部结构都是相同的（例如球面的情况，见示例 1 和 2），因此，位于不同点的切空间在结构上是等价的。群结构保证了：

• 流形上元素的复合仍然属于该流形（封闭性，上式 (1)）；
• 每一个元素都在流形内具有对应的逆元（上式 (3)）。

其中，有一个特殊的元素——单位元（上式 (2)）；相应地，也存在一个特殊的切空间，即单位元处的切空间。这个切空间被称为该李群的李代数（Lie algebra）。

综上所述，李群将：

• 光滑流形的局部性质（使得我们可以进行微积分运算），
• 与群的整体性质（使得我们可以对相距较远的对象进行非线性组合）

优雅地结合在了一起。

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

我们的第一个李群示例，也是最容易进行可视化理解的一个例子，是单位复数在复数乘法下所构成的群（见图 3）。单位复数可表示为
z = cos ⁡ θ + i sin ⁡ θ . z = \cos\theta + i\sin\theta .z=cosθ+isinθ.

• 作用（Action）：
  向量x = x + i y x = x + i yx=x+iy可通过与单位复数相乘而在平面中旋转一个角度θ \thetaθ，即
  x ′ = z x . x' = zx .x′=zx.

• 群的性质（Group facts）：
  单位复数的乘积仍然是单位复数；
  群的单位元为1 11；
  群中元素的逆为其共轭：
  z − 1 = z ∗ . z^{-1} = z^* .z−1=z∗.

• 流形的性质（Manifold facts）：
  单位范数约束定义了复平面中的单位圆（也可视为一维球面，因此称为S 1 S^1S1）。
  这是一个嵌入在二维空间中的1 自由度（1-DoF）曲线。
  单位复数随时间在该圆周上演化。
  该群（即圆周）在局部上类似于一个线性空间（其切线），但在整体上并非如此。

图 3.流形S 1 S^1S1是复平面C \mathbb{C}C中的单位圆（蓝色），其上的元素为满足
z ∗ z = 1 z^\ast z = 1z∗z=1
的单位复数。李代数s 1 = T E S 1 \mathfrak{s}^1 = T_E S^1s1=TE​S1是虚数轴i R i\mathbb{R}iR（红色），而任意切空间T S 1 T S^1TS1都与实直线R \mathbb{R}R同构（红色）。

切向量（红色线段）沿流形“缠绕”，在流形上生成圆弧（蓝色弧线）。指数映射exp ⁡ \expexp与对数映射log ⁡ \loglog（箭头所示）在i R i\mathbb{R}iR与S 1 S^1S1的元素（蓝色圆弧）之间进行映射，即分别实现“包裹”和“展开”。

单位复数之间的增量通过群复合与指数映射在切空间中表示（为此我们将定义特殊的运算符⊕ \oplus⊕、⊖ \ominus⊖）。具体解释请参见正文，类似的群结构示意见图 4。

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

第二个李群示例，同样也是一个相对容易进行可视化理解的例子，是单位四元数在四元数乘法下所构成的群（见图 4）。单位四元数可表示为
q = cos ⁡ ( θ 2 ) + u sin ⁡ ( θ 2 ) , q = \cos(\tfrac{\theta}{2}) + \mathbf{u}\sin(\tfrac{\theta}{2}),q=cos(2θ​)+usin(2θ​),
其中
u = i u x + j u y + k u z \mathbf{u} = i u_x + j u_y + k u_zu=iux​+juy​+kuz​
是一个单位旋转轴，θ \thetaθ为旋转角。

• 作用（Action）：
  向量
  x = i x + j y + k z \mathbf{x} = i x + j y + k zx=ix+jy+kz
  可通过双四元数乘积，即
  x ′ = q x q ∗ , \mathbf{x}' = q\mathbf{x}q^* ,x′=qxq∗,
  在三维空间中绕单位轴u \mathbf{u}u旋转角度θ \thetaθ。

• 群的性质（Group facts）：
  单位四元数的乘积仍然是单位四元数；
  群的单位元为1 11；
  群元素的逆为其共轭：
  q − 1 = q ∗ . q^{-1} = q^* .q−1=q∗.

• 流形的性质（Manifold facts）：
  单位范数约束定义了三维球面S 3 S^3S3，这是一个嵌入在四维空间中的三维球面流形。
  单位四元数随时间在该曲面上演化。
  该群（即球面）在局部上类似于线性空间（其切超平面
  R 3 ⊂ R 4 \mathbb{R}^3 \subset \mathbb{R}^4R3⊂R4
  ），但在整体上并非如此。

图 4.流形S 3 S^3S3是四元数空间H \mathbb{H}H中的单位三维球面（蓝色），其上的元素为满足
q ∗ q = 1 q^\ast q = 1q∗q=1
的单位四元数。其李代数是纯虚四元数空间i x + j y + k z ∈ H p ix + jy + kz \in \mathbb{H}_pix+jy+kz∈Hp​，该空间与三维欧氏空间R 3 \mathbb{R}^3R3同构（红色网格），而任意切空间T S 3 T S^3TS3也同样与R 3 \mathbb{R}^3R3同构。

切向量（红色线段）在流形上“缠绕”，沿着大圆弧或测地线（虚线）展开。中间和右侧的图展示了沿该测地线的侧向剖切（可以注意到它与图 3 中的S 1 S^1S1非常相似）。

指数映射exp ⁡ \expexp与对数映射log ⁡ \loglog（箭头所示）在H p \mathbb{H}_pHp​与S 3 S^3S3的元素（蓝色弧线）之间进行映射，即分别实现“包裹”和“展开”。四元数之间的增量通过运算符⊕ \oplus⊕、⊖ \ominus⊖在切空间中表示（详见正文）。

B. 群作用

重要的是，李群自带将其他集合元素进行变换的能力，例如产生旋转、平移、缩放及它们的组合。这些在机器人学中被广泛使用，无论是二维还是三维。

给定一个李群M MM和一个集合V VV，我们记X ⋅ v X \cdot vX⋅v为X ∈ M X \in MX∈M对v ∈ V v \in Vv∈V的作用，

⋅ : M × V → V ; ( X , v ) ↦ X ⋅ v . (5) \cdot : M \times V \to V \ ; \ (X, v) \mapsto X \cdot v \ . \tag{5}⋅:M×V→V;(X,v)↦X⋅v.(5)

为了使⋅ \cdot⋅成为一个群作用，它必须满足以下公理：

单位元（Identity）：
E ⋅ v = v (6) E \cdot v = v \tag{6}E⋅v=v(6)

兼容性（Compatibility）：
( X ∘ Y ) ⋅ v = X ⋅ ( Y ⋅ v ) . (7) (X \circ Y) \cdot v = X \cdot (Y \cdot v) \ . \tag{7}(X∘Y)⋅v=X⋅(Y⋅v).(7)

常见示例包括旋转矩阵群S O ( n ) SO(n)SO(n)、单位四元数群以及刚体运动群S E ( n ) SE(n)SE(n)。它们各自对向量的作用为：

• S O ( n ) SO(n)SO(n)：旋转矩阵R ⋅ x ≜ R x R \cdot x \triangleq RxR⋅x≜Rx
• S E ( n ) SE(n)SE(n)：欧几里得矩阵H ⋅ x ≜ R x + t H \cdot x \triangleq Rx + tH⋅x≜Rx+t
• S 1 S_1S1​：单位复数z ⋅ x ≜ z x z \cdot x \triangleq zxz⋅x≜zx
• S 3 S_3S3​：单位四元数q ⋅ x ≜ q x q ∗ q \cdot x \triangleq q x q^\astq⋅x≜qxq∗

更详细的说明请参见表 I 以及附录。

群的组合运算 (公式 1) 可以看作是群自身的一个作用：
∘ : M × M → M \circ : M \times M \to M∘:M×M→M

另一个有趣的作用是伴随作用（adjoint action），我们将在第 II-F 节中讨论。

C. 切空间与李代数

给定X ( t ) X(t)X(t)为在李群流形M MM上运动的点，其速度X ˙ = ∂ X / ∂ t \dot X = \partial X / \partial tX˙=∂X/∂t属于X XX处流形的切空间，我们记为T X M T_XMTX​M（见图 2）。流形的光滑性，即不存在棱角或尖点，意味着每个点都有唯一的切空间。所有切空间的结构都是一致的。

1. 李代数m \mathfrak{m}m：

位于单位元处的切空间T E M T_E MTE​M被称为李群M MM的李代数，记作m \mathfrak{m}m，
Lie algebra: m ≜ T E M . (8) \text{Lie algebra: } \mathfrak{m}\triangleq T_E M \ . \tag{8}Lie algebra:m≜TE​M.(8)

每个李群都有一个关联的李代数。我们通过以下事实将李群与其李代数联系起来 [5]（见图 1 和图 6）：

图 6.
流形M MM与其在原点处切空间T E M T_E MTE​M的表示之间的映射关系。切空间在这里既可以表示为李代数m \mathfrak{m}m，也可以表示为笛卡尔向量空间R m \mathbb{R}^mRm。

映射hat( ⋅ ) ∧ (\cdot)^\wedge(⋅)∧和vee( ⋅ ) ∨ (\cdot)^\vee(⋅)∨（∨ \vee∨）是线性的、可逆的映射（同构）（见公式 (10)–(11)），用于在向量空间R m \mathbb{R}^mRm与李代数m \mathfrak{m}m之间相互转换。

映射exp ⁡ ( ⋅ ) \exp(\cdot)exp(⋅)和log ⁡ ( ⋅ ) \log(\cdot)log(⋅)用于在李代数m \mathfrak{m}m与流形M MM之间相互映射；而E x p ( ⋅ ) \mathrm{Exp}(\cdot)Exp(⋅)和L o g ( ⋅ ) \mathrm{Log}(\cdot)Log(⋅)则是便捷写法，用于直接在向量空间R m \mathbb{R}^mRm与流形M MM之间进行映射。

• 李代数m \mathfrak{m}m是一个向量空间。其元素可以与R m \mathbb{R}^mRm中的向量对应，其中m mm是李群M MM的自由度数。
• 指数映射（exponential map）exp ⁡ : m → M \exp : \mathfrak{m} \to Mexp:m→M可以将李代数的元素精确映射到群元素，
  对应的对数映射（log map）是其逆操作。
• 位于X XX处的切空间向量可以通过线性变换转换到单位元E EE的切空间，这个变换称为伴随（adjoint）。

李代数也可以在某个切点X XX局部定义，从而为T X M T_XMTX​M建立局部坐标系（见图 5）。我们将用“帽子”（hat）符号标记李代数的元素，例如速度v ^ \hat vv^或一般元素τ ^ = ( v t ) ^ = v ^ t \hat \tau = (v t\hat ) = \hat v tτ^=(vt)^​=v^t。也可以加上左上标以指定精确的切空间，例如
X v ∧ ∈ T X M 和 E v ∧ ∈ T E M ^Xv^\wedge \in T_X M \quad \text{和} ^Ev^\wedge \in T_E MXv∧∈TX​M和Ev∧∈TE​M

图 5.设一点z ∈ S 1 z \in S_1z∈S1​以恒定旋转速率ω \omegaω运动，z ( t ) = cos ⁡ ω t + i sin ⁡ ω t z(t) = \cos \omega t + i \sin \omega tz(t)=cosωt+isinωt。当点经过1 11和z zz时，其速度分别位于对应的切空间T 1 S 1 T_1 S^1T1​S1和T z S 1 T_z S^1Tz​S1。

在T z S 1 T_z S_1Tz​S1​的情况下，速度在全局坐标下表示为

z ˙ = z i ω = − ω sin ⁡ ω t + i ω cos ⁡ ω t \dot{z} = z i \omega = -\omega \sin \omega t + i \omega \cos \omega tz˙=ziω=−ωsinωt+iωcosωt

而在局部表示下为

z v ∧ = i ω ^zv^\wedge = i \omegazv∧=iω

它们之间的关系由

z v ∧ = z − 1 z ˙ = z ∗ z ˙ ^zv^\wedge = z^{-1} \dot{z} = z^* \dot{z}zv∧=z−1z˙=z∗z˙

给出。

在T 1 S 1 T_1 S^1T1​S1的情况下，这个关系就是恒等式

1 v ∧ = z ˙ = i ω ^1v^\wedge = \dot{z} = i \omega1v∧=z˙=iω

显然，所有切空间的结构都是i R i\mathbb{R}iR，这就是李代数的结构。这也是在单位元处速度的结构，因此李代数被定义为单位元处的切空间。

李代数的结构可以通过对群约束公式 (3) 求时间导数得到（见示例 3 和 5)。对于乘法群，这产生新的约束

X − 1 X ˙ + X ˙ − 1 X = 0 , X^{-1} \dot X + \dot X^{-1} X = 0,X−1X˙+X˙−1X=0,

它适用于X XX处的切元素（其中X ˙ − 1 \dot X^{-1}X˙−1是逆元的导数）。因此李代数的元素形式为

v ^ = X − 1 X ˙ = − X ˙ − 1 X . (9) \hat v = X^{-1} \dot X = - \dot X^{-1} X \ . \tag{9}v^=X−1X˙=−X˙−1X.(9)

例 3：旋转群 SO(3)、其李代数 so(3) 及向量空间R 3 \mathbb{R}^3R3

在 3×3 旋转矩阵R RR组成的旋转群 SO(3) 中，我们有正交条件：

R ⊤ R = I . R^\top R = I.R⊤R=I.

切空间可以通过对该约束取时间导数得到，即

R ⊤ R ˙ + R ˙ ⊤ R = 0 , R^\top \dot{R} + \dot{R}^\top R = 0,R⊤R˙+R˙⊤R=0,

我们可以重排为

R ⊤ R ˙ = − ( R ˙ ⊤ R ) ⊤ . R^\top \dot{R} = -(\dot{R}^\top R)^\top.R⊤R˙=−(R˙⊤R)⊤.

这个表达式表明R ⊤ R ˙ R^\top \dot{R}R⊤R˙是一个反对称矩阵（其转置的负值）。反对称矩阵通常记作[ ω ] × [\omega]_\times[ω]×​，其形式为

[ ω ] × = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] . [\omega]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}.[ω]×​=​0ωz​−ωy​​−ωz​0ωx​​ωy​−ωx​0​​.

因此有

R ⊤ R ˙ = [ ω ] × . R^\top \dot{R} = [\omega]_\times.R⊤R˙=[ω]×​.

当R = I R = IR=I时，我们得到

R ˙ = [ ω ] × , \dot{R} = [\omega]_\times,R˙=[ω]×​,

即[ ω ] × [\omega]\times[ω]×属于 SO(3) 的李代数，我们记作 so(3)。由于[ ω ] × ∈ so(3) [\omega]\times \in \text{so(3)}[ω]×∈so(3)有 3 个自由度，SO(3) 的维数为m = 3 m = 3m=3。李代数是一个向量空间，其元素可以分解为

[ ω ] × = ω x E x + ω y E y + ω z E z , [\omega]_\times = \omega_x E_x + \omega_y E_y + \omega_z E_z,[ω]×​=ωx​Ex​+ωy​Ey​+ωz​Ez​,

其中生成元为：

E x = [ 0 0 0 0 0 1 0 − 1 0 ] , E y = [ 0 0 1 0 0 0 − 1 0 0 ] , E z = [ 0 1 0 0 0 0 − 1 0 0 ] , 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 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{bmatrix},Ex​=​000​00−1​010​​,Ey​=​00−1​000​100​​,Ez​=​00−1​100​000​​,

而ω = ( ω x , ω y , ω z ) ∈ R 3 \omega = (\omega_x, \omega_y, \omega_z) \in \mathbb{R}^3ω=(ωx​,ωy​,ωz​)∈R3是角速度向量。上述一一对应的线性关系允许我们将 so(3) 与R 3 \mathbb{R}^3R3对应，即：

so(3) ∼ R 3 . \text{so(3)} \sim \mathbb{R}^3.so(3)∼R3.

我们可以通过线性算子 hat 和 vee 在 so(3) 与R 3 \mathbb{R}^3R3之间转换：

Hat 映射：

Hat : R 3 → so(3) , ω ↦ ω ∧ = [ ω ] × \text{Hat} : \mathbb{R}^3 \to \text{so(3)}, \quad \omega \mapsto \omega^\wedge = [\omega]_\timesHat:R3→so(3),ω↦ω∧=[ω]×​

Vee 映射：

Vee : so(3) → R 3 , [ ω ] × ↦ [ ω ] ∨ = ω . \text{Vee} : \text{so(3)} \to \mathbb{R}^3, \quad [\omega]_\times \mapsto [\omega]^\vee = \omega.Vee:so(3)→R3,[ω]×​↦[ω]∨=ω.

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

我们在例 3 中已经看到：

R ˙ = R [ ω ] × ∈ T R SO(3) . \dot{R} = R [\omega]_\times \in T_R \text{SO(3)}.R˙=R[ω]×​∈TR​SO(3).

对于常量ω \omegaω，这是一个常微分方程（ODE），其解为：

R ( t ) = R 0 exp ⁡ ( [ ω ] × t ) . R(t) = R_0 \exp([\omega]_\times t).R(t)=R0​exp([ω]×​t).

在原点R 0 = I R_0 = IR0​=I时，我们得到指数映射：

R ( t ) = exp ⁡ ( [ ω ] × t ) ∈ SO(3) . R(t) = \exp([\omega]_\times t) \in \text{SO(3)}.R(t)=exp([ω]×​t)∈SO(3).

现在定义向量θ ≜ u θ ≜ ω t ∈ R 3 \theta \triangleq u \theta \triangleq \omega t \in \mathbb{R}^3θ≜uθ≜ωt∈R3为积分旋转的角-轴形式，其中角度为θ \thetaθ，单位轴为u uu。因此[ θ ] × ∈ so(3) [\theta]_\times \in \text{so(3)}[θ]×​∈so(3)表示在李代数中的总旋转。将其代入上式，并将指数展开为幂级数：

R = exp ⁡ ( [ θ ] × ) = ∑ k θ k k ! ( [ u ] × ) k . R = \exp([\theta]\times) = \sum_k \frac{\theta^k}{k!} ([u]\times)^k.R=exp([θ]×)=k∑​k!θk​([u]×)k.

为了找到闭式表达，我们写出[ u ] × [u]_\times[u]×​的几个幂：

[ u ] × 0 = I , [ u ] × 1 = [ u ] × , [ u ] 2 × = u u ⊤ − I , [ u ] × 3 = − [ u ] × , [ u ] 4 × = − [ u ] × 2 , … [u]^0_\times = I, \quad [u]^1_\times = [u]\times, \\ \quad [u]^2\times = u u^\top - I, \quad [u]^3_\times = -[u]\times,\\ \quad [u]^4\times = -[u]^2_\times, \dots[u]×0​=I,[u]×1​=[u]×,[u]2×=uu⊤−I,[u]×3​=−[u]×,[u]4×=−[u]×2​,…

可以发现所有幂都可以用I II、[ u ] × [u]\times[u]×或[ u ] 2 × [u]^2\times[u]2×的倍数表示。因此将级数重写为：

R = I + [ u ] × ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 − … ) + [ u ] 2 × ( 1 2 θ 2 − 1 4 ! θ 4 + 1 6 ! θ 6 − … ) , R = I + [u]\times \left(\theta - \frac{1}{3!} \theta^3 + \frac{1}{5!} \theta^5 - \dots \right) +\\ [u]^2\times \left(\frac{1}{2} \theta^2 - \frac{1}{4!} \theta^4 + \frac{1}{6!} \theta^6 - \dots \right),R=I+[u]×(θ−3!1​θ3+5!1​θ5−…)+[u]2×(21​θ2−4!1​θ4+6!1​θ6−…),

识别出正弦和余弦的级数后，得到闭式形式：

R = exp ⁡ ( [ u θ ] × ) = I + [ u ] × sin ⁡ θ + [ u ] × 2 ( 1 − cos ⁡ θ ) . R = \exp([u \theta]\times) = I + [u]\times \sin \theta + [u]^2_\times (1 - \cos \theta).R=exp([uθ]×)=I+[u]×sinθ+[u]×2​(1−cosθ).

该表达式即著名的 Rodrigues 旋转公式。通过大写指数函数可以直接使用：

R = Exp ( u θ ) = exp ⁡ ( [ u θ ] × ) . R = \text{Exp}(u \theta) = \exp([u \theta]_\times).R=Exp(uθ)=exp([uθ]×​).

2. 笛卡尔向量空间R m \mathbb{R}^mRm：

李代数中的元素τ ^ \hat \tauτ^具有非平凡的结构（例如斜对称矩阵、虚数、纯四元数，见表 I），但关键在于它们可以表示为某些基元素E i E_iEi​的线性组合，E i E_iEi​被称为m \mathfrak{m}m的生成元（它们是X XX在第i ii个方向原点附近的导数）。

因此，我们可以仅操作它们在R m \mathbb{R}^mRm中的坐标，记作τ \tauτ。我们可以通过两种互为逆的线性映射（也称为 hat 与 vee 映射）在m \mathfrak{m}m与R m \mathbb{R}^mRm之间转换（见图 6）：

Hat 映射：
Hat: R m → m ; τ ↦ τ ^ = ∑ i = 1 m τ i E i . (10) \text{Hat: } \mathbb{R}^m \to \mathfrak{m} ; \quad \tau \mapsto \hat \tau = \sum_{i=1}^m \tau_i E_i \ . \tag{10}Hat:Rm→m;τ↦τ^=i=1∑m​τi​Ei​.(10)

Vee 映射：
Vee: m → R m ; τ ^ ↦ τ ^ ∨ = τ = ∑ i = 1 m τ i e i , (11) \text{Vee: } \mathfrak{m} \to \mathbb{R}^m ; \quad \hat \tau \mapsto \hat \tau^\vee = \tau = \sum_{i=1}^m \tau_i e_i \ , \tag{11}Vee:m→Rm;τ^↦τ^∨=τ=i=1∑m​τi​ei​,(11)

其中e i e_iei​是R m \mathbb{R}^mRm的基向量（满足e ^ i = E i \hat e_i = E_ie^i​=Ei​）。

这意味着m \mathfrak{m}m与向量空间R m \mathbb{R}^mRm同构，即m ≅ R m \mathfrak{m} \cong \mathbb{R}^mm≅Rm，或者τ ^ ≅ τ \hat \tau \cong \tauτ^≅τ。

向量τ ∈ R m \tau \in \mathbb{R}^mτ∈Rm比它们在m \mathfrak{m}m中的同构元素τ ^ \hat \tauτ^更便于操作，因为它们可以堆叠成更大的状态向量，并且可以使用矩阵运算进行线性代数处理。在本文中，我们优先使用R m \mathbb{R}^mRm而非m \mathfrak{m}m，几乎所有定义的算子和对象（尤其是伴随、雅可比矩阵、扰动及其协方差矩阵等）都是在R m \mathbb{R}^mRm上进行的。

图 6. 流形M MM与其在原点T E M T_E MTE​M处切空间的表示之间的映射（李代数m \mathfrak{m}m和笛卡尔空间R m \mathbb{R}^mRm）。

映射hat( ⋅ ) ∧ (\cdot)^\wedge(⋅)∧和vee( ⋅ ) ∨ (\cdot)^\vee(⋅)∨是线性可逆映射或同构（公式 10–11），
exp ⁡ ( ⋅ ) \exp(\cdot)exp(⋅)和log ⁡ ( ⋅ ) \log(\cdot)log(⋅)将李代数映射到流形或从流形映射回李代数，
而Exp ( ⋅ ) \text{Exp}(\cdot)Exp(⋅)和Log ( ⋅ ) \text{Log}(\cdot)Log(⋅)是直接在向量空间R m \mathbb{R}^mRm与M MM之间映射的快捷方式。

图6

D. 指数映射

指数映射exp ⁡ ( ) \exp()exp()允许我们将李代数的元素精确地映射到李群上（见图 1），这一操作通常称为重traction。直观上，exp ⁡ ( ) \exp()exp()将切向量沿着流形包裹，沿大圆弧或测地线移动（就像把绳子绕在球上，见图 1、3 和 4）。逆映射为log ⁡ ( ) \log()log()，即解开操作。指数映射自然地来源于对流形上X ∈ M X \in MX∈M的时间导数的考虑，如下。

由公式 (9) 可得：

X ˙ = X v ∧ . (12) \dot{X} = X v^\wedge. \tag{12}X˙=Xv∧.(12)

对于常量v vv，这是一个常微分方程（ODE），其解为：

X ( t ) = X ( 0 ) exp ⁡ ( v ∧ t ) . (13) X(t) = X(0) \exp(v^\wedge t). \tag{13}X(t)=X(0)exp(v∧t).(13)

由于X ( t ) X(t)X(t)和X ( 0 ) X(0)X(0)都是群的元素，因此：

exp ⁡ ( v ∧ t ) = X ( 0 ) − 1 X ( t ) \exp(v^\wedge t) = X(0)^{-1} X(t)exp(v∧t)=X(0)−1X(t)

也必须属于群，所以exp ⁡ ( v ∧ t ) \exp(v^\wedge t)exp(v∧t)将李代数的元素v ∧ t v^\wedge tv∧t映射到李群。这就是所谓的指数映射。

为了给出更通用的定义，定义切向增量τ ≜ v t ∈ R m \tau \triangleq v t \in \mathbb{R}^mτ≜vt∈Rm为单位时间的速度，使得τ ∧ = v ∧ t ∈ m \tau^\wedge = v^\wedge t \in \mathfrak{m}τ∧=v∧t∈m是李代数中的一点。指数映射及其逆映射对数映射可以写为：

exp ⁡ : m → M ; τ ∧ ↦ X = exp ⁡ ( τ ∧ ) (14) \exp : \mathfrak{m} \to M ; \tau^\wedge \mapsto X = \exp(\tau^\wedge) \tag{14}exp:m→M;τ∧↦X=exp(τ∧)(14)

log ⁡ : M → m ; X ↦ τ ∧ = log ⁡ ( X ) (15) \log : M \to \mathfrak{m} ; X \mapsto \tau^\wedge = \log(X) \tag{15}log:M→m;X↦τ∧=log(X)(15)

乘法群中指数映射的闭式形式可通过绝对收敛的泰勒级数得到：

exp ⁡ ( τ ∧ ) = E + τ ∧ + 1 2 τ ∧ 2 + 1 3 ! τ ∧ 3 + … , (16) \exp(\tau^\wedge) = E + \tau^\wedge + \frac{1}{2} \tau^{\wedge 2} + \frac{1}{3!} \tau^{\wedge 3} + \dots , \tag{16}exp(τ∧)=E+τ∧+21​τ∧2+3!1​τ∧3+…,(16)

并利用τ ∧ \tau^\wedgeτ∧的代数性质（见例 4 和 5 中 SO(3) 与 S³ 的指数映射推导）求得对数映射。指数映射的关键性质为：

exp ⁡ ( ( t + s ) τ ∧ ) = exp ⁡ ( t τ ∧ ) exp ⁡ ( s τ ∧ ) (17) \exp((t+s)\tau^\wedge) = \exp(t\tau^\wedge) \exp(s\tau^\wedge) \tag{17}exp((t+s)τ∧)=exp(tτ∧)exp(sτ∧)(17)

exp ⁡ ( t τ ∧ ) = ( exp ⁡ ( τ ∧ ) ) t (18) \exp(t\tau^\wedge) = (\exp(\tau^\wedge))^t \tag{18}exp(tτ∧)=(exp(τ∧))t(18)

exp ⁡ ( − τ ∧ ) = ( exp ⁡ ( τ ∧ ) ) − 1 (19) \exp(-\tau^\wedge) = (\exp(\tau^\wedge))^{-1} \tag{19}exp(−τ∧)=(exp(τ∧))−1(19)

exp ⁡ ( X τ ∧ X − 1 ) = X exp ⁡ ( τ ∧ ) X − 1 , (20) \exp(X \tau^\wedge X^{-1}) = X \exp(\tau^\wedge) X^{-1}, \tag{20}exp(Xτ∧X−1)=Xexp(τ∧)X−1,(20)

其中公式 (20) 是一个令人惊讶且强大的性质，可通过展开泰勒级数并简化大量X − 1 X X^{-1}XX−1X项轻松证明。

1. 大写指数映射：大写Exp \text{Exp}Exp和Log \text{Log}Log映射是将向量元素τ ∈ R m ( ∼ T E M ) \tau \in \mathbb{R}^m (\sim T_E M)τ∈Rm(∼TE​M)直接映射到X ∈ M X \in MX∈M的便捷方法。即：

Exp : R m → M ; τ ↦ X = Exp ( τ ) (21) \text{Exp} : \mathbb{R}^m \to M ; \tau \mapsto X = \text{Exp}(\tau) \tag{21}Exp:Rm→M;τ↦X=Exp(τ)(21)

Log : M → R m ; X ↦ τ = Log ( X ) (22) \text{Log} : M \to \mathbb{R}^m ; X \mapsto \tau = \text{Log}(X) \tag{22}Log:M→Rm;X↦τ=Log(X)(22)

显然根据图 6：

X = Exp ( τ ) ≜ exp ⁡ ( τ ∧ ) (23) X = \text{Exp}(\tau) \triangleq \quad \exp(\tau^\wedge) \tag{23}X=Exp(τ)≜exp(τ∧)(23)

τ = Log ( X ) ≜ ( log ⁡ ( X ) ) ∨ (24) \tau = \text{Log}(X)\triangleq \quad (\log(X))^\vee \tag{24}τ=Log(X)≜(log(X))∨(24)

不同流形上这些映射的实现细节请参见附录。

图6

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

在群S 3 S^3S3中（回顾例 2，参见例如 [8]），单位范数条件q ∗ q = 1 q^* q = 1q∗q=1的时间导数为：

q ∗ q ˙ = − ( q ∗ q ˙ ) ∗ . q^* \dot{q} = - (q^* \dot{q})^*.q∗q˙​=−(q∗q˙​)∗.

这表明q ∗ q ˙ q^* \dot{q}q∗q˙​是一个纯四元数（其实部为零）。纯四元数u v ∈ H p u_v \in \mathbb{H}_puv​∈Hp​形式为：

u v = ( i u x + j u y + k u z ) v = i v x + j v y + k v z , u_v = (i u_x + j u_y + k u_z) v = i v_x + j v_y + k v_z,uv​=(iux​+juy​+kuz​)v=ivx​+jvy​+kvz​,

其中u = i u x + j u y + k u z u = i u_x + j u_y + k u_zu=iux​+juy​+kuz​是纯单位四元数，v vv是模长，i , j , k i, j, ki,j,k是李代数s 3 = H p s^3 = \mathbb{H}_ps3=Hp​的生成元。重写上述条件得：

q ˙ = q u v ∈ T q S 3 , \dot{q} = q u_v \in T_q S^3,q˙​=quv​∈Tq​S3,

其积分为：

q = q 0 exp ⁡ ( u v t ) . q = q_0 \exp(u_v t).q=q0​exp(uv​t).

令q 0 = 1 q_0 = 1q0​=1并定义ϕ ≜ u ϕ ≜ u v t \phi \triangleq u\phi \triangleq u v tϕ≜uϕ≜uvt，得到指数映射：

q = exp ⁡ ( u ϕ ) = ∑ k ϕ k k ! u k ∈ S 3 . q = \exp(u \phi) = \sum_k \frac{\phi^k}{k!} u^k \in S^3.q=exp(uϕ)=k∑​k!ϕk​uk∈S3.

u uu的幂次遵循模式1 , u , − 1 , − u , 1 , … 1, u, -1, -u, 1, \dots1,u,−1,−u,1,…，因此我们将项分组为1 11和u uu，并识别出cos ⁡ ϕ \cos \phicosϕ和sin ⁡ ϕ \sin \phisinϕ的级数，得到闭式表达：

q = exp ⁡ ( u ϕ ) = cos ⁡ ( ϕ ) + u sin ⁡ ( ϕ ) , q = \exp(u \phi) = \cos(\phi) + u \sin(\phi),q=exp(uϕ)=cos(ϕ)+usin(ϕ),

这是欧拉公式exp ⁡ ( i ϕ ) = cos ⁡ ϕ + i sin ⁡ ϕ \exp(i\phi) = \cos \phi + i \sin \phiexp(iϕ)=cosϕ+isinϕ的优美扩展。

李代数的元素ϕ = u ϕ ∈ s 3 \phi = u \phi \in s^3ϕ=uϕ∈s3可以通过 Hat 和 Vee 映射与旋转向量θ ∈ R 3 \theta \in \mathbb{R}^3θ∈R3对应：

Hat : R 3 → s 3 ; θ ↦ θ ∧ = θ 2 \text{Hat} : \mathbb{R}^3 \to s^3 ; \theta \mapsto \theta^\wedge = \frac{\theta}{2}Hat:R3→s3;θ↦θ∧=2θ​

Vee : s 3 → R 3 ; ϕ ↦ ϕ ∨ = 2 ϕ , \text{Vee} : s^3 \to \mathbb{R}^3 ; \phi \mapsto \phi^\vee = 2 \phi,Vee:s3→R3;ϕ↦ϕ∨=2ϕ,

其中因子 2 考虑了四元数在旋转作用x ′ = q x q ∗ x' = q x q^*x′=qxq∗中的双重效应。使用此 Hat 和 Vee，四元数指数映射

q = Exp ( u θ ) = cos ⁡ ( θ / 2 ) + u sin ⁡ ( θ / 2 ) q = \text{Exp}(u \theta) = \cos(\theta/2) + u \sin(\theta/2)q=Exp(uθ)=cos(θ/2)+usin(θ/2)

等价于旋转矩阵

R = Exp ( u θ ) . R = \text{Exp}(u \theta).R=Exp(uθ).

E. 加号和减号算子

加号（Plus）和减号（Minus）允许我们在（曲面）流形的元素之间引入增量，并将其表达在其（平坦的）切向量空间中。它们分别记为⊕ \oplus⊕和⊖ \ominus⊖，结合一次Exp/Log操作与一次组合操作。由于组合操作的非交换性，它们根据操作数的顺序定义了右侧和左侧版本。

右侧算子为（见图 4 右侧）：

right- ⊕ : Y = X ⊕ X τ ≜ X ∘ Exp ( X τ ) ∈ M (25) \text{right-}\oplus : Y = X \oplus ^X\tau \triangleq \quad X \circ \text{Exp}(^X\tau) \in M \tag{25}right-⊕:Y=X⊕Xτ≜X∘Exp(Xτ)∈M(25)

right- ⊖ : X τ = Y ⊖ X ≜ Log ( X − 1 ∘ Y ) ∈ T X M (26) \text{right-}\ominus : X\tau = Y \ominus X \triangleq \quad \text{Log}(X^{-1} \circ Y) \in T_X M \tag{26}right-⊖:Xτ=Y⊖X≜Log(X−1∘Y)∈TX​M(26)

由于在公式 (25) 中Exp ( X τ ) \text{Exp}(X\tau)Exp(Xτ)出现在组合的右侧，X τ X\tauXτ属于点X XX的切空间（见公式 (26)），我们约定X τ X\tauXτ在X XX的局部参考系中表示 —— 参考系用左上标表示。

左侧算子为：

left- ⊕ : Y = E τ ⊕ X ≜ Exp ( E τ ) ∘ X ∈ M (27) \text{left-}\oplus : Y = E\tau \oplus X \triangleq \quad \text{Exp}(E\tau) \circ X \in M \tag{27}left-⊕:Y=Eτ⊕X≜Exp(Eτ)∘X∈M(27)

left- ⊖ : E τ = Y ⊖ X ≜ Log ( Y ∘ X − 1 ) ∈ T E M (28) \text{left-}\ominus : E\tau = Y \ominus X \triangleq \quad \text{Log}(Y \circ X^{-1}) \in T_E M \tag{28}left-⊖:Eτ=Y⊖X≜Log(Y∘X−1)∈TE​M(28)

在公式 (27) 中Exp ( E τ ) \text{Exp}(^E\tau)Exp(Eτ)出现在左侧，因此E τ ∈ T E M ^E\tau \in T_E MEτ∈TE​M，我们称E τ ^E\tauEτ在全局参考系中表示。

注意，虽然左、右⊕ \oplus⊕可通过操作数顺序区分，但公式 (26) 和 (28) 中的⊖ \ominus⊖符号存在歧义。在本工作中，我们默认将扰动表达为局部坐标，因此默认使用右侧形式的⊕ \oplus⊕和⊖ \ominus⊖。

≜ \triangleq≜