Home / 机器人经典基础 / LIO-SAM：让激光、惯性、GPS 与回环共同解释运动

097｜LIO-SAM：让激光、惯性、GPS 与回环共同解释运动

论文：Tixiao Shan 等，LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping，IROS 2020。论文正文 · PDF · 官方代码与运行说明。

先接住前作的问题

LeGO-LOAM利用地面和特征改善雷达里程计，但惯性初值与全局漂移校正仍需更紧密连接；LIO-SAM 承接雷达建图路线，并组合预积分与iSAM2。

五个前置概念

五个前置概念是：因子图把状态和观测关系分开表示；预积分将高频惯性数据形成区间约束；外参描述传感器之间的刚体关系；可观测性判断哪些状态能被数据确定；回环利用重访建立跨时间约束。

一、问题设定：扫描时序与惯性偏差怎样进入同一轨迹约束

激光雷达告诉机器人“周围表面在哪里”，IMU 告诉它“刚才转了多快、加速了多少”。激光几何比较稳定，但一帧扫描不是同时拍摄；IMU 更新快，却会累积偏差。单纯把两个输出平均，无法充分利用这种互补关系。

为什么一帧点云可能是一张歪地图

下面是自构情景：雷达转一圈需要 0.1 秒，机器人在这段时间转了 10 度。扫描开始和结束时，墙面点在不同的传感器坐标系里。若当作同一瞬间的数据叠在一起，平墙就可能被描成弯墙。

“去畸变”是根据每个点的采样时刻，把点搬回共同参考时刻。它不同于后续地图配准：前者修正扫描内部运动，后者估计扫描相对地图的位置。如果输入时间字段错误，后端优化面对的就是一份有系统变形的几何观测。

状态可写成 $x_i=(R_i,p_i,v_i,b_i)$ ：姿态、位置、速度和 IMU 偏置。任务是估计状态序列与地图，不是学习一个动作策略。没有奖励函数，也没有环境交互训练预算。

二、连接预积分、激光、GPS与回环因子

概率目标与因子图

为了说明因子图，使用通用高斯观测模型的教学形式：

$X^*=\arg\min_X\left[ \|r_0(X)\|^2_{\Sigma_0^{-1}}+ \sum_{(i,j)}\|r^{imu}_{ij}(X)\|^2_{\Sigma_{imu}^{-1}}+ \sum_{(i,j)}\|r^{lidar}_{ij}(X)\|^2_{\Sigma_{lidar}^{-1}}+ \sum_i\|r^{gps}_i(X)\|^2_{\Sigma_{gps}^{-1}}+ \sum_{(i,j)}\|r^{loop}_{ij}(X)\|^2_{\Sigma_{loop}^{-1}} \right].$

$\|r\|^2_W=r^TWr$ ，表示按不确定性加权的残差。某个 GPS 测量很不可靠，就应给它较低权重，而不是因为它叫“绝对定位”就强行相信。没有 GPS 或回环时，对应求和项可以不存在。

这是帮助理解的统一目标，不意味着发布代码每次都在一张无限增长的图中联合重算所有变量。官方实现说明区分持续维护的地图图和周期重置的惯性预积分图：读论文的概念图与读代码的线程、图结构，应分别对照。

紧耦合的关键在于是否反馈偏置

如果 IMU 只给出一次初始猜测，随后激光匹配完全独立运行，激光观测未必能修正 IMU 偏置。紧耦合则让惯性残差与几何残差共同影响状态和偏置估计。一个简单的偏置例子是：陀螺仪静止时仍持续输出小角速度，如果不估计这项偏置，积分的角度就会不断增长。

预积分把两个关键时刻之间的大量 IMU 数据压缩成相对运动约束及其不确定性。它保留的是时间区间上的测量作用，不是把几百次读数简单平均。优化状态改变后，还需要处理偏置线性化和适用范围。

激光残差为什么选线和平面

沿一堵平墙滑动时，某些方向不容易从几何中辨别；面对多个朝向不同的表面，约束通常更充分。点到面的残差可用下式解释：

$r_k=n_k^T(Rp_k+t-q_k).$

$n_k$ 为地图平面的单位法向， $q_k$ 是平面上的点。它衡量变换后点沿法向偏离多少，而不要求两个传感器恰好采到同一个物理点。点到线约束则检查点偏离一条局部边缘的程度。

论文沿用这类几何特征，用关键帧及局部子地图控制计算规模。保留全部历史原始点并不必然更准确：冗余点会增加计算，旧的错误对应还可能妨碍当前估计。关键在于留下足够约束，而不是只追求地图点数。

一个 GPS 因子怎样改变之前的状态

原创一维因子图。 固定初始位置 $p_0=0$ ，两个里程计增量都测为 1 m、方差都为 $1\,\mathrm{m^2}$ ；GPS 只在末端给出位置 0 m，方差记为 $\gamma\,\mathrm{m^2}$ 。这不是现实路线参数，而是展示信息传播的最小例子。忽略共同的常数因子，目标为

$E=(p_1-1)^2+(p_2-p_1-1)^2+\frac{p_2^2}{\gamma}.$

对两个位置求导，有 $2p_1-p_2=0$ 和 $(1+1/\gamma)p_2-p_1-1=0$ ，解得

$p_1=\frac{\gamma}{\gamma+2},\qquad p_2=\frac{2\gamma}{\gamma+2}.$

当 $\gamma=1$ ，得到 $(p_1,p_2)=(1/3,2/3)$ ；GPS 不仅改变最后一个点，也经相邻因子改变历史位置。若 $\gamma=100$ ，得到约 $(0.9804,1.9608)$ ，结果更接近原里程计的 $(1,2)$ 。因此全局修正不是把最后一个位置硬设为 GPS，再将前面轨迹原样留下。

原文III-D按位置协方差比较决定是否加入 GPS 因子。这个门槛控制冗余和可信度，却不能单靠一个很小的协方差排除系统性 GPS 偏差。Campus 中新增 GPS、再新增回环后首尾误差改变，体现的是新增约束及其信息传播；该例和实验都不能将更小首尾误差自动解释为每个中间状态同幅改善。

相对位姿要取逆，不能直接转置

若 $T_i$ 把第 $i$ 帧坐标变到世界，则正确的齐次相对变换为：

$T_{ij}=T_i^{-1}T_j, \qquad \begin{bmatrix}R&t\\0&1\end{bmatrix}^{-1} =\begin{bmatrix}R^T&-R^Tt\\0&1\end{bmatrix}.$

原文式 (12) 显示为转置符号，PDF 第 4 页也如此。按本文齐次变换约定，不能直接照写成 $T_i^TT_j$ ；只有旋转矩阵的逆等于转置。这里指出的是表达与实现时应检查的数学边界，不能据此断言官方系统计算了错误相对位姿。

惯性先帮助扫描时序补偿，激光与全局因子再校正轨迹。下面按信息来源检查更新，再分开评价额外传感器带来的收益。

三、交替去畸变和估计，检查约束信息来源

输入：带逐点时间的激光数据、IMU、标定参数；可选 GPS
缓冲并对齐传感器时间
使用惯性运动预测，对扫描逐点去畸变
提取几何特征，与当前局部子地图配准
达到关键帧条件时，形成新的状态与激光约束
用激光校正约束联合更新惯性状态和偏置
有可信 GPS 时加入位置约束
发现历史候选且几何验证通过时加入回环约束
增量更新地图相关估计，输出位姿和地图

这是流程伪代码。回环候选生成与几何验证是两个环节；空间上接近并不必然意味着观测到同一个场景。

四、按是否使用GPS回环与惯性读精度结果

Fig. 4：卫星参考与三种地图同列显示，在 Walking 数据中比较建筑边缘是否反复重影；这组结果没有使用 GPS — 原论文结果图／表摘引：Tixiao Shan 等（2020），Fig. 4，PDF 第 6 页；核对原文。原版权保留。

读图：卫星参考与三种地图同列显示，在 Walking 数据中比较建筑边缘是否反复重影；这组结果没有使用 GPS。LIO-SAM 更整齐是此序列的定性证据，不能由地图观感得出每个时刻的定位误差。

实验：准确性提升是否用了更多信息

GPS、回环和 IMU 辅助改变了信息条件；首尾闭合误差不能替代全程真值，原始 LIO-SAM 与后续分支能力分开。

检查项	原文设置或阅读边界
平台	手持设备、地面车与船，见图 2
数据	表 I 包含 Rotation、Walking、Campus、Park、Amsterdam 五组
输入比较	LIO-odom 无 GPS/回环；LIO-GPS 加 GPS；完整 LIO-SAM 使用可用因子
基线	LOAM、LIOM，以及上述融合配置
运行约束	§IV 要求 LOAM/LIO-SAM 实时；允许 LIOM 更长处理时间
设备	原实验报告 CPU 执行；不能当作现代任意设备速度承诺
主要指标	地图与轨迹可视化、首尾平移误差；首尾误差不是完整轨迹 ATE
重复与调参	原文所示结果不能直接提供跨随机种子的置信区间；完整调参预算未核实

尚未内嵌：图 2、图 5。平台组合照片对当前因子图与误差评论不是必要数据；另一个复合图含校园轨迹及 Google Earth 底图叠加，后者未核清完整 Google/数据提供商署名。只展示轨迹面板不能算整幅图已完成，故保留完整图入口。

三项证据应连起来读：

图 3–图 4：快速运动。 比较地图结构是否保持。它展示这些数据上的配准稳定性，但视觉上更整齐不能替代逐时刻真值误差。

原论文图3：旋转测试的实景、LOAM地图和LIO-SAM地图 — Tixiao Shan 等（2020），图 3，PDF 第 5 页；原文与图注。原版权保留。

读图：上图给出场景结构，下两图检查旋转时几何是否保持一致；这是困难运动下的定性比较。

表 II：增量加入全局约束。 Campus 的 LIO-odom、LIO-GPS、LIO-SAM 首尾误差分别为 9.44、6.87、0.12 米。它支持额外位置和回环约束的作用；不支持“仅靠改了局部配准就提高到同样水平”。
图 5–6：GPS 可用性与轨迹形状。 它们帮助判断修正发生在什么位置。把回到起点的位置拉近，并不能保证中间每一段轨迹都同等准确。

原论文图6：公园数据中多种估计轨迹及LIO-SAM地图配准结果 — Tixiao Shan 等（2020），图 6，PDF 第 7 页；原文与图注。原版权保留。

读图：上图的GPS可用区间与各方法轨迹一起读，下图仅提供地理背景；地图对齐不等于逐点真值评估。

此外，表 I 与 §IV-A/B 对 Rotation、Walking 的最大角速度存在互换：表中为 213.9 与 133.7 度/秒，正文相反。PDF 第 5 页可见这一差异，具体归属应查原始数据澄清，不宜用其中一个数字制造精确的鲁棒性上限。

用一个小实验建立理解

自构、未运行：在同一段闭环数据上，分别启用 IMU+激光、再加 GPS、再加回环。保留相同前端参数，记录全轨迹误差、首尾误差、处理延迟、回环接受情况。接着只给 IMU 时间加一个小偏移，观察哪种指标最先恶化。

这个实验能区分三个问题：融合增加了什么信息；闭环指标是否掩盖途中形变；好算法是否被错误的输入时间破坏。若只保存最终地图截图，几乎无法回答后两个问题。

五、保留时间同步、标定与异常全局观测风险

作者提供了紧耦合融合与不同信息来源的实验；下面是应用层分析。长期漂移不是“优化还不够努力”，也可能是缺少能观测该方向的信号。长走廊、稀少结构、水面或大量运动物体，都可能削弱几何约束。IMU 能撑过短期退化，却不能永远补上不存在的绝对位置观测。

官方运行说明明确要求逐点时间和相应点云通道，并讨论 IMU 姿态、外参及同步条件。原始分支的传感器支持范围不应扩写成“任意雷达与 IMU 即插即用”。两个错误时钟之间的固定延迟，可能被优化器误解释成运动或偏置；这是一种模型错误，不只是随机噪声变大。

同样要分开局部高频里程计和全局回环校正：后者可能使地图坐标中的轨迹调整。把这种调整直接送给需要连续速度的控制器，可能产生不必要的跳变。系统集成应明确控制使用哪个坐标系、哪种输出和怎样的更新语义。

六、追问：加入GPS后漂移变小，怎样验证不是把额外全局信息误称为激光前端改进？

加入GPS后漂移变小，怎样验证不是把额外全局信息误称为激光前端改进？

参考答案：固定前端与局部因子，分别比较有无GPS及其覆盖区间，再单独测试去畸变和扫描匹配。全局约束能够改善整条轨迹，但不证明每帧激光估计独立变准。实验还应报告GPS质量及异常处理条件。

七、比较因子图平滑与迭代滤波

平行路线：FAST-LIO2：直接用点云定位，为什么先要解决地图更新。用迭代滤波和增量地图实现激光惯性里程计，系统全局能力不同。

八、自测与参考答案

1．为什么不能把 IMU 去畸变直接等同于紧耦合？

答：去畸变可以只是预处理，使用一次惯性预测后就不再更新偏置。紧耦合强调不同传感器残差共同约束状态，激光信息还能反馈到惯性状态和偏置。要看估计依赖关系，不能只看系统接了几种传感器。

2．首尾误差从数米降到厘米，能说明整段轨迹都达到厘米精度吗？

答：不能。中间轨迹可以发生局部伸缩或弯曲而最终闭合。需要有独立真值的全程轨迹指标、局部相对误差和地图几何检查；回环强制吻合的位置更不能成为唯一验证依据。

3．因子图中加入更多 GPS 测量一定更好吗？

答：不一定。存在多路径、错误协方差或时间偏移时，更多有偏测量会更强地拉偏估计。应先检查质量与同步，并给出合理噪声模型；增加观测数量不能替代可靠的观测关系。