未知环境下基于视觉与激光融合的SLAM系统研究与实现

曾光 黄健盛 佟景泉 黄杨灵

收稿日期：2023-07-30

基金项目：2021年广东省科技创新战略专项资金（“攀登计划”专项资金）项目（pdjh2022b0855，pdjh2022b0854）；2023年广东省科技创新战略专项资金（大学生科技创新培育）项目（pdjh202360852）

DOI：10.19850/j.cnki.2096-4706.2024.04.015

摘  要：在未知环境中实现机器人的自主导航与环境建图是一个极具挑战的研究课题，SLAM是应对这一挑战的关键技术之一。基于研究现状分析、系统方案设计、技术实现与实验验证等内容，结合视觉与激光各自的优势，设计一种未知环境下基于视觉与激光融合的SLAM系统。通过针对关键帧和滑动窗口的平衡选取策略以及分类优化策略对初始位姿和特征点进行优化，提高了系统的鲁棒性，可以输出高精度地图；通过实现以智能小车为载体的演示系统验证了所提出算法的可行性和实用性。

关键词：未知环境；视觉；激光；SLAM

中图分类号：TP389.1；TP242；TN958.98   文献标识码：A 文章编号：2096-4706（2024）04-0074-05

Research and Implementation of SLAM System Based on Vision and Laser Fusion in Unknown Environment

ZENG Guang， HUANG Jiansheng， TONG Jingquan， HUANG Yangling

（Guangdong Communication Polytechnic， Guangzhou  510650， China）

Abstract： Realizing autonomous navigation and environmental mapping of robots in unknown environments is a highly challenging research topic， and SLAM is one of the key technologies to address this challenge. Based on the analysis of research status， system scheme design， technical implementation， and experimental verification， combined with the advantages of vision and laser， a SLAM system based on vision and laser fusion in an unknown environment is designed. By using a balanced selection strategy for keyframes and sliding windows， as well as a classification optimization strategy， the initial pose and feature points are optimized， which improves the robustness of the system and can output high-precision maps; the feasibility and practicality of the proposed algorithm are verified through the implementation of a demonstration system using intelligent cars as carriers.

Keywords： unknown environment; vision; laser; SLAM

0  引  言

在搜索救援等復杂场所（如隧道、矿井、仓库、大型写字楼和地下停车场等场所），人们经常面对充满未知和挑战的环境。在这些环境中，构建实景地图以及实现自主导航的能力至关重要。此外，为了保障工作人员的安全和提高生产效率，需要提供一种能够在复杂环境中自主运动的解决方案。移动机器人因其自身的灵活性和适应性，成为解决诸多棘手问题的绝佳选择。

然而，在未知环境中，移动机器人面临着三个基本问题：“我在什么地方？”“我周围环境是怎样的？”“我如何到达目标位置？”这些问题涉及机器人的定位、环境感知和路径规划[1]。只有进行准确的定位和建图，机器人才能在未知环境中自主导航，实现高效的任务执行。

为了积极应对这些挑战，同步定位与地图构建（SLAM）技术应运而生。SLAM是一种关键技术，指的是安装相机、激光雷达等传感器的运动主体，通过传感器获取的信息在未知环境中估计自身的运动轨迹，实时建立环境模型。尤其是基于视觉与激光融合的SLAM系统，使移动机器人能够在未知环境中获取并感知自身定位与环境的空间几何结构信息。借助SLAM技术，机器人可以构建栅格地图或拓扑地图，从而增强其对环境的感知和导航能力。

1  研究现状

目前，室内导航的两个主要研究主题分别是基于视觉的SLAM技术和基于激光的SLAM技术，这两个研究主题在计算机视觉和移动机器人导航领域都受到广泛关注。此外，将视觉和激光数据进行融合（得出视觉激光融合SLAM），能够有效克服各自单独应用时的局限性，获得更准确和鲁棒性更高的导航与建图能力。

1.1  视觉SLAM

视觉SLAM利用低成本相机和计算机视觉技术，对连续图像序列的特征运动进行感知，从而实现对相机运动的估计和导航位姿的计算。这种技术使移动机器人能够在未知环境下自主定位和建立地图，为准确导航和路径规划提供关键信息。视觉SLAM因其独有的灵活性和成本效益，在计算机视觉和移动机器人领域得到了广泛的应用与研究[2]。RGBD-SLAM[3]、LSD-SLAM[4]、MonoSLAM[5]和ORB-SLAM[6]等都属于典型的视觉SLAM算法。相关研究内容如下：

1）提高匹配鲁棒性。在光照条件较差或纹理特征较弱的环境下，视觉SLAM面临着匹配精度下降的困境。相关学者致力于开发鲁棒的特征提取和匹配算法，以应对复杂环境的变化影响和噪声影响。

2）解决尺度不确定性问题。在纯视觉SLAM中，单目相机无法直接测量场景中的绝对尺度，因此会存在尺度不确定性的问题。相关学者提出了多视图几何约束和结构化光度误差等方法，用以解决尺度不确定性问题并提高定位精度。

3）拓展应用范围。当前视觉SLAM主要应用于室内环境，在室外和大场景下，其性能和效用受到一定的限制。相关学者致力于解决视觉SLAM在室外场景中的定位问题，通过增加传感器（如IMU）的方式提高视觉SLAM在复杂场景中的性能。

1.2  激光SLAM

激光SLAM利用激光雷达快速扫描场景，获取丰富的三维点云数据，对连续帧的激光点云进行匹配，实现导航计算和实时建图。GMapping[7]、Cartographer[8]、Hector SLAM[9]和Karto SLAM[10]等都属于典型的激光SLAM算法。相关研究内容如下：

1）提高实时计算效率。激光SLAM需要处理大量的三维点云数据，为此对激光SLAM的实时计算性能提出了更高的要求。研究人员通过优化算法和硬件加速等手段，提高激光SLAM的实时计算性能，以适应环境的快速变化。

2）解决动态场景稳健重定位问题。在存在动态障碍物的环境中，激光SLAM面临着受障碍物阻挡而重新定位的挑战。研究人员提出了滤波、回环检测和特征选择等方法，用以提高激光SLAM的稳健性和可靠性。

3）粗差处理和闭环处理。激光SLAM中由于存在传感器噪声和运动估计误差，会产生粗差数据。研究人员致力于开发粗差处理算法，并利用闭环检测与优化方法进一步提高激光SLAM的建图精度和稳定性。

1.3  视觉与激光融合SLAM

视觉激光融合SLAM是指将视觉和激光数据相互融合，优势互补，获得更强大的定位与建图能力。Zhang等学者提出的V-LOAM（Visual-Lidar Odometry and Mapping）算法紧耦合运作，利用等速度高斯过程建模运动，实现高精度的导航位姿估计[11]。而LVIO（Laser-Visual-Inertial Odometry）算法是一种融合3D激光雷达、相机和惯性测量单元数据的技术，通过整合这些传感器的信息来构建大范围的环境地图[12]。这种融合技术能够克服单一传感器的局限性，可提供更加全面和准确的环境感知能力。这些算法通过顺序的、多层处理线程解决了传统方法中卡尔曼滤波或因子图优化难以解决的问题，从而可实现更优秀的定位与建图效果。

总体而言，视觉SLAM、激光SLAM和视觉激光融合SLAM都是目前移动机器人导航领域具有重要研究意义的技术。视觉SLAM通过低成本相机和计算机视觉技术实现导航位姿计算，适用于相对较小的场景和资源受限的应用。激光SLAM则依赖激光雷达获取三维点云数据，适用于相对较大的场景和实时性要求较高的场景。而视觉激光融合SLAM则充分实现视觉和激光数据的优势互补，提供更加鲁棒和精确的导航和建图结果，适用于复杂的未知环境。

然而，视觉SLAM、激光SLAM和视觉激光融合SLAM仍将面临一系列的挑战和机遇。对于视觉SLAM，如何进一步提高匹配鲁棒性、解决尺度不确定性以及拓展应用范围将是重要的研究方向。对于激光SLAM，同样存在一些具有挑战性的问题，例如如何提高实时计算效率、如何解决动态场景下的稳健重定位问题以及如何处理粗差和闭环问题。而对于视觉激光融合SLAM，如何更好地融合视觉和激光数据，进一步提高定位和建图的精度和稳定性，以及如何应用于更广泛的领域，也将是未来的重点研究方向。

2  系统方案

未知环境下基于视觉与激光融合的地图构建与定位导航系统的研究方案，通过不同类型的场景感知传感器实现同时定位导航与地图构建，主要包括前端处理和后端处理两个阶段，进一步探讨视觉激光融合的关键技术，主要流程如图1所示。

2.1  前端處理

2.1.1  数据采集与时空基准对齐

在前端处理阶段，首先进行数据采集，同时从不同传感器（如激光雷达、视觉相机和惯性测量单元）获取环境信息。数据采集的关键是对多传感器数据进行时空基准对齐，确保融合后的数据具有一致的时间戳和坐标系，为后续的数据处理奠定基础。

图1  本系统方案实现流程图

2.1.2  激光点云与视觉特征融合

对前端处理过程中用到的数据融合策略进行优化，特别是激光点云与视觉特征的融合。激光点云可提供丰富的三维空间信息，而视觉特征则具有更强的纹理信息。通过将两者融合，可以进一步提高定位的准确性和鲁棒性。本研究将采用深度学习技术、卷积神经网络对激光点云进行特征提取，并与视觉特征进行融合。这样的深度学习融合方法有助于解决尺度不确定性和环境变化对定位导航的影响。

2.2  后端处理

2.2.1  回环检测与全局优化

在后端处理阶段，引入回环检测技术来判断传感器是否曾到过之前的位置。回环检测可有效解决传感器长时间运动后的漂移问题，提供更准确的位置约束。本研究将应用全局平差优化技术对前端推算的位姿轨迹进行优化，以进一步提升导航位姿计算的精度和稳定性。全局平差优化能够兼顾多个时刻的观测数据和约束信息，对位姿轨迹进行全局调整，从而减小误差累积，提高定位精度[2]。

2.2.2  计算量优化策略

在后端位姿和特征点大范围优化过程中的计算量较大。为了解决这个问题，本研究将采用基于关键帧和滑动窗口的平衡选取策略来平衡优化计算负担。在处理关键帧的时候，将根据其重要性进行优先级排序，以排除不必要的计算。此外，本研究还将采用基于特征点和位姿的分类优化策略，将优化过程划分为不同的层次和优先级，以减少计算量并提高算法的效率和实时性。

2.3  场景重建与地图构建

在全局优化的位姿与传感器数据进行融合后，本研究将利用重建后的位姿信息和融合后的数据，构建场景的三维地图。地图构建过程涉及点云配准、建图算法以及数据融合等关键技术。通过将视觉和激光数据相融合，系统将获得更准确和更丰富的环境信息，实现对未知环境的建图与重建。

2.4  算法实现与系统集成

为了验证本研究提出的视觉与激光融合的SLAM系统，我们将在实际场景中进行算法实现与系统集成。选择合适的硬件平台（包括激光雷达、相机和惯性测量单元），搭建移动机器人实验平台。开发基于前端处理和后端处理的算法模块，并进行实时数据处理和优化。在算法实现过程中，将重点测试视觉激光融合的效果，验证其对定位导航和地图构建的性能提升。

3  技术实现

未知环境下基于视觉与激光融合的SLAM系统的技术实现，涉及软件和硬件两个层面的内容，具体包括问题描述与建模、数据处理与优化以及软硬件系统的实现。

3.1  问题描述与建模

在本研究中，首先需要描述SLAM问题并进行建模。为了实现机器人的自主导航与环境建图，我们采用了运动轨迹分解的方法。具体而言，我们将小车在一段连续时间内的运动轨迹划分为离散的点（时刻），并记录每个点（时刻）的位置信息，从而构建车辆的运动轨迹[12]。同时，地图构建是基于许多特征点的集合。每个时刻，传感器会测量一部分特征点，并获取它们的观测数据。通过分析这些数据，我们可以建立数学模型来描述机器人的运动轨迹以及观测过程[13]。运动方程和观测方程的建立是关键步骤，它们使得机器人能够在未知环境中实现自主定位与地图构建。在对这些方程进行求解的过程中，将传感器数据的不确定性和运动过程中可能的误差考虑在内，以确保复杂环境下定位与建图的准确性和稳定性。通过结合特征点和传感器数据，我们能够获得丰富的环境信息，进而构建出3D地图，为机器人在未知环境中的正确导航提供重要支持。

3.2  数据处理与优化

视觉和激光数据的融合是本项目的关键技术之一。在数据处理与优化阶段，我们将结合前端数据处理和后端算法优化两部分来实现视觉与激光的融合，具体流程如图2所示。

图2  本系统数据处理与优化模块实现流程图

3.2.1  前端数据处理

在前端数据处理阶段，我们专注于对预处理后的雷达和相机数据进行特征提取。对于雷达数据，我们着重提取ORB特征点，而对于相机数据，我们主要关注点云边缘平面特征点的提取。对这些特征点的选取是为了在融合视觉与激光数据时，能够提供丰富而有效的信息，从而实现更准确、鲁棒性更强的定位与建图。通过对特征点的提取和匹配，我们可以将激光和视觉数据松耦合地输入到位姿估计模块中，从而实现对机器人运动轨迹的初步估计[12]。这一前端数据处理的步骤为后续的全局优化和地图构建提供了重要的初始信息，为机器人在未知环境中的自主导航和建图奠定了基础。

3.2.2  后端算法优化

后端算法优化的目标是进一步优化初始位姿和地图特征点。后端算法优化涉及多个关键步骤，其中包括将所得到的初步估计位姿映射到世界坐标系下，并将特征点输入到全局优化模块。在全局优化阶段，我们采用平衡选取策略来处理关键帧和滑动窗口，同时还采用分类优化策略对初始位姿和特征点进行进一步的优化，以得到最终的高精度地图[12]。这一后端算法的优化过程对于整个系统的性能提升至关重要，它能够有效地改进定位导航的精度和稳定性，构建出更为准确和全面的环境地图。通过这样的后端算法优化，视觉与激光融合的SLAM系统能够在未知复杂环境中实现自主导航与建图，为搜索救援、室内定位、自主巡航等领域带来更广泛的应用前景。

3.3  软硬件系统实现

在软件层面，我们将编写上位机终端控制软件，实现对移动机器人的控制和导航。同时，进行前端数据处理和后端算法优化的软件开发，整体实现框架如图3所示。

圖3  软硬件系统实现框图

在硬件层面，我们将搭建移动机器人实验平台，如图4所示。该平台将基于树莓派，以智能小车的形式呈现。我们会选择合适的激光雷达、相机和惯性测量单元作为传感器，搭建硬件设备，进行调试和测试。通过软硬件系统的集成，我们能够实现完整的视觉与激光融合的SLAM系统。

图4  本研究对应系统硬件载体

4  实验验证

为验证未知环境下基于视觉与激光融合的SLAM系统的性能和有效性，我们进行了一系列的实验，实验结果如图5所示。实验涵盖了系统的SLAM建图能力、机器视觉功能以及智能操控特性。

图5  上述实验对应的实现结果展示

4.1  SLAM建图实验

在SLAM建图实验中，我们使用了Hector、Gmapping和Karto等多种算法，分别进行了激光SLAM自主建图以及视觉与激光融合的3D地图构建。为了模拟未知环境，我们在实验室内搭建了一个复杂的场景，包含走廊、房间和障碍物等元素。

实验结果表明，激光SLAM算法在未知环境中能够准确地建立地图，实现自主导航。同时，通过加载RGB-D深度相机，我们成功实现了视觉与激光的融合，构建出更为丰富的3D地图。该功能在搜索救援、室内定位和自主巡航等领域具有广泛的应用前景。

4.2  机器视觉实验

在机器视觉实验中，我们使用RGB-D深度相机和激光辅助，对多种视觉功能进行了验证。我们实现了人脸识别、目标检测、边缘检测、遁迹巡航以及AR标签检测等功能。

实验结果显示，系统能够准确地进行人脸识别和目标检测，在边缘检测和遁迹巡航方面也表现出良好的性能。AR标签检测功能在工业自动化生产等领域具有潜在的应用价值。机器视觉功能的验证进一步证明了系统在复杂环境下的实用性和稳定性。

4.3  智能操控实验

在智能操控实验中，我们对系统进行了不同方式的控制，包括语音控制、手柄控制、PC端控制以及手机端APP控制。我們还测试了系统的跟随、避障和巡航功能。

实验结果显示，系统对于不同控制方式均具有良好的响应性和准确性。跟随功能能够实现对目标的稳定跟踪，避障功能有效地避免了碰撞。智能巡航功能使得机器人能够在未知环境中自主导航，表现出较高的智能化水平。

5  结  论

本文研究了未知环境下基于视觉与激光融合的SLAM系统，并设计了一套完整的技术实现路线。通过系统的实验验证，我们证明了该系统在SLAM建图、机器视觉和智能操控方面的优良性能。然而，本系统仍然存在一些待解决的问题，例如进一步提高SLAM建图算法的实时性和精确性，增强机器视觉功能的鲁棒性和准确性，以及优化智能操控策略，使其能够应对更复杂的环境和任务。在未来的研究中，我们将继续改进和优化系统的各个模块，推动视觉与激光融合的SLAM技术在实际应用中发挥更大的作用，为搜索救援、室内定位、自主巡航等领域带来更多的创新和进步。

参考文献：

[1] 颜世博.未知环境中移动机器人自定位技术研究 [D].哈尔滨：哈尔滨工业大学，2012.

[2] 高扬，胡庆武，郭浩.视觉激光匹配导航技术及其应用 [J].现代导航，2021，12（5）：313-318.

[3] SCHERER S A，ZELL A. Efficient onbard RGBD-SLAM for autonomous MAVs [C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo：IEEE，2013：1062-1068.

[4] 王丹，黄鲁，李垚.基于点线特征的单目视觉同时定位与地图构建算法 [J].机器人，2019，41（3）：392-403.

[5] DAVISON A J，REID I D，MOLTON N D，et al. MonoSLAM：real-time single camera SLAM [J].IEEE Transactions on Pattern Analysis and Machine intelligence，2007，29（6）：1052-1067.

[6] MUR-ARTAL R，MONTIEL J M M，TARDOS J D. ORB-SLAM： A Versatile and Accurate Monocular SLAM System [J].IEEE Transactions on Robotics，2015，31（5）：1147-1163.

[7] BALASURIYA B L E A，CHATHURANGA B A H，JAYASUNDARA B H M D，et al. Outdoor robot navigation using Gmapping based SLAM algorithm [C]//2016 Moratuwa Engineering Research Conference （MERCon）. Moratuwa：IEEE，2016：403-408.

[8] 陆世东，涂美义，罗小勇，等.基于图优化理论和GNSS激光SLAM位姿优化算法 [J].激光与光电学进展，2020，57（8）：216-225.

[9] WEI W C，SHIRINZADEH B，ESAKKIAPPAN S，et al. Orientation Correction for Hector SLAM at Starting Stage [C]//2019 7th International Conference on Robot Intelligence Technology and Applications （RiTA）. Daejeon：IEEE，2019：125-129.

[10] GAO H M，ZHANG X B，WEN J，et al. Autonomous Indoor Exploration Via Polygon Map Construction and Graph-Based SLAM Using Directional Endpoint Features [J].IEEE Transactions on Automation Science and Engineering，2018，16（4）：1531-1542.

[11] ZHANG J，SINGH S. Laser-visual-inertial odometry and mapping with high robustness and low drift [J].Journal of Field Robotics，2018，35（8）：1242-1264.

[12] 余祖俊，张晨光，郭保青.基于激光与视觉融合的车辆自主定位与建图算法 [J].交通运输系统工程与信息，2021，21（4）：72-81.

[13] 林义忠，马凯.室内移动机器人自主导航系统设计 [J].自动化与仪表，2021，36（6）：38-42.

作者简介：曾光（1990—），男，汉族，广东河源人，讲师，硕士研究生，研究方向：信息通信、机器视觉；黄健盛（2002—），男，汉族，广东汕头人，研究方向：城轨通信信号；佟景泉（1984—），男，汉族，黑龙江哈尔滨人，讲师，硕士研究生，研究方向：信号控制、交通运输；黄杨灵（2001—），男，汉族，广东高州人，研究方向：城轨通信信号。