LE-VINS：固态激光雷达增强的视觉惯性导航系统-平芜编程栈

1. 摘要

在光照剧烈变化、动态物体、弱纹理等视觉退化场景，视觉路标点的深度估计难度急剧增加，导致视觉惯性导航系统 (VINS) 的精度和鲁棒性恶化。采用非重复扫描原理的固态激光雷达，为解决视觉路标点深度估计问题带来便利。面向低速机器人应用，我们提出了一种固态激光雷达增强的视觉惯性导航系统 (LE-VINS)，惯性导航系统 (INS) 位姿辅助了固态激光雷达点云积累，实现了视觉特征和激光雷达点云的有效关联，为视觉路标点提供了精确的深度测量，提高了系统在复杂环境下导航定位的精度和鲁棒性。

2. 引言

视觉惯性导航系统 (VINS) 被广泛应用于移动机器人、自动驾驶、三维重建等领域。一般地，视觉路标点深度由视觉特征的三角测量得到，其深度估计精度直接影响系统定位精度和鲁棒性。然而，路标点深度估计的准确性一方面受光照变化、动态物体和弱视觉纹理等环境因素影响，另一方面受基于微机电系统 (MEMS) 的惯性导航系统 (INS) 位姿精度影响。多目相机能直接估计路标点的深度，但会受到相机间基线长度、外参标定精度及环境因素等的影响。

激光雷达 (LiDAR) 能够直接进行精确的深度测量，可以被用于增强视觉导航系统。三维机械扫描式LiDAR能够实现360°水平视角的测量，但垂直视场角较小且垂直方向角分辨率低，导致相机和激光雷达的视场重叠区域少（尤其是低线束LiDAR），不利于视觉特征和激光雷达点云的关联。采用非重复扫描模式的低成本固态激光雷达，如Livox Mid-70，通过累积一段时间的激光雷达点云，可以获得相对稠密的点云地图，从而有效覆盖相机图像的大部分区域，为视觉特征和激光雷达点云深度关联带来了便利，如图1所示。

图 1：固态激光雷达Livox Mid-70（70°圆形视场角）和相机（水平视场角约80°）的视场角示意图（此处点云累积了20秒，根据点云的反射强度进行的颜色渲染）

面向低速机器人导航应用，我们提出了一种固态激光雷达增强的视觉惯性导航系统LE-VINS (Tang Hailiang et.al, 2023)，实现了大尺度复杂环境的实时、稳健、精确的定位。LE-VINS是在我们前期工作IC-GVINS (Niu Xiaojiet. al, 2023) 基础上，引入固态激光雷达，为视觉路标点提供精确的深度测量。LE-VINS的设计原理见图2，主要贡献如下：

采用非重复扫描模式的固态激光雷达为视觉路标点提供精确的深度信息。在因子图优化 (FGO) 的框架进行视觉特征点、激光雷达深度和IMU测量的紧耦合，实现了最大后验状态估计。
提出了一种鲁棒的深度关联算法，实现了视觉特征和INS位姿辅助的固态激光雷达点云的有效关联，精确估计了视觉路标点的深度。同时，提出了一种有效的深度校验方法，显著提高了路标点深度估计的鲁棒性。
估计的深度信息不仅被用作路标点的初始深度，还被用于在FGO中构建激光雷达深度因子，以直接约束路标点的深度。如果路标点被边缘化，则可以将激光雷达深度因子转换为先验因子，从而保留对位姿状态的精确约束。
基于低速机器人载体在大尺度复杂环境中进行了实测实验。结果表明，相比于IC-VINS系统，LE-VINS显著提升了导航定位的鲁棒性和准确性。与此同时，LE-VINS优于主流的激光雷达视觉惯性导航系统和激光雷达增强方法。

3. 系统概述

4. 固态激光雷达增加的视觉惯性导航系统

4.1. 点云投影

4.2. 鲁棒的深度关联

4.3. 因子图优化

5. 实验结果

6. 总结

本工作充分发挥了固态激光雷达非重复式扫描的特性，实现了视觉特征和激光雷达点云深度的有效关联。通过构建激光雷达深度测量因子，在因子图优化框架下为视觉路标点提供了有效的深度测量。激光雷达深度测量直接有效地约束了视觉路标的深度估计，从而显著提高了导航定位精度。同时，提高了视觉特征跟踪的连续性，进一步提高了视觉惯性导航系统的鲁棒性。本工作在我们前期开源项目 IC-GVINS (Tang Hailiang, 2022) 基础上研究并实现的。

参考文献

LE-VINS：固态激光雷达增强的视觉惯性导航系统

Hailiang Tang, Xiaoji Niu, Tisheng Zhang, Liqiang Wang, and Jingnan Liu, “LE-VINS: A Robust Solid-State-LiDAR-Enhanced Visual-Inertial Navigation System for Low-Speed Robots,” IEEE Trans. Instrum. Meas., vol. 72, pp. 1–13, 2023, doi: 10.1109/TIM.2023.3260279.

Xiaoji Niu, Hailiang Tang, Tisheng Zhang, Jing Fan, and Jingnan Liu, “IC-GVINS: A Robust, Real-Time, INS-Centric GNSS-Visual-Inertial Navigation System,” IEEE Robotics and Automation Letters, vol. 8, no. 1, pp. 216–223, Jan. 2023, doi: 10.1109/LRA.2022.3224367.

Hailiang Tang, “IC-GVINS: A Robust, Real-time, INS-Centric GNSS-Visual-Inertial Navigation System,” 2022. [Online]. Available: https://github.com/i2Nav-WHU/IC-GVINS.