建设网站公司前景wordpress侧边小图标联系方式

建设网站公司前景,wordpress侧边小图标联系方式,姜堰哪里有网站建设的,wordpress科技模板摘要 李群#xff08;Lie group#xff09;是一种历史悠久的数学抽象对象#xff0c;其理论可追溯到19世纪#xff0c;当时数学家 Sophus Lie 奠定了连续变换群理论的基础。此后多年#xff0c;李群的影响逐渐扩展到科学与技术的诸多领域。近年来#xff0c;在机器人领域…摘要李群Lie group是一种历史悠久的数学抽象对象其理论可追溯到19世纪当时数学家 Sophus Lie 奠定了连续变换群理论的基础。此后多年李群的影响逐渐扩展到科学与技术的诸多领域。近年来在机器人领域尤其是在状态估计问题中并且在导航中的运动估计方面我们正在见证李群使用上的一个重要趋势。然而对于绝大多数机器人研究者而言李群仍然是一种高度抽象的数学结构因此难以理解和实际应用。在机器人领域的估计问题中往往并不需要充分动用李群理论的全部能力因此有必要对相关内容进行取舍和筛选。本文将围绕李群理论中最基础的原理展开旨在传达清晰且实用的思想而将相当一部分更为完整的李群理论内容置于一旁。尽管如此即便经过这样的“裁剪”本文所涵盖的材料在现代机器人估计算法中仍然被证明极具价值尤其是在 SLAM、视觉里程计等领域。在这套“微型李群理论”的介绍之外我们还提供了一章包含若干应用示例的内容并汇总了机器人领域中常用的主要李群的公式参考其中包括大部分雅可比矩阵及其便捷的操作方式。此外本文还介绍了一个全新的、仅使用模板的 C 库实现了文中所描述的全部功能。I. 引言近年来机器人学界在对估计问题进行规范化建模方面投入了大量努力。这一趋势源于人们对解的精度、一致性以及稳定性不断增长的需求。事实上只有对状态和观测、它们之间的函数关系以及相应的不确定性进行恰当建模才能实现这些目标。由此人们开始采用所谓的流形manifolds进行系统设计——在这里流形并非抽象的概念而是李群Lie groups所对应的光滑拓扑曲面状态表示正是在这些曲面上演化的。借助李理论Lie TheoryLT我们能够构建一套严格而系统的微积分工具用以精确而方便地处理不确定性、导数以及积分问题。通常这类研究主要集中在众所周知的旋转群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。李理论绝非简单。若要对其内容有一个最低限度的认识可以参考以下三本著作Abbaspour 的《Basic Lie theory》[1]全书超过 400 页Howe 的《Very basic Lie theory》[2]虽仅有 24 页但内容高度凝练常被视为必读的入门文献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/manifmanif实现了机器人领域中广泛使用的群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 TheoryA. 李群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群运算需满足以下公理封闭性ClosureX ∘ Y ∈ G . (1) X \circ Y \in G . \tag{1}X∘Y∈G.(1)单位元IdentityE EEE ∘ X X ∘ E X . (2) E \circ X X \circ E X . \tag{2}E∘XX∘EX.(2)逆元InverseX − 1 X^{-1}X−1X − 1 ∘ X X ∘ X − 1 E . (3) X^{-1} \circ X X \circ X^{-1} E . \tag{3}X−1∘XX∘X−1E.(3)结合律Associativity( X ∘ Y ) ∘ Z X ∘ ( Y ∘ Z ) . (4) (X \circ Y) \circ Z X \circ (Y \circ Z) . \tag{4}(X∘Y)∘ZX∘(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 .zcosθisinθ.作用Action向量x x i y x x i yxxiy可通过与单位复数相乘而在平面中旋转一个角度θ \thetaθ即x ′ z x . x zx .x′zx.群的性质Group facts单位复数的乘积仍然是单位复数群的单位元为1 11群中元素的逆为其共轭z − 1 z ∗ . z^{-1} z^* .z−1z∗.流形的性质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∗z1的单位复数。李代数s 1 T E S 1 \mathfrak{s}^1 T_E S^1s1TE​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}),qcos(2θ​)usin(2θ​),其中u i u x j u y k u z \mathbf{u} i u_x j u_y k u_zuiux​juy​kuz​是一个单位旋转轴θ \thetaθ为旋转角。作用Action向量x i x j y k z \mathbf{x} i x j y k zxixjykz可通过双四元数乘积即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−1q∗.流形的性质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∗q1的单位四元数。其李代数是纯虚四元数空间i x j y k z ∈ H p ix jy kz \in \mathbb{H}_pixjykz∈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⋅成为一个群作用它必须满足以下公理单位元IdentityE ⋅ v v (6) E \cdot v v \tag{6}E⋅vv(6)兼容性Compatibility( X ∘ Y ) ⋅ v X ⋅ ( Y ⋅ v ) . (7) (X \circ Y) \cdot v X \cdot (Y \cdot v) \ . \tag{7}(X∘Y)⋅vX⋅(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≜RxS E ( n ) SE(n)SE(n)欧几里得矩阵H ⋅ x ≜ R x t H \cdot x \triangleq Rx tH⋅x≜RxtS 1 S_1S1​单位复数z ⋅ x ≜ z x z \cdot x \triangleq zxz⋅x≜zxS 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}mLie 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 mapexp ⁡ : 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ωtisinω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ωtiω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˙−1X0,它适用于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⊤RI.切空间可以通过对该约束取时间导数得到即R ⊤ R ˙ R ˙ ⊤ R 0 , R^\top \dot{R} \dot{R}^\top R 0,R⊤R˙R˙⊤R0,我们可以重排为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 IRI时我们得到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 3m3。李代数是一个向量空间其元素可以分解为[ ω ] × ω 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,[ω]×​↦[ω]∨ω.例 4SO(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.Rexp([θ]×)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),RI[u]×(θ−3!1​θ35!1​θ5−…)[u]2×(21​θ2−4!1​θ46!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).Rexp([uθ]×)I[u]×sinθ[u]×2​(1−cosθ).该表达式即著名的 Rodrigues 旋转公式。通过大写指数函数可以直接使用R Exp ( u θ ) exp ⁡ ( [ u θ ] × ) . R \text{Exp}(u \theta) \exp([u \theta]_\times).RExp(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之间转换见图 6Hat 映射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_{i1}^m \tau_i E_i \ . \tag{10}Hat:Rm→m;τ↦τ^i1∑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_{i1}^m \tau_i e_i \ , \tag{11}Vee:m→Rm;τ^↦τ^∨τi1∑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–11exp ⁡ ( ⋅ ) \exp(\cdot)exp(⋅)和log ⁡ ( ⋅ ) \log(\cdot)log(⋅)将李代数映射到流形或从流形映射回李代数而Exp ( ⋅ ) \text{Exp}(\cdot)Exp(⋅)和Log ( ⋅ ) \text{Log}(\cdot)Log(⋅)是直接在向量空间R m \mathbb{R}^mRm与M MM之间映射的快捷方式。图6D. 指数映射指数映射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;τ∧↦Xexp(τ∧)(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​τ∧23!1​τ∧3…,(16)并利用τ ∧ \tau^\wedgeτ∧的代数性质见例 4 和 5 中 SO(3) 与 S³ 的指数映射推导求得对数映射。指数映射的关键性质为exp ⁡ ( ( t s ) τ ∧ ) exp ⁡ ( t τ ∧ ) exp ⁡ ( s τ ∧ ) (17) \exp((ts)\tau^\wedge) \exp(t\tau^\wedge) \exp(s\tau^\wedge) \tag{17}exp((ts)τ∧)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项轻松证明。大写指数映射大写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;τ↦XExp(τ)(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)显然根据图 6X Exp ( τ ) ≜ exp ⁡ ( τ ∧ ) (23) X \text{Exp}(\tau) \triangleq \quad \exp(\tau^\wedge) \tag{23}XExp(τ)≜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∗q1的时间导数为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​)vivx​jvy​kvz​,其中u i u x j u y k u z u i u_x j u_y k u_zuiux​juy​kuz​是纯单位四元数v vv是模长i , j , k i, j, ki,j,k是李代数s 3 H p s^3 \mathbb{H}_ps3Hp​的生成元。重写上述条件得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).qq0​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.qexp(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),qexp(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)qExp(uθ)cos(θ/2)usin(θ/2)等价于旋转矩阵R Exp ( u θ ) . R \text{Exp}(u \theta).RExp(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-⊕:YX⊕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-⊕:YEτ⊕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≜