知识点得逻辑关系如下

引言

非线性问题由于复杂的数学结构，多样的解空间，局部极值等问题求解难度大大增加。所以在求解时需要把非线性问题转化为更容易处理的形式，例如

1. 数值优化方法：数值优化方法包括梯度下降、共轭梯度法、牛顿法、拟牛顿法等。这些方法可以用于求解非线性优化问题，例如最小化损失函数、拟合非线性模型等。
2. 迭代法：迭代法通过不断迭代更新来逼近非线性问题的解。例如，迭代法可以用于求解非线性方程组、求解微分方程的数值解等。
3. 泰勒展开法：泰勒展开法是一种将非线性函数近似为多项式的方法，通过保留函数在某一点的一阶、二阶甚至更高阶导数来实现。这种方法常用于局部近似和线性化处理。
4. 李群与李代数方法：对于具有连续对称性的对象的变换问题，李群与李代数方法提供了一种有效的工具。例如，在机器人路径规划、SLAM（同时定位与地图构建）等问题中，李群与李代数方法常用于处理非线性变换。
5. 数值模拟方法：对于复杂的非线性系统，数值模拟方法可以通过数值仿真来模拟系统的行为，并分析系统的稳定性和性能。
6. 符号计算方法：符号计算方法利用计算机代数系统进行符号计算，可以对非线性方程组进行求解、符号推导等操作。
7. 统计方法：统计方法包括贝叶斯推断、蒙特卡洛方法等，用于估计非线性模型的参数、进行概率推断等。
8. 机器学习方法：机器学习方法可以用于建立非线性模型，并通过训练数据来学习模型的参数和结构，例如神经网络、支持向量机、随机森林等。早期的SLAM问题被看为统计学问题，使用粒子滤波进行求解（Gmapping），但会存在粒子数目选择困难、样本退化、对传感器噪声敏感等问题。SLAM是非线性估计问题，将其转化为线性问题可以大大降低求解难度。但旋转矩阵是一种特殊的结构，旋转矩阵的乘积是一个典型的非线性变换，它涉及到角度的叠加，而角度的叠加是非线性的。虽然泰勒展开可以用来近似非线性函数，由于角度叠加的非线性特性和截断误差的影响，泰勒展开并不是一个很好的选择。旋转矩阵不符合加法特性，两个旋转矩阵相加不再是旋转矩阵，这意味着不能对旋转矩阵求导，求导的本质是衡量函数在给定点处的变化率，不能加法运算就不利于求导。旋转矩阵位于流形空间，流形空间中的每个点都对应一个局部线性空间，所以需要找到旋转矩阵对应的线性表达，这就涉及到了李群李代数的转换。

一、为啥用四元数

位姿的表达

SLAM的一个目标是求解位姿，而位姿是由平移和旋转构成，变换矩阵构成如下

• 旋转矩阵（Rotation Matrix）：3x3的旋转矩阵表示物体的旋转姿态。它描述了相机或机器人在空间中的方向变化。
• 平移向量（Translation Vector）：3维向量表示物体从一个位置平移到另一个位置的平移量。它描述了相机或机器人在空间中的位置变化。其中，R11到R33是3x3的旋转矩阵，描述了物体的旋转姿态；T1到T3是平移向量，描述了物体的位置变化。最后一行是[0 0 0 1]，用于保持齐次坐标的一致性。这时你会觉得，位姿这不很好被表达了，为什么要引入什么四元数、李群等等一大堆东西。我们需要注意一点就是表达和优化计算的区别，可以看到变换矩阵用9个变量表达旋转，在优化计算过程中是十分复杂的。因此需要一种简单的表达旋转的方法，我们会想到欧拉角。欧拉角( Euler Angles)将旋转分解为三个方向上的转动例，按Z-Y-X顺序转动轴可以是定轴或动轴，顺序亦可不同常见的有 : yaw-pitch-roll，这样表达旋转参数量大大减少。然而，欧拉角存在着一些奇异性，这些奇异性可能导致旋转表示的不唯一性和不连续性。1.万向锁（Gimbal Lock）： 当俯仰角（pitch）接近 ±90 度时，会出现万向锁的情况。在这种情况下，两个旋转轴会趋向于重合，导致旋转系统丧失了一个自由度。这样一来，物体的旋转将变得不稳定，且不再具有唯一性。2.奇异点（Singularities）： 欧拉角存在一些奇异点，即某些特定的姿态下，无法唯一地确定欧拉角的值。这些奇异点通常出现在欧拉角的转换公式中，导致计算过程中的不连续性和不确定性。3.旋转顺序的影响： 欧拉角的表示方式取决于旋转的顺序，不同的旋转顺序可能导致不同的结果。这种依赖旋转顺序的性质使得欧拉角的表示方式并不唯一。4.理论上可以证明，用3个实数来描述旋转问题都会出现奇异性问题，因为这个问题，欧拉角不适用与插值或者迭代。

四元数

旋转矩阵用9个量描述3自由度旋转具有冗余性，欧拉角虽然紧凑但存在奇异性，所以需要寻求一种新的旋转描述方式——四元数，它可以更有效地表示和计算旋转操作，并且相对于欧拉角等其他旋转表示方法，四元数不易受到奇异性的影响。首先需要知道复数可以表达旋转。复数域定义如下，对应了复数的平面坐标系称为复平面，x轴和y轴分别称为实轴和虚轴。当r等于1时称为单位复数。特别的我们拿i乘以一个复数，对应的几何意义是旋转90°。在知道了可以用复数表达旋转后，引入了四元数。四元数：三维情况下，四元数可作为复数的扩充，具有三个虚部和一个实部。四元数相比于角轴、欧拉角的优势：紧凑、无奇异性。且四元数与角轴之间可以相互转换。四元数与旋转向量之间的转换这下可以用四元数来表示旋转了，可以求解计算了吧。

二、为啥用李群和李代数

李群

但真实情况好像没那么简单，在SLAM中，要对位姿估计和优化，这避免不了插值、求导、迭代等操作。四元数微分的数学形式比较复杂，涉及到对四元数的实部和虚部的导数进行定义和计算。所以四元数只是表达比较方便，迭代优化还得用旋转矩阵（欧拉角有奇异性）。由旋转矩阵R和平移向量t可以构成如下矩阵这下子可以优化计算了吧，但还有一点问题。旋转矩阵不满足加法，旋转矩阵必须满足一些特定的性质，包括正交性和行列式等于1。当两个旋转矩阵相加时，它们的和可能不再满足这些性质。不满足相加就意味着不能微分，不能求导等操作，那还怎么优化。我们知道对于线性空间要满足加法和乘法，对于这种只满足其中一种运算的集合叫做群。对于连续性质的群就叫做李群。所以上述的SO(3)和SE(3)都是李群李群是一个群，即它包含了一组元素和一个群运算，通常是乘法或加法。群结构要求群运算满足封闭性、结合律、单位元素和逆元素等性质。

李代数

同时李群是流形，在数学中，流形是局部类似于欧几里得空间的空间，但在全局上具有更复杂的拓扑结构的对象。在流形上的每个点，我们可以定义一个与该点相切的线性空间，这个空间就是正切空间。正切空间是局部近似流形的一个线性空间，它包含了所有可能的在该点的切向量。正切空间可以看作是流形在某一点的“平坦”近似。根据流形的定义，我们知道李群可以在局部找到一个线性空间的同胚，且一一对应，同胚是在线性空间中，满足加乘的运算性质，就可以进行迭代优化了，那么现在问题就是如何找到这个一一对应的线性同胚。对于任意旋转矩阵R满足则A是一个反对称矩阵，对于任意的反对称矩阵，存在对应的唯一向量，该向量被称为反对称矩阵的旋转向量或轴向量。所以可以找到一个三维向量与之对应

指数与对数映射

求出李代数后，下一个问题是如何求解矩阵得导数，自然而然想到泰勒展开则虽然泰勒展开可以用来近似非线性函数，但在表示旋转矩阵的乘积时存在一些问题：

1. 角度叠加不可线性化：泰勒展开是将一个函数在某一点附近进行多项式展开的方法。对于一般的非线性函数，这种展开在某个点附近可以比较好地近似函数的局部行为。但是对于角度的叠加来说，由于角度的周期性和叠加特性，无法在某个点附近找到一个合适的线性化点。
2. 截断误差问题：泰勒展开通常是截断到一定阶数，截断后的高阶项会引入截断误差。对于旋转矩阵的乘积，特别是当乘积中的角度较大时，高阶项的影响可能会很大，导致泰勒展开的近似不准确。
3. 旋转矩阵的特殊结构：旋转矩阵是特殊的正交矩阵，它们具有一些特殊的性质，例如单位化、正交性等。这些特殊性质使得泰勒展开的近似更加复杂，因为展开后的矩阵必须满足这些额外的约束条件。是否对于任意的R都能找到一个唯一的ϕ?很遗憾，指数映射只是一个满射，并不是单射。这意味着每个SO(3)中的元素,都可以找到一个so(3)元素与之对应;但是可能存在多个so(3)中的元素，对应到同一个 SO(3)。但是，如果我们把旋转角度固定在 之间，那么李群和李代数元素是一一对应的。整理李群和李代数得转换关系如下

李代数求导

• 李代数与李群计算关系知道李代数和李群之间得对应关系是不够的，还需要知道怎么求导，以及李代数相加是怎么影响李群的。理想状态是李代数相加对应李群相乘
• 求导假设我们对一个空间点p进行了旋转，得到了Rp根据李代数定义求导扰动模型：根据李群定义求导知道怎么求导以及计算对应关系，就能进行迭代优化了。

总结

在众多SLAM系统中，采用四元数接收发布里程计，因其简单明了。在非线性优化过程中，旋转矩阵在流形上，需要找到对应得李代数进行求导，再作用到李群上进行迭代优化。