1、第 50 卷 第 2 期2 0 2 3 年 2 月Vol.50，No.2Feb.2 0 2 3湖 南 大 学 学 报（自 然 科 学 版）Journal of Hunan University（Natural Sciences）基于改进ORB-SLAM2算法的RGB-D稠密地图构建韩彦峰，唐超超，肖科（重庆大学 机械与运载工程学院，重庆 400044）摘 要：针对传统ORB-SLAM2系统无法稠密建图以及缺少可应用于移动机器人在特定场景下导航与路径规划的八叉树地图等问题，提出了一种改进RGB-D ORB-SLAM2算法.该算法利用深度信息，通过针孔成像模型计算出关键帧点云的三维空间位置，采用离
2、群点滤波去除多余杂点，体素滤波保留具有特征信息的点云进行拼接，降低地图冗余度，并经过稠密回环处理进一步优化更新关键帧的点云位姿，构建出精确的稠密点云地图并转换为八叉树地图.试验数据表明，相比于RGB-D SLAMV2系统，RGB-D ORB-SLAM2系统的全局轨迹误差和相对位姿误差提升有50%以上，均方根误差为0.89%，均值误差为0.76%；在建图性能方面，相比于同类型算法，点云数量平均降低约30%.此外，八叉树地图相比于点云地图，仅占其内存的0.6%，更加满足了高精度、快速性的导航需求.关键词：同时定位与地图构建；体素滤波；离群点滤波；稠密回环；八叉树地图中图分类号：TP242.6 文献
3、标志码：ARGB-D Dense Map Construction Based on Improved ORB-SLAM2AlgorithmHAN Yanfeng，TANG Chaochao，XIAO Ke（College of Mechanical and Vehicle Engineering，Chongqing University，Chongqing 400044，China）Abstract：An improved RGB-D ORB-SLAM2 algorithm is proposed to address the problems that the traditional OR
4、B-SLAM2 system cannot build a dense map and lack an octree map that can be applied to the navigation and path planning of mobile robots in specific scenarios.The algorithm uses depth information to calculate the 3D spatial position of the point clouds in key frames through the pinhole imaging model
5、and uses outlier filtering to remove redundant clutter and voxel filtering to retain point clouds with feature information for stitching and reduce map redundancy.And through dense loopback processing，the point cloud poses are further optimized and updated in keyframes，and an accurate dense point cl
6、oud map is constructed and converted into an octree map.The experimental data shows that compared with the RGB-D SLAMV2 system，the global trajectory error and relative positional error of the RGB-D ORB-SLAM2 system are improved by more than 50%，the root mean square error is 0.89%，and the mean error
7、is 0.76%；in terms of map-building performance，the number of point clouds is reduced by about 30%on average when compared with the same type of algorithms.In addition，the octree map occupies only 0.6%of its 收稿日期：2022-05-30基金项目：国家重点研发计划资助项目（2018YFB1304800），National Key R&D Program of China（2018YFB1304
8、800）作者简介：韩彦峰（1987），男，山西定襄人，重庆大学副教授，博士 通信联系人，E-mail：fyh-文章编号：1674-2974（2023）02-0052-11DOI：10.16339/ki.hdxbzkb.2023270第 2 期韩彦峰等：基于改进ORB-SLAM2算法的RGB-D稠密地图构建memory when compared with the point cloud map，which better meets the high precision and fast navigation demands.Key words：simultaneous localization
9、 and mapping construction；voxel filtering；outlier filtering；dense loopback；octree map近年来，自动驾驶技术、VR技术、无人机日新月异的发展，对地图和定位的精度需求愈来愈高.作为各种创新性技术中的关键环节，地图和定位精度的可靠性和准确性具有重要的意义，同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）成为目前国内外学者的热门研究领域1-4.SLAM 技术可搭载雷达、相机等传感器，因此基于相机传感器的视觉SLAM技术相比于激光SLAM技术具有设备元件成本低、功
10、耗低等优点，更受到研究者们的青睐.相机可提供鲁棒性更优质的图像信息，能够更准确地识别位置信息，而位置识别是 SLAM技术系统中的关键模块.2007年，Klein等人5提出的PTAM系统，是视觉SLAM 发展中重要的里程碑，一举开创了多线程SLAM系统的先河，将前端跟踪线程和后端优化建图线程并行化处理，后端优化开创性地采用了非线性优化方案，而非传统的滤波器优化（如卡尔曼滤波6、粒子滤波7）.2014年，Endres8团队设计完成了基于深度相机的RGB-D SLAMV2系统，首次利用RGB-D 相机特性进行稠密点云地图的构建.2015年，Mur-Artal 等人9提出了 ORB-SLAM 系统，在
11、PTAM算法框架上增加了地图初始化和闭环检测环节，能够很好地消除运行产生的累积误差，显著提升了地图跟踪的快速性和构建地图的准确性，也成为了一个完整的SLAM系统.2016年，原团队在此基础上提出了 ORB-SLAM2 系统10，在回环检测模块后又增加了全局BA（Bundle Adjustment）环节，提高了系统的鲁棒性；支持单目、双目、RGB-D等多种常见相机.2018年，Bescos等人11提出基于ORB-SLAM2的DynaSLAM算法系统，采用Mask-CNN算法检测潜在动态物体，增加了动态目标检测和背景修复的能力，在单目、立体和RGB-D配置的动态场景中非常强大；同年，清华大学提出了
12、DS-SLAM算法12，采用SegNet进行语义分割并建立语义八叉树地图，精确定位图像场景下的各个物体.2021年，中国科学技术大学团队提出一种新的基于稀疏特征的视觉SLAM算法（DP-SLAM13），该算法基于移动概率传播模型进行动态关键点检测，结合几何约束和语义分割的结果，在贝叶斯概率估计框架中跟踪动态关键点，克服了几何约束和语义信息的偏差，提高了视觉SLAM系统的准确性和鲁棒性.ORB-SLAM2 系统具有精确度高、实时稳定性好、应用场景多等优点，但存在如下缺点：所构建的地图比较稀疏而非稠密，点云地图存储空间大，无法有效描述空间占据状态导致无法满足导航、路径规划以及高层次的人机交互 14
13、-17.针对上述问题，本文提出一种基于 RGB-D 模式下 ORB-SLAM2 框架的VSLAM方案，添加稠密建图线程，实时记录关键帧位姿进行点云拼接，引入双层滤波算法降低点云冗余度，位姿稠密回环处理优化点云空间位置，并最终转换成八叉树地图.本文测试多个开源数据集，在轨迹精度和建图效果等方面均有提高，验证了改进算法的有效性.1 ORB-SLAM2系统框架简介ORB-SLAM2是一种前端视觉里程计基于“Oriented FAST”关键点和BRIEF描述子，实现特征点的提取匹配，后端基于非线性优化 BA 方式的视觉SLAM系统.系统共有三个线程，由跟踪线程（Tracking）、局部建图线程（Loc
14、al Mapping）和闭环检测线程（Loop Closing）组成18.RGB-D 模式下，跟踪线程负责根据提供的特征点深度信息，进行实时位姿的定位跟踪并优化处理.局部建图线程通过所获得的关键帧创建新地图点并剔除地图外点，将关键帧的位姿进行局部BA优化，删除冗余关键帧和地图点.闭环检测线程利用数学模型评价相邻关键帧的相似性程度，判断关键帧闭环情况，有助于减少轨迹的累积漂移.2 改进RGB-D ORB-SLAM2系统框架本文VSLAM系统方案在原有RGB-D模式下的ORB-SLAM2算法框架上进行扩展，添加稠密建图功53湖南大学学报（自然科学版）2023 年能，使其能够对周围场景实时构建点云地
15、图，并转换为可表征周围环境的3D八叉树地图.系统整体构架如图1所示，图中虚线方框内表示本文主要的扩展工作.2.1 稠密点云地图与八叉树地图构建2.1.1 稠密点云地图构建原理RGB-D 相机提供的深度图像和彩色图像实现二维坐标转换为三维点云数据.定义任意一个空间三维点X，坐标表示为X=xw，yw，zw，在相机坐标系下三维点所对应已知点的像素坐标表示为：x=u，v，1，根据相机针孔模型成像原理可以变换得到点云的空间位置信息：z|uv1=K(R|t)|xwywzw1（1）式中：K代表相机内参，由相机标定所得；R代表旋转矩阵，t代表平移向量；z是深度值和实际空间距离的比例关系因子.世界坐标系原点与相
16、机坐标系原点重合，即没有旋转和平移，所以R为单位矩阵，t为零矩阵，通过公式（1）变换得到点云的三维空间位置坐标：|xw=z()u-cxfxyw=z()u-cyfyzw=z（2）2.1.2 八叉树地图的构建与更新本文提出了一种在导航中比较常用的、压缩性能好的构建八叉树地图方式.如图2（a）所示，一个大立方体不断均匀地分成八块，直到变成最小的方块为止，白色表示未被占据，黑色表示被占据不可再分割.图 2（b）中为八叉树结构，每一个节点类比成体素，未被占据的节点继续展开形成八个子节点，被信息占据的节点停止展开.当向地图中添加信息时，由于实际的物体经常连在一起，空白的地方也会常常连在一起，所以大多数八叉树节点无须展开到叶子层面19，因此八叉树地图比点云地图节省了大量存储空间.八叉树节点存储了它是否被占据的信息，从点云层面来讲，可以用0表示空白，1表示被占据.由于噪声的影响，会造成节点所存储的占据信息判别不准确，出现未占据、占据两种情况往复循环的现象，甚至存在未知状态.因此本文提出使用概率对数值（Log-odds）来表达某节点是否被占据.设y R为概率对数值，x为01的概率，那么它们之间的变换由公
