摘要
回环是基于激光的同时定位和建图研究中的一个众所周知的问题，特别是对于大规模环境中的应用。 无论使用基于粒子滤波器还是基于图形的SLAM方法，估计的位姿和建图中的累积误差都使得回环检测变得困难。 相机具有信息丰富的优点，但是距离短，计算负担相对较高。 在本文中，我们提出了一种新方法来解决大规模激光SLAM中的回环问题，其中激光和相机传感器集成在一起。 应用ORB特征和词袋来获得快速且鲁棒的回环检测性能。 然后使用公认的LRGC SLAM框架和SPA优化算法来实现SLAM。 最后，在不同的大规模环境中进行了几次实验，以验证所提出方法的有效性。
1.简介
基于激光的同时定位和建图（SLAM）（例如[1] - [3]）是机器人感知研究中最受欢迎的主题之一，因为激光具有高精度，长距离和避障能力的优点。 在许多实际应用中，机器人需要同时定位姿势并构建未探测环境的地图。 如图1所示，应用包括服务机器人，工业无人地面车辆和无人驾驶飞行器等。在过去的几十年中，已经开发了许多方法来实现基于激光的SLAM，如EKF-SLAM [1]，GMapping [ 2]和基于图的SLAM [4]。 这些方法在小规模环境中是有效的。 然而，它们在大规模环境中遭受回环检测问题，特别是在具有对称性或几何简单性的环境中。（激光在对称性或几何简单环境有问题）
Tardós等人[5] 检测声纳传感器的线或角特征，然后利用扩展卡尔曼滤波器估计声纳特征的位置以及机器人姿态。 然而，由于数据关联不稳定和计算负担高，基于EKF的方法难以用于大规模激光SLAM。
Montemerlo等人[6] 利用rao-blackwellized粒子滤波器提出了FastSLAM 1.0来处理数据关联和计算负担的问题。 和蒙特梅罗等人[7] 通过在提案分布中集成运动预测和测量不确定性分布，进一步开发了FastSLAM2.0。 这种新方法可以大大提高FastSLAM1.0的准确性和鲁棒性。 可以进一步集成扫描匹配技术和自适应重采样技术，以提高FastSLAM的鲁棒性。
Lu和Milios [8]提出了一种基于图形的方法，通过使用基于ICP（迭代最近点）的扫描匹配来构建扫描之间的图形边缘。 迭代线性化进一步用于优化图。 最初的基于图形的方法耗时，因此难以实时应用。 Dellaert等人[3] 利用基于图形的方法的内部稀疏性来加速计算，并利用束调整算法[9]开发了一种名为SAM的新方法。Konolige等人[4] 使用另一种称为稀疏位姿调整（Sparse Pose Adjustment，SPA）的调整方法，并提出了一种“continuable LM”方法。 该方法类似于SAM，但在重新线性化和矩阵构造方面具有更高的效率。 SPA考虑姿势约束中的协方差信息，从而产生更准确的解决方案。 此外，LM方法的快速收敛有利于SPA的低计算成本。
基于具有束调整算法（bundle adjustment algorithm）的激光SLAM可以在小规模环境中表现出良好的性能。 然而，由于累积的估计误差和来自激光测距仪的信息限制测量，在大规模环境中尤其是在具有相似几何结构的环境中检测轨迹环是困难的。 在本文中，我们的目标是通过集成单目系统来开发一种解决上述问题的新方法。
基于摄像头的方法属于SLAM研究的另一个重要分支。 在文献中已经提出了许多带有照相机的SLAM方法。 Davison等人[10]在标准EKF框架下实现了实时单目SLAM。 Civera等[11]在基于EKF的MonoSLAM框架中采用了1点RANSAC方法，可以成功区分内点和异常值。 Endres等人[12]提出了一种名为RGBD-SLAM的新型视觉SLAM算法，其中来自Kinect的3D点云被记录以计算两个相机姿势之间的变换。 Klein和Murray [13]提出了一种新的实时SLAM框架，称为并行跟踪和映射（PTAM），基于bundle adjustment。 基于PTAM，Mur-Artal等[14] 利用ORB方法[15]检测跟踪和建图的特征，并使用词袋模型进行位置识别。 ORB功能的使用有助于PTAM算法更快地执行，而无需GPU或多线程技术。
通常，基于相机的SLAM可以实现3D定位和建图，但是鲁棒性低，尤其是当操作环境缺乏可检测特征时。 然而，由于从相机可提取的丰富信息，视觉SLAM通常在数据关联和回环检测中比基于激光的方法执行得更好。 受到工作[16]的启发，我们将灰度相机与激光传感器集成在一起，以提供强大的回环检测实现。 此外，在所提出的方法中使用ORB特征检测和词袋技术来保证鲁棒性和实时计算性能。
2. 方法框架
在本文中，我们将视觉和激光信息集成在一起，并提出一种新颖有效的SLAM方法，可以解决基于激光的SLAM中的回环检测问题。 图2说明了所提方法的主要框架。 该框架导入了公认的SLAM框架，旨在继承高鲁棒性和低计算负担。 主要思想是通过位姿图表示机器人的历史姿势，其中每个图形节点涉及位姿估计和该位姿的传感器测量。 两个节点之间的边缘编码为由连接的测量的空间约束。
如图2所示，我们的方法框架中有两个主要过程，即数据关联（前端）和地图校正（后端）。 数据关联通过匹配传感器测量来识别图边缘（graph edges），因为环境中存在潜在的模糊或对称性，这是一个困难的过程。 地图校正通过优化关于位姿约束的一些目标函数来校正所有位姿节点和地图; 它寻求节点的配置，以最大化在约束中编码的测量的可能性。
前端包括帧到帧对齐（frame-to-frame alignment）和回环检测。 帧到帧的对齐建立了观察到的数据帧之间的对应关系。 回环检测是检测机器人当前是否在已经访问过的地方附近，实际上是一种地点识别。 回环检测在SLAM中起着非常重要的作用，因为正确的回环检测可以解决累积到当前时间的大型里程表误差。 相反，不正确的回环会对后续的图优化产生干扰，甚至可能破坏整个地图。 由于激光器的传感器测量信息受到限制，因此传统的激光方法需要很长时间来收集足够的传感器数据以进行有效的回环检测和闭合，因此误报率和负误差都可能在激光专用SLAM中频繁发生。
为了解决上述问题，我们尝试通过在机器人上安装灰度相机来导入视觉信息。受ORB-SLAM [14]工作的启发，提出的方法采用了两种关键技术，包括ORB方法[15]和词袋模型，如图3所示。ORB特征，面向多尺度FAST每个具有256位描述符关联的角点，计算和匹配都非常快。并且它们对视点高度不变，这使得它们对于不同视觉帧之间的特征匹配具有鲁棒性。这些词袋用于加速特征匹配，并在大规模环境中实现实时地点识别。然而，在许多情况下，ORB特征跟踪可能失败，例如，机器人移动得太快，存在遮挡或抖动移动，或者可检测的特征太少。为了解决这个问题，提出了一种全局重定位方案，如图3所示。在所提出的方法中，通过可视方法的每个回环检测将触发基于激光的部分扫描匹配和地图校正过程，这将在第3节中讨论。
3.雷达-相机SLAM
为了实现视觉辅助激光SLAM，我们应用了 Local Registration and Global Correlation（LRGC）方法[17]，这是一个公认的SLAM框架。 在我们提出的方法中，需要考虑扫描匹配，基于视觉的回环检测和地图关联（map correlation）的三种关键技术。
A. 扫描匹配
扫描匹配是通过平移和旋转范围扫描（从诸如激光测距仪的范围测量设备获得）以使得与先验图最大重叠的过程。它返回在现有地图或先前获得的本地地图中放置扫描的位置的位姿概率分布；姿势概率通常限于高斯分布。有效的扫描匹配有助于在位姿图中提供准确的预优化边缘（edges），这可以极大地有益于回环检测和图优化，尤其是在大规模环境中。文献[17,18]中提出了许多扫描匹配方法。在本文中，利用[17]中的方法，可以获得高性能的匹配结果。该方法将扫描点与先验图中的线段匹配，并且不依赖于多边形结构来匹配一对扫描。在这种扫描匹配方法下，随后越来越多地获得位姿之间的拓扑关系。
触发回环检测意味着机器人回到先前访问过的地方。 然后使用部分地图匹配来获得当前姿势与先前位姿之间的空间关系。 在这里，我们提出了一种基于相关匹配和贝叶斯规则的方法来计算位姿l的机器人的后验概率p（l | r，m）。 给定当前map patch r和地图m。 根据贝叶斯规则，我们有：
部分扫描匹配被识别为global registration，并且应该由回环检测触发。 一旦基于匹配的拓扑连接被添加到位姿图中，就很难在激光SLAM中移除，因为所有姿势都被更新并且没有保留历史。 仅激光SLAM的性能差可能会造成许多错误连接，从而导致SLAM过程失败。 幸运的是，视觉回环检测方案可以提供鲁棒的位置识别，从而有助于获得更好的全局global registration。
4. 基于视觉的回环检测
从相机图像提取的视觉信息用于帮助回环检测和global registration。 如图3所示，本文使用ORB方法从图像中提取特征。 首先在8个比例级别提取FAST角，比例因子为1.2，并且应在每个比例级网格中提取每个单元至少5个角点以确保均匀分布。 如果找不到足够的角，则角数阈值需要适应。 最终在保留的FAST角上计算方向和ORB描述符。 图4说明了ORB功能示例。
在大规模环境中存在数千个位姿节点，因此不可能通过匹配256位ORB描述符来实现实时global registration。 这里使用了词袋，以加速ORB功能之间的匹配。 该方法将描述符空间离散化为视觉词汇表，其使用从大量图像集中提取的ORB创建离线描述符。 最后通过为已经看到词汇表中的每个视觉单词存储倒排索引来构建数据库。 方法DBoW2 [19]甚至通过将时间上接近的图像的分数相加来考虑图像重叠。 数据库查询返回所有帧匹配，其分数高于最佳分数的75％。
关键帧的选择对于实现特征有效匹配的是有用的，因为短时间内的连续帧几乎相同。 在本文中，滑动时间窗口内的局部地图用于查找关键帧以降低大型地图的复杂性。 假设本地地图具有一组与当前帧共享ORB特征的关键帧Ks，以及与当前帧共享大多数特征的参考关键帧Kref。 有关Ks和Kref的选择，请参阅[14]。新的关键帧应满足以下四个条件。 首先，最后一次全局重定位必须超过20帧; 这确保了良好的重新定位。 其次，从最后一个关键帧插入开始已超过20帧。 第三，当前帧跟踪至少50个点; 这确保了良好的跟踪。 最后，当前帧跟踪的点数比Kref少90％。 这些条件可以在鲁棒性和关键帧数之间保持良好的平衡。
如果环境完美且没有ORB跟踪失败，则使用最新检测到的关键帧来匹配地图数据库中的要素以回环检测。如果存在几个与当前位置具有相似外观的地方，则可能存在若干回环候选，并且利用连续匹配结果和里程计信息来消除歧义。计算当前关键帧和回环候选关键帧之间的ORB特征的对应关系;如果有足够的内点，则检测到回环。由于图像模糊，遮挡或相机抖动，或缺乏可检测的功能，ORB功能的跟踪有时可能会失败。如果发生跟踪丢失，则从当前帧中提取的ORB特征将被转换为一包单词，用于全局重定位。新转换的单词包与从识别数据库查询的每个关键帧匹配。然后使用RANSAC迭代和PnP算法[20]来找到最可能的机器人位姿。最后，将执行与现有关键帧的更多匹配的引导搜索以获得更准确的位姿估计。
我们添加视觉信息以帮助的回环检测。 根据我们的实验，我们发现视觉检测可以比基于激光的方法更精确。 因此，我们仅将基于视觉的方案应用于回环检测，其触发部分地图匹配以关闭回环并校正地图。
C. 地图校正
一旦检测到回环并将其添加到位姿图中，则执行图优化以校正姿势节点以及构建的地图。 基于Levenberg-Marquardt（LM）的SPA方法由于其对稀疏姿态图优化的效率而应用于图优化算法[21,22]。 沿着其轨迹的第i个全局机器人位姿表示为：
与所有约束相关联的目标函数表示为
在位姿图中的位姿数n = | c | ，在大规模环境中可以是10k甚至更多，因此（7）中的处理矩阵H非常昂贵。 然而，他非常稀疏并且可以通过特殊修改快速处理，因为约束的数量随着位姿的数量线性增长。 Cholesky分解求解器可用于以稀疏方式求解（7）。
5.实验结果
在实际室内环境中进行了几次实验以说明所提出方法的有效性。 还提供了与经典的基于激光的SLAM算法[4]在性能上的比较。 我们的实验平台是通过在Turtlebot2.0上配备激光（Hokuyo UTM-30lx）和灰度相机（PointGreyFMVU-03MTM-CS）来设置的。 用记录在HITSZ的学术建筑-D和图书馆，地图尺寸分别为26m×47m和30m×100m的两个ROSbag用于验证和比较。 数据集和算法最终在具有Intel Corei3（3.30GHZ）CPU和4GB内存的计算机上运行。 请注意，算法是实时运行的。
参考文献
[1] Bar-Shalom Y., Li X.R., and Kirubarajan. T. “Estimation with Applications to Tracking and Navigation,” Williey Inter Science, July, 2001.
[2] Grisetti G., Stachniss C., and Burgard W.. “Improved techniques for grid mapping with rao-blackwellized particle filters.”IEEE Transactions on Robotics, 23(1), 2007:34-46.
[3] Dellaert F., and Kaess M. “Square root SAM: simultaneous localization and mapping via square root information smoothing.”International Journal of Robotics Research, 25(12), 2006:1181-1203.
[4] Konolige K. “Efficient Sparse Pose Adjustment for 2D mapping,” In proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, 2010:22-29.
[5] Tardós J.D., Neira J., Newman P.M., and Leonard J.J.. “Robust mapping and localization in indoor environments using sonar data,” Int. J. Robotics Research, 21(4), 2002:311-330.
[6] Montemerlo M., Thrun S., Koller D., and Wegbreit B.. “FastSLAM: a factored solution to the simultaneous localization and mapping problem.”In proceedings of the AAAI National Conference on Artificial Intelligence, 2003:593-598.
[7] Montemerlo M., Thrun S., Koller D., and Wegbreit B. “ FastSLAM 2.0: an improved particle filtering algorithm for simultaneous localization and mapping that provably converges,” In proceedings of international joint conference on Artificial intelligence, 18, 2003:1151-1156.
[8] Lu F., and Milios E. “Globally consistent range scan alignment for environment mapping,” Autonomous Robots, 4(4), 1997:333-349.
[9] Triggs B., Mclauchlan P.F., Hartley R.I., and Fitzgibbon A.W. “Bundle adjustment: A modern synthesis,” In proceedings of the International Workshop on Vision Algorithms: Theory and Practice Springer-Verlag, 1999:298-372.
[10] Davison A.J., Reid I.D., Molton N.D., and Stasse O. “MonoSLAM: Real-Time Single Camera SLAM,”IEEE Transactions on Pattern Analysis & Machine Intelligence, 29(6), 2007:1052-1067.
[11] Civera J., Grasa O.G., Davison A.J., and Montiel J.M.M . “1-Point RANSAC for extended Kalman filtering: Application to real-time structure from motion and visual odometry,” Journal of Field Robotics,27(5), 2010:609-631.
[12] Endres F., et al. “An evaluation of the RGB-D SLAM system,” in proceeding of IEEE International Conference on Robotics and Automation, 2012:1691-1696.
[13] Klein G., and Murray D. “Parallel Tracking and Mapping for Small AR Workspaces,” IEEE & Acm International Symposium on Mixed & Augmented Reality IEEE Computer Society, 2007:1-10.
[14] Mur-ArtalR., MontielJ.M.M., TardosJ.D.“ORB-SLAM: a versatile and accurate monocular SLAM system,” IEEE Transactions on Robotics, 2015.
[15] Rublee E., Rabaud V., Konolige K., and Bradski G. “ORB: An efficient alternative to SIFT or SURF,” In proceeding of IEEE int. conf. on computer vision, 2011:2564-2571.
[16] Newman P., Cole D., and Ho K.. “Outdoor SLAM using visual appearance and laser ranging,” In proceeding of IEEE int. conf. on robotics and automation, 2006:1180-1187.
[17] Gutmann J.S., and KonoligeK. “Incremental mapping of large cyclic environments. ”Computational Intelligence in Robotics and Automation,1999:318-325.
[18] Lu F., and MiliosE. “Robot pose estimation in unknown environments by matching 2D range scans.”Journal of Intelligent & Robotic Systems,18(3), 1997:249-275.
[19] Galvez-Lopez D., and Tardós J.D. “Bags of Binary Words for Fast Place Recognition in Image Sequences.”IEEE Transactions on Robotics, 28(5), 2012:1188-1197.
[20] Lepetit V., Moreno-Noguer F., and Fua P. “EPnP: An Accurate O(n) Solution to the PnP Problem.” International Journal of Computer Vision, 81(2),2009:155-166.
[21] Liu M. and Siegwart R., “Topological mapping and scene recognition with lightweight color descriptors for an omnidirectional camera,” IEEE Transactions on Robotics, 30(2), 2014: 310-324.
[22] Chen H., Wang C., Li X.J., and Sun D.. “Transportation of multiple biological cells through saturation-controlled optical tweezers in Crowded Microenvironments,” IEEE/ASME Transactions on Mechatronics, 21(2), 2016: 888-899.
[23] Chen H., and Sun D., “Moving groups of microparticles into array with a robot-tweezers manipulation system,” IEEE Transactions on Robotics, 28(5), 2012: 1069-1080.
