【图灵智库】机器人状态估计中的简明李群理论(一)

来源：arVix

作者：Joan Sola, Jeremie Deray, Dinesh Atchuthan

编译 : 李璟 湖南大学

审核：wyc

github: https://github.com/artivis/manif

这是泡泡图灵智库推送的第618篇文章，欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

摘要

李群是一个古老抽象的数学对象，可以追溯到十九世纪，多年后，在现在的机器人领域中，至少是在状态估计领域尤其是针对导航的运动估计，其扮演了越来越重要的角色。然而，对于绝大多数机器人专家来说，李群是高度抽象的结构，因此很难理解和使用。在状态估计中，我们不需要应用其全部的理论，因此需要从中挑选出我们需要的部分。在这篇文章中，我们将展示最基本的李群相关理论，旨在传达一些清晰有用的想法，为机器人状态估计领域留下一份重要的关于李群的资料。尽管存在缺陷，但这里包含的内容在机器人估计算法中非常有用，特别是在SLAM、视觉里程计等领域。除此之外，我们还提供了几个应用示例以及常用的公式，还展示了一个C++模板库，它实现了本文描述的所有功能。

一、背景与贡献

Fig1. 该图展示了李群与李代数之间的关系。李代数 (红色平面)是李群的流形 （蓝色表示）在幺元  处的切空间。通过指数映射，通过李代数上原点的每条直线路径 会产生围绕流形的路径 ，该路径沿着各自的测地线延伸。相反，群中的每个元素在李代数中都有一个等价元素。这种关系的意义是如此深刻，以至于（几乎）弯曲和非线性的群中的所有运算在李代数中都是完全等价的，而李代数是一个线性向量空间。这里需要说明的是 中的球体不是李群（我们只是用它来表示以使得其可以在纸上画出来），但是 中的球体是，它代表单位四元数。

借助李群的理论，我们可以建立一个严格的微积分语料库来处理状态估计问题中的不确定性、导数和积分。典型地，这些工作集中在旋转和刚性运动流形上。第一次接触李群时，尝试从不同的角度去看待它是十分重要的。从拓扑的角度来说，如Fig1，它描述了流形的形状，并直观地展示了李群与其切空间的关系以及什么是指数映射。从代数的角度来说，它涉及到群运算以及其具体实现，通过代数性质来推出封闭形式的公式或化简它们。从几何的角度来说（在机器人学中非常有用），它将群元素与本体或参考系的位置、速度、旋转相联系。初始系可能被定义为李群中的幺元，流形中的其它点代表着某一确定的局部系。通过这些类比，李群中许多抽象的数学概念可以更加接近向量空间、运动学和其他更经典的领域中的直观概念。李群绝不简单，为了掌握李群的基本概念，可以参考以下三份材料：1.Abbaspour的“Basic Lie theory”（400多页）；2.Howe的“Very basic Lie theory”（24页）；3.Stillwell的“Naive Lie theory”（200多页）。本篇论文只有17页，目的是进一步简化李群理论。

我们通过两种方式来实现这一点：1.我们只选取了李群中很小的一部分，但是这些内容对于机器人中的估计问题（如惯性预积分、里程计、SLAM、视觉伺服等）十分有用，它使得优化器严格和优雅的设计成为可能；2.我们使用一种类似教学的方式，用大量的冗余介绍来降低学习李群的门槛。正文是通用的，我们通过插入一些例子来介绍一些基础的概念，同时用大量的图表来再次解释相同的概念。我们特别关注雅可比的计算，这对于大多数优化器来说是必要的，也是设计新算法时许多麻烦的来源。我们在一章中提供了定位与建图的示例，基于李群完成了EKF和非线性优化算法的设计。最后，几个附录中包含机器人中常用的一些群的相关内容：单位复数、四元数、2D\3D旋转矩阵、2D\3D刚性运动矩阵与平移群。

在这项工作中，我们将位于 向量空间的李代数降为到二维平面进行表示，而且不会引入李括号的概念。之所以这样做是因为在我们预见的目标应用领域，这些内容不是必要的。如果将其包括在内，我们很难进行清晰有用的描述，读者将不得不进入数学概念，这些概念由于其抽象性或微妙性而使得问题变的复杂。

二、简明李群理论

A.李群

李群包含了群和光滑流形的概念：李群是一个光滑流行，它的元素满足群的公理。在将两个概念结合在一起之前，我们先简单介绍一下这两个概念。

Fig2. 流形 与其向量空间 在处相切。速度项 不属于流形 但属于其切向空间 。

一方面，可微或光滑流形是一个局部类似线性空间的拓扑空间。读者应当能够想象流形的形状（Fig2）,它就像一个弯曲的、光滑的（超）曲面，没有边缘或尖刺，嵌在更高纬度的空间里。在机器人学中，我们认为状态向量在这个曲面上变化，这就是说，流行由强加于状态的约束来定义或描述。例如，具有单位模长约束的向量定义了半径为1的球面流形。流形的光滑性意味着在每个点上存在唯一的切空间，这个空间是允许我们做微积分的线性或向量空间。

另一方面，群由集合和运算组成，对于，满足以下公理：

在李群中，流形上的每个点看起来都是一样的，因此任意点的切空间都是相似的。群的结构规定了群元素的运算仍保持在流形上（封闭性，等式1），存在一个特殊元素幺元（等式2）而且每个元素在流形上存在一个逆（等式3），因此存在一个特殊的切空间，它是群在幺元处的切空间，我们将之称之为李群的李代数。在本文中，为了简单起见，我们将李群称之为“流形”。

Fig3. 流形  是一个位于平面 上的单位圆（蓝色)，其中包含所有的单位复数  。李代数 是由虚数构成的直线 （红色），与 同构。切向向量（红色线段）通过指数映射包裹在流形上产生圆上的弧（蓝色弧）。 （箭头）将虚数  中的元素映射到  中的元素， 将  中的元素映射到虚数  中的元素。单位虚数在切空间中的增量通过复合和指数运算表示（类似Fig4, 我们将其定义为 和 ）。

Fig4. 流形  是四元数空间  中的一个三维单位球，其中包含了所有的单位四元数  。李代数位于纯虚四元数空间 ，与超平面  （红色网格）同构。切向向量（红色线段）通过指数映射包裹在流形上的大弧线或测地线（虚线上）。中间与右边的图显示了这个测地线的侧面切割（由图可见与 非常相似）。 （箭头）将  中的元素映射到  中的元素， 将  中的元素映射到  中的元素。四元数在切空间中的增量通过符号 和 表示。

B.群作用

李群的重要性之一在于其具有变换其他集合元素的能力，例如旋转、平移、缩放以及他们的组合。这些被广泛应用关于机器人技术，包括2D和3D。

给定一个李群和一个集合，我们将作用于记作，其中：

将  定义为一种群作用，它必须满足以下公理：

常见的例子包括旋转矩阵构成的群，单位四元数构成的群，刚性运动构成的群。它们对向量的作用满足(详细内容见table1与附录)：

群的合成可以视作群对自身的作用，还有一些有趣的作用如伴随作用，将在后文中看到。

C.切向空间与李代数

Fig5. 如图，点   以一恒定角速度  运动，有 。在通过  和  的时候，它们的速度分别位于不同的切向空间中： 和 。以  为例，在全局系下其速度为 ，在局部系下有 。二者有如下关系： 。在  处，二者相等：。显然，所有的切空间都是 ，也就是李代数。这也是  在幺元处的结构，这就是为什么李代数被定义为在幺元处的切空间。

Fig6.流行与其幺元处切空间的映射关系。小写 代表李代数与流行间的变换，大写 代表向量空间与流行之间的映射。

 在流形 上移动，其速度属于在处的切向空间记为（Fig2）。流形的光滑性，即没有边或尖峰，意味着在每个点都存在唯一的切空间。这样的切空间的结构在任何地方都是一样的。

1）李代数 :位于李群幺元处的切空间，称为李代数，记做：

每一个李群都有李代数与之相关联，我们将通过以下事实将李群与李代数联系起来（Fig1，Fig6）：

• 李代数是一个向量空间。因此，它的元素可以用中的向量来定义，其中代表的自由度。
• 指数映射，： ，精确地将李代数中的元素      转换为群中的元素，对数映射是逆运算。
• 通过线性变换，可以将处切空间中的向量转换到幺元处的切空间，这称作伴随变换。

李代数可以局部定义为以为切点建立的坐标系（Fig5）。我们用来修饰李代数中的元素，用左上标来指定其所属切向空间，如，。李代数的结构可以通过对群约束（逆）的时间微分来找到（例3,5），对于乘法群产生约束：，则李代数具有如下形式：

2）笛卡尔向量空间 ：李代数中的元素具有不一般的结构（斜对称矩阵，虚数，纯四元数，见table1）,但是对我们来说最关键的是它们可以表示为一些基元的线性组合，其中记做的生成元（它们是围绕原点在第个方向上的导数）。因此，将李代数写作中的向量是十分方便的，我们记做。我们可以通过两个互逆的映射，通常称作和，将转换到，反之亦然（fig6）。

其中为的基向量（有）。这意味着与向量空间同构，记做或者。对于我们来说，比更加方便，这是因为它们能够嵌入在更大的状态向量中，更重要的是，能够利用矩阵算子进行线性运算。在本文中，我们更倾向于使用，因此我们定义的大多数算子和对象（特别是：伴随矩阵、雅可比矩阵、扰动矩阵和协方差矩阵）都在上。

D.指数映射

指数映射能够让我们精确地将李代数中的元素转换到李群中（Fig1），这种操作通常称作。直观来说，指数映射将切平面中的元素沿着大弧线或测地线包裹在流行上（就想用绳子绕球，Fig1，3，4）。逆映射为，即展开操作。通过考虑关于时间的导数，可以很自然地推出指数映射，由公式（9）：

对于常数，这是一个常微分方程，它的解是：

由于都是群中的元素，那么一定也是群中的元素，因此将李代数中的元素映射到群中，这就是指数映射。为了提供更一般的指数映射的定义，我们定义，那么即为李代数中的一点。通过指数映射和对数映射有：

通过泰勒级数展开，得到乘法群中指数的封闭形式：

其关键性质包括：

其中公式（20）由泰勒展开可以很容易得到。

1）大写的指数映射 :

与小写的指数映射的关系（Fig6）：

E.加减运算

加减运算在（平坦的）切向量空间中表示（弯曲的）流行上元素之间的增量。分别用 和 表示，在每一次合成都包含了操作。由于合成的非交换性，根据操作数的顺序，它们被定义为右版本和左版本。右操作如下：

由于（25）式中的出现在合成式的右边，位于处的切空间：我们按照惯例称在的局部系中（注意我们在左上角标注的参考系）。左操作：

在式（27）中，位于左边，有：我们认为为全局系下的表示。

注意到左右可以通过操作数顺序区分，却无法区分。因此，在本文中，我们默认为局部扰动，即默认使用右操作形式。

F.伴随和伴随矩阵

Fig7.图中有两条路径， 和 ，连接了原点  和点  。他们分别是局部系和全局系下的增量与  的组合。由于非交换性，  和  并不相等。他们对应的切空间中的向量  和  也不相等。它们通过线性变换相关联：，其中  是  在  处的伴随矩阵。

如果令（25）（27）中的为幺元，那么有。这代表局部和全局切空间中元素的转换关系（Fig7）。由（20）（25）（27）有：

1) 伴随： 我们定义流形在处的伴随作用，记为：

因此。这定义了群在其李代数上的伴随作用。伴随有两个有趣的性质：

*2) 伴随矩阵：*由于是线性的作用，因此我们能够找到等价的矩阵算子  来映射位于笛卡尔切空间中的向量 和：

我们称其为伴随矩阵。可以通过对式（29）执行来计算：

将右边展开可以得到伴随矩阵（见附录和例6）。伴随矩阵的其他性质包括：

注意（33）（34）中左边的计算通常比右边更简单。我们通常将伴随矩阵用作将切空间处向量转换到原点处切空间向量的方法，即。

G.李群求导

在定义李群导数的众多方式中，我们关注于以雅克比矩阵映射切向空间向量的形式。在本文中这就已经足够了，因为在这些空间中，不确定性和增量可以正确和轻松地定义。使用这些雅克比矩阵，李群中关于不确定性管理的公式将在很大程度上类似于向量空间中的公式。

下文描述的雅克比矩阵满足链式法则，因此我们可以很容易地从关于逆、合成、指数、作用 的部分雅克比推导出任意雅克比。

1) 关于向量空间的雅克比： 对于多元函数，雅克比定义为的矩阵：

用下面这个形式定义雅克比矩阵很方便，令，其中是第列向量。列向量对应：

其中是的第个自然基向量。关于分子，注意到（37）是当在方向进行扰动时的变化，并且相应的雅克比正好是。

在本项工作中，为了方便起见，我们引入了紧凑形式：

其中。式（38）只是一个方便的符号（就像（35）一样），因为的除法是未定义的，正确的计算需要式（36）。然而，通过将分子展开乘关于的线性形式，并且将左边认为是雅克比矩阵，则有：

其中关于的小量，具有线性近似：

Fig8.函数  的右雅克比：基向量方向上的扰动  通过 得到扰动向量 ，。通过变化  ,在  中的扰动  （粗红色线）在  上沿着测地线产生了一些路径（蓝色）。在  中，由于的非线性，从  中映射过来的路径（蓝色实线）通常不位于测地线（蓝色虚线）上。这些映射路径投射至切向空间  ，产生平滑的曲线路径（细红色实线）,得到扰动向量  。雅克比的列向量  。每一个  都会对应得到  。因此，雅克比矩阵将  线性映射到  。

*2) 关于李群的右雅克比：*受到式（38）的启发，我们可以使用和定义流行上函数的雅克比（Fig8）。用右版本代替，我们可以得到一个类似于标准导数的形式：

进一步得到：

我们称之为的右雅克比。注意（41c）是复杂函数的标准导数。把它写成（41a）更加直观：它是关于的导数，只不过我们使用正切空间中的无穷小变化来进行表示。事实上，多亏了右版本的运算符，和的变化能够表示为局部切空间中的向量，其中切空间分别处于和。因此，这个导数是一个的雅克比矩阵，它代表局部切向空间之间的映射：。类似于向量空间，这个矩阵的列对应于方向导数。

（42）表示在方向上变化时的变化。它对应的雅克比的列是。

我们使用（41a）来计算雅克比。例如，对于3D旋转，有，因此：

注意，当函数从一个流形传递到另一个流形上时，（41a）中的也需要进行改变：使用定义域的，使用值域的。对于微小量，如下近似成立：

Fig9.该图展示了  中涉及的所有线性映射。线性映射 ，构成了循环映射（实线），即公式（46）。交叉雅克比（虚线）构成更多的循环映射，即式（47）（48）。

3) 关于李群的左雅克比： 使用左版本的，可以计算左雅克比：

注意此时，分子属于，因此左雅克比是映射全局切空间的矩阵：。对于小量，下式成立：

由（32）（43）（40）可知左右雅克比之间通过伴随矩阵相关联（Fig9）：

4) 交叉左右雅克比： 同样可以使用右加左减来定义雅克比，反之亦然。这在进行局部切空间与全局切空间之间的映射时是很有用的。我们可以通过伴随将其与其他雅克比联系起来：

这里，上下的注脚表示在什么参考系下进行求导。相应对于小量：

5) 流形中的不确定性，协方差矩阵的传播

Fig10.点  周围的不确定性被表示为该点切向空间上的协方差（红色）。使用  (式51)，切向空间中的概率椭圆包裹在流形上（蓝色），从而说明了在群上的概率集中区域。

定义在点处切空间中的局部扰动，使用右:

协方差矩阵在这个切空间中可以通过标准的期望算子进行计算：

由此定义流形上的高斯变量，如Fig10。注意，尽管我们写作，但是协方差是指切空间中扰动的协方差。由于切空间的维度与流行的自由度相匹配，所以这些协方差的定义是明确的。

扰动也可以在全局参考系中表示，即在幺元处的切空间中，使用左:

那么（52）中的协方差定义将使用左，这提供了全局的协方差矩阵定义。例如，对于已知水平面内的3D旋转可以关联至协方差矩阵。这是因为“水平”是一个全局的定义，因此必须定义在全局系下。但事实上，全局和局部扰动的协方差可以通过伴随矩阵进行关联：

协方差通过函数的线性化进行传播：6) 流形上的离散积分Fig11.流形上的运动积分。每一次运动数据都产生 ，包裹在流形上成为局部增量，然后与 组合得到 。

指数映射以恒定速度在流形上进行连续时间的积分。对于非恒定速度通常通过在短时间内认为速度恒定对其进行分段得到，得到离散积分：同样（Fig11），我们可以定义，将积分构造为离散的一系列切空间小量的“和”：。递归的形式写作：常见的例子是将三维角速度积分到旋转矩阵或者积分到四元数。