基于ORB-SLAM2的三维占据网格地图的实时构建

王 飞, 王耀力

(太原理工大学信息与计算机学院,太原 030024)

同时定位与地图构建(simultaneous localization and mapping,SLAM)。是指搭载特定传感器主体,在没有环境先验信息的情况下,于运动过程中建立环境的模型,同时估计自己的运动[1-2]。按传感器的不同,SLAM可以分为两大类,激光SLAM(lidar SLAM)和视觉SLAM(visual SLAM)。激光SLAM理论研究已相对成熟,但由于激光传感器动辄几万到几十万的价格,极大地限制了激光SLAM在移动机器人上的应用。因此,搭载视觉传感器来实现定位和导航的视觉SLAM成为了近年来研究的热点[3-5]。视觉SLAM使用视觉传感器来获得外界图像信息,进而进行机器人位姿估算与定位。

自从Davision在2007年提出了第一个实时的单目视觉SLAM系统MonoSLAM[6]以来,各种SLAM系统都使用各自的方式对环境地图实现了实时的构建。目前在V-SLAM 中,较流行的方案有MSCKF[7]、ROVIO[8-9]、ORB-SLAM2[10-11]等,然而以上SLAM系统仅输出相机运动的轨迹图及稀疏的点云地图,但点云地图形式不紧凑,因此无法为移动机器人提供导航和避障的支持。Santana等[12]在 2011 年介绍了一种基于单目相机使用平面信息(单应矩阵)构建2D地图的方法,该方法在分割步骤通过判断图像特征点是否在同一平面来快速判断是否有障碍物,但由于单目视觉没有尺度信息,其所构建的 2D 网格地图并不适用于机器人的定位和导航。Dia等[13]和Rakotovao等[14]介绍了一种新型的用于占据网格地图构建的逆传感器模型,然而并没有SLAM方面的应用。文献[15-16]将点云地图进行投影后实现了二维占用网格地图(occupy grid map)的构建,但二维地图对空间中障碍物信息表示不清晰,重叠信息处理不明确。Meagher[17]和Freeman等[18]提出了一种八叉树的存储方式,将三维空间中的每个平面一分为二,得到八个大小相同的立方体,每个立方体用0-1中的数表达是否被占用,通过调整分割的次数,可以实现对地图精度的调整,以这一存储方式构造的八叉树地图是导航领域较为常见的方式[19-20]。但八叉树的时间复杂度较大,计算代价较高。对于以SLAM为底层支撑的机器人系统来说,并不希望其占用太多的资源。

目前的V-SLAM系统多生成点云地图,无法为导航和避障提供有效的支持,而现有构建三维网格地图的方法由于计算代价高、时间复杂度大等缺点,无法实现三维网格地图的实时构建。因此针对上述不足,利用一种高效的跳表结构来存储地图信息,减少计算成本,实现三维网格地图的实时构建。

1 跳表地图

自主机器人导航的关键是能够从地图中获得足够丰富的感知信息,从而允许机器人规划路径并避开障碍物,其中三维网格地图在导航中占有重要地位。现提出一种基于SkipList存储结构的三维地图模型,并描述关键数据结构以及详细的地图存储与查询方法。

1.1 跳表结构

一般将树形结构根据地图坐标对体素进行分组。如图1所示,根节点根据x的坐标划分第一层,第一层根据y的坐标划分第二层,第三层同理使用z坐标划分。整棵树的最大深度为3,深度为d3的节点为地图体素,它可以用于存储地图数据,例如占用概率。然而,采用经典的树结构来实现图1所示的概念效率并不高,如果每个节点的子节点数无限多,而对每个节点的子节点都用普通链表进行储,则执行随机访问将出现O(n)的复杂度,为了提升效率,采用SkipList数据结构[21]进行存储。

图1 体素分组示意图Fig.1 The diagram voxel grouping

图2 SkipList结构Fig.2 Structure of SkipList

如图2所示,SkipList在普通链表的基础上,从内部节点向上层拓展为一个新的链表,通过几次延拓,形成一个沿横向分层、沿纵向相互耦合的多个链表。为了查表方便,同层节点都按关键码(key)排序。尽管每一列的节点相同,但这种重复不仅可以加快查找,而且只要策略得当,也不至于造成空间的实质浪费。得益于结构中的这种重复,使得跳表将有序链表中O(n)的随机复杂度降低到O(lgn),因而为提高本文的时间效率提供了理论支持。

1.2 地图的分组与排布

在存储方式上,1.1节中对树形结构进行了分组,相对应的,对整个占据网格地图也应将其离散为大小相同的体素(voxel),将图1中所示的概念实际实现为图3。图1的d1层用来在x轴上构造SkipList,称其为xNode,d2层和d3层分别在在y轴和z轴上构造为yNode和zNode。通过三层的Skiplist嵌套实现了将地图体素用Skiplist表示的目的,并且体素的坐标可以通过迭代其前驱节点来获得。

SkipList数据结构的每个节点都可以通过key寻址,可以使用它将真实世界坐标映射到量化索引,即3D点P(x,y,z)的体素索引v(Ix,Iy,Iz)为

(1)

式(1)中：r表示地图的分辨率。根据本文的数据结构,查询一个体素v=f(Ix,Iy,Iz),相当于执行一个迭代序列h{g[f(Ix),Iy],Iz}=v,结合图3,定义f()表示检索xNode,g()表示检索yNode,h()表示检索zNode。三个查询函数f()、g()和h()中的每一个都可以得到匹配和错误两个结果,泛型函数φ[f(Ix)]可以在函数执行之初根据xNode将多个查询函数定位到不同的SkipList树的不同分支上,因此每个泛型函数φ[f(Ix)]均可以同时执行,这在很大程度上提升了本文的算法的执行效率。

图3 SkipList嵌套示意图Fig.3 Schematic diagram of SkipList nesting

1.3 并行化操作

嵌套的SkipList数据结构为算法提供了高度的并行化操作。此外,即使单个SkipList也可以使用基于锁定的技术来实现一定程度的并行化,但这种算法通常是不可预测的,因此并不适合在实时的任务中进行使用,在1.2中提出了体素索引的概念,可以操作结构的高并行性,适用于实时任务。以下将从遍历和更新两种操作说明本文结构提高运行效率的具体措施。

遍历操作:遍历整棵树(即到达地图的每个体素)包括并行访问所有第一层节点,并收集结果。由于网格地图按照x坐标将不同组的体素分布在不同的第一层SKipList中,因此,可以同时操作不在同一组数据,即可以并行的实现体素的遍历,该过程可表示为

(2)

更新操作:在执行更新操作时,无法预先知道其是否产生新的分支亦或仅更新现有体素的内容而不执行进一步搜索。只要两个更新操作属于两个单独的第一级分支,则不会发生冲突,避免了更新操作与其他操作并发的可能。假设传感器观测的三维点为C={P(x,y,z)},可以根据它们的第一个量化坐标将这两点分组在子集中:在确保没有并发内存访问的情况下,并行地执行处理每个子集的集成操作。以上过程可表示为

(3)

基于以上两种操作的并行化不仅有助于改善计算性能,而且有助于分离传感器可能发生的地图更新情况从而为本文算法的高效运行提供保障。

2 占用网格地图模型

确定了网格地图的存储方式后,为使机器人能判断某一位置是否可以通过,还需要将每一体素的占据概率存入图中,该过程中传感器根据不同的权重将信息融合入体素信息中,网格占用的概率可以表示为

(4)

W′(v)=W(v)+wi(v)

(5)

式中：P′(v)和P(v)分别表示体素v的新旧占用概率；W′(v)和W(v)表示体素v的新旧权重。通过式(4)和式(5)实现对地图持续的更新。以下将对占据网格地图的概率模型进行推导,从而获得体素的概率P。在机器人位姿已知的情况下,假设网格地图的概率模型为P(M|Z0:t),其中M为当前的地图,Z0:t表示传感器从开始到t时刻的测量值。

在占用网格地图中,通过数值0、1、0.5 的概率来表示其单元格的状态,其中0表示空闲,1表示被占用,0.5则表示状态未知。如果用mi表示地图中第i个单元格,其状态可用二进制占用变量表示(占用或空闲)。在上文中已将占据网格地图按一定的规则离散为不同的体素,而体素个数则由地图分辨率决定,故每个体素的概率模型为

P(mi|z0:t)

(6)

在栅格地图中,将一个点是空的概率表示为p(m=0),有障碍物表示为p(m=1),两者的概率和为1。

(7)

以mi=0为例,0-t时刻的mi的概率为

(8)

对式(8)做Markov假设,可得

P(mi|z0:t-1,m)=P(zt|m)

(9)

P(zt|z0:t-1)=P(zt)

(10)

由于地图被划分为一系列体素,因此可将这一假设扩展为

P(mi|z0:t-1,mi)=P(zt|mi)

(11)

将式(9)～式(11)代入式(8)可得

(12)

同理可得m=0的情况:

(13)

为减少不必要的计算,将式(12)与式(13)取比值可得

(14)

进一步变换为

(15)

由于0-1概率的不稳定性,故采用概率对数值log-odds来描述,l为概率对数值,p为0-1的概率,l与p之间由logit变换描述为

(16)

其反变换为

(17)

可以看到,当l从-∞变到+∞时,p相应的从0变到1。而当l取值为0时,取p=0.5。因此存储l来表示节点是否被占据,当不断观察到“占据”时,只需更新l的值即可。当查询概率时,再用逆logit变换,将l转换至概率p。

由以上分析,将式(16)带入式(15)并简化可得

(18)

(19)

通过式(19)可以看出,当前时刻地图体素m的状态为当前的观测值加上先验信息。通过这一机制,将体素状态更新转换为简单的加减法,大大提高了地图更新实现的效率。

3 ORB-SLAM2实现三维占据网格地图

由Mur-Artal等提出的ORB-SLAM2系统,是PTAM的继承者中非常有名的一位,它提出于2015年,是现代SLAM系统中非常完善和易用的系统之一,它代表着主流特征点SLAM的一个高峰。

3.1 ORB-SLAM2概述

ORB-SLAM 2是一种基于图像特征和非线性优化的视觉 SLAM系统。它包括Tracking、LocalMapping和LoopCLose三个线程,并在全部处理进程中使用ORB特征来进行检测和描述,并辅以DBoW词袋模型进行回环检测,在复杂度和准确度上得到了显著的提高。其主要结构和流程如图4所示。

图4 ORB-SALM2流程Fig.4 The flowchart of ORB-SALM2

3.2 三维地图构建框架

ORB-SLAM2连接Microsoft的Kinect V2相机进行真实场景的实时运行时,需要在ROS(robot operating system)系统中订阅相机发布的彩色图像和深度图像,ORB-SLAM2获取到相机节点和图像后,进行关键帧和地图点的提取,计算机器人的姿态,在建图线程初步生成局部点云地图,最终经过闭环检测和全局优化生成全局点云地图。将ORB-SLAM2生成的关键帧、地图点以及机器人位姿通过ROS进行发布,SkipList Map在获得信息后对地图进行分组,并结合tf位姿变换,将信息存入SkipList中,通过高并行的更新和查找操作,实现三维网格地图模型的构建,具体流程如图5所示。

图5 ORB-SLAM2的三维地图构建框架Fig.5 Framework of 3D map construction of ORB-SLAM2

4 实验结果与分析

为了验证本文算法的有效性,首先对跳表地图的效率进行验证,其次将构建实验环境,利用KinectV2在室内环境及走廊环境进行三维占据网格地图的实时构建实验。

4.1 效率对比实验

本文算法以跳表(SkipList)为基础进行构建,跳表是一种高效的词典结构,它是基于有序链表结构来定义和实现,其对地图的操作在平均意义下复杂度仅为O(lgn)。以下将在各种条件下,对地图的插入、删除和搜索与传统的Octree进行对比。

实验使用TUM数据集中具有代表性的rgbd dataset freiburg2 large with loop 和rgbd dataset freiburg3 long office household 两组数据进行实验。图6表示的是新的测量值插入原有地图进行更新操作时的性能,为了获得更全面的评估,考虑了0.05、0.1、0.2 m三种不同的体素的分辨率。在更新过程中,地图分辨率越低,SkipList结构的上层结构所含节点个数越少,故当分辨率变化时所耗费时间也将有较大的改变,而Octree中,分辨率的改变并不能有效减少结构的分支,因此在三种不同的分辨率下Octree执行更新操作时所用的时间基本一致且都大于本文使用的算法。

图7所示为两组数据集在三种不同分辨率下对所生成地图进行遍历所耗费的时间。由图7可知,在遍历整个地图时,充分使用了1.2节与1.3节提出的高并行性,使整体遍历时间均比Octree短,且分辨率越高差别越明显。

图6 插入时间对比Fig.6 Comparison in the insertion time

图7 遍历时间对比Fig.7 Comparison in the traversal time

表1对比了SkipList Map、KD-Tree和Octree在TUM四组数据集中的搜索时间。与图6、图7相比,表1额外考虑了Kd-tree,因为它在空间搜索任务中被广泛采用。括号中的数值为其余两种算法以本文方法为基准而得到的比值。从表1中可以看出,提出的SkipList Map在实际操作中较Kd-tree时间缩短约40%,较Octree缩短约50%,表明本文的结构能以更短的时间完成搜索操作。

表1 搜索时间对比Table 1 Comparison of search time

以上三组试验表明,本文的算法在执行插入、更新和搜索时都能以更短的时间完成,与所期结果一致,因而验证了本文算法具有良好的时间性能。

4.2 实验平台的搭建

以轮式机器人为实验平台,搭载NVIDIA的Jetson TX2核心板,它是一台基于NVIDIA PascalTM架构的单模块超级计算机,性能强大,外形小巧,节能高效,适合机器人等智能终端设备,因其优异的性能,本实验选择其作为算法的运行平台。在TX2上运行的是Ubuntu16系统,配置ROS环境以及一些必要的功能包。本实验所选相机为Microsoft Kinect V2,它的Depth传感器,采用的是time of flight(TOF)的方式,通过从投射的红外线脉冲反射回来的时间来获得Depth的信息。相机的实际图像由Color Camera获得,通过对齐深度信息与彩色信息可以获得任意点的深度,从而为构建三维地图提供强有力的数据基础。图8所示即为实验平台。

图8 轮式机器人实验平台Fig.8 The platform wheeled robot experiment

4.3 建图实例

将使用4.2节中搭建的实验平台在室内环境及走廊环境进行两组地图构建实验。

首先在实验室内部进行实验,最终获得运动过程中产生的轨迹和识别到的点云信息如图9(b)所示,在点云地图中显示了相机的运动轨迹,并能粗略标识出实验室的大致轮廓,但无法对机器人的导航和定位提供有效的障碍物位置信息。实验生成的三维网格地图如图9(a)所示。实验中体素分辨率为0.05 m(即三维地图由边长为0.05 m的正方体构成),图9(a)较好地呈现了两侧放置的桌子和物品,对真实环境中右上角的小物体也能清晰表示,为了更好地表示实验室内部的空间信息,将Kinect摄像头所在的0.2 m 高的平面设置为0 m,使用不同颜色表示不同高度,其中负值表示障碍处于摄像头之下,颜色与高度的对应值由图9(a) 的图例给出。

图9 实验室场景Fig.9 The lab scene

将地图通过ROS系统中的rviz(ROS visualization)显示,并利用工具测量地图中的尺寸信息,与真实环境的对比如表2所示。从表2中可以看到,地图中的测量值与真实环境的误差均在3～6 cm,足以支持一般情景的导航和避障。

表2 实验地图与真实环境尺寸对比Table 2 Comparison of experimental map and real environment size

在完成室内场景的实验后,选择走廊作为大场景环境下的实验验证。走廊场景下的运动轨迹和点云地图如图10(b)所示。使用0.05 m的体素大小构建的网格地图如图10(a)所示,其中不同颜色代表的不同高度由图例给出,图10(a)能够清晰地表示走廊的墙壁和门框等不同物体,在楼道与电梯间之间也可以表现二者的连接部分。实验说明本文的算法在大场景下也可以较好地实现三维网格地图的实时构建。

图10 走廊场景Fig.10 The corridor scene

5 结论

提出了一种基于ORB-SLAM2的跳表地图(SkipList Map)算法,用于三维占据网格地图实时构建。首先根据跳表数据结构进行网格地图的重组与分布；其次讨论了三维占据网格地图的表示和更新的方法；最后研究了ORB-SLAM2应用该地图的流程步骤。实验部分首先对比了本文算法与Octree和kd-tree的性能差异,表明本文的算法优于后两种结构,平均意义下时间复杂度仅为O(lgn)；其次展示了本文的算法在真实场景中的效果。实验表明,本算法具有较强的实用性,能够实时构建用于机器人导航和路径规划的三维网格占据地图。