论文阅读《机器人状态估计中的李群》

摘要

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

在机器人估计技术中，通常不需要充分利用李群理论的能力，因此需要努力选择合适的阅读材料。在本文中，我们将通过李理论的最基本原理，目的是传达清晰和有用的想法，并留下一个重要的李理论语料库。即使有这样的缺陷，这里包含的材料已经被证明在机器人的现代估计算法中非常有用，特别是在SLAM，视觉里程计等领域。

除了这个微李理论之外，我们还提供了一个章节，其中包含一些应用示例，以及机器人中使用的主要李群的大量公式参考，包括大多数雅可比矩阵和容易操作它们的方法。我们还提供了一个新的C++模板库，实现了这里描述的所有功能。

1 介绍

在过去的几年里，机器人社区在正确地表述估计问题方面做出了显著的努力。这是由于对解决方案的精度、一致性和稳定性的要求越来越高。事实上，对状态和测量、与它们相关的函数及其不确定性进行适当的建模，对于实现这些目标至关重要。这导致了涉及所谓的“流形”的设计，在这种情况下，流形不亚于李群的光滑拓扑表面，状态表示在这里演变。依托李理论，我们能够构建一个严格的微积分语料库，以精确和轻松地处理不确定性、导数和积分。通常，这些工作集中在众所周知的旋转$SO(3)$和刚性运动$SE(3)$流形上。

当你第一次听说李群时，试着从不同的角度来看待它们是很重要的。拓扑学的观点，见图1，涉及流形的形状，并传达了它与切空间和指数映射关系的强大直觉。代数观点涉及群运算及其具体实现，允许利用代数性质来开发封闭形式的公式或简化它们。几何观点在机器人技术中特别有用，它将群元素与物体或参考系的位置、速度、方向和/或其他修改联系起来。原点坐标系可以用群的同一性来标识，流形上的任何其他点都代表某个“局部”坐标系。通过采用这些类比，李群的许多数学抽象可以更接近于向量空间、几何、运动学和其他更经典领域中的直观概念。

图1 李群与李代数关系的表示。李代数$T_{\epsilon }\mathcal{M}$（红色平面）是李群流形$\mathcal{M}$（这里用蓝色球体表示）在值$\epsilon$处的切空间。通过指数映射，经过李代数原点的直线$vt$，变为沿着流型自测地线的路径$exp(vt)$。相反的，李群中的每一个元素在李代数中都能找到一个等价。这种关系是如此深刻，以至于（几乎）群中的所有运算，它是弯曲的和非线性的，在李代数中有一个完全等价的，李代数是一个线性向量空间。虽然${\mathbb{R}}^3$中的球不是李群（我们只是把它作为一种可以在纸上画出来的表示），但${\mathbb{R}}^4$中的球是李群，并描述了单位四元数群——见图4和例5。

李理论绝不简单。为了对李理论可能是什么有一个最起码的概念，我们可以考虑以下三个参考文献。第一篇，Abbaspour的《基本李理论》，它有400多页。第二篇，Howe的《非常基本的李理论》，共24页（密集），有时被认为是必读的介绍。第三篇，更现代、更著名的Stillwell的《朴素李理论》有200多页。有了这些被标记为“基本”、“非常基本”和“朴素”的先例，这篇只有17页的论文的目的是进一步简化李理论（因此我们在标题中使用了形容词“微”）。我们有两种方法。首先，我们从LT中选择一小部分材料。这个子集是如此之小，以至于它仅仅是在探索LT的潜力。然而，它对于我们在机器人技术中处理的估计问题中的不确定性管理（例如惯性预积分，里程计和SLAM，视觉伺服等）似乎非常有用，从而实现最优优化器的优雅和严格设计。其次，我们以教学的方式进行解释，并带有大量冗余，以进一步缩小LT的理解差距，我们认为这仍然是必要的。也就是说，我们坚持朝着这个方向努力，命名一个典型的标题，Stillwell的，并提供一个更简化的版本。本文主体是通用的，尽管我们试图将抽象级别保持在最低限度。当应用于已知的群（旋转和运动矩阵、四元数等）时，插入的例子可以作为一般概念的基础。此外，大量带有冗长标题的图表再次解释了相同的概念。我们特别关注雅可比矩阵的计算（这个主题在中没有讨论），这对于大多数最优估计是必不可少的，也是设计新算法时的许多麻烦的来源。在这一章中，我们给出了机器人定位和建图的一些应用实例，实现了EKF和基于LT的非线性优化算法。最后，几个附录包含了机器人中最常用群的相关细节的大量参考：单位复数，四元数，2D和3D旋转矩阵，2D和3D刚性运动矩阵，以及平凡的平移群。

然而，我们对李理论最重要的简化是在范围方面。以下来自Howe的段落可以帮助我们说明我们留下的内容：“李理论的本质现象是，人们可以以一种自然的方式将李群$\mathcal{G}$与它的李代数$g$联系起来。李代数$g$首先是一个向量空间，其次被赋予一个称为李括号的双线性非结合积[…]。令人惊奇的是，李群$\mathcal{G}$几乎完全由李代数$g$和它的李括号决定。因此，在许多情况下，我们可以用李代数$g$来代替李群$\mathcal{G}$。因为李群$\mathcal{G}$是一个复杂的非线性对象，而李代数$g$只是一个向量空间，所以用李代数$g$来处理通常要简单得多。这是李理论力量的来源之一。”在[3]中，Stillwell甚至谈到了“李理论的奇迹”。在这项工作中，我们有效地将李代数降至第二平面，以支持其等效向量空间${\mathbb{R}}^n$，并且根本不引入李括号。因此，李群和它的李代数之间的联系将不会在这里做得像它应该做的那样深刻。我们的立场是，考虑到我们预见的目标应用领域，这种阅读材料通常是不必要的。此外，如果包括在内，那么我们将无法实现清晰和有用的目标。由于它们的抽象或微妙，读者将不得不进入数学概念，这是不必要的复杂。

我们的努力与最近其他关于该主题的工作一致，这些工作也确定了使LT更接近机器人专家的需求。我们的方法旨在使本文的目标受众熟悉：精通状态估计（卡尔曼滤波，基于图的优化等）的受众，但还不熟悉李理论。为此，我们在符号上做了一些改进，特别是在导数的定义上，使它接近向量的对应项，从而使链式法则清晰可见。如前所述，我们的选择实际上避免了李代数的材料，而更喜欢在它的同构切向量空间${\mathbb{R}}^n$上工作，这是我们最终表示不确定性或（小）状态增量的地方。所有这些步骤在精度和准确性上都绝对不会有任何损失，我们相信它们使得对LT的理解和其工具的操作变得更容易。

图2 流形$\mathcal{M}$和向量空间${\mathcal{T}}_{\mathcal{X}}\mathcal{M}$（在这种情况下$\cong {\mathbb{R}}^2$）相切于点$\mathcal{X}$。同时，也展示了侧切图。速度元素$\dot{\mathcal{X}}=\partial X/\partial t$不属于流形$\mathcal{M}$，但属于正切空间${\mathcal{T}}_{\mathcal{X}}\mathcal{M}$。

本文附带了一个新的开源C++头文件库，称为manif，可以在https://github.com/artivis/manif上找到。manif实现了广泛使用的群$SO(2)$，$SO(3)$，$SE(2)$和$SE(3)$，并支持创建解析雅可比矩阵。该库的设计是为了易用性、灵活性和性能。

2 微李理论

2.1 李群

李群既包含了群的概念也包含了光滑流形的概念：李群$\mathcal{G}$是一个光滑流形，它的元素满足群公理。在将这两个概念结合在一起之前，我们将简要介绍它们。

一方面，可微流形或光滑流形是局部类似于线性空间的拓扑空间。读者应该能够想象出流形的概念（图2）：它就像一个弯曲的、光滑的（超）表面，没有边缘或尖峰，嵌入在更高维度的空间中。在机器人技术中，我们说我们的状态向量在这个表面上演化，也就是说，流形描述或由施加在状态上的约束来定义。例如，具有单位范数约束的向量定义半径为1的球流形。流形的光滑性意味着在每一个点上存在唯一的切空间。这个空间是一个线性或向量空间，在这个空间上我们可以做微积分。

另一方面，群$(\mathcal{G},\circ )$是带有操作$\circ$的集合$\mathcal{G}$。对于$\mathcal{X},\mathcal{Y},\mathcal{Z}\in \mathcal{G}$，它满足如下定理，
$$closureunder\circ :\mathcal{X}\circ \mathcal{Y}\in \mathcal{G}\text{\hspace{1em}(1)}$$
$$identity\epsilon :\epsilon \circ \mathcal{X}=\mathcal{X}\circ \epsilon =\mathcal{X}\text{\hspace{1em}(2)}$$

$$inverse{\mathcal{X}}^{-1}:{\mathcal{X}}^{-1}\circ \mathcal{X}=\mathcal{X}\circ {\mathcal{X}}^{-1}=\epsilon \text{\hspace{1em}(3)}$$

$$结合性:(\mathcal{X}\circ \mathcal{Y})\circ \mathcal{Z}=\mathcal{X}\circ (\mathcal{Y}\circ \mathcal{Z})\text{\hspace{1em}(4)}$$

在李群中，流形在每个点上看起来都是一样的（例如在球面上，参见例1和例2），因此任何点上的所有切空间都是相似的。群结构规定流形元素的组合仍然在流形上，即公式(1)；并且每个元素在流形中也有一个逆，公式(3)。这些元素中的一个特殊元素是单位元，即公式(2)，因此切空间中的一个特殊元素是单位元处的切，我们称之为李群的李代数。李群结合了光滑流形的局部性质，使我们能够进行微积分，同时结合了群的全局性质，使我们能够对远处的物体进行非线性组合。

图3 $S^1$流形是平面$\mathbb{C}$中的单位圆（蓝色），其中单位复数存在$z\ast z=1$。李代数$s^1=T_{\epsilon }{S}^1$是虚数$i\mathbb{R}$的线（红色），任何切空间$T{S}^1$都同构于直线$\mathbb{R}$（红色）。切向量（红色段）包裹流形，形成圆弧（蓝色弧）。映射$exp$和$log$（箭头）将$i\mathbb{R}$的元素映射（包裹和展开）到$S^1$的元素（蓝色弧）。单位复数之间的增量通过复合和指数映射在切空间中表示（针对这个，我们将定义特殊算子$\oplus$、$\ominus$）。见正文解释，图4为类似的群。

示例1：单位复数群$S^1$。 李群的第一个例子是复数乘法下的单位复数群，这是最容易想象的（图3）。单位复数的形式是$z=cos\theta +isin\theta$。

action：向量$\mathbf{x}=x+iy$在平面上旋转一个角度$\theta$，通过复数乘法，${\mathbf{x}}^{\prime }=\mathbf{zx}$。

group facts：单位复数的乘积是一个单位复数，identity是1，其逆是共轭${\mathbf{z}}^{\ast }$。

manifold facts：单位范数约束定义了复平面上的单位圆（可以看作是1球，因此称为$S^1$）。这是二维空间中的一自由度曲线。单位复数在这个圆上随时间变化。群（圆）局部类似于线性空间（切线），但不是全局的。

2.2 group actions

重要的是，李群具有变换其他集合元素的能力，例如产生旋转、平移、缩放和它们的组合。它们被广泛应用于机器人技术中，无论是2D还是3D。

给定李群$\mathcal{M}$和集合$\mathcal{V}$，我们注意到$\mathcal{X}\cdot v$是$\mathcal{X}\in \mathcal{M}$对$v\in \mathcal{V}$的影响：

$$\cdot :\mathcal{M}\times \mathcal{V}\to \mathcal{V};(\mathcal{X},v)\mapsto \mathcal{X}\cdot v\text{\hspace{1em}(5)}$$

对于群运算$\cdot$，它必须满足以下公理：
$$identity:\mathcal{E}\cdot v=v\text{\hspace{1em}(6)}$$
$$兼容性:(\mathcal{X}\circ \mathcal{Y})\cdot v=\mathcal{X}\cdot (\mathcal{Y}\cdot v)\text{\hspace{1em}(7)}$$

常见的例子有旋转矩阵组$SO(n)$、单位四元数组和刚体运动组$SE(n)$。它们各自对向量的作用满足：
$$SO(n):rotationmatrix\mathbf{R}\cdot \mathbf{x}\triangleq \mathbf{Rx}$$
$$SE(n):Euclideanmatrix\mathbf{H}\cdot \mathbf{x}\triangleq \mathbf{Rx}+\mathbf{t}$$
$$S^1:单位复数\mathbf{z}\cdot \mathbf{x}\triangleq \mathbf{zx}$$
$$S^3:单位四元数\mathbf{q}\cdot \mathbf{x}\triangleq \mathbf{qx}{\mathbf{q}}^{\ast }$$

更详细的说明见表1和附录。

图4 在四元数空间$\mathbb{H}$中，流形$S^3$是一个单位的三维球体（蓝色）。对于单位四元数，满足${\mathbf{q}}^{\ast }\mathbf{q}=1$。李代数是纯虚四元素空间$ix+jy+kz\in {\mathbb{H}}_p$，它与超平面${\mathbb{R}}^3$（红色网格）同构。任何其他正切空间$T{S}^3$也同样与${\mathbb{R}}^3$同构。正切向量（红色段）将流形包裹在测地线（虚线）上。中间和右边的图形显示了通过测地线的侧切（注意它与图3中的$S^1$有多相似）。指数映射将${\mathbb{H}}_p$中的元素映射到$S^3$中；对数映射则相反。四元数的增量通过运算符$\oplus$、$\ominus$表示在正切空间中。

示例2：单位四元数群$S^3$。 李群的第二个例子，也相对容易可视化，是四元数乘法下的单位四元数群（图4）。单位四元数用$\mathbf{q}=cos(\theta /2)+\mathbf{u}sin(\theta /2)$表示，其中$\mathbf{u}=i{u}_x+j{u}_y+k{u}_z$为统一的轴，$\theta$为旋转角度。

Action：向量$\mathbf{x}=ix+jy+kz$通过双四元数积${\mathbf{x}}^{\prime }=\mathbf{qx}{\mathbf{q}}^{\ast }$在三维空间中绕单位轴$\mathbf{u}$旋转一个角度$\theta$。

Group fact：单位四元数的乘积是单位四元数。identity是1。单位四元数的逆是它的共轭${\mathbf{q}}^{\ast }$。

Manifold fact：单位范数约束定义了三维球面$S^3$，即四维空间中的球面三维曲面或流形。单位四元数在这个表面上随时间演化。群（球）局部类似于线性空间（切超平面${\mathbb{R}}^3\subset {\mathbb{R}}^4$），但不是全局。群构成(1)可被视为群对自身的作用，$\circ :\mathcal{M}\times \mathcal{M}\to \mathcal{M}$。另一个有趣的作用是伴随作用，我们将在第2.6节中看到。

2.3 正切空间和李代数

给定在李群流形$\mathcal{M}$上运动的点$\mathcal{X}(t)$，其速度$\dot{\mathcal{X}}=\partial \mathcal{X}/\partial t$属于在$\mathcal{X}$点处与$\mathcal{M}$相切的空间（图2），我们记为$T_{\mathcal{X}}\mathcal{M}$。流形的平滑性，即没有边或尖峰，意味着在每个点上存在唯一的正切空间。这种切空间的结构在任何地方都是一样的。

表1 二维和三维运动中的典型李群，包括平凡的${\mathbb{R}}^n$。参见附录以获得完整的参考资料。

 

图5 设点$\mathbf{z}\in S^1$以恒定的旋转速率$\omega$移动，$\mathbf{z}(t)=cos\omega t+isin\omega t$。它通过1和$\mathbf{z}$时的速度分别在切空间$T_1{S}^1$和$T_{\mathbf{z}}{S}^1$中。在$T_{\mathbf{z}}{S}^1$情况下，全局系下的速度表示为$\dot{\mathbf{z}}=\mathbf{z}i\omega =-\omega sin\omega t+i\omega cos\omega t$，局部系为${}^{\mathbf{z}}{\mathbf{v}}^{\wedge }=i\omega$。它们之间的联系为${}^{\mathbf{z}}{\mathbf{v}}^{\wedge }={\mathbf{z}}^{-1}\dot{\mathbf{z}}={\mathbf{z}}^{\ast }\dot{\mathbf{z}}$。在$T_1{S}^1$的情况下，它们之间的联系是${}^1{\mathbf{v}}^{\wedge }=\dot{\mathbf{z}}=i\omega$。显然，所有切空间的结构都是$i\mathbb{R}$，也就是李代数。这也是$\dot{\mathbf{z}}$在单位矩阵处的结构，这就是为什么李代数被定义为单位阵处的切空间。

李代数$\mathfrak{m}$。单位矩阵的切空间，$T_{\epsilon }\mathcal{M}$，称为$\mathcal{M}$的李代数，记为$\mathfrak{m}$，
$$李代数:\mathfrak{m}\triangleq T_{\epsilon }\mathcal{M}\text{\hspace{1em}(8)}$$
每个李群都有其对应的李代数。我们通过以下事实将李群与其李代数联系起来（见图1和图6）：

（1）李代数$\mathfrak{m}$是一个向量空间。