LIO-SAM

2020

Paper: LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

Code: https://github.com/TixiaoShan/LIO-SAM

LOAM + IMU 融合 + 因子图优化 + 滑动窗口 的“进阶版”。它把激光和惯导紧耦合在一个全局优化框架里，既保证实时性，又能方便地插入 GPS、回环等绝对约束来抑制漂移

背景

LOAM 局限：依赖全局特征地图配准，地图稠密时回环难插入、融合 GPS 等绝对测量不方便；漂移累积快

LIO‑SAM 思路：

  • 用 IMU 预积分先去除扫描畸变（deskew），并给激光里程计初值
  • 把 IMU、激光、GPS、回环等都建成因子，放进因子图统一优化（iSAM2 增量平滑）
  • 滑动窗口 + 局部子地图配准：新关键帧只与固定数量的近邻子关键帧匹配，降低实时优化开销

系统结构

四类因子 + 一类状态节点

状态节点

在$i$时刻系统的状态向量为:

\[\mathbf{x}_i = \begin{bmatrix} \mathbf{p}_i & \mathbf{v}_i & \mathbf{q}_i & \mathbf{b}^a_i & \mathbf{b}^g_i \end{bmatrix}\]

其中 $p_i \in \mathbb{R}^3 $为世界坐标系位置; $v_i \in \mathbb{R}^3 $为世界坐标系速度; $q_i \in \mathbb{H} $ 为IMU到世界的姿态四元数; $ b_i^a, b_i^g $ 分别为加速度计、陀螺仪零偏

IMU预积分因子

IMU 给的是高频原始测量：角速度 $\omega_t$ 和线加速度 $a_t$ 。如果每来一帧激光都用全部 IMU 数据做积分，会不断重复积分计算，开销大。预积分的思路是：把两个关键帧之间的所有 IMU 测量先在本地坐标系里积成一个“相对位姿、速度、零偏变化”的包，再作为一个因子加入因子图。这样在图优化时，就不用重新积分原始 IMU 序列，只需在零偏变化时做一次轻量级的“修正积分”

连续时间运动模型

世界坐标系下, IMU 动力学方程:

\[\dot{\mathbf{p}} = \mathbf{v}\] \[\dot{\mathbf{v}} = \mathbf{R}(\mathbf{q}) \left( \mathbf{a}_m - \mathbf{b}^a - \mathbf{n}^a \right) + \mathbf{g}\] \[\dot{\mathbf{q}} = \frac{1}{2} \,\mathbf{q} \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_m - \mathbf{b}^g - \mathbf{n}^g \end{bmatrix}\]

其中 $ a_m, w_m $ 为IMU原始测量; $ n^a, n^g $ 为高斯白噪声; $ R\left( q \right) $ 为四元数对应旋转矩阵; $ g $ 为重力加速度

预积分变量定义

在关键帧 $i$ 与 $j$ 间累积的预积分量:

  • 位置增量（IMU 本体系）$\Delta p_{ij}$
  • 速度增量（IMU 本体系） $\Delta v_{ij}$
  • 姿态增量 $\Delta R_{ij}$

它们通过消除零偏初值影响，被定义为去偏后的积分

离散积分形式（采样间隔 $ \Delta t_k $ ）:

\[\Delta \mathbf{R}_{ij} = \prod_{k=i}^{j-1} \exp\!\left[ \left(\boldsymbol{\omega}_m^k - \mathbf{b}^g_i\right)\,\Delta t_k \right]\] \[\Delta \mathbf{v}_{ij} = \sum_{k=i}^{j-1} \mathbf{R}_{ik} \left( \mathbf{a}_m^k - \mathbf{b}^a_i \right) \,\Delta t_k\] \[\Delta \mathbf{p}_{ij} = \sum_{k=i}^{j-1} \mathbf{v}_{ik} \,\Delta t_k + \frac12 \,\mathbf{R}_{ik} \left( \mathbf{a}_m^k - \mathbf{b}^a_i \right) \,\Delta t_k^2\]

这里 $ R_{ik} $、$ v_{ik} $ 都是在 IMU 初始坐标系下累积

因子残差

在因子图里，IMU 预积分因子连接两个状态节点 $ x_i $ 和 $ x_j $ , 残差向量为:

\[\mathbf{r}_{\text{IMU}} = \begin{bmatrix} \mathbf{R}_i^\top \left( \mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i \Delta t_{ij} - \frac12 \mathbf{g} \Delta t_{ij}^2 \right) - \Delta \mathbf{p}_{ij} \\ \mathbf{R}_i^\top \left( \mathbf{v}_j - \mathbf{v}_i - \mathbf{g} \Delta t_{ij} \right) - \Delta \mathbf{v}_{ij} \\ \mathrm{Log}\!\left[ \left( \Delta \mathbf{R}_{ij} \cdot \exp(\mathbf{J}^g \,\delta \mathbf{b}^g) \right)^\top \left( \mathbf{R}_i^\top \mathbf{R}_j \right) \right] \\ \mathbf{b}^a_j - \mathbf{b}^a_i \\ \mathbf{b}^g_j - \mathbf{b}^g_i \end{bmatrix}\]

前两行是位置、速度预测误差（都转回 IMU $i$ 系下比较）

第三行是姿态误差，用李群上的对数映射求旋转差

最后两行是零偏随机游走约束

其中 $ \delta $ 表示在线性化点附近的“小增量/扰动”，主要用于当零偏估计发生变化时对预积分量做一阶校正

$ J^g $ 是预积分对陀螺零偏的一阶雅可比，用来在线性化时快速修正预积分量

零偏变化快速矫正

典型地，对陀螺和加计零偏有:

\[\delta b^g = b^g - b_{\mathrm{lin}}^g, \space\space\space \delta b^a = b^a - b_{\mathrm{lin}}^a\]

预积分量对零偏的一阶近似为

\[\Delta \mathbf{R}_{ij}(\mathbf{b}^g) \approx \Delta \mathbf{R}_{ij}(\mathbf{b}^g_{\text{lin}}) \cdot \exp\!\big(\mathbf{J}^g\,\delta\mathbf{b}^g\big)\] \[\Delta \mathbf{v}_{ij}(\mathbf{b}^a,\mathbf{b}^g) \approx \Delta \mathbf{v}_{ij} + \mathbf{J}^{v}_{\mathbf{b}^a}\,\delta\mathbf{b}^a + \mathbf{J}^{v}_{\mathbf{b}^g}\,\delta\mathbf{b}^g\] \[\Delta \mathbf{p}_{ij}(\mathbf{b}^a,\mathbf{b}^g) \approx \Delta \mathbf{p}_{ij} + \mathbf{J}^{p}_{\mathbf{b}^a}\,\delta\mathbf{b}^a + \mathbf{J}^{p}_{\mathbf{b}^g}\,\delta\mathbf{b}^g\]

$ J^{\left ( \cdot \right ) } $ 是预积分对零偏的一阶雅可比。直觉上，$ \delta $ 就是“从当前线性化点到最新估计的偏移”，用来快速修正预积分而无需重积分

另外，在整体优化中也常写全局增量 $ \delta x $ 表示对状态的高斯–牛顿/LM 小步更新，这与上面的偏置增量是同一类“线性化周围的小扰动”思想

协方差 & 线性化

预积分过程同时递推协方差 $ P $ 和雅可比 $ J_{b^a}, J_{b^g} $:

\[\mathbf{P}_{k+1} = \mathbf{F}_k \,\mathbf{P}_k \,\mathbf{F}_k^\top + \mathbf{G}_k \,\mathbf{Q} \,\mathbf{G}_k^\top\]

其中 $ Q $ 为加速度计、陀螺仪噪声、零偏随机游走噪声协方差。这样一旦零偏估计在优化中发生变化，就可以用一阶近似修正：

\[\Delta \mathbf{p} \leftarrow \Delta \mathbf{p} + \mathbf{J}_{\mathbf{b}^a} \delta \mathbf{b}^a + \mathbf{J}_{\mathbf{b}^g} \delta \mathbf{b}^g\]

速度、姿态的变化量 $ \Delta v, \Delta R $ 同理。

因子目标函数

优化目标是最小化各个部分残差其加权二乘（马氏距离）之和。对于每一个 $ \left ( i,j \right ) $对，有：

\[\min \; \frac{1}{2}\,\mathbf{r}_{ij}^{\top}\,\mathbf{\Sigma}_{ij}^{-1}\,\mathbf{r}_{ij}, \quad \mathbf{r}_{ij} = \begin{bmatrix} \mathbf{r}_p\\ \mathbf{r}_v\\ \mathbf{r}_R\\ \mathbf{r}_{b^a}\\ \mathbf{r}_{b^g} \end{bmatrix}\]

整个因子图的目标函数就是对所有因子求和：

\[\min_{\{\mathbf{x}\}}\; \sum_{(i,j)\in \mathcal{E}_{\text{IMU}}} \frac{1}{2}\,\mathbf{r}_{ij}^{\top}\,\mathbf{\Sigma}_{ij}^{-1}\,\mathbf{r}_{ij}\]

在因子图/贝叶斯图中，因子是把若干状态变量联系起来的概率约束；IMU 预积分因子就是“把 i 到 j 两关键帧之间的整段 IMU 原始测量压缩成一个高斯似然”的约束。其测量模型可写为

\[p\big(\mathcal{Z}^{\text{IMU}}_{ij} \mid \mathbf{x}_i,\mathbf{x}_j\big) \;\propto\; \exp\!\left( -\tfrac{1}{2}\,\mathbf{r}_{ij}^{\top}\,\mathbf{\Sigma}_{ij}^{-1}\,\mathbf{r}_{ij} \right)\]

其中：

  • $ \mathbf{x}_i={\mathbf{p}_i,\mathbf{v}_i,\mathbf{q}_i,\mathbf{b}^a_i,\mathbf{b}^g_i}, \quad \mathbf{x}_j={\mathbf{p}_j,\mathbf{v}_j,\mathbf{q}_j,\mathbf{b}^a_j,\mathbf{b}^g_j} $
  • $ \mathcal{Z}^{\text{IMU}}{ij} $ 是i到j之间IMU原始序列，被压缩为 $ \Delta R{ij}, \Delta v_{ij}, \Delta p_{ij} $ 等“预积分测量”
  • $ {\sum}_{ij} $ 来自预积分过程对噪声的传播（离散–线性化后迭代的 $P$）

把它放到最大后验估计中，取负对数似然就得到标准的加权非线性最小二乘目标

里程计因子

从 deskew 后的点云提取 Edge / Plane 特征，选择关键帧（位移 > 1 m 或转角 > 10°）。

建立局部子地图（最近关键帧 n=25 ），分别生成边缘/平面体素地图。

用 LOAM 特征匹配方式，求新关键帧到局部地图的相对变换

GPS因子 (可选)

GPS 信号可用且协方差优于当前估计时，添加到图中，修正全局漂移

过滤条件:

  • 有关键帧：GPS 因子只约束关键帧节点。
  • 运动距离足够：首尾关键帧距离太近（<5m）不加，避免刚启动或回环时重复约束。
  • 当前位姿协方差较大：如果 iSAM2 优化结果的 x,y 方向不确定性低于阈值，就不需要 GPS 修正。
  • GPS 数据质量好：根据 pose.covariance 判断，若 x,y 方向方差大于阈值则丢弃。 -时间同步：GPS 时间戳需与当前关键帧时间差在 ±0.2s 内

构建约束:

  1. 将 GPS 经纬度转换到笛卡尔坐标
  2. 如果 useGpsElevation=false，则忽略 GPS 的 z 值，用 LiDAR 里程计的高度代替（因为 GPS 高度精度差）
  3. 构造 PriorFactor\<Pose3\>（单节点先验）约束关键帧的平移部分，噪声模型由 GPS 协方差决定

回环因子 (可选)

独立线程，默认 1Hz 检测频率

  1. 候选帧搜索：使用 KD‑Tree 在历史关键帧位置中查找距离当前帧 <15m 的候选帧; 时间间隔必须 >30s，避免刚经过的地方被误判

  2. 局部地图构建：提取候选历史帧及其前后若干帧（默认 25 帧）的特征点，组成局部地图。提取当前关键帧及其邻近帧的特征点。

  3. ICP 配准：将当前关键帧点云与历史局部地图进行 ICP（迭代最近点）匹配。若收敛且残差小于阈值，则认为回环成立。

构建约束:

  1. 回环检测成功后，得到两帧之间的相对位姿
  2. 构造 BetweenFactor\<Pose3\>（双节点约束），连接当前关键帧节点和历史关键帧节点
  3. 噪声模型根据 ICP 配准的残差设定

关键细节

  • Deskew：IMU 提供扫描期间传感器运动轨迹，每个点按相对时间戳插值补偿，消除旋转/平移畸变。

  • 滑动窗口：只匹配最新关键帧到固定数量子关键帧的局部地图，避免全局重配准导致计算爆炸。

  • 因子图优化：iSAM2 增量优化，实时更新轨迹和零偏。

  • 多源融合：可以无缝接入高度计、罗盘等其他绝对传感器，只需添加对应因子。

其他

iSAM2

incremental Smoothing And Mapping 2（增量平滑与建图 2）是 GTSAM 库里实现的一个增量式非线性优化算法，专门用于因子图（Factor Graph）的实时更新

传统的批量优化（Batch Optimization）要在新约束出现时重新解整个系统，计算量大、延迟高，不适合实时机器人定位建图

iSAM2 则采用：

  • 增量式更新：当新的状态节点和因子加入时，只更新与这些新增部分相关的子图，而不动其他部分。
  • Bayes Tree 数据结构：把因子图分解成类似树的结构，方便局部重排和更新。
  • 流式处理：每次关键帧生成后就立刻优化，不必等到积累一批再算。

输入：一串“未知数”（状态节点）+ 一堆“等式/约束”（因子）。

输出：所有未知数的当前最优解 + 它们的置信度。