一种低成本的机器人室内可通行区域建模方法

2020-01-02张釜恺芮挺何雷杨成松

北京航空航天大学学报 2019年12期

张釜恺，芮挺，何雷，杨成松

（1.中国人民解放军陆军工程大学研究生院，南京210000； 2.中国人民解放军陆军工程大学野战工程学院，南京210000）

近年来，随着计算机、传感器以及图像处理技术［1］的进步，机器人技术也得到了蓬勃的发展。基于视觉的同步定位与建图（Simultaneous Localization and Mapping，SLAM）［2］技术是机器人领域的重要技术之一。SLAM 可以利用输入的连续图像来实时解算摄像机的运动轨迹，并且构建场景的地图用以指导机器人的行进。

室内环境是目前小型机器人的主要应用领域。在包括仓储物流、工厂物料搬运、家居服务等应用场景中，小型机器人都有很大的应用潜力。对于以室内为主要工作环境的小型机器人来说，单目摄像头视觉传感器由于较低的成本、较小的体积以及较高的布置灵活性，是机器人获取周围信息的主要来源。利用单目视觉信息获取场景信息以指导机器人的行动，是相关研究的重点方向之一。其中，地面可通行区域的信息，包含了地面的纹理与可通行地面的范围，可以直接用于对机器人的导航。地面可通行区域的建模可以为指导机器人的行动提供很大的便利。

然而，由于室内小型机器人的体积小、视角低、运算能力弱、视场有限，应用视觉SLAM 方法来实时理解场景较为困难。能够构建稠密地图的SLAM方法对于计算资源有着较高的要求，不适合运算能力较弱的小型机器人使用。而且，由于其视野内的主要部分为地面且纹理特征较少，基于特征与梯度的SLAM方法只能获得极为稀疏的地图，对于场景的表现能力较差，无法指导机器人行动。但另一方面，绝大多数室内地面均为平面，而且纹理分布较为均匀，这又为地面的建模提供了方便。

针对此问题，本文提出了一种融合图像分割与单目SLAM的室内地面快速建模方法来获取机器人的可通行区域。该方法基于单目特征点SLAM，并结合图像分割，利用SLAM 生成的点云与图像分割之间的相互印证来提高地面构建的准确性。最终，生成地面可通行区域的稠密建模，用以指导机器人的行动。

通过实验与对比，在小型移动机器人平台上，相对于主流SLAM 方案，本文方法可以显著提高对于地面建模的效果，并且满足在移动平台上实时处理的要求。相比较于经典的单目SLAM 方法，本文方法的运行速度接近特征点SLAM，而对地面可通行区域的建模效果则接近稠密，对于机器人的行动有较大的参考价值。

1 相关工作

SLAM问题自1988年被提出，经过近30年的发展，取得了丰硕的成果。单目SLAM 目前主要有2条技术路线。直接法SLAM直接利用像素的亮度或梯度来匹配图像间的像素，并可以在解算相机运动的同时生成稠密［3-4］或者半稠密［5-6］的场景地图。特征法SLAM 利用点［7-8］或者线特征［9］来匹配图像间的点或者线，进而解算相机运动并生成只包括特征点或线的场景地图。

在建模方面，直接法生成的稠密或半稠密地图相对于特征法生成的稀疏地图有着更多的点，对于场景的描述也更加具有表现力。其中，代表性的系统有2011年的DTAM（Dense Tracking and Mapping）［10］、2014年的LSD-SLAM（Large-Scale Direct monocular SLAM）［11］等。然而，虽然基于特征的SLAM系统只能获得稀疏的地图，但是一般来说具有更快的速度。而且由于便于使用束集调整（Bundle Adjustment，BA［12］）进行优化，并进行回环检测［13］，特征法获取的轨迹一般来说更加准确与鲁棒。在特征法SLAM 中，2007年的Mono-SLAM系统［14］与同一年的PTAM（Parallel Tracking and Mapping）［15］是这一领域具有开创性的成果。2015 年的 ORB-SLAM （ORiented Brief SLAM）［16］系统各项功能较为完备，已经被广大研究者用作后续二次开发研究的基础系统。

为了在保持稳定跟踪的同时，获得更加稠密的地图，有一些方法在特征法的基础上进行场景重建。2010年，Newcombe和Davison［17］提出的方法是在获取了相机位姿与稀疏点云的基础上，利用光流来重建场景表面。2015年，Murartal和Tardos基于ORB-SLAM系统，利用逆深度滤波像素匹配，构建了基于概率的半稠密场景地图［18］。一般来说，此类方法的基本思想为基于SLAM 获得的相机轨迹对图像像素进行匹配，并计算其空间位置。此类思路虽然可以结合直接法和特征点法的优点，但是运算量也大大提升，并不符合小型移动平台的运算能力。

为了提高运算的速度，以适应小型移动平台较低的运算能力，近年来，基于视觉的低成本建模方法也成为了一个有意义的研究方向。2016年，Hinzmann等［19］提出了一种针对无人机平台的低成本地面建模方法，利用无人机自身的传感器感知姿态，并结合图像处理方法获得对于地面的稠密建模。但是，该方法基于的图像内容全部为地面，不适用于小型地面无人车。2017年，蒙山和唐文名将直线引入点特征单目SLAM，并通过点线特征以及直线增强的J-Linkage算法构建包含特征平面的场景地图［20］，但是该地图相对于纯特征点地图，对场景描述效果的提升有限。2017年，von Stumberg等［21］针对小型旋翼无人机，基于LSD-SLAM提出了一种本地探索避障策略与体素模型构建方法，但是对于地面的建模效果较差。2018年，蒋林等［22］对基于嵌入式平台三维重建算法进行了研究，针对差动式机器人的运动特点对算法进行了针对性的简化，但是所利用的视觉传感器为深度相机而非单目相机。

总体来说，现有的主流单目SLAM 系统在小型室内机器人上运行时，由于平台自身的特点，或是地图构建过于稀疏；或是虽地图稠密，但不能实现实时运行。一般并不能在构建场景方面取得令人满意的效果。而现有的低成本建模方法中，针对小型地面机器人特点的研究不多，尚未出现较为合适的地面区域低成本建模方法。

2 室内地面快速建模方法

2.1 整体流程

本文提出的融合图像分割与单目SLAM的室内地面快速建模方法构建在单目特征点SLAM的基础上，利用SLAM 系统获得的相机位姿与稀疏点云，结合地面区域的分割图像获得地面的稠密建模。

系统的基本流程如下：首先，利用SLAM系统获得的相机位姿与稀疏点云。其次，利用地面区域的分割图像，将稀疏点云中投影在地面区域的点提取出来，获取粗略的地面稀疏点云。然后，进行滤波后，利用优化模型对这些地面点拟合平面，获取地平面的数学表达。之后，通过计算稀疏点云中的点与该地平面之间的位置关系，将它们相对精确地分为地面与非地面点云。再利用该分类来筛选地面分割图像。最后，通过相机的位姿，将筛选过的地面分割图像投影到地面平面上，并进行滤波与降噪，获得稠密的地面建模。整个系统的流程示意图如图1所示。

后文将对本文方法流程中的各主要功能模块逐一进行介绍。

图1 本文方法整体流程图Fig.1 Flowchart of proposed method

2.2 单目SLAM 模块

本文使用ORB-SLAM［16］作为系统的单目SLAM模块。ORB-SLAM 是一个功能完善，接口便捷的单目SLAM 系统。整个系统围绕ORB点特征建立，在效率与精度之间达到了较好的平衡，自发表以来就被研究者们所广泛使用。该系统包含3个平行的线程：跟踪线程（tracking thread）、局部建图线程（local mapping thread）和回环检测线程（loop closing thread）。其中，跟踪线程负责计算相机的运动，以及指定关键帧，并在跟踪丢失时负责重定位；局部建图线程主要负责用局部束集调整（local BA）来对局部地图进行优化，以及地图点的插入和删除；回环检测线程主要负责进行回环检测，在有回环出现时进行闭环全局优化。

2.3 地面区域的分割

在SLAM进程对相机位姿进行跟踪的同时，系统对每一帧输入图像进行分割，以获得地面区域的分割图像。由于系统获得的是一个地面分割的图像序列，并且还需要和点云数据进行比对，所以对于单张地面分割图像的精度要求不高，但是对于图像分割的速度有着较高的要求，这使得采用目前主流的深度学习语义分割的方法［23］较为困难。基于以上分析，本文采用基于HSV（Hue，Saturation，Value）色彩空间的自适应阈值分割方法来对地面区域进行分割。

计算机存储和处理图片时一般采用的颜色编码方式是RGB颜色空间，其将颜色按红、绿、蓝3个维度上的分量进行编码，而HSV颜色空间是按照色度、饱和度和亮度来进行编码，分别对应H、S、V 3个分量，更贴近人眼的视觉感知，受光照的影响也更小［24］。在本文的分割方法中，先将待分割的图像转化到HSV色彩空间。

由于室内小型机器人在运动过程中会与障碍物保持一定距离，所以其摄像头拍摄图像的中间底部一般为地面区域。基于此假设，对于输入图像I，本系统设定图像的底部中间区域为默认地面区域MG，其位置与尺寸如图2（a）所示。首先，统计得到默认地面区域MG内H、S、V 3个分量的最值。然后对全图像的每个像素pi进行判断：

式中：HG、SG、VG分别为默认地面区域MG中所有像素H、S、V分量的集合；hi、si、vi分别为像素pi的H、S、V分量。若像素pi的3个分量均在默认地面区域MG的分量范围内，该像素就被加入候选地面区域G1，如图2（b）所示。

在获取候选地面区域之后，还需要进行后续的滤波与处理。首先进行开操作（采用运算符号“○”表示）。开操作用于使对象的轮廓变得光滑，断开狭窄的间断和消除细的突出物。开操作的流程是先用结构元素S对G1腐蚀（采用运算符号“⊕”表示），然后再用S对结果进行膨胀（采用运算符号“⊖”表示）［25］。将候选地面区域G1用开操作处理后，得到区域G2：

区域G2如图2（c）所示。其中，本文方法采用的结构元素S为半径为2像素的圆形结构。

最后对区域进行连通域划分，划分结果如图2（d）所示。选取默认地面区域MG所在的连通域作为分割得到的地面区域G3，如图2（e）所示。获得的地面分割图像如图2（f）所示。

2.4 地面点云的获取、滤波与地平面的计算

在SLAM模块向稀疏点云地图中添加每一个空间地图点Qi时，都会判断该点在其对应的地面分割图像上是否位于地面区域。如果位于地面区域，则将该点加入地面点云Cg。

然后，利用该地面点云来获得地平面的数学表达。由于点云位置计算误差以及地面分割误差的存在，在后续处理之前，需要先去除地面点云中明显的误差离群点。本文采用统计滤波的方法进行离群点的去除［26］：

1）计算点Qi与其邻域范围内k个邻域点的平均距离。

2）分析所有点k邻域平均距离，计算其均值m和标准差σ。

3）设置标准差倍数阈值std，确定距离阈值为

4）若点Qi的k邻域平均距离大于距离阈值d，则为噪声点，去除。

本文方法的应用环境是地面为平面的室内环境。建立优化模型对经过统计滤波的地面点云Cg′进行平面拟合，使点到平面的总距离最小。设地平面F的表达式为

优化模型为

2.5 对地面分割图像的筛选

在获取了地平面F的表达式后，系统对地面分割图像进行筛选。对于每一个关键帧，系统计算其观察到的所有地图点与地平面F的位置关系，并将所有位于地平面F上方μ（单位为cm）以上的点视为非地面点，其余视为地面点。

若某张地面分割图像的地面区域上有超过n个非地面点的投影，则略去这张分割。在本文的实验中，略去的分割如图3所示（其中红色为非地面点，绿色为地面点）。

从图3可以看出，将墙体下沿的部分瓷砖也划入了地面分割，从而包括了大量非地面点，并因此被略去。

图3 被略去的地面分割图像示例Fig.3 An example of excluded ground segmentation image

2.6 稠密地面建模

至此，系统通过对稀疏点云与地面分割图像的交叉比对，已经获得了相对精确的地平面与分割图像。利用关键帧的位姿，将地面分割图像投影到地平面上即可得到稠密的地面点云模型，如图4所示。

首先，通过相机内参矩阵K，将地面分割图像上的像素pi从图像坐标［u，v］T反投影到相机坐标系中，获得相机坐标系下的坐标PC=［Xc，Yc，］T：

式中：深度值ZC为未知量。

然后，通过该帧的相机位姿变换矩阵Twc，将相机坐标系下的坐标转换到世界坐标系坐标Pw，即

图4 将分割图像投影到地平面对地面建模Fig.4 Projecting segmented images to groundplane for ground modeling

该世界坐标（xw，yw，zw）应该在平面F上，即

利用式（8）解出未知深度ZC，即可以得到该像素点对应的世界坐标。对所有地面分割图像如此操作后，再进行体素滤波，设定滤波半径为0.01，即半径0.01的范围内只保留一个点。最终，得到地面的稠密点云模型。

3 实验与分析

为了验证本文方法的可行性与实用性，使用搭载了单目摄像头的小型机器人获取数据。数据的处理使用笔记本电脑，搭载Ubuntu 16.04系统，处理器（CPU）为Intel 4 200 H，内存4 G。并同样的环境中用同样的平台运行了ORB-SLAM 系统，与本文的系统相对比。

分别以办公室与教室作为实验环境。其中，办公室的地面为瓷砖，颜色与纹理相对连续，但是如图5（a）所示，地面存在很严重的倒影现象。教室场景较为简单，光照较为均匀，但是如图5（b）所示，教室的地面上存在明显的黑色地砖缝，地面色彩与纹理不连续。2个实验环境对于地面区域的建模均具有一定的挑战。

经过预实验，在实验中将点云的统计滤波参数设置为k=10，std=1，将地面分割图像筛选过程中的参数设置为μ=3 cm，n=50。

图5 实验环境Fig.5 Experimental environment

3.1 场景与建模效果

3.1.1 办公室场景

办公室环境和机器人的运动轨迹如图6所示。本文方法得到的稠密地面建模与非地面稀疏点云如图7所示。ORB-SLAM得到的场景稀疏点云如图8所示。

图6 办公室环境与机器人运动轨迹Fig.6 Office environment and robot movement track

办公室场景的地面可通行区域边界本身就并不明晰，所以建模的边缘并不规则。但是本文方法得到的模型能够基本准确地反映通路的宽窄与通过情况，例如中间一段通路中最细的部分反映了因墙面突起造成的障碍。

图7 本文方法得到的办公室稠密地面建模与非地面稀疏点云Fig.7 Office dense ground modeling and non-ground sparse point cloud obtained by proposed method

图8 ORB-SLAM得到的办公室稀疏点云Fig.8 Office sparse point cloud obtained by ORB-SLAM

3.1.2 教室场景

教室环境和机器人的运动轨迹如图9所示。本文方法得到的稠密地面建模与非地面稀疏点云如图10所示。ORB-SLAM 得到的场景稀疏点云如图11所示。

图9 教室环境与机器人运动轨迹Fig.9 Classroom environment and robot movement track

图10 本文方法得到的教室稠密地面建模与非地面稀疏点云Fig.10 Classroom dense ground modeling and non-ground sparse point cloud obtained by proposed method

图11 ORB-SLAM得到的教室稀疏点云Fig.11 Classroom sparse point cloud obtained by ORB-SLAM

教室场景的地面比较规则，本文方法所建的模型较好地还原了通行区域的地面情况。但是由于通路狭窄，在转弯的位置相机面对旁边的课桌，地面的图像区域不足，造成转弯部分建模效果稍差。

3.1.3 建模效果

从图11中可以看出，原ORB-SLAM 生成的点云十分稀疏，只是标记了特征点的位置，根本无法从中获得地面的位置以及场景的有效信息，对场景的还原效果较差，基本不能用于对场景的描述，指导机器人行动。而本文方法获得了对于地平面的稠密建模，基本涵盖了机器人视野范围内的可通行区域。相比较原ORB-SLAM 算法，本文方法生成的地图更具有实用性。

3.2 运算速度

在实际的运行中，本文方法与ORB-SLAM 算法平均每一帧各阶段的运算时间如表1所示。

从表1数据可以看出，相比较于ORB-SLAM，本文方法中的SLAM运算时间基本不变。增加的计算时间中，占比最大的部分是图像的分割与地面稠密建模。由于本文方法添加的操作全部是基于关键帧的，所以实际平均到每一帧的时间都很短。总体来看，本文方法能够达到21帧/s的平均运行速度，ORB-SLAM的平均运行速度约为30帧/s。本文方法约为ORB-SLAM的70%，能够满足预期的应用要求。

表1 平均每帧运算时间Table 1 Average operation time per frame

3.3 精度分析

由于单目SLAM方法生成的空间数据具有尺度不确定性，所以为分析本文方法的建模精度，首先将生成的相机轨迹与真实的轨迹进行配准，得到数据的正确尺度后，再与实际数据相对比，进行精度分析。而ORB-SLAM得到的稀疏点云地图则过于稀疏，基本上无法从中提取通路的位置与具体范围，精度也无从谈起，故不对其进行精度分析。

3.3.1 地平面的位置还原精度

本文方法对地面进行还原的重要步骤是拟合地平面。由于搭载的相机高度已知，通过对比相机真实高度与相机和拟合地平面之间的平均距离，可以得到地平面的位置还原精度，如表2所示。可见，地平面位置的还原平均绝对误差为5.8%。

3.3.2 场景建模精度

在2个场景中，分别在系统生成的稠密地面模型上选取3处通路的宽度进行测量，与实际地面上通路的宽度进行对比，以分析方法的场景建模精度。由于转弯处存在遮挡，相机拍不到通路的全貌，故选取直线前行的部分通路作为实验分析的对象。

1）办公室

对地面上3段通路的宽度进行测量（见图12）。

与真实的宽度进行对比，得到的结果如表3所示。

通过对地面3处通路的宽度进行测量后，我们发现a处和b处精度较高，而c处误差较大。经分析，这部分的误差较大主要是由于地面倒影的影响，分析详见后文3.4.1节。

2）教室

对系统生成的稠密地面模型进行测量，对地面上3段通路的宽度进行测量（见图13）。