融合深度视觉信息的单线激光雷达回环检测方法及系统与流程

1.本发明涉及激光雷达的同时定位与建图(slam)领域，尤其涉及地图构建过程中的回环检测部分，更为具体地，涉及一种融合深度视觉信息的单线激光雷达回环检测方案。


背景技术：

2.地图构建过程中的回环检测部分通常采用机器人的轨迹信息来检测回环，当机器人经过一个区域时，如果在后续的轨迹中再次经过这个区域，就可以将这两个轨迹闭合成一个回环。回环检测的作用是修正slam算法中的漂移误差，当机器人发生漂移时，回环检测可以将之前建立的地图和当前的轨迹进行匹配，并根据匹配结果调整地图和机器人位置，从而减少漂移误差，提高地图的准确性和鲁棒性。
3.当前的回环检测方法主要包括基于视觉的回环检测方法和基于激光雷达的回环检测方法，基于视觉的回环检测方法可以在计算资源有限的情况下实现实时回环检测，但对环境光照变化敏感，对纹理丰富度要求较高。基于激光雷达的回环检测方法使用激光点云数据进行回环检测，单线激光雷达结构简单，数据量小，使用方便，不受环境光照和纹理影响，但在诸如地下停车场、图书馆、公寓等场景结构相似的环境中，容易造成错误的回环匹配，从而导致整个地图的构建失败。


技术实现要素：

4.为了解决上述现有技术中存在的不足，本发明提出了一种融合深度视觉信息的单线激光雷达回环检测方法。
5.为了实现上述目的，本发明提出的技术方案为一种融合深度视觉信息的单线激光雷达回环检测方法，通过激光数据进行机器人的运动估计，构建出原始的占据栅格地图；回环检测部分利用到视觉数据，并使用激光数据和视觉数据进行回环验证，融合二者进行回环校正，最后通过图优化方式，用全局约束来求解优化构建最终的校正地图。
6.而且，所述的激光雷达建图采用单线激光雷达，基于单线激光雷达进行前端配准，包括激光帧构建、插入子图、求解优化位姿、最后将激光帧中的点云投影到子图，并插入到子图的占据栅格地图中。
7.而且，融合视觉与激光的回环检测流程中，将来自相机和激光传感器的数据进行关联，并构建成关键帧，然后利用关键帧中的视觉数据进行回环检测；当检测到回环以后，利用视觉和激光数据进行回环的验证；若回环通过了验证，则接受该回环，以用于后续校正。
8.而且，实现过程包括如下步骤，
9.s1.构建关键帧；
10.s2.使用图像的单词向量搜索回环候选帧并检验；
11.s3.使用图像的全局描述子对回环候选帧进行排序，根据分数从大到小排序回环候选帧；
12.s4.依次对每个回环候选帧检查连续一致性，以减少误检率；
13.s5.使用激光数据和视觉地图点进一步验证回环；
14.而且，构建关键帧时，进行激光雷达和相机坐标系变换的求解、视觉帧的构建、视觉3d地图点构建，获得关联的激光帧、视觉帧以及子图，且视觉帧中的特征点被构建为具有空间信息的3d地图点，然后生成融合关键帧。
15.而且，所述使用激光数据和视觉地图点进一步验证回环，实现方式为首先检验激光数据来验证回环，使用快速相关性扫描匹配，设激光雷达放在地图的每个栅格上，点云与地图重合程度最高的位置就是激光雷达的真实位姿，按此原理检测当前帧所对应的激光点云与回环候选帧对应的子图之间的匹配度，若匹配度大于相应的阈值，则接受该回环候选帧，否则丢弃该回环帧；
16.然后使用视觉地图点进一步验证回环，将回环候选帧所对应的子图中的3d地图点重投影到当前帧中来通过视觉地图点验证回环，基于投影点的坐标，在搜索空间中寻找最近且距离小于相应距离阈值的匹配特征点，若匹配特征点对数小于设定的相应阈值，则丢弃该回环帧，否则接受该回环帧。
17.另一方面，本发明提供一种融合深度视觉信息的单线激光雷达回环检测系统，用于实现如上所述的一种融合深度视觉信息的单线激光雷达回环检测方法。
18.而且，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如上所述的一种融合深度视觉信息的单线激光雷达回环检测方法。
19.或者，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如上所述的一种融合深度视觉信息的单线激光雷达回环检测方法。
20.本专利结合视觉获得的丰富纹理信息与激光雷达提供的更精准的环境结构信息，联合激光点云和视觉特征点进行回环的验证，基于帧到局部地图的方式进行激光点云匹配，以减少单线激光雷达稀疏点云带来的计算误差。具体而言即使用快速相关性扫描匹配方法，即进行激光数据的匹配以验证回环，同时，也使用局部地图中的视觉地图点重投影匹配方法来进一步确保回环的准确性。
21.本发明方案实施简单方便，实用性强，解决了相关技术存在的实用性低及实际应用不便的问题，能够提高用户体验，具有重要的市场价值。
附图说明
22.图1为本发明实施例的slam流程图；
23.图2为本发明实施例融合回环流程图；
24.图3为本发明实施例关键帧生成及回环检测原理图。
具体实施方式
25.以下结合附图和实施例具体说明本发明的技术方案。
26.本发明实施例中提出，一种融合深度视觉信息的单线激光雷达回环检测方法：首先，单线激光雷达作为建图的主要传感器，生成2d栅格地图；然后，单目相机提取深度视觉特征信息；最后，在统一的坐标系下用视觉提取的纹理信息来检测回环，用激光检测的结构信息来验证回环，融合两者信息判断出正确回环。
27.参见图1，本发明的slam框架以单线激光传感器作为核心传感器来进行定位和建图，因为单目相机在运动估计时的精度和鲁棒性较低，因此单目视觉信息仅用于回环阶段。系统在经典的单线激光slam方法——cartographer的基础上进行构建，前端配准、后端优化和地图构建主要遵循cartographer中的算法进行设计。本发明所提出的融合视觉和激光雷达两种传感器的回环检测方法，替换原有单激光雷达回环检测，并保持与原系统的兼容性。
28.首先是地图构建。前端配准模块通过激光数据进行机器人的运动估计，构建出原始的占据栅格地图；然后回环检测部分利用到视觉数据，并使用激光数据和视觉数据进行回环验证，融合二者进行回环校正，最后通过图优化的方法考虑全局额约束来求解优化构建最终的校正地图。所述的图优化方式是现有最流行的优化算法之一。当出现回环误差之后，构建节点，并且找到一个最优的配置(各节点的位姿)，让预测值与观测值的误差最小即完成优化，本发明不予赘述。
29.本发明实施例中提出的一种融合深度视觉信息的单线激光雷达回环检测方法，具体实现包括以下步骤：
30.步骤1，所述的激光雷达数据采用单线激光雷达，成本低的同时快速有效，在单线激光雷达前端配准的步骤为激光帧构建、插入子图、求解优化位姿、最后将激光帧中的点云投影到子图，并插入到子图的占据栅格地图中。
31.本发明的前端配准模块通过激光数据进行机器人的运动估计，其步骤为：
32.(1)激光帧构建：当激光数据加入到系统时，通过点云校正后构成激光帧。
33.(2)插入子图：系统中维护由多帧激光帧构成的局部地图，称为子图，所有子图构成全局地图。若当前子图未满，则将激光帧插入到该子图，否则创建新子图并插入激光帧，以激光帧的位姿作为子图位姿。
34.(3)求解优化位姿：使用非线性优化方法，通过最大化激光帧点云在子图占据栅格地图中的占据概率，在初始位姿的基础上求解激光帧到子图的优化位姿(即从当前激光帧坐标系到子图坐标系的变换)，初始位姿由上一帧激光点云的运动估计结果提供。
35.(4)将激光帧中的点云投影到子图，并插入到子图的占据栅格地图中。
36.步骤2，进行回环检测流程部分：需要将来自相机和激光传感器的数据进行关联，并构建成关键帧，然后利用关键帧中的视觉数据进行回环检测，视觉数据中的特征点提取基于深度学习网络进行；当检测到回环以后，利用视觉和激光数据进行回环的验证；若回环通过了验证，则接受该回环，以用于后续校正。
37.本发明的融合视觉与激光的回环流程如图2，与现有技术相比，本方法同时利用相机和激光雷达在回环检测时的优点，同时利用两种传感器的数据进行回环的检测与验证，克服传统单传感器方法在相似环境建图时容易回环失败的问题。在本方法中，回环首先需要将来自相机和激光传感器的数据进行关联，并构建成关键帧，这是数据融合的前提；然后利用关键帧中的视觉数据进行回环检测，视觉数据中的特征点提取基于深度学习网络进行；当检测到回环以后，利用视觉和激光数据进行回环的验证；若回环通过了验证，则接受该回环，并基于视觉数据和激光数据的紧组合算法计算当前帧与回环帧所对应的子图之间的相对位姿作为回环位姿，以校正回环。
38.参见图3，实施例采用的关键帧生成及回环检测具体包括如下步骤：
39.s1.构建关键帧；
40.本发明关键帧的构建有4个关键步骤：激光雷达和相机坐标系变换的求解、视觉帧的构建、视觉3d地图点构建、生成融合关键帧。
41.(1)激光雷达和相机坐标系变换的求解
42.本发明进行单目相机和单线激光雷达间的外部标定，来确定两种传感器的空间相对位置。通过外部标定可以得到激光雷达坐标系和相机坐标系之间的变换矩阵t
cl
＝(r
cl
,t
cl
)，其中r
cl
和t
cl
分别是旋转矩阵和平移矩阵，根据变换矩阵可以实现数据在两个坐标系之间的位姿变换，保证空间一致性。
43.(2)视觉帧的构建
44.当通过视觉特征提取算法获得图像帧的特征点、局部描述子和全局描述子后，采用词袋模型将局部描述子转换为单词向量，以便于回环计算时能够快速搜索回环。词袋模型的词汇树由离线训练，并通过词汇树将局部描述子转换为单词向量。为了规避单目相机在相机位姿估计上的不准确性，本发明利用激光帧的位姿来求解对应视觉帧位姿。相机的帧数相比激光雷达较低，而且激光雷达扫描的时间间隙较小，当两者时间同步后，用插帧的方法来将激光帧与视觉帧对齐，并采用上一步中得到的变换矩阵，就能通过前端匹配得到的激光帧位姿，求解出视觉帧的位姿；最终将上述的数据构建为视觉帧。
45.(3)视觉3d地图点的构建
46.本发明采用三角化的方法计算视觉特征点的空间坐标，并通过提供多项约束来降低错误匹配的情况，提升算法的可靠性。首先对两帧影像计算它们对应的相机位姿，并计算两帧影像的相对位姿变换；检查两帧图像的基线(即两帧图像所对应的相机光心距离)，若基线小于相应阈值(根据实际设置，例如实施例采用50厘米)，三角化时可能会带来较大的计算误差，因此放弃使用这两帧图像进行三角化；搜索两帧图像中的匹配特征点对，并通过来验证特征点匹配是否有效；对所有匹配特征点对，使用线性三角化方法，求解其3d坐标，构建成3d地图点；最后通过卡方检验来检查所求得的3d坐标，如果地图点坐标处于相机的前方、地图点投影到两帧影像的重投影误差小于相应阈值(根据实际设置，例如实施例采用5.99)，则接受该3d地图点，否则丢弃。
47.(4)生成融合关键帧
48.通过上述步骤，可以获得关联的激光帧、视觉帧以及子图，且视觉帧中的特征点被构建为具有空间信息的3d地图点，因此，将这些数据构建为一帧融合关键帧。
49.s2.使用图像的单词向量初步搜索回环候选帧并检验；
50.本步骤使用图像的单词向量搜索回环候选帧。
51.首先计算匹配阈值t1：在使用词袋模型计算两帧图像的匹配度时，因为不同场景的信息量不同，因此通过图像帧的共视关系来设置阈值t1。当两帧图像共同可见的地图点数超过相应阈值(根据实际设置，例如实施例采用15个)时，就将两帧视为共视，有共视关系的图像帧在空间上是相邻的，因此计算当前帧f
cur
和其所有共视帧的匹配分数，以最小的匹配分数s
min
作为搜索回环帧时的相应匹配阈值t1；
52.然后，搜索回环候选帧：通过搜索图像数据库，获得与当前帧f
cur
共词且无共视关系的所有历史图像帧，并计算当前帧f
cur
与它们的匹配分数，若匹配分数小于t1，则丢弃该历史图像帧，同时，记录最大共词数n
max_common
。
53.最后，检验回环候选帧：实施例优选采用的检验条件是，对上一步操作后留存的历史图像帧，如果与当前帧f
cur
的共词数n
common
不满足n
common
≥0.8
×nmax_common
，则丢弃该历史图像帧，否则保留该历史图像帧。
54.s3.使用图像的全局描述子对回环候选帧进行排序，根据分数从大到小排序回环候选帧；
55.本步骤使用图像的全局描述子对回环候选帧进行排序，实现方式包括使用回环候选帧和当前帧的全局描述子衡量二者的匹配分数s
global
，该分数通过全局描述子的欧氏距离来表示，其计算公式如下式，然后根据分数从大到小排序回环候选帧。
[0056][0057]
其中，x是当前帧f
cur
的全局描述子，xi是x的第i位，y是回环帧的全局描述子，yi是y的第i位。
[0059]
s4.依次对每个回环候选帧检查其连续一致性，以减少误检率；
[0060]
进行连续一致性检查。仅使用单帧图像来检测回环，容易产生误检测，因此可以利用动态的信息来进行回环的验证，也即对于排序后的所有回环候选帧，依次对每个回环候选帧检查其连续一致性，若其满足连续一致性，则接受该候选帧，并继续步骤s5来验证回环，否则忽略该帧，并回到步骤s3中排好的序列来检查下一个候选帧。
[0061]
s5.使用激光数据和视觉地图点进一步验证回环；
[0062]
本步骤使用激光数据和视觉地图点进一步验证回环，实现方式为首先检验激光数据来验证回环，使用快速相关性扫描匹配：激光雷达放在地图的每个栅格上，点云与地图重合程度最高的位置就是激光雷达的真实位姿。按照此原理检测当前帧所对应的激光点云与回环候选帧对应的子图之间的匹配度，若匹配度大于相应的阈值t2，则接受该回环候选帧，否则丢弃该回环帧；
[0063]
然后使用视觉地图点进一步验证回环。将回环候选帧所对应的子图中的3d地图点重投影到当前帧中来通过视觉地图点验证回环，记投影点的坐标为(x0,y0)，在搜索空间
[0064]
w中寻找最近且距离小于相应距离阈值t3的匹配特征点，其中w如下式所示：
[0065]
w＝{(x,y)|(x
0-r)≤x≤(x0 r),(y
0-r)≤y≤(y0 r)}
[0066]
其中，r是预先设置的搜索半径，若匹配特征点对数小于设定的相应阈值t4丢弃该回环帧，否则接受该回环帧。
[0067]
具体实施时，本发明技术方案提出的方法可由本领域技术人员采用计算机软件技术实现自动运行流程，实现方法的系统装置例如存储本发明技术方案相应计算机程序的计算机可读存储介质以及包括运行相应计算机程序的计算机设备，也应当在本发明的保护范围内。
[0068]
在一些可能的实施例中，提供一种融合深度视觉信息的单线激光雷达回环检测系统，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如上所述的一种融合深度视觉信息的单线激光雷达回环检测方法。
[0069]
在一些可能的实施例中，提供一种融合深度视觉信息的单线激光雷达回环检测系统，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，
实现如上所述的一种融合深度视觉信息的单线激光雷达回环检测方法。
[0070]
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。