APP下载

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

2024-01-10李燊阳邓三鹏权利红祁宇明丁昊然刘天慧

机器人技术与应用 2023年6期
关键词:关键帧位姿激光雷达

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

(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仿真平台上进行了实验验证,结果表明,所提出的方法具有较好的时效性和鲁棒性。

猜你喜欢

关键帧位姿激光雷达
手持激光雷达应用解决方案
法雷奥第二代SCALA?激光雷达
基于激光雷达通信的地面特征识别技术
基于激光雷达的多旋翼无人机室内定位与避障研究
基于改进关键帧选择的RGB-D SLAM算法
基于共面直线迭代加权最小二乘的相机位姿估计
基于CAD模型的单目六自由度位姿测量
基于相关系数的道路监控视频关键帧提取算法
小型四旋翼飞行器位姿建模及其仿真
基于聚散熵及运动目标检测的监控视频关键帧提取