LIO-SAM 这篇论文很多搞激光雷达定位建图的同学应该都翻过。全称是 Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping翻译过来就是“通过平滑与建图的紧耦合激光雷达惯性里程计”——名字不短但关键词已经把技术底牌全都亮出来了激光雷达、惯性测量单元、紧耦合、因子图优化、建图。最近我又把这套方案的论文、开源代码和实际运行数据放在一起对照着捋了一遍感触比第一次读的时候深了不少。这篇文章就顺手整理一下我对论文思路的拆解、复现的完整流程还有调试中踩过的坑给正在读这篇论文或者准备上手跑一遍的人做个参考。1. LIO-SAM到底做了什么先看整体设计和设计动机1.1 核心痛点纯激光里程计为什么会飘激光雷达测距精度在传感器里确实算高的但做过实际平台的人都有体会纯靠点云配准的里程计在长直走廊、空旷场地、隧道这些结构弱的环境里特别容易退化。退化这个词说白了就是某个方向几乎没有观测约束比如走廊里沿着长度方向有很多点云可以匹配但垂直于墙面的横向约束其实很弱误差就在这个没约束的方向上一厘米一厘米地累积轨迹慢慢就飘了。即便是在特征丰富的园区里scan-to-scan 或者 scan-to-map 匹配也会因为两帧之间视角变化、物体遮挡、运动畸变等因素在长距离行驶之后产生不可忽略的累积漂移。这正是 LIO-SAM 想解决的核心问题再引入一个廉价、高频的惯性传感器IMU通过因子图把惯性观测和雷达观测融合起来而不是简单拿 IMU 结果去补偿一帧点云就完事。这样做的好处非常直观——在退化场景里惯性信息可以兜底在正常场景里激光雷达的观测又不断修正 IMU 的漂移两者形成互补。你可以把它理解成一个团队里两个人相互盯着干活一个人发现另一个偏了就及时拉一把而不是一个人闷头干完另一个人只在旁边喊两声加油。1.2 因子图框架为什么选择“优化”而不是“滤波”早期很多激光惯性里程计走的是滤波框架比如扩展卡尔曼滤波那一套状态估计是一步一步往前递推的历史信息只保留在协方差矩阵里一旦当前估计出了问题很难回头去把过去的误差翻出来重新修正。LIO-SAM 选择的是另外一条路平滑与建图。它把所有关键帧的位姿当作待优化变量把各种传感器的约束抽象成因子构建成一张因子图然后用批量优化求最大后验估计。用优化框架有什么实际收益最明显的一点是回环检测。如果走滤波路线回环出现的瞬间几乎不可能把过去所有状态重新推一遍只能近似处理而因子图优化天生就支持把回环约束加进去一优化整个历史轨迹都会跟着被修正这是滤波方案做不到的。另外因子图框架做多传感器融合时扩展性非常好。今天只有雷达和 IMU明天想加个 GPS 或者视觉往图里加一种新因子就行不需要把底层状态模型推翻重写。这个优点在做工程选型的时候尤其重要后面你会感受得到。1.3 对比前代方案紧耦合和松耦合差在哪里理解 LIO-SAM 之前最好先知道它跟 LOAM 这类经典方案的关系。LOAM 的核心是激光里程计和建图两个模块并行跑IMU 在里面的作用主要就是做点云畸变补偿和提供初始位姿并没有把 IMU 的零偏、速度这些状态放进优化问题里一起求解这属于松耦合。LIO-SAM 则把 IMU 预积分因子和激光雷达里程计因子放在同一个优化图里联合估计位姿、速度和 IMU 零偏这才是真正的紧耦合。区别可以用一个比方说清楚松耦合是“激光雷达先自己算一版结果算完我再拿 IMU 校验一下”紧耦合是“雷达和 IMU 拿着原始数据一起算谁也别说自己没问题”。后者在传感器短暂失效、退化环境下明显更稳代价是模型和解算更复杂计算量也更高。从工程复现的角度说我建议把 LOAM 和 LIO-SAM 的代码都跑一遍对比着看同一段数据下两者的轨迹差异你会非常直观地理解“紧耦合”三个字的分量。2. LIO-SAM核心细节拆解四个因子和两个关键机制2.1 激光雷达里程计因子角点和面点怎么配准激光里程计因子是 LIO-SAM 的主力约束。每一帧原始点云先做去畸变和特征提取把点云分成角点和面点两大类。角点对应棱边结构比如墙角、柱子边缘、栏杆面点对应平面结构比如地面、墙面、停车场地板。提取特征的方法是看局部光滑度邻域里曲率大的选成角点曲率小的选成面点同时会做均匀化采样避免特征扎堆在某个区域导致配准约束分布失衡。特征提取之后系统会把当前帧的角点和面点往局部地图或者附近的关键帧点云上配准。这个配准过程用到的是一套迭代最近点类的优化思路通过最小化点到直线、点到平面的距离来解算相对位姿。注意这里有个细节LIO-SAM 的激光里程计因子是在相邻关键帧之间建立的而不是每一帧都建立一个因子因为关键帧的数量远远少于原始帧这样优化图规模才不会爆炸。实际调参时角点和面点的数量上限是高频调整项。数量太少配准容易缺约束数量太多计算量增加明显但精度不一定提升。我自己的经验是先按默认参数跑一遍把提取结果可视化出来看分布如果某个场景里角点稀疏就适当调提取阈值而不是盲目提高数量。2.2 IMU预积分因子时间和算力之间的平衡IMU 的输出频率通常远高于激光雷达比如雷达 10HzIMU 100 到 200Hz。如果每一帧 IMU 数据都作为独立因子加入图优化变量和因子数量会多到无法实时计算。预积分的思路是把两次关键帧之间的 IMU 测量累积起来形成这两个关键帧之间的相对运动约束。这样不管中间有多少个 IMU 采样最后都只消耗一个因子。预积分里最有技术含量的部分是零偏处理。IMU 零偏如果不修正预积分结果会越来越偏修了零偏预积分值本身又会变总不能每次都重新积分一遍。LIO-SAM 的做法是预积分值对零偏做一阶线性化每次零偏更新后直接修正预积分结果避免重复运算。这个细节在代码里体现在雅可比补偿上初读论文时很容易跳过但实际自己写一版就会发现这个设计有多关键。调参时零偏噪声这个参数对轨迹影响很大。设太紧系统会过度信任 IMU万一 IMU 有温度漂移就会带偏轨迹设太松IMU 反馈的约束太弱退化场景下又起不到兜底作用。建议根据传感器手册的噪声密度反推再在实测数据上微调。2.3 GPS因子和回环因子全局修正从哪来GPS 因子提供了一个全局绝对位置约束作用是抑制累积漂移。但 GPS 只有在开阔环境下才有稳定解城市峡谷、隧道、室内全都不可用。LIO-SAM 的做法是先用状态估计模块判断 GPS 解是否可用比如位置协方差小于某个阈值时才把它加入因子图。这样能避免把飘得离谱的 GPS 位置当成硬约束反而毁了整个优化结果。一个容易被忽略的坑是GPS 给不了航向角。也就是说单靠 GPS 因子无法直接约束偏航漂移。如果所有约束都来自前方和侧方光线又没变化偏航角可能会慢慢溜掉只能靠回环或者激光特征去拽回来。回环因子的来源也很有意思。它不像视觉 SLAM 那样靠词袋模型识别场景而是直接用当前关键帧的位置去历史轨迹里找空间邻近的帧如果距离够近就认为可能构成回环再用点云配准验证。这种方法简单高效适合已知区域重复遍历的场景比如园区巡检、仓库盘点。缺点是如果轨迹漂移已经很大了空间搜索会找不到真实回环所以回环检测是兜底方案不是万能解药。工程上尽量让路径设计成能周期性回到已知地标把回环触发率提上来精度才可控。2.4 滑动窗口与关键帧实时性是怎么保住的如果所有历史关键帧都留在因子图里优化规模会无限增长最后拖垮实时性。LIO-SAM 用了一个很标准的处理手段滑动窗口。优化窗口内保留最近的关键帧和它们关联的约束窗口之外的老帧被边缘化处理。边缘化听起来复杂实际理解可以简单一点把老帧已经统计出的信息压缩成一个先验因子留在优化问题里当作约束。这样既控制了优化规模又没有完全丢掉历史信息。关键帧的选取不是每帧都进优化而是运动量超过阈值时才创建新的关键帧比如平移超过 1 米或者旋转超过 0.1 弧度。这样做的好处是减少优化节点数量同时保证帧间有足够的视差来做可靠配准。如果你发现地图太重、关键帧太多就把阈值调大一点如果发现轨迹精度不够把阈值调小一点让关键帧更密。这个权衡没有标准答案完全取决于你的算力和精度需求。3. 复现LIO-SAM环境、数据与参数配置全流程3.1 环境准备依赖项、编译和节点结构LIO-SAM 的工程依赖不算复杂典型的一套是一个干净的 Linux 开发环境、点云处理库、线性代数库、因子图优化库。我建议在独立工作空间里编译先装依赖再编译不要图省事跳过任何一项。很多源码里的特征提取和点云配准函数都封装在点云库里漏掉会导致编译错误而且这类错误查起来特别费时间。源码编译完成后工作流大致可以拆成几个独立模块点云去畸变与投影、特征提取、建图优化以及一个 IMU 预积分线程。它们的协作方式是通过消息队列串联起来一条典型数据链是原始点云和 IMU 数据进入去畸变模块输出校正后的点云校正点云进入特征提取模块输出角点和面点角点和面点再进入建图优化模块输出位姿、轨迹和地图。刚开始看代码时不用一层层抠细节先把这条数据链在脑子里画清楚后面调参会顺很多。3.2 数据准备公开数据集和自采数据的处理差异最简单的复现方式是用公开的自动驾驶数据集。这类数据包一般同时包含高线束机械雷达、高频 IMU 以及实时差分定位结果拿来就能跑通整个流程非常适合验证环境是否正确。第一次跑建议直接用这类数据把预期结果找出来再折腾自己的设备。如果要用自采数据有三件事必须提前做第一传感器时间戳必须对齐雷达和 IMU 的时间基准要统一否则后处理外参标定得再准也没用第二必须做外参标定至少要有 IMU 到雷达的旋转和平移直接用理想安装值硬扛地图很快会出双层重影第三启动时设备不要完全静止要做一段小幅的变速运动让 IMU 完成重力方向的初始化和零偏估计。很多人在这一步吃亏觉得静止放置就能初始化好实际航向角在这种情况下是完全不可观的。3.3 关键参数配置按这张表来调基本少踩坑下面这组参数是我整理过最容易影响结果的关键项不同平台必须按传感器特性自己改。参数作用经验值或说明激光雷达频率去畸变和运动补偿的时间轴常见 10Hz 左右需与点云时间戳一致IMU 频率预积分的时间步长常见 100-200Hz太低会导致运动估计粗糙IMU 到雷达外参两组传感器的坐标变换必须标定不能使用理想安装值IMU 零偏噪声决定 IMU 漂移被优化的信任程度太紧约束过死太松轨迹慢慢飘关键帧平移阈值控制优化节点密度一般 1 米左右环境大可以放宽关键帧旋转阈值控制优化节点密度一般 0.1-0.2 弧度回环搜索半径回环候选帧的搜索范围根据场景范围设置不能无限大GPS 位置协方差阈值决定 GPS 因子是否参与优化城市峡谷里要调得更严格不要直接抄一套别人的参数用到自己设备上。我试过把一套园区参数直接搬到另一台设备结果地图重影严重最后重新标定了外参和零偏噪声才恢复正常。传感器不同内参、噪声特性、频率都不一样参数没有一次到位的都是跑数据、看轨迹、微调这样的循环。3.4 跑通流程从启动到地图输出的操作顺序第一次上手按这个顺序来会比较稳。先单独播放一段数据确认点云和 IMU 话题都在正常输出然后启动 IMU 预积分模块观察初始姿态在几百毫秒内是否快速收敛再启动特征提取在可视化界面里确认角点和面点提取正常没有大量点云堆积或者特征缺失最后启动建图优化观察地图是否随着行驶逐步收敛清晰。这里有个我实测过的技巧刚开始调参时先关掉回环检测把单趟里程计调到尽量少的漂移再开回环验证修正效果。这样如果回环一开地图变了你能判断是回环约束本身的问题还是基础里程计的问题而不是一堆因素搅在一起排查。这个方法看起来保守但能帮你省掉大量无效调试时间。4. 实测常见问题与排查技巧4.1 时间同步和外参标定地图重影的元凶我遇到过最普遍的现象是建出来的地图整体看起来还行但过弯之后墙面出现双层甚至三层轮廓。九成原因不是算法不行而是外参标定误差或者时间戳错位。外参标定里尤其容易出问题的是旋转部分旋转差个一两度近距离看不出来跑几十米后地图就开始分层。排查技巧可以记住两条先让设备原地旋转几圈如果点云里柱子出现拖影大概率是时间同步问题如果点云整体倾斜大概率是外参旋转部分没标准。时间同步的常见原因是雷达每个点的采集时刻不一样没有做线性插值补偿导致运动畸变没消干净。外参标定最好用标定板做多位置观测来解算不要靠尺量安装角度。我在一个项目上试过用尺量外参硬跑结果重建的墙面像被揉过一样后来换成标定流程后立刻正常。4.2 初始化阶段翻车静止启动和丢帧LIO-SAM 初始化阶段那几百毫秒非常关键很多轨迹发散问题其实从这里就埋下了。如果传感器完全静止IMU 预积分只能估计到重力方向航向角完全是不可观的如果这时候代码直接给一个朝向估计后续优化会把错误当作真值去拟合轨迹自然偏到不知哪去了。我的做法是启动时让设备做小幅“8 字”运动或者前后晃动让加速度计观测把姿态完整激励起来。如果是播放录制数据尽量保证起始阶段有足够的运动不要为了省时间把这一段剪掉。另外如果数据包前几帧因为运动畸变过大被丢弃也不要慌先确认正常输出后再启动建图不要在数据不完整时强行开优化。初始位姿一旦扎错后面很难拉回来。4.3 退化场景漂移长走廊、隧道和空旷场地怎么办退化问题在实际平台里太常见了尤其是园区里的一些长走廊和地下车库通道。LIO-SAM 比 LOAM 在退化方向上有明显改善因为有 IMU 约束兜底但 IMU 本身也有缓慢漂移长时间走长直走廊垂直于行驶方向的约束仍然很弱轨迹会慢慢往一侧偏。应对办法有几个方向第一在退化场景降低该区域里程计结果的信任权重别让错误的匹配把整体优化带偏第二利用任务约束比如让机器人保持离墙固定距离行驶在走廊里墙面的法向距离本身就是一种稳定观测第三如果室内没有 GPS回环是唯一强修正手段所以要保证路径能周期性地回到已知区域让回环因子有机会触发。做巡检或者扫地机器人时这条建议比调任何参数都见效快。4.4 因子图不收敛或轨迹发散按这个顺序排查轨迹突然发散是最吓人的现象但排查顺序比速度更重要。第一看 IMU 零偏初值是否合理初值错会导致预积分从一开始就是歪的第二看雷达特征提取得是否太少尤其是空旷场景里如果全是地面点角特征缺失配准约束就会不够第三看外参是不是设备换装后忘了改第四看关键帧之间运动量是否过大导致 scan-to-map 匹配的初始位姿离真值太远。我自己的固定排查顺序是先关掉回环因子看是不是起始点就在发散。如果是起始点就偏问题大概率在初始化和外参如果起始点没问题但跑到某一段突然跳变重点查时间同步和 IMU 噪声参数。发散问题不要一上来就调后端优化参数先确认输入数据质量很多时候把时间戳对齐问题解决掉轨迹自己就回来了。最后再说一点个人体会。读 LIO-SAM 这篇论文最容易忽略但也最值得反复琢磨的其实是它“让传感器互相兜底”的设计哲学激光雷达给 IMU 做长期修正IMU 给激光雷达做短期补偿GPS 提供绝对位置约束回环负责纠正历史。单独看每一个因子都不复杂组合在一起才体现出平滑与建图的优势。如果读者你现在正准备复现这套方案我建议别急着调参先把论文里数据流的逻辑和代码对应起来再在公开数据集上跑一遍最后换成自己的设备。等这一步走通你再看那些调参问题基本都能自己判断出原因在哪了。