基于VSLAM的自主移动机器人三维同时定位与地图构建

2017-12-14林辉灿王国胜

计算机应用 2017年10期

关键词：移动机器人位姿鲁棒性

林辉灿,吕强,王国胜,张洋,梁冰

(1.装甲兵工程学院控制工程系,北京 100072; 2.江西理工大学信息工程学院,江西赣州 341000) (*通信作者电子邮箱rokyou@live.cn)

基于VSLAM的自主移动机器人三维同时定位与地图构建

林辉灿1,吕强1*,王国胜1,张洋1,梁冰2

(1.装甲兵工程学院控制工程系,北京 100072; 2.江西理工大学信息工程学院,江西赣州 341000) (*通信作者电子邮箱rokyou@live.cn)

移动机器人在探索未知环境且没有外部参考系统的情况下,面临着同时定位和地图构建(SLAM)问题。针对基于特征的视觉SLAM(VSLAM)算法构建的稀疏地图不利于机器人应用的问题,提出一种基于八叉树结构的高效、紧凑的地图构建算法。首先,根据关键帧的位姿和深度数据,构建图像对应场景的点云地图;然后利用八叉树地图技术进行处理,构建出了适合于机器人应用的地图。将所提算法同RGB-D SLAM(RGB-Depth SLAM)算法、ElasticFusion算法和ORB-SLAM(Oriented FAST and Rotated BRIEF SLAM)算法通过权威数据集进行了对比实验，实验结果表明,所提算法具有较高的有效性、精度和鲁棒性。最后,搭建了自主移动机器人,将改进的VSLAM系统应用到移动机器人中,能够实时地完成自主避障和三维地图构建,解决稀疏地图无法用于避障和导航的问题。

计算机视觉;同时定位与地图构建;自主移动机器人;八叉树地图;避障

0 引言

在现实环境中自主移动的机器人必须具备在大尺度、非结构化、动态和未知空间中进行导航的能力。为此,移动机器人必须能够构建环境地图并实现自定位,即同时定位和地图构建(Simultaneous Localization And Mapping, SLAM)。SLAM在过去的近30年中得到了广泛的关注与研究,学者们提出了大量的SLAM方法,包括各种各样的传感器、优化技术和地图描述方式。激光雷达能够以高频率提供精确的环境几何测量,存在的主要不足是价格高、耗能大和体积重量大,而摄像机能够克服上述不足,且能够提供更多的信息。

视觉SLAM(Visual SLAM, VSLAM)方法使用摄像机作为传感器估计机器人的位姿和环境地图[1]。Klien等[2]提出在单目VSLAM系统中将跟踪和地图构建分为两个并行线程,实现了基于捆集调整(Bundle Adjustment, BA)的并行跟踪与地图构建(Parallel Tracking And Mapping, PTAM)系统。RGB-D(RGB-Depth)摄像机能提供彩色图像和深度图像,相对于单目摄像机能够获得绝对尺度,避免了深度求取和尺度模糊等问题；相对于双目摄像机，由于深度信息能直接获取,所以计算量较小,降低了实现难度。

Kerl等[3]提出优化光测一致性误差项以配准连续RGB-D帧,构建RGB-D里程计,随后又提出一种结合光测一致性误差和基于稠密深度数据的误差项的鲁棒方法[4],注重位姿估计而忽略地图构建。Henry等[5]提出使用RGB-D摄像机的SLAM系统,结合迭代最近点(Iterative Closest Point, ICP)算法和视觉特征计算摄像机位姿并优化,构建了稠密的点云地图但不具备实时性。Endres等[6]提出的RGB-D SLAM(RGB-Depth SLAM)算法与Kerl算法[3]一样在CPU上实时运行,采用相同的硬件进行评估实验,显示出更好的鲁棒性和精度。这些算法中使用随机采样一致性(RANdom SAmple Consensus,RANSAC)估计关联视觉特征点之间的闭环,并使用非线性方法优化位姿图。最后,生成环境的体素三维地图,能够应用于机器人的避障、路径规划和导航。由于算法使用ICP进行位姿图优化,过度依赖于良好的初始化估计且缺乏对整体匹配质量的衡量。

由于视觉特征倾向于在实物的边缘,导致深度测量数据易受噪声干扰,为了减小估计误差,文献[7]通过可靠的深度数值选择和提取特征,提出基于平面特征的RGB-D SLAM算法。采用平面点特征有利于提高传统的ICP的精度和鲁棒性,同时保持合理的计算量以保证实时性。ElasticFusion[8]通过融合ICP和稠密的光度重投影误差实现视觉里程计,并构建了点云地图,具有较高的精度但是需要GPU加速；同文献[5]和文献[7]一样,由于构建的是点云地图,存在资源消耗严重和不利于维护的问题,不适合机器人应用。Mur-Artal等[9]提出的ORB-SLAM(Oriented FAST and Rotated BRIEF SLAM)根据图像序列实时地估计位姿和地图,精度和鲁棒性均优于上述方法,支持主流的单目、双目和RGB-D摄像机。ORB-SLAM能够在小尺度和大尺度、室内和室外环境实时地完成位姿估计的同时构建稀疏特征点地图,实现宽基线的闭环检测和重定位,并且能够实现全自动的初始化。该系统是迄今为止基于稀疏特征的VSLAM的最好系统,其精度和鲁棒性是很难超越的。相对于RGB-D SLAM,该方法存在的不足是构建的稀疏特征点地图应用领域有限,侧重于定位功能,不能够用于机器人的避障、路径规划和自主导航。文献[10]基于ORB-SLAM构建点云地图,通过手柄遥控Turtlebot运动并构建地图。

本文基于ORB-SLAM,使用RGB-D传感器提供的彩色图像与深度图像将原系统构建的稀疏地图扩展成稠密的点云地图,在此基础上提出使用八叉树结构[11]构建可供机器人应用的八叉树地图。最后,将本文算法成功应用于自主移动机器人,实验结果表明本文算法能够在充满挑战的条件下、长距离轨迹的情况下,精确地跟踪机器人的位姿并构建环境3D地图,并实现自主避障。

1 八叉树地图构建

通常将VSLAM分为前端、后端和最终地图描述三个模块:前端解决传感器数据关联和位姿估计问题,后端解决局部图优化和闭环检测及全局优化问题。本文根据ORB-SLAM算法优化后的数据构建八叉树地图,并成功应用于移动机器人的地图构建与避障,也能够应用于机器人路径规划、导航和交互式操作。

ORB-SLAM中的关键帧存储与之对应的摄像机位姿、彩色图像与深度图像,根据当前关键帧中的数据构建点云地图,进而创建八叉树地图,用于机器人的自主避障。

(1)

(2)

其中:R和t分别表示摄像机的旋转矩阵和位移矢量。根据式(2)变换得到世界坐标系下的坐标,则获得对应关键帧的点云地图。点云地图包含关键帧的所有像素构成的大量的由三维位置信息构成的点,随着关键帧规模的增大,维护显得更加困难。同时,对于机器人应用而言,如此大规模的空间点,冗余量大并且不实用。

八叉树地图能够描述任意形式的3D环境,具有更新方式灵活、无需先验知识和地图形式紧凑的优点。相对于点云地图,以八叉树结构分层描述方式存储地图,能够高效地压缩和更新地图,并且地图分辨率可调,可解决点云地图不紧凑和不利于避障和导航的问题。

八叉树结构中的叶子节点,采用概率的形式表示该空间的占据、空旷或未知状态,即3D空间中该点是否存在障碍或可通行。在构建地图的过程中,对于叶子节点n,在1,2,…,k时刻的观测数据分别为z1,z2,…,zk,由贝叶斯理论可知,该节点记录的信息为:

Q(n|z1:k)=

其中Q(n)一般情况下假设为0.5,此处引入logit变换:

logit(q)=ln(q/(1-q))

对式(3)两边取logit变换,代入Q(n)=0.5,化简后得:

L(n|z1:k)=L(n|z1:k-1)+L(n|zk)

(4)

2 自主移动机器人平台

为将本文算法构建的八叉树地图应用到地面移动机器人的自主避障中,搭建自主移动机器人平台,其中车身架构为Axial AX10,摄像机为Kinect 2.0,控制器为Pixhawk[12-13],数据处理单元为微型计算机,硬件条件是4核i7处理器,16 GB内存,256 GB固态硬盘,系统的结构框架和搭建的移动机器人平台实物见图1。

微型计算机通过USB3.0接口读取Kinect 2.0的彩色图像和深度图像,并以ROS(Robot Operating System)[14]主题的形式发布,本文算法通过订阅该主题的数据完成位姿估计与跟踪、特征地图构建与优化、闭环检测和八叉树地图构建。将摄像机的位姿估计作为移动机器人的状态估计,将创建的八叉树地图用于移动机器人自主避障。其中,运动控制指令采用ROS主题的形式发布(geometry_msgs::Twist),Pixhawk控制器订阅该主题数据,并输出信号到电子调速器以控制电机,实现移动机器人平台的运动控制。

图1 自主移动机器人平台

3 自主避障策略

有关自主移动机器人平台根据八叉树地图进行路径规划与导航,不属于本文研究的范畴。为验证本文算法能够成功地应用于移动机器人在未知的、复杂的动态环境中的自主导航能力,运动控制部分主要采用漫游式的自主避障策略。根据已构建的八叉树地图,检测移动机器人前方障碍物的距离。当机器人前方没有障碍物或障碍物距离大于安全距离时,给定直行前进的命令。当前方障碍物的距离小于等于安全距离时,则给定左转前进的命令。若转向后障碍物仍小于安全距离,则给定右转后退的命令,直到前方没有障碍,再给定直行前进的命令继续探索环境。避障策略代码如下。

输入障碍物最近距离(closestRange)。

输出运动控制指令。

如果closestRangegt;安全距离且正在前进

throttle← 0.4

//throttle为推力，前进

yaw← 0

//yaw为转向，直行

如果closestRange≤ 安全距离且直行前进

throttle← 0.4

// 前进

yaw← -0.6

// 左转

如果closestRange≤安全距离且左转前进

throttle← -0.4

// 后退

yaw← 0.6

// 右转

10)

如果closestRangegt;安全距离且右转后退

11)

throttle← 0.4

// 前进

12)

yaw← 0

//直行

4 实验评估

为验证本文算法的鲁棒性、精度和实时性,采用TUM(Technische Universität München)的RGB-D数据集[15]进行实验评估,并将本文算法应用于移动机器人的三维同时定位和三维地图构建。

4.1 RGB-D 标准数据集

(5)

本文选择的误差度量虽然不能直接衡量地图的质量,但是SLAM系统同时完成位姿(轨迹)估计和地图构建,且二者相辅相成,因此轨迹偏差程度与地图偏差程度直接相关。

4.2 精度与鲁棒性

对比本文算法同RGB-D SLAM算法[6]、ElasticFusion算法[8]和ORB-SLAM算法[9]的精度和鲁棒性,如图2所示。由图2可知,运行文献[6]中评估性能所采用的4个数据集,本文算法的RMSE约为文献[6]的25%,精度提高了4倍。相对文献[6]采用ICP进行位姿图优化,本文基于ORB-SLAM采用BA同时优化摄像机的位姿和地图,有利于提升算法的精度和鲁棒性。文献[8]在fr2_large_no_loop数据集失败,本文算法的精度与之相当,但是鲁棒性更好。本文与文献[9]的精度和鲁棒性相当。

图2 不同算法的精度对比

图3 不同算法的运行时间对比

4.3 实时性

本文采用数据集进行验证的硬件条件为4核i5处理器,4 GB内存,文献[6]采用i7处理器和GTX570显卡。将实验的耗时情况绘制如图3,可见本文算法在处理器性能较低的情况下,平均耗时降低为文献[6]的56%,这主要得益于加入了关键帧的剔除和优化功能,尤其在环境规模大、长时间构图的情况下,优势更加明显。文献[8]需要GPU加速,因此不进行对比。本文基于文献[9]扩展了地图构建方法,耗时约为其2倍,主要是本文构建点云地图和八叉树地图时耗时较大。

4.4 3D地图构建

ORB-SLAM侧重于提升摄像机位姿估计的精度、实时性和鲁棒性,所构建的稀疏地图主要应用于提升位姿估计精度以及回环检测以保证构建地图的全局一致性,更适合于虚拟现实(Virtual Reality,VR)而不适合于机器人应用,如图4(a)为运行“fr2_large with loop”数据集获得的稀疏地图。本文基于ORB-SLAM扩展使用RGB-D传感器构建稠密的点云地图和适合机器人应用的八叉树地图,图4(b)、(c)为本文算法运行fr2_large_with_loop和fr2_pioneer_slam数据集创建稠密点云地图和八叉树地图。此外,图4(b)、(c)两个点云地图和八叉树地图占用的存储空间分别为96.5 MB和220 KB,显然后者的形式更紧凑、易于维护。

图4 稀疏地图、点云地图和八叉树地图

此外,将本文算法获得的摄像机轨迹估计与由数据集提供的高精度运动捕获系统测量的摄像机轨迹真实值进行对比,对比结果表明本文算法有效且精度高,如图5所示(RMSE分别为0.10 m和0.15 m)。

图5 绝对轨迹误差

4.5 移动机器人应用

最终,在实验室环境且地图未知的情况下,基于第2章搭建的移动机器人平台,测试本文算法。测试结果表明,本文算法能够实时地实现自主避障和八叉树地图重构,所有数据采集与处理均依靠在板的传感器和计算机完成,图6为构建的3D地图。在实际运行的过程中,随着地图规模的增大,算法的实时性降低,原因是为提高点云地图和八叉树地图的可视化效果,地图创建的精度为0.02 m,远高于移动机器人导航所需。

图6 移动机器人构建三维地图

5 结语

本文基于ORB-SLAM系统,使用RGB-D传感器提出了一种新的VSLAM系统,利用关键帧的数据改进了原系统的地图构建方法,由不利于机器人应用的稀疏地图扩展为适合机器人避障与导航,且易于维护的八叉树地图。本文算法在精度和速度上要优于RGB-D SLAM方法[6],相对于ElasticFusion[8]精度相当而鲁棒性更好,相对于ORB-SLAM方法扩展其地图构建技术,实时地构建八叉树地图,并成功应用于所搭建的自主移动机器人平台,实现未知复杂环境下的自主避障与探索,但是耗时增加。下一步将对图优化算法进行改进,进一步提升实时性,并研究将八叉树地图应用于路径规划和自主导航。

References)

[1] FUENTES-PACHECO J, RUIZ-ASCENCIO J, RENDON-MANCHA J M. Visual simultaneous localization and mapping: a survey[J]. Artificial Intelligence Review, 2015, 43(1): 55-81.

[2] KLEIN G, MURRAY D. Parallel tracking and mapping for small AR workspaces[C]// ISMAR 2007: Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality. Piscataway, NJ: IEEE, 2007: 225-234.

[3] KERL C, STURM J, CREMERS D. Robust odometry estimation for RGB-D cameras[C]// Proceedings of the 2013 IEEE International Conference on Robotics and Automation. Piscataway: IEEE Press, 2013: 3748-3754.

[4] KERL C, STURM J, CREMERS D. Dense visual SLAM for RGB-D cameras[C]// Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 2013: 2100-2106.

[5] HENRY P, KRAININ M, HERBST E, et al. RGB-D mapping: using depth cameras for dense 3D modeling of indoor environments[C]// Proceedings of the 12th International Symposium on Experimental robotics. Berlin: Springer, 2014: 477-491.

[6] ENDRES F, HESS J, STURM J, et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics, 2014, 30(1): 177-187.

[7] GAO X, ZHANG T. Robust RGB-D simultaneous localization and mapping using planar point features[J]. Robotics and Autonomous Systems, 2015, 72: 1-14.

[8] WHELAN T, SALAS-MORENO R F, GLOCKER B, et al. ElasticFusion: real-time dense SLAM and light source estimation[J]. The International Journal of Robotics Research, 2016, 35(14): 1697-1716.

[9] MUR-ARTAL R, MONTIEL J M M, TARDS J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.

[10] 侯荣波, 魏武, 黄婷, 等. 基于ORB-SLAM的室内机器人定位和三维稠密地图构建[J]. 计算机应用, 2017, 37(5): 1439-1444. (HOU R B, WEI W, HUANG T, et al. Indoor robot localization and 3D dense mapping based on ORB-SLAM[J]. Journal of Computer Applications, 2017, 37(5): 1439-1444.)

[11] HORNUNG A, WURM K M, BENNEWITZ M, et al. OctoMap: an efficient probabilistic 3D mapping framework based on octrees[J]. Autonomous Robots, 2013, 34(3): 189-206.

[12] MEIER L, TANSKANED P, HENG L, et al. PIXHAWK: a micro aerial vehicle design for autonomous flight using onboard computer vision[J]. Autonomous Robots, 2012, 33(1/2): 21-39.

[13] MEIER L, HONEGGER D, POLLEFEYS M. PX4: a node-based multithreaded open source robotics framework for deeply embedded platforms[C]// Proceedings of the 2015 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2015: 6235-6240.

[14] MARTINEZ A, FERNANDEZ E. Learning ROS for Robotics Programming[M]. Birmingham: Packt Publishing, 2013: 45-48.

[15] STURM J, ENGELHARD N, ENDRES F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]// Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 2012: 573-580.

3DsimultaneouslocalizationandmappingformobilerobotbasedonVSLAM

LIN Huican1, LYU Qiang1*, WANG Guosheng1, ZHANG Yang1, LIANG Bing2

(1.DepartmentofControlEngineering,AcademyofArmoredForceEngineering,Beijing100072,China;2.SchoolofInformationEngineering,JiangxiUniversityofScienceandTechnology,GanzhouJiangxi341000,China)

The Simultaneous Localization And Mapping (SLAM) is an essential skill for mobile robots exploring in unknown environments without external referencing systems. As the sparse map constructed by feature-based Visual SLAM (VSLAM) algorithm is not suitable for robot application, an efficient and compact map construction algorithm based on octree structure was proposed. First, according to the pose and depth data of the keyframes, the point cloud map of the scene corresponding to the image was constructed, and then the map was processed by the octree map technique, and a map suitable for the application of the robot was constructed. Comparing the proposed algorithm with RGB-Depth SLAM (RGB-D SLAM) algorithm, ElasticFusion algorithm and Oriented FAST and Rotated BRIEF SLAM (ORB-SLAM) algorithm on publicly available benchmark datasets, the results show that the proposed algorithm has high validity, accuracy and robustness. Finally, the autonomous mobile robot was built, and the improved VSLAM system was applied to the mobile robot. It can complete autonomous obstacle avoidance and 3D map construction in real-time, and solve the problem that the sparse map cannot be used for obstacle avoidance and navigation.

computer vision; Simultaneous Localization And Mapping (SLAM); autonomous mobile robot; octree map; obstacle avoidance

2017- 04- 05;

2017- 06- 18。

国家自然科学基金资助项目(61663014)。

林辉灿(1989—),男,福建厦门人,博士研究生,主要研究方向:VSLAM、自主移动机器人; 吕强(1962—),男,北京人,教授,博士,主要研究方向:机器人控制、机器视觉; 王国胜(1975—),男,北京人,副教授,博士,主要研究方向:非线性控制、计算机视觉; 张洋(1987—),男,吉林四平人,博士研究生,主要研究方向:计算机视觉、SLAM; 梁冰(1975—),女,副教授,博士,主要研究方向:鲁棒容错控制。

1001- 9081(2017)10- 2884- 04

10.11772/j.issn.1001- 9081.2017.10.2884

TP242.6

This work is partially supported by the National Natural Science Foundation of China (61663014).

LINHuican, born in 1989, Ph. D. candidate. His research interests include visual simultaneous localization and mapping, autonomous mobile robot.

LYUQiang, born in 1962, Ph. D., professor. His research interests include robot control, machine vision.

WANGGuosheng, born in 1975, Ph. D., associate professor. His research interests include non-linear control, computer vision.

ZHANGYang, born in 1987, Ph. D. candidate. His research interests include computer vision, simultaneous localization and mapping.

LIANGBing, born in 1975, Ph. D., associate professor. Her research interests include robust tolerant control.