基于视觉的机器人自主定位与障碍物检测方法

丁斗建 赵晓林 王长根 高关根 寇磊

摘 要：针对稀疏型同时定位与地图构建（SLAM）算法环境信息丢失导致无法检测障碍物问题，提出一种基于视觉的机器人自主定位与障碍物检测方法。首先，利用双目相机得到观测场景的视差图。然后，在机器人操作系统（ROS）架构下，同时运行定位与建图和障碍物检测两个节点。定位与建图节点基于ORB-SLAM2完成位姿估计与环境建图。障碍物检测节点引入深度阈值，将视差图二值化;运用轮廓提取算法得到障碍物轮廓信息并计算障碍物凸包面积;再引入面积阈值，剔除误检测区域，从而实时准确地解算出障碍物坐标。最后，将检测到的障碍物信息插入到环境的稀疏特征地图当中。实验结果表明，该方法能够在实现机器人自主定位的同时，快速检测出环境中的障碍物，检测精度能够保证机器人顺利避障。

关键词：视觉定位;障碍物检测;视觉同时定位与地图构建;机器人操作系统;立体视觉;机器人

中图分类号： TP242.6智能机器人

文献标志码：A

Abstract： Aiming at the obstacle detection problem caused by the loss of environmental information in sparse Simultaneous Localization And Mapping （SLAM） algorithm， an autonomous location and obstacle detection method of robot based on vision was proposed. Firstly， the parallax map of the observed scene was obtained by binocular camera. Secondly， under the framework of Robot Operating System （ROS）， localization and mapping node and obstacle detection node were operated simultaneously. The localization and mapping node completed pose estimation and map building based on ORB-SLAM2. In the obstacle detection node， a depth threshold was introduced to binarize the parallax graph and the contour extraction algorithm was used to obtain the contour information of the obstacle and calculate the convex hull area of the obstacle， then an area threshold was introduced to eliminate the false detection areas， so as to accurately obtain the coordinates of obstacles in real time. Finally， the detected obstacle information was inserted into the sparse feature map of the environment. Experiment results show that this method can quickly detect obstacles in the environment while realizing autonomous localization of the robot， and the detection accuracy can ensure the robot to avoid obstacles smoothly.

Key words： visual localization; obstacle detection; Visual Simultaneous Localization And Mapping （VSLAM）; robot operating system; stereo vision; robot

0 引言

隨着机器人应用领域的不断拓展，对其自主性的要求也在不断提升[1]。要实现机器人自主移动，自主定位技术和自主避障技术是最为关键的两个方面。在机器人自主定位与自主避障研究中，视觉传感器由于具有功耗低、价格便宜、可以获得更加丰富的环境信息等优点[2]，得到了广泛应用[3-4]。

基于视觉的机器人自主定位研究已有近二十年的历史。文献[5]中提出视觉里程计以后，基于视觉里程计的视觉同时定位与地图构建（Visual Simultaneous Localization And Mapping， VSLAM）算法受到广泛的关注。VSLAM根据所利用的图像信息可分为基于特征的稀疏方法和直接的稠密方法。稠密方法可以建立密集的环境地图，便于后续的自主导航，具有代表性的工作是LSD-SLAM[6]。但是，稠密方法计算量很大，难以做到较好的实时性，因此，出于减少计算量、提高系统实时性的目的，稀疏型SLAM受到了许多研究者的青睐。文献[7]使用尺度不变特征变换（Scale Invariant Feature Transform， SIFT）特征检测方法，解决了特征匹配中图像旋转、缩放以及光照变化的影响，但SIFT特征的提取本身就比较耗时。文献[8]在特征匹配过程中应用了极线约束，提高了匹配的速度与精度。文献[9]提供了一个大场景VSLAM框架，采用关键帧优化方法，成为了VSLAM的一个特色，但由于缺少回环检测，定位的精度并不高。针对这一问题，文献[10]提出一种基于ORB（Oriented FAST and Rotated BRIEF）特征的SLAM解决方案，通过加入回环检测线程和自动初始化功能，全面提高了系统的性能。但是，以上提到的方法都是通过特征点来建图，得到的只是稀疏的点图，只反映了环境的局部特征信息。因此，只依赖稀疏型SLAM算法，能够实现较好的定位，却无法辨别出环境中的障碍物，难以直接用于后续的避障与路径规划等工作中。

为解决上述问题，文献[11]提出了一种基于全局观测地图模型的SLAM方案，将全局密集信息嵌入到稀疏特征地图中，并将EKF-SLAM算法与全局地图观测模型相结合，得到了较为可信的密集地图。文献[12]基于双目立体视觉，提出了一种栅格地图构建方法，得到了包含环境几何信息的地图。文献[13]利用迭代滤波以及高斯分布对数据进行稠密化处理，实现了对稀疏性VSLAM地图的数据补插。以上方法基本都是致力于对稀疏特征地图进行数据补充，在得到密集地图的同时，也会给系统引入较大的计算量。然而，机器人实现一般的自主移动，并不都需要所有的环境信息，很多情况下只需检测出环境中的障碍物即可满足自主移动要求。

针对上述情况，本文利用立体视觉法检测障碍物来弥补稀疏型SLAM环境交互能力弱的缺陷，提出了一种基于视觉的机器人自主定位与障碍物检测方法。首先，利用双目相机得到场景视差图，在视差图的基础上，同时运行两个节点。定位与建图节点依赖成熟的视觉SLAM算法ORB-SLAM2[14]实现相应功能。障碍物检测节点首先对视差图进行三维重建与伪彩色处理，得到特征点的三维信息并将疑似障碍物区域红色显示;然后依据机器人运动特性将视差图二值化，在二值图像基础上提取障碍物轮廓与位置信息并将障碍物信息插入环境地图。通过实验验证可知，该方法能够在机器人SLAM的同时，快速检测出环境中的障碍物，且检测效果较好。

1 算法设计

1.1 算法整体架构

算法主要由三个模块组成，如图1所示，包括视差图获取模块、障碍物检测模块和定位与建图模块。

视差图获取模块首先由双目相机采集场景图像，并转换为灰度图像;基于相机标定模型，消除图片畸变并对图片进行立体校正;然后，利用ORB特征提取算法提取左右图像的特征点信息;最后，采用半全局立体匹配（Semi-Global Block Matching， SGBM）算法[15]计算视差，得到较为精确的致密视差图。得到视差图之后，定位与建图模块依据ORB-SLAM2算法，实现相机位姿的跟踪并建立环境的稀疏特征地图。与此同时，障碍物检测模块依据立体视觉法检测出环境中障碍物，并将障碍物信息插入到定位与建图模块所建立的环境地图当中。

算法最终得到一幅具有障碍物信息的环境地图，这一地图包含了障碍物坐标信息、特征点三维信息和相机位姿，在ORB-SLAM2基础上实现了环境信息的补充，为机器人与环境的交互提供依据，同时为后续的路径规划打下基础。

1.2 基于ORB-SLAM2的定位与建图算法

定位与建图算法基于ORB-SLAM2实现，ORB-SLAM2功能非常全面，并且具有良好的精度和实时性，是当前稀疏型SLAM的代表。因此，本文选择ORB-SLAM2的双目部分完成算法的定位与建图模块，其整体的结构框图如图2所示，主要包含了位姿跟踪、局部构图和回环检测三个线程。

1.3 基于立体视觉的障碍物检测算法

双目立体视觉是通过两个不同位置的攝像机采集场景图像，根据两幅不同角度的场景图中的对应点来计算视差。本文在视差图的基础上，设计了障碍物检测算法。

首先，在视差图基础上，利用三角测量原理将图像转换成三维点云。给定视差d、重投影矩阵Q和点坐标（x，y），根据最简单的相似三角形关系，就可以得到对应的三维坐标（X/W，Y/W，Z/W），其转换公式为：

2 基于ROS的软件架构设计

本文算法基于机器人操作系统（Robot Operating System， ROS）架构实现，系统的整体流程如图3所示。

系统在开始阶段，首先从外部文件中读取摄像头参数并将参数初始化;然后，由图像采集节点不断地发布相机采集的场景图像，得到图像信息后，ORB-SLAM2节点与障碍物检测节点同时运行并订阅图像信息。ORB-SLAM2节点逐渐完成系统的初始化并针对输入的图像不断执行位姿跟踪、局部构图和回环检测三个线程，从而实现相机位姿的跟踪和环境特征地图构建。障碍物检测节点首先对摄像头参数进行检查，在获得正确的参数情况下完成图像的立体匹配得到视差图，然后在视差图的基础上依次执行算法的各个步骤，从而实现环境中障碍物的检测并将障碍物信息发布出去，这一信息将会被ORB-SLAM2节点订阅并将其插入到环境地图中。当两个节点同时判断系统关闭时关闭整个系统。

系统的节点总体设计如图4所示。虚线框外的方框代表的是外部节点，分别是机器人控制节点和传感器之间的坐标转换节点。虚线框内的是本文运行的节点，图像采集节点主要完成图像的采集和分发;障碍物检测节点订阅来自图像采集节点的图像信息并实现障碍物检测和发布障碍物的信息;ORB_SLAM2节点订阅来自图像采集节点的图像信息处理后，实时发布相机的位姿信息，并订阅来自障碍物检测节点的障碍物信息插入环境地图。

3 实验与结果分析

3.1 数据集测试

针对系统的定位性能，本文利用著名的双目数据集KITTI[18]对系统进行测试，实验设备为一台配备Intel core i7处理器的笔记本电脑，并装有64位的Ubuntu16.04操作系统。各数据子集上不同方法的定位均方根误差结果对比如表1所示。

从表1中可以看出，在大多数数据子集的测试中，本文算法的定位精度优于稠密方法LSD-SLAM，且和ORB-SLAM2的定位精度基本一致，这是由于本文的定位与建图算法是基于ORB-SLAM2的双目模块实现的，因此能够保证较高的定位精度。

3.2 室内实物实验

3.2.1 实验硬件选型

实验硬件系统由两个同一型号的USB摄像机和图像处理终端共同组成，并将其安装在轮式机器人Arduino 4WD上进行实验。为了实现左右图像序列的获取，本文选择两个内参数相近的JD-202USB摄像机，如图5（a）所示，其感光元件为CMOS，最大分辨率为1280×720，帧数率为30frame/s。为了提高图像处理能力，本文选择了香蕉派BPI-M3作为图像处理终端，如图5（b）所示。香蕉派BPI-M3是一个拥有8核1.8GHz处理器和2GB LPDDR3内存的超级单板电脑，它可以运行Android、Ubuntu等操作系统，具有计算处理速度快、外设接口丰富、体积小巧轻便易挂载和开源的社区等特点，满足本文算法的设计需求。

3.2.2 实验方案设计

实验数据来源于实验室运动场景，运动场景的平面图以及设计的运动轨迹如图6所示。机器人在室内按预定轨迹运动一周，运动的途中，为在场景图标注的三个障碍物点设定三种不同的障碍物环境，以验证算法在不同环境下的障碍物检测效果。障碍物1为简单环境，只设置一个纹理清晰的静态障碍物;障碍物2为复杂环境，是在简单环境的基础上添加了多个不同的静态障碍物;障碍物3为动态环境，是指将静态障碍物换成运动的履带小车。

简单环境和复杂环境实验过程中，分别在机器人距离障碍物0.3m、0.8m、1.5m、2.0m、2.5m、3.0m的地方，对相应场景进行障碍物檢测，分别记录系统的检测距离、单帧处理时间和误检率。动态障碍物环境下，履带车在机器人前以不同的速度进行运动，以此来检验系统对于运动目标的检测能力。深度阈值取对应的场景距离分别为1.8m、2.5m、3.2m的三个值，在不同阈值下进行多次实验;面积阈值取为100个像素点，将凸包面积小于100个像素点的区域视为误检测区域，予以剔除。

3.2.3 结果分析

对于实物实验的自主定位性能，本文只针对实验结果作简要分析。精度方面，形成闭环路径前，系统生成的路径会出现局部的偏移，闭环路径形成后，系统开始进行全局优化，对轨迹图和位姿进行调整。将生成的路径与设计的路径比较，可以发现整体生成图与设计的路线拟合得比较好，没有出现较大的偏差。实时性方面，系统启动以后，每秒处理帧数在14～18，可以满足机器人一般运动状态下的定位需要。稳定性方面，实验过程中，有时会出现跟踪丢失的情况，但是由于采用了关键帧存取，实现了系统快速的重定位能力，可以保证稳定的定位输出。

障碍物检测方面，三种环境下不同时刻的障碍物检测结果如图8～10所示，图中子图（a）、（b）、（c）分别为机器人在距离障碍物2.0m、1.5m和0.8m处的实验结果。在每幅子图中，从上到下依次为左图像，右图像，伪彩色图像和障碍物提取图像。简单环境和复杂环境的测试距离分别由远到近进行，动态障碍物的运动分为沿相机坐标系的X轴和Z轴方向运动。

4 结语

本文主要研究了基于视觉的机器人自主定位和障碍物检测问题，利用典型稀疏性SLAM算法ORB-SLAM2实现了算法的自主定位功能。针对ORB-SLAM2存在的环境信息丢失的问题，在ROS架构下，引入了基于深度的障碍物检测算法，解决了其不能检测障碍物的问题。在基于立体视觉的障碍物检测算法中，进一步引入深度与面积阈值，提高算法的检测精度。将所提算法在搭建的实验平台上进行验证，实验结果表明本文系统是有效可行的。如何在具有障碍物信息的环境地图中进行路径规划是未来需要进一步研究的方向。

参考文献 （References）

[1] 杨洋，童东兵，陈巧玉.面向未知地图的六足机器人路径规划算法[J].计算机应用，2018，38（6）：1809-1813.（YANG Y， TONG D B， CHEN Q Y. Six-legged robot path planning algorithm for unknown map [J]. Journal of Computer Applications， 2018， 38（6）： 1809-1813.）

[2] NAYA B， SABRINA H， STEFAN W， et al. Navigation and vision system of a mobile robot [C]// Proceedings of the 2018 19th International Conference on Research and Education in Mechatronics. Piscataway， NJ： IEEE， 2018： 99-104.

[3] 胡章芳，鲍合章，陈旭，等.基于改进闭环检测算法的视觉同时定位与地图构建[J].计算机应用，2018，38（3）：873-878.（HU Z F， BAO H Z， CHEN X， et al. Visual simultaneous location and mapping based on improved closed-loop detection algorithm [J]. Journal of Computer Applications， 2018， 38（3）： 873-878.）

[4] 张永，杨浩.基于优化视觉词袋模型的图像分类方法[J].计算机应用，2017，37（8）：2244-2247，2252.（ZHANG Y， YANG H. Image classification method based on optimized bag-of-visual words model [J]. Journal of Computer Applications， 2017， 37（8）： 2244-2247， 2252.）

[5] NISTER D， NARODITSKY O， BERGEN J R. Visual odometry [C]// Proceedings of the 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Piscataway， NJ：IEEE， 2004： 652-659.

[6] ENGEL J， SCHOPS T， CREMERS D. LSD-SLAM： large-scale direct monocular SLAM [C] // Proceedings of the 2014 13th European Conference on Computer Vision， LNCS 8690. Berlin： Springer， 2014： 834-849.

[7] SIM R， ELINAS P， GRIFFIN M， et al. Design and analysis of a framework for real-time vision-based SLAM using Rao-Blackwellised particle filters [C]// CRV 2006： Proceedings of the 3rd Canadian Conference on Computer and Robot Vision. Washington， DC： IEEE Computer Society， 2006： 21-29.

[8] YANG M L， LIU Y G， YOU Z S. The Euclidean embedding learning based on convolutional neural network for stereo matching [J]. Neurocomputing， 2017， 267： 195-200.

[9] KLEIN G， MURRAY D. Parallel tracking and mapping for small AR workspaces [C]// ISMAR 2007： Proceedings of the 2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality. Washington， DC： IEEE Computer Society， 2007： 225-234.

[10] MUR-ARTAL R， MONTIEL J M M， TARDOS J D. ORB-SLAM： a versatile and accurate monocular SLAM system [J]. IEEE Transactions on Robotics， 2015， 31（5）： 1147-1163.

[11] 周武，趙春霞，沈亚强，等.基于全局观测地图模型的SLAM研究[J].机器人，2010，32（5）：647-654.（ZHOU W， ZHAO C X， SHEN Y Q， et al. SLAM research based on global observation map model [J]. Robot， 2010， 32（5）： 647-654.）

[12] 王轩，叶平，贾庆轩.双目立体视觉栅格地图构建方法[J].软件，2012，33（11）：233-236.（WANG X， YE P， JIA Q X. Stereo vision location and grid map building method [J]. Computer Engineering & Software， 2012， 33（11）： 233-236.）

[13] 陈炜楠，朱蕾，张宏，等.稀疏视觉SLAM对平面激光雷达传感的稠密化模拟[J].机器人，2018，40（3）：273-281.（CHEN W N， ZHU L， ZHANG H， et al. Planar LiDAR densified simulation from sparse visual SLAM [J]. Robot， 2018， 40（3）： 273-281.）

[14] MUR-ARTAL R， TARDOS J D. ORB-SLAM2： an open-source SLAM system for monocular， stereo and RGB-D cameras [J]. IEEE Transactions on Robotics， 2016， 33（5）： 1255-1262.

[15] HIRSCHMULLER H. Accurate and efficient stereo processing by semi-global matching and mutual information [C]// CVPR 2005： Proceedings of the 2005 IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Washington， DC： IEEE Computer Society， 2005： 807-814.

[16] GALVEZ-LOPEZ D， TARDOS J D. Bags of binary words for fast place recognition in image sequences [J]. IEEE Transactions on Robotics， 2012， 28（5）： 1188-1197.

[17] SUZUKI S， ABE K. Topological structural analysis of digitized binary images by border following [J]. Computer Vision， Graphics， and Image Processing， 1985， 30（1）： 32-46.

[18] GEIGER A， LENZ P， STILLER C， et al. Vision meets robotics： the KITTI dataset [J]. International Journal of Robotics Research， 2013， 32（11）： 1231-1237.