面向嵌入式平台的单目ORB-SLAM稠密化建图实现

马靖煊，王红雨，曹 彦，乔文超，韩佼志，吴昌学

1.上海交通大学 电子信息与电气工程学院，上海 200240

2.上海宇航系统工程研究所，上海 200240

同时定位与地图构建（simultaneous localization and mapping，SLAM）技术是机器人技术领域的一个研究热点和关键技术。在室外环境中，机器人可以依靠以卫星定位为基础的技术实现对其自身的定位，进一步实现路径规划与避障探索。但人类活动时间超过70%是在室内，而室内环境复杂，需要从多种测量数据中估计移动节点的实时位置[1]。在未知的室内环境中，机器人需要构建地图来执行各种任务，但构建地图的前提条件是机器人知道自己的位置。

在过去的十几年里，很多学者尝试使用多种传感器来实现SLAM技术。Davison使用单目摄像头实现了实时SLAM系统[2]，是第一个实时的单目视觉SLAM系统；Asada 等人设计了自学习双目立体视觉伺服系统，通过双目立体视觉三角测量原理跟踪移动方向未知的目标[3]；在2007 年，Klein 等人提出了并行追踪与建图（parallel tracking and mapping，PTAM）方法[4]，提出并实现了跟踪与建图过程的并行化，并且PTAM是第一个使用非线性优化，而非传统的滤波器（如卡尔曼滤波[5]、粒子滤波[6]和组合滤波[7]）作为后端的方案。国内学者在文献[8]中对基于RGB-D的SLAM系统也进行了深刻的研究。随着PTAM的提出，许多优秀的SLAM系统在其基础上被设计出来，ORB-SLAM 正是PTAM 继承者中最完善易用的SLAM 系统之一。2020 年7 月提出了最新的ORBSLAM3 系统[9]，该系统是第一个同时支持单目、双目、RGB-D 相机的SLAM 系统。与它的前身ORB-SLAM2系统相比，ORB-SLAM3融合了基于紧耦合方法的视觉-惯性里程计，完全依赖于最大后验概率（maximum-aposteriori，MAP）估计，精度比前者高2～5 倍；第二个创新点是多地图系统，可以在丢失追踪时创建一张新的地图，并在重新追踪时与先前的地图无缝合并。

与其他的SLAM方案相比，ORB-SLAM3在保留了回环检测的基础上，在跟踪阶段加入了IMU，在地图构建阶段引入了多地图系统，这些特点使得ORB-SLAM3有着远超其他方案的稳健性。当然也存在一些不足之处，如其构建的稀疏特征点地图仅能满足机器人对定位的需求，而无法提供导航、避障、交互等诸多功能。为了克服上述缺点，为机器人导航避障提供基础，本文将论述基于ORB-SLAM3 系统实现的稠密建图，并在Nvidia Jetson AGX Xavier 嵌入式平台上测试了基于TUM 数据集本文方法的效率与精度，取得了一定的实验效果。

1 ORB-SLAM3系统概述

ORB-SLAM 是一种基于ORB（oriented fast and rotated BRIEF）关键点和非线性优化方式的，适用于单目、双目、RGB-D 等多种相机模型的视觉SLAM 系统。该系统围绕ORB 特征进行计算，包括视觉里程计部分和用于回环检测词袋（bag of words，BoW）[10]，这一特征是现阶段计算平台的一种优秀的效率与精度之间的折中方案。ORB不像SIFT（scale-invariant feature transform）或SURF（speed up robust feature）那样费时，在CPU上面即可实时计算；亦比Harris等简单角点具有更好的旋转和缩放不变性。同时，ORB-SLAM 创新式地使用了三个线程来完成其整个SLAM系统：实时跟踪特征点的Tracking跟踪线程、局部地图构建线程以及回环检测线程，如图1所示。下文将分别介绍这三个线程的作用。

图1 ORB-SLAM3系统线程Fig.1 ORB-SLAM3 system thread

1.1 Tracking跟踪线程

Tracking 跟踪线程负责处理传感器采集到的数据，并实时计算当前帧相对于当前活动地图的姿态，尽可能地降低符合地图要素的重投影误差。同时，跟踪线程还判断当前帧是否可以作为关键帧使用。在视觉-惯性模式下，跟踪线程通过在优化中加入惯性残差来估计相机速度和IMU 的偏差。当跟踪丢失时，跟踪线程会尝试在所有地图集中重新定位当前帧，如果成功重新定位的话会恢复跟踪，并在适当的时候切换活动地图；如果重定位失败，在一定时间后活动地图会存储为非活动地图，并创建新的活动地图。在传统的纯视觉SLAM系统中，估计的状态只包括当前的相机姿态，而在ORBSLAM3 的视觉惯性SLAM 系统中，还需要考虑以下参数：真实世界坐标系下的姿态Ti、速度vi，以及陀螺仪和加速度计的漂移和bai，这两个漂移假设为布朗运动，i为当前时刻的视频帧。可以得出如下状态向量：

对于视觉-惯性SLAM，通过文献[11]中的方法在连续的视频帧i和i+1 之间预积分IMU，并通过文献[12]的方法形成公式，从而得到预测量的旋转、速度和位置信息。最终，视觉惯性优化问题可通过下式表示：

式中，sˉk是一组数量为k+1 的关键帧的状态，x是一组数量为h的3D点的状态，rli-1,i是惯性残差，Vli,i+1是整个测量向量的信息矩阵，ρHub是鲁棒Huber 核，kj是一组数量为j的3D观测点。这种优化方法需要较为精确的初始值，从而实现更快地收敛和得到更精确的解。

1.2 局部地图构建线程

局部地图构建线程负责向当前活动的地图添加关键帧和地图点，同时剔除冗余的关键帧和地图点，并通过视觉或视觉-惯性约束来细化地图。剔除冗余地图点的原则有以下几条：（1）这些点在接下来的图像帧中无法跟踪匹配；（2）投影光线通过三角化计算点处于低视差；（3）三角测量点产生的重投影误差会较大。这三条地图点筛选原则保证了所构建地图的鲁棒性。为了保证地图的简洁性，降低BA过程的复杂度，提高系统的运行速度，在ORB-SLAM3 的关键帧集合中，如果某个关键帧中90%的地图点在其他至少3 个关键帧中被检测到，则会剔除该关键帧。

1.3 回环检测线程

在ORB-SLAM2系统中，回环检测线程利用各个关键帧的相互关联性进行有效的闭合回环，在本质图上优化位姿，这样就可以将累计的误差分散到位姿图中，并通过相似变换校正尺度偏差，从而降低视觉里程计过程中的累计误差[13]。在ORB-SLAM3系统中，回环检测线程不光具有ORB-SLAM2中的功能，还会以关键帧速率来检测活动地图和整个地图集之间的公共区域。如果公共区域属于活动地图，回环检测线程会执行前文所述的循环校正；如果属于不同的地图，则两张地图会无缝地合并成一张地图，并成为新的活动地图。循环校正后，ORB-SLAM3系统会在独立线程中启动一个完整的BA来进一步细化地图映射，而不会影响实时性能。

ORB-SLAM3 虽然鲁棒性更好，性能更强，但其构建的仍是三维稀疏特征地图，无法满足机器人的导航、避障、路径规划等需求。为了克服这个缺点，本文在ORB-SLAM3系统上进行改进，使其能对三维场景进行稠密化建图。

2 稠密化建图实现

改进后的OS3-D（ORB-SLAM3-Dense）系统框架图如图2所示，图中虚线框内为OS3-D系统新增的稠密建图线程。在Tracking 跟踪线程中对满足要求的关键帧进行特征点二次采样，二次采样仅仅是为了提取ORB特征点，不需要进行匹配，也不用于估计相机姿态和运动。相机位姿则由局部BA进行初始化，并在回环检测和全局BA阶段进行更新，使点云拼接更为精确。

图2 加入稠密建图线程系统结构图Fig.2 System structure diagram of adding dense mapping thread

稠密建图线程主要分为三部分：数据输入、位姿估计和地图构建。数据输入部分采集RGB 图像、深度图和ORB特征点，为地图构建提供环境源数据；位姿估计部分利用ORB-SLAM3 为每一帧满足建图需求的关键帧提供姿态估计；地图构建节点接收前两部分的图像数据和位姿信息，进行三维点云拼接，并维护和管理点云数据，后续可增加点云滤波、语义识别等功能。稠密建图线程结构如图3所示。

图3 稠密建图线程结构图Fig.3 Structure diagram of dense mapping thread

三维点云可以根据针孔相机成像原理，由二维RGB图像和深度图像生成：式中，cx、cy、fx和fy是相机内参，u、v是像素坐标，x、y、z是像素坐标系坐标，d是像素点深度（即像素点到成像平面距离），单位为毫米（mm），s是深度尺度，根据经验取1 000。

此时得到的某一帧图像的三维点云是基于此时刻的相机坐标系，为了得到像素点在真实世界中的坐标，需要利用前文所述的位姿信息将像素点的相机坐标变换为世界坐标：

式中，Ti为前文中第i个关键帧相机在真实世界坐标系下的位姿，Xci,j是第i个关键帧在相机坐标系上的坐标，Xw,j是变换后的世界坐标。

在实际使用过程中，深度相机的depth 量程是有限的，通过如下公式去掉“不可能”采集到的点：

式中，dmin、dmax分别是实际深度相机的上下量程，单位为毫米（mm），dtrust是实际深度相机可以采集到的点。以Intel Realsense D455相机为例，dmin为400，即0.4 m，dmax为8 000，即8 m。

3 实验结果和分析

为了验证OS3-D算法有效性，对文献[14]所提供的RGB-D 标准数据集进行了测试，包括彩色图、深度图、相机内参与高精度动作捕捉系统获得的相机运动位姿，并且该数据集还提供了计算估计位姿和真实位姿的均方根误差（root mean square error，RMSE）工具来评估算法的定位精度。值得一提的是，为了后续针对无人机室内建图研究，用于评测OS3-D的测试平台是Nvidia公司的ARM架构嵌入式平台Jetson AGX Xavier，其CPU是8个ARM V8.2核心，GPU是拥有32 TOPS计算能力的Volta核心，整个系统的最大功耗仅为30 W，不带电源适配器重量672 g，十分适合无人机这种重量、功耗敏感的平台。之前已有学者在ORB-SLAM2 上增加半稠密建图线程，并在笔记本平台测试了性能[15]。

测试OS3-D 主要采用TUM 数据集中的RGB-D 标准数据进行实验，估计相机的运动轨迹，并进行稠密建图。三维稠密建图效果如图4所示，估计轨迹与真实轨迹对比图如图5所示。

图4 部分fr数据集稠密建图效果图Fig.4 Dense mapping renderings of some fr data sets

从图4 中可以看出，OS3-D 算法可以较好地实现三维稠密建图，不论是对于细节较多的（a）fr1/desk、（b）fr1/desk2、（c）fr1/xyz 和（e）fr2/xyz 等场景，还是大范围场景（d）fr2/desk 和（f）fr3/long_office，都可以较好地还原。从图5可以看出，OS3-D对于轨迹的估计有一定的漂移，但总体来说估计轨迹较为准确。表1为OS3-D与ORB-SLAM3 算法的性能对比，表中“OS3”为ORBSLAM3缩写。

图5 真实轨迹与估计轨迹图Fig.5 Real trajectory and estimated trajectory

从表1中可以知道：因为OS3-D算法与ORB-SLAM3算法相比，增加了稠密建图线程，构建了三维稠密地图场景，并且使用了性能较弱的计算平台，故本文算法的每帧耗时、合计耗时均弱于ORB-SLAM3 算法，并且由于每帧耗时增加，进而影响了算法的定位精度。

表1 本文算法与ORB-SLAM3性能对比Table 1 Performance comparison between algorithm in this paper and ORB-SLAM3

与ORB-SLAM3算法相比，OS3-D算法的每帧平均耗时为0.092 2 s，即每秒10.8 帧，每帧处理耗时增加了3.7%；合计处理耗时增加了7%；对于定位精度RMSE，本文算法仅增加了2%，在6个数据集的测试中，本文算法的平均定位精度达到了0.213%。综合来看，整个系统可以满足机器人定位与建图在精度和速度上的要求。

4 结束语

为了实现机器人在室内的定位、导航、避障与建图需求，本文基于ORB-SLAM3算法设计了三维稠密地图重建系统，并在嵌入式平台Jetson AGX Xavier上进行了数据集测试。实验表明该系统相较于ORB-SLAM3系统，实现了三维稠密建图，同时定位速度可达10.8 frame/s，定位精度可达0.213%，满足机器人室内定位需求，为机器人室内导航避障提供了基础。

目前本文方法仍存在不足，仅实现了在TUM 数据集上的测试。并且因为本文方法是基于ORB角点特征实现位置识别和回环检测，所以在角特征点较少的环境中容易定位失败。未来，将进一步改进实现三维稠密地图的实时构建，加入滤波算法和基于像素的直接SLAM，提高系统的运行效率和定位效果。