基于激光雷达增强的视觉SLAM 多机器人协作地图构建方法的研究

李燊阳 邓三鹏* 权利红 祁宇明 丁昊然 刘天慧

（1天津职业技术师范大学机器人及智能装备研究院，天津，300222；2 天津市智能机器人技术及应用企业重点实验室，天津，300350；3 天津博诺智创机器人技术有限公司，天津，300350）

0 引言

随着机器人技术的快速发展，多机器人协作系统在工业自动化、无人驾驶等领域得到广泛应用。地图构建作为多机器人系统中的重要任务，对于机器人的导航、定位和路径规划具有关键意义。然而，环境中的不确定性和动态性给地图构建带来了挑战，特别是在复杂、大规模的场景中，这种不确定性和动态性使得多机器人协作地图构建变得越发艰难。

为应对多机器人协作地图构建问题，CAMPOS 等[1]提出的ORB-SLAM3 算法，在原有的SLAM 系列算法的基础上，添加并构建了一个Atlas 地图集和一个DBoW2关键帧数据库。通过对DBoW2 关键帧的跟踪匹配，实现了多机器人协作地图构建的重定位、循环闭合和地图合并。但ORB-SLAM3 算法仅能构建出稀疏点云地图，地图包含的信息量少。林沛杨等[2]提出的基于视觉SLAM多机器人协作地图构建算法，虽构建出了稠密点云融合地图，但在光线强度等环境因素变化的情况下，视觉SLAM 算法的鲁棒性和时效性会受到较大的影响。

针对以上问题，本文提出了一种结合激光雷达和视觉传感器的多机器人协作SLAM 方法。首先，各机器人以ORB-SLAM3 为基础，以自身为主机器人开始建图，利用改进的卡尔曼滤波算法计算出雷达相机的融合位姿，根据该位姿得到局部稠密点云地图。然后，利用关键帧跟踪模型和ICP 算法得到机器人之间的坐标转换关系，进而得到各机器人的世界坐标系。最后，在世界坐标系上将局部点云拼接与滤波，得到稠密点云融合地图。同时，本文将多个机器人进行有效的通信和协调，实现地图数据共享和信息交换，提高了整个系统的定位精度和地图构建精度。

1 单机器人局部地图的创建

1.1 ORB-SLAM3 简介

ORB-SLAM3 是在基于ORB 特征的三维定位与地图构建算法ORB-SLAM2 和ORB-SLAM-VI 的基础上扩展改进得到的开源的、支持多种相机的实时视觉SLAM 系统。它能够借助针孔和鱼眼模型，使用单目、立体或红绿蓝—深度（RGB-D）传感器在纯视觉或者视觉惯导模式下工作。相关实验表明，ORB-SLAM3 在EuRoC 数据集上的单目惯性单会话操作比VINS-Mono（一个基于视觉惯性里程计的闭环和多重地图功能的单目视觉系统）准确2.6 倍，且ORB-SLAM3 的地图集基于DBoW2 关键帧数据库构建，采用了具有高召回率的地点识别技术[3]。ORB-SLAM3 的整体框图如图 1 所示，图中，IMU、BA、Sim3、SE3 分别是惯性测量单元（Inertial Measurement Unit）、束调整（Bundle Adjustment）、相似变换（Similarity Transformation in 3D）、 特殊欧几里德群（Special Euclidean Group in 3D）的英文首字母缩写。

图1 ORB-SLAM3 框图

1.2 激光雷达与视觉传感器的数据融合

现有的激光雷达融合视觉传感器的做法都是在SLAM 前端的集成部分进行了传感器的数据融合[4]。ORB-SLAM3 在ORB-SLAM2 的基础上进行了扩展，现有ORB-SLAM3 的前端数据融合被称为RGB-D 模式，该模式是在ORB-SLAM3 系统框架外部进行相机与雷达数据的实时处理，将处理完成的数据放入ORB-SLAM3 系统模型中，进而估计出相机的位姿和地图点位置。这种模式下，由于传感器数据在系统模型外部处理，导致系统整体过于松散，时效性较差。

RGB-D 模式可以将之前生成和保存的深度图进行前端的数据集成，即将雷达传感器的点云数据（pcd）通过对应节点转换成深度图[5]，通过SLAM 接口集成进去，RGB-D 模式的数据融合流程图如图 2 所示。

图2 RGB-D 数据融合流程图

本文所设计的方法使用已开源的Depth 模块，在RGB-L（L 为雷达（LiDAR）的英文首字母）模式下使用传统的计算机视觉（Computer Vision，CV）在线对激光雷达数据进行上采样，将采样后的结果提供给SLAM接口，交由直接集成到ORB-SLAM3 的Depth 模块进行处理[6]。RGB-L 数据融合流程图如图3 所示。

图3 RGB-L 数据融合流程图

图4 基本原理图

图5 多机器人协作融合框图

图6 Gazebo 仿真环境

图7 局部地图和融合地图

Depth 模块通过扩展卡尔曼滤波器（EKF）进行实现。在机器人的实际运动中，相机和雷达的传感器位姿估计可能出现其中一个失效的情况。当实际环境不够明亮时，单独以雷达点云信息完成位姿估计，而当环境明亮时，将同时使用两种传感器，由相机的视觉数据提供地图点的视觉观测，激光雷达数据提供关于地图点的距离和角度观测，通过不断的观测和预测，对系统的状态进行估计和更新，进而结合ORB-SLAM3 系统模型，估计相机的位姿和地图点位置。基本原理如图 4 所示。

默认两个传感器都符合高斯分布，系统的方程为非线性可微分函数。观测模型采用雷达和相机的观测模型，而相机的位姿需要转换到雷达的极坐标系下，因此系统采用雷达、相机联合标定的转换矩阵进行转换，通过传感器模型实现观测信息的更新[7]。

2）预测步骤：根据上一个时刻的状态估计和控制输入，使用传感器模型对相机载体的状态进行预测。将时间段内的位姿增量作为输入，和分别代表相邻的时间戳：

3）更新步骤：将相机和激光雷达的测量数据与预测的状态进行对比，利用卡尔曼滤波的思想来融合测量和预测信息。EKF 使用线性化的近似方法来处理非线性的测量和状态模型。通过计算卡尔曼增益，将测量数据的可靠性与预测数据进行权衡，更新目标的位姿状态估计。

更新后的观测方程为：

通过RGB-L 模式下得到的机器人精准的位姿和深度图像，ORB-SLAM3 可以将稀疏特征点转换为稠密的3D 点云。通过调整ORB-SLAM3 的参数来进行稠密点云地图的构建，即设置参数“DENSE_RECONSTRUCTION=true”，使用半全局匹配（Semi-Global Matching， SGM）算法来估计连续帧之间的稀疏特征点的深度值。最后，通过积分这些深度值构建出每个机器人的局部稠密点云地图。

2 多机器人协作地图的构建

本文所提出的基于激光雷达增强的视觉SLAM 多机器人地图构建方法，分别在各自的机器人上，通过建立世界坐标系将局部地图进行完整地图的融合和构建。

2.1 世界坐标系的建立

在上述提到的ORB-SLAM3 主体框架里，存在着存放不同机器人关键帧的数据库DBoW2。数据库中相邻关键帧的位姿估计可以由关键帧跟踪模型和ICP 算法求得[7-8]。利用其估计的位姿可以将构建世界坐标系的过程分为如下步骤：

1）将协作地图构建的机器人放置在一起，并将机器人雷达相机朝向同一方向，使得机器人相机存在共识关系，以捕捉到足够多的特征点；

2）启动机器人各自相机的launch 文件，将观测到的关键帧通过ORB-SLAM3 系统处理后存放到DBoW2 的数据库中；

3）将各机器人DBoW2 关键帧数据库中存放的关键帧作为自身的基础帧，各机器人使用对应的基础帧进行世界坐标系的构建；

4）在构建完各自的世界坐标系后，机器人使用上文所设计的RGB-L 模块进行稠密点云地图的创建。

2.2 多机器人协作地图的融合

根据上文所述，各个机器人得到各自协作地图构建系统的世界坐标系后，利用融合RGB-L 的ORB-SLAM3算法进行整体稠密点云地图的创建。多机器人协作融合框图如图 5 所示。

地图融合创建的步骤如下：

1）在关键帧数据库中读取雷达点云和相机图片数据；

2）在相机图片关键帧具有共识关系的情况下，激活第二代机器人操作系统（Robot Operating System2,ROS2）相关的数据传输话题，根据关键帧追踪模型和ICP 算法计算出它们的坐标转换关系[9]，再根据坐标转换关系进行机器人世界坐标系的创建；

3）利用改进的卡尔曼滤波进行雷达、相机位姿的融合，获得机器人的位姿状态，通过积分深度图，获得局部稠密点云地图；

4）以步骤2）中的世界坐标系为基础，利用位姿转换关系进行局部地图的映射转换，将其映射到世界坐标系中，融合扩展为全局稠密点云地图。

3 实验结果及分析

本次实验在ROS2 通信框架和Gazebo 仿真平台上进行，ROS2 利用了分布式通信架构和域ID（ROS_DOMAIN_D）技术，不同机器人的节点只要在同一局域网内，就能实现节点间的相互通信[10]。Gazebo 是一款机器人动力学仿真软件，其内嵌于ROS2 中，提供高保真度的物理环境模拟与一套完整的传感器模型，并具备友好的用户交互界面，用户可以根据需求进行准确的动力学仿真和数据模拟。

实验利用亚马逊物流机器人的书店场景模型模拟地图构建的场景，如图 6 所示。利用机器人模型读取雷达和相机的数据，将雷达和相机的数据输入到ORB-SLAM3框架中进行处理，然后将处理完的数据进行融合。

根据设计的协作地图构建方法进行地图融合。在地图构建过程中，各模块运行时间如表1 所示。由表1 可知，本文的融合方法处理机器人的每一帧关键数据帧所需的时间不长，各个机器人的数据处理并行运行，使得算法运行效率较高，时效性好。

表1 各模块运行时间

机器人各自构建的局部地图和融合地图如图 7所示。本方法在灯光较暗的情况下依旧可以保持局部的创建，但地图边缘较为模糊不清，具体原因不明，有待进一步实验研究。在整体地图的融合上，本方法可以较好地实现融合且误差较小，总体上实现了鲁棒性较好的激光雷达增强视觉SLAM 建图。

4 结束语

本文提出了一种基于激光雷达增强的视觉SLAM 多机器人协作地图构建方法，将激光雷达和视觉传感器的数据进行了融合，避免环境光照等因素的影响，得到机器人的精确位姿数据。本文的地图构建方法将存在共识关系的机器人利用关键帧跟踪和ICP 算法进行机器人的位姿变换，得到机器人全局世界坐标系。最后在全局世界坐标系上进行了稠密点云地图的融合扩展。在Gazebo仿真平台上进行了实验验证，结果表明，所提出的方法具有较好的时效性和鲁棒性。