Lio-sam导航算法的各因子详解

LIO-SAM的优化器（GTSAM）的工作就是找到一组状态XXX，使得所有加权的残差平方和最小，从而得到一个在所有可用传感器信息下最一致、最可能的状态估计。

1. 最终目标：非线性最小二乘问题

LIO-SAM最终求解的是一个非线性最小二乘问题，其通用表达式为：

其中：

XX 是所有待优化状态变量的集合（位姿、速度、偏差等）。

r∗ 是某种因子对应的残差函数，计算预测值与测量值之间的差异。

∣r∣Σ2=r⊤Σ−1r是马哈拉诺比斯范数，这里的 Σ是该因子测量值的协方差矩阵，其逆矩阵 Σ−1 被称为信息矩阵，直接决定了该因子在总体优化中的权重， 协方差越小（测量越确信），信息矩阵越大，该因子的权重就越高。

2. IMU预积分因子

功能： 约束相邻两个关键帧 i 和 j 之间的状态变化，并提供IMU偏差的估计。

状态变量：旋转、位置、速度、陀螺仪偏差、加速度计偏差

权重 ΣI： IMU的协方差矩阵，是一个15x15的矩阵，通常在预积分过程中通过递推得到，反映了IMU测量噪声（陀螺仪和加速度计的白噪声）在积分过程中传播所导致的不确定性，IMU通常具有非常高的置信度，因此其因子权重很高。

3. 激光里程计因子

功能： 通过当前帧点云与局部地图匹配，提供两个位姿 Ti 和 Tj 之间的相对变换约束。

状态变量：SE(3)位姿

残差函数 rL： 激光匹配算法（如点到面、点到线的ICP）会计算出一个相对位姿变换 ΔTij，残差定义为这个测量值与状态预测值之间的差异（在李群上计算）。

这里 Log() 是SE(3)群上的对数映射，将一个变换矩阵转换为一个6维向量（平移3维+旋转3维）。

权重 ΣL： 激光里程计的协方差 是一个6x6的矩阵，在LIO-SAM可能采用：

• 固定经验值： 根据传感器性能和匹配算法经验设定。
• 基于匹配的不确定性： 例如，通过评估点云匹配的拟合程度（点-面距离的方差）来近似估计协方差，激光匹配通常非常精确，但频率较低，其权重通常与IMU相当或略低。

4. GPS因子

功能： 提供绝对位置观测，防止导航漂移。

状态变量： SE(3)位姿

残差函数 rG： GPS测量提供的是一个3维全局位置 ，残差是状态估计的位置与GPS测量位置之差。

权重 ΣG： GPS的协方差是一个3x3的矩阵，通常设为对角阵，其对角线上的值（σx2,σy2,σz2）直接反映了对GPS信号的信任程度。

• 开阔环境： 协方差小，权重高，优化器会信任GPS。
• 城市峡谷/树林： 协方差大，权重低，优化器会更信任IMU和激光。
• LIO-SAM通常只在位置变化大于一定阈值时才加入GPS因子，避免过度约束。

5. 回环因子

功能： 当检测到回环时，校正累积误差。

状态变量： 历史位姿和当前位姿

残差函数 rLC： 与激光里程计因子形式完全一样，回环检测和匹配后，会得到一个相对变换 ΔTloop。

权重 ΣLC： 回环因子的协方差 ΣLC 也是一个6x6的矩阵，通常被赋予非常高的权重（即很小的协方差），因为回环匹配通常非常可靠，是校正长期漂移的最有力证据，一个强回环因子可以导致整个姿态图的显著调整。

6. 权重机制

核心思想： 不同传感器的可靠性和不确定性通过其对应的协方差矩阵 ΣΣ 自然地融入到优化问题中，这种权重机制是自适应和客观的，基于对传感器物理特性的理解，而不是手动调参。

• 高精度、高置信度的传感器（如IMU、成功匹配的激光）： 赋予小协方差（大信息矩阵），使其在优化中占据主导地位。
• 低精度、低置信度的传感器（如漂移的GPS）： 赋予大协方差（小信息矩阵），使其影响变小。