企业官网建设流程全解析

最近在梳理多传感器融合SLAM的相关论文，顺手把VIL-SLAM这篇完整翻译了一遍。标题全称是“Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping”，也就是“立体视觉惯性激光雷达同时定位与建图”，应该有不少做机器人、自动驾驶定位的同学对这篇感兴趣，所以把翻译和阅读笔记整理出来，结合我在实际工程里踩过的坑聊聊这套系统到底解决了什么问题，以及我们能从里面抄到什么作业。

先说结论，VIL-SLAM是一套基于因子图紧耦合的立体视觉惯性激光雷达SLAM系统，核心贡献是把三类传感器（双目相机、IMU、激光雷达）放在一个统一优化框架里，让视觉负责短时间内的稳健跟踪，IMU负责帧间运动预测，激光雷达负责消除长时间漂移。这里面有一个非常关键的工程点：系统只有在视觉跟踪质量差到一定程度时才会触发激光雷达的扫描匹配来约束位姿，而不是每一帧都做LiDAR的scan-to-map匹配，这个设计直接决定了整个系统的计算开销和实时性。文章适合正在做激光视觉融合、想把手里的机器人平台精度往上再拉一截的研究生和工程师参考。

1. 论文要解决的实际问题

1.1 单传感器SLAM的痛点在哪里

做SLAM做得久了你会发现，没有哪一种传感器能包打天下。纯视觉SLAM（比如ORB-SLAM系列）在纹理丰富的环境里表现不错，可一旦遇上白墙、玻璃、夜间的低光照场景，特征点数量直接断崖式下跌，跟踪随时会崩。纯激光SLAM（比如LOAM、LIO-SAM）依赖几何结构，在走廊、隧道这类结构化环境里很稳，但遇到空旷的大场地、或者几何特征退化的场景，点云匹配同样会飘。

IMU倒是短时间内的运动估计非常平滑，不受光照和纹理影响，但偏偏它的积分会随时间快速累积漂移，几秒钟不矫正，航向角就可以偏出好几度。这就是为什么现在主流方案都在做多传感器融合，本质上就是用各自的优势去补对方的短板：视觉给你丰富的语义和纹理约束，激光给你精确的几何距离约束，IMU填上两个传感器帧之间的运动空白。

这段开头的比喻我用了很久：视觉像人眼，负责看轮廓和特征；激光像卷尺，能随时量出精确的距离；IMU像内耳前庭，负责感知加速和旋转。三者加在一起，才是一套能走稳路的感知系统。

1.2 VIL-SLAM给了一个什么方案

VIL-SLAM的思路在论文里表达得很清楚：它不追求三个传感器全时段、全频率的强融合，而是采取一种按需触发的松-紧耦合混合策略。系统主体是一个寄生在视觉惯性里程计上的因子图框架，激光雷达作为另一个独立的传感器因子被接入同一个后端优化器。

这个设计和我想象中的多传感器全频融合不一样。它最聪明的地方在于，激光雷达的观测不需要每帧都进入优化。论文提出一套基于局部地图质量评估的触发机制，只有当视觉跟踪质量下降时，系统才会主动调用LiDAR的几何约束来修正累积漂移。这样一个设计让系统在计算资源有限的情况下依然能保持稳定，也为后面的工程部署留出了充足的性能余量。

另外需要留意的是，这套系统采用双目视觉而不是单目，这个选择直接消除了单目初始化难和尺度不确定性的问题。对于室外机器人和自动驾驶这类直接要求米制尺度的应用，双目自带基线就是一个天然的标尺，不需要额外做尺度恢复。

2. 系统框架与各模块的设计逻辑

2.1 因子图后端是怎么组织信息的

VIL-SLAM的后端建立在因子图（Factor Graph）上。如果对图优化还不熟悉，可以把它想象成一张由节点和边组成的网络：节点就是机器人不同时刻的位姿，以及路标点的位置；边就是传感器给出来的约束关系。

这套系统的图里实际运行着三类节点：关键帧的位姿节点、IMU的偏置节点、以及激光雷达局部地图对应的子图节点。边则来自四个方向：双目视觉的视觉重投影因子、IMU的预积分因子、LiDAR扫描匹配产生的几何因子、以及回环检测产生的闭环因子。

因子图框架最大的好处就是模块化。我想往里面加一种新传感器，只需要定义好残差表达式和雅可比矩阵，剩下的交给优化器去统一求解。这和早期松耦合方案把各传感器独立定位，再用滤波或者插值去“缝合”完全不同——紧耦合的价值就在于，所有传感器在同一优化问题里相互牵制，任何一个传感器的约束都能帮助系统同时优化自身的位姿和其他传感器的状态。

2.2 前端视觉：ORB特征与LK光流

视觉前端沿用了一套比较成熟的组合：ORB特征提取加上LK金字塔光流跟踪，这个组合在ORB-SLAM和VINS-Mono里都能看到影子。为什么不用重特征提取而选择光流跟踪？直接原因就是性能。每帧重新提一遍ORB特征的成本远高于在上一帧的特征位置上做光流搜索，而且光流跟踪在相邻帧之间保持了特征点的一致性，省掉了特征点匹配这步最耗时的暴力搜索。

但光流跟踪有个先天的风险，就是跟丢和漂移。特征点会随着视角变化、遮挡、快速运动而逐渐丢失，所以VIL-SLAM的策略是：每一帧都提取一批新的ORB特征点和上一帧跟踪过来的老特征混合使用，按照像素坐标均匀分布的方式去挑选特征，避免特征扎堆在某个图像区域。论文里对特征数量给出了一个范围，我翻译时的理解是，这需要根据实际场景的纹理丰富度去调整，没有绝对最优的固定值。

双目立体视觉部分的深度值是通过左右目三角化算出来的。这一步在初始化时给系统提供了可靠的尺度信息，同时视觉特征的深度也为后面视觉惯性的对齐提供了初始值。

2.3 LiDAR特征提取：角点与平面点

LiDAR端使用了类似于LOAM的特征提取思路，将点云按照曲率分成两大类：角点特征，代表边缘几何结构；平面点特征，代表平坦的表面结构。这两类特征在scan-to-map匹配中承担的角色完全不一样。

角点特征的约束性极强，能同时限制位姿的六个自由度，但角点在自然环境中往往稀疏且不稳定。平面点则多得多，随处可见的地面、墙面、桌面都是平面点，但单个平面点只能约束两个自由度的法向方向。VIL-SLAM的做法是将两类特征点在匹配后同时放入非线性优化中，用最小化点到线、点到面距离的形式来构建激光因子。

这里有个容易忽视的小细节：提取特征的时候要去除激光点云的运动畸变。因为激光雷达是一个点一个点扫描的，在扫描过程中机器人自身在动，导致一帧点云里的不同点对应的是不同时刻的机器人位姿。VIL-SLAM的处理方法是，利用前端视觉惯性里程计在激光帧时间戳范围内的短时位姿估计，对点云的每个点做运动补偿，把所有点都变换到激光扫描结束时刻的坐标系下。这个步骤做不好，后面的所有匹配都会带着系统性误差。

2.4 IMU预积分与状态传递

IMU预积分这个概念现在已经是多传感器融合SLAM的基础操作了。它的核心思想是：把两帧图像或者两个关键帧之间的所有IMU测量值预先积分出一个相对运动增量，这个增量不考虑起点状态，所以可以直接放进因子图里作为一个相对约束。

VIL-SLAM沿用并改进了这一套路。IMU预积分因子同时约束了相对旋转、相对速度和相对位置，还顺带估计了加速度计偏置和陀螺仪偏置。这里有一个在工程里很要命的细节：IMU偏置项在优化过程中如果被更新，预积分的结果理论上也要跟着变化，否则残差会变得不一致。一般做法是一个周期内保持偏置不变，周期结束再用最新的偏置重新传播一遍。论文里对这块的处理没有展开太多，但实际操作时这一块的实现细节直接决定了整个系统长时间运行的稳定性。

3. 关键环节的工程实现细节

3.1 局部地图与关键帧管理

VIL-SLAM对地图的管理方式和ORB-SLAM比较相近，维护一个局部地图窗口，里面包含当前关键帧的共视关键帧及其观测到的路标点。激光雷达部分则单独维护一份局部体素地图，用于scan-to-map匹配。体素地图的好处是支持增量式更新，新扫描进来的点只需要更新对应的网格，而不是整个重建。

关键帧的插入策略直接关系到优化频率和系统漂移。论文里给出的判据包括：当前帧跟踪特征点比例是否过低、当前帧与最近关键帧的平移量是否超过阈值、旋转角度是否超过阈值等。这个策略的目的是让关键帧既不会太密导致优化频繁，也不会太疏导致约束不足。我在自己的系统里控制的阈值大概是平移0.2米、旋转10度，但具体数值还是要看机器人的运动速度和使用场景来调。

3.2 触发式LiDAR因子是如何工作的

这是整篇论文里我最喜欢的设计。大多数融合方案，比如LIO-SAM，是把LiDAR因子默认为每一帧都开启的，也就是说每来一帧激光，都要做一次scan-to-map匹配并把结果送进因子图。这个方式精度确实高，但它要求LiDAR帧率不能太低，而且scan-to-map匹配本身相当耗计算量。

VIL-SLAM换了一个思路：先跑一段视觉惯性里程计，一旦发现当前帧跟踪的特征数和平均视差发生明显退化，就触发一次LiDAR扫描匹配，把当前位姿和最近的局部点云地图对齐，然后把这个对齐后的相对位姿作为一次性的强约束丢进因子图。这样一来，系统在视觉良好的时候几乎不消耗LiDAR匹配的计算量，只有视觉开始不靠谱时，激光雷达才出来“救场”。

触发条件本身也值得细读，论文里综合了特征数量和视差两个维度。特征数量低代表当前图像可用的信息量不足，视差小代表相机在快速旋转或者移动缓慢，这两种情况都容易让纯视觉里程计漂移。测试里这个机制表现得比全时段稠密融合还要稳，原因在于LiDAR匹配在视觉退化时引入的约束是“精确打击”，而不是高频的噪声式修正，给了优化器更强的可观测性。

3.3 回环检测与全局位姿修正

回环检测模块依旧由视觉来完成。VIL-SLAM维护了一个DBoW2的词袋模型数据库，当前关键帧的词袋向量在数据库里搜索，找到相似度足够高的历史关键帧就认为存在回环候选。这一步计算成本不高，因为词袋检索本质上就是向量空间里的最近邻搜索。

当回环被确认后，系统会执行一次位姿图优化，把累积的漂移一次性拉回正确轨道。这个优化只调整关键帧位姿的相对变化，然后通过生成新的路标点坐标来保持地图的一致性。注意，这里做的是全局的位姿图优化而不是全部路标点的联合优化，因为后者计算量太大，对于实时系统来说不可接受。

我实际使用中发现，回环检测在有大量重复结构的工厂、仓库环境里容易产生假阳性。应对办法是增加连续几帧的确认机制和几何一致性校验，VIL-SLAM里用的是相对位姿校验，也就是用当前帧和候选帧之间通过特征匹配算出的基础矩阵来判断物理上是不是真的闭环。

3.4 程序初始化与传感器时间同步

一套融合系统能不能跑起来，很多时候卡在初始化而不是算法本身。VIL-SLAM的初始化分两步：先利用双目视觉重建出带有尺度信息的初始地图，然后和IMU的预积分对齐，估计出重力方向、尺度修正量和IMU偏置的初值。这跟VINS-Mono初始化思路相似，但双目的存在让尺度问题天然被解决了一大半。

时间同步也是个极其容易被忽视的坑。三个传感器的数据频率完全不同，IMU可能到200Hz，相机30Hz，激光雷达10Hz，如果时间戳对不齐，融合出来的结果会让误差直接翻倍。实际工程中我一般先用硬触发方式把相机和IMU做时间同步，LiDAR的时间戳单独标定补偿，采样系统时钟校准，保证整套系统的延迟偏差在几毫秒以内。

4. 实验效果和不同方案对比

4.1 数据集评估的维度

论文在自建数据和公开数据上都做了测试。评估指标主要包括绝对轨迹误差（ATE）和相对位姿误差（RPE），同时统计了系统各模块的运行耗时。ATE衡量的是全局建图轨迹的准确程度，RPE衡量的是局部里程计的平滑性和稳定性，两个指标一起看才能对系统性能有个全面判断。

对比方法里很自然的选了ORB-SLAM2（视觉基准）、LIO-SAM（激光惯性基准）、以及VINS-Fusion（视觉惯性基准）。这几套系统基本代表了单传感器和双传感器融合的天花板，和它们对比才能体现出三传感器融合的增量价值。

4.2 实验结果说明了什么

在视觉退化场景，比如灯光忽明忽暗的走廊、大量白墙的室内环境，纯视觉方案（ORB-SLAM2、VINS-Fusion）的轨迹基本会在一段时间后发散，而VIL-SLAM因为有激光的兜底约束，轨迹保持得比较稳。在几何特征退化的场景，比如宽敞平坦的停车场，纯激光方案LIO-SAM出现了垂直方向的漂移，VIL-SLAM则因为视觉提供了额外的非几何约束，漂移被明显抑制。这种互补性正是多传感器融合方案最核心的价值。

从运行耗时上看，VIL-SLAM在开启触发式LiDAR因子时，单帧平均耗时仍然在实时范围以内。这个结果非常关键，因为很多精度优秀的方案跑不到实时，而VIL-SLAM在保证精度的前提下维持了实时性，这也是它能落地到实际机器人的原因。

5. 从论文到工程落地的几点思考

5.1 论文里没写但实践必须要做的几件事

第一件事就是传感器标定。外参标定的精度直接决定了融合系统性能的上限。我当时专门花了一周时间做相机到IMU、LiDAR到IMU的外参标定，使用了多种标定工具交叉验证，确保误差在厘米级。如果外参标定不准，后面所有的融合都像戴着度数不对的眼镜看东西，看什么都模糊。

第二件事是坐标系的统一。双目左目坐标系、IMU坐标系、LiDAR坐标系、车身坐标系，这四个坐标系之间的变换关系必须理清楚。我见过不少系统跑飞，最后查到原因是坐标系变换用反了。建议在代码里把每个坐标系和变换矩阵的变换方向用注释写明确，建议统一使用刚体变换的齐次矩阵表示，避免欧拉角的万向锁问题。

第三件事是失效降级策略。再好的融合系统也不能保证所有场景100%不失效。实际工程里我会额外实现一个监控模块，实时检查优化后的残差、特征数量、匹配得分等指标，一旦异常就触发系统级降级，比如从三传感器融合切换到视觉惯性模式，至少保证机器人还能安全停下来，而不是直接崩溃。

5.2 关于计算平台和实车部署

在算力有限的平台上跑这种系统需要提前做调整。比如用RK3588这类ARM平台时，建议把ORB特征的图像分辨率降到640x480，同时限制特征点数量，关闭光流金字塔的第三层，这样能省出不少CPU资源。激光雷达部分如果用机械式雷达，可以适当减小点云体素滤波的分辨率，比如从0.2米放宽到0.3米，换回一些实时性。

部署的时候还建议加上动态物体过滤。论文默认场景偏向静态，但实际道路上有车、有人，动态物体会给视觉特征和激光点云引入很大的异常值。动态物体的处理是个大课题，简单做法是用语义分割把人和车检测出来，然后把对应区域的特征点和点云剔除，虽然粗暴，但效果立竿见影。

5.3 这个框架还能往哪个方向扩展

VIL-SLAM的框架设计有一个很大的优势——传感器因子可以灵活替换和添加。比如把双目相机换成RGB-D相机，或者添加一个毫米波雷达因子到同一张因子图上，不需要改动后端主体结构，只需要定义好新因子的残差模型。这种扩展性在目前的研究和工程中非常有用。

另外，近年来出现的可学习特征、端到端里程计也可以作为新的视觉因子接入到这个框架中，理论上能够处理更多退化场景。不过要注意，任何深度学习模块的引入都要考虑推理延迟和平台算力，不能为了性能牺牲实时性。

我个人在实际操作中的体会是，VIL-SLAM最大的贡献不是某一项单独的技术创新，而是它把多传感器融合从“拼参数”提升到了“拼设计”的层面，按需触发、紧耦合、模块化扩展，这套设计哲学对于任何做机器人定位系统的人来说都有参考价值。翻译完整篇论文以后，我又回头把自己手头的融合代码重构了一遍，就是按照它的思路把LiDAR约束改成了触发式加入，定位精度不仅没有下降，耗时还降低了将近30%。

最后再分享一个小技巧：如果你也想在自己的系统里复现这个方案，可以先不管回环检测和全局优化，先把视觉惯性里程计和触发式LiDAR因子跑通，这两块做好了，系统的整体骨架就已经立住了，后面再逐步加回环和其他模块，排查问题会容易得多。按照这个节奏，不建议一上来就全量实现，代码调试会难度陡增。