CN114187418A

CN114187418A - 回环检测方法、点云地图构建方法、电子设备及存储介质

Info

Publication number: CN114187418A
Application number: CN202111526258.1A
Authority: CN
Inventors: 袁昊东; 张煜东; 范圣印; 李雪
Original assignee: Beijing Yihang Yuanzhi Technology Co Ltd
Current assignee: Beijing Yihang Yuanzhi Technology Co Ltd
Priority date: 2021-12-14
Filing date: 2021-12-14
Publication date: 2022-03-15

Abstract

本发明涉及一种回环检测方法、点云地图构建方法、电子设备及存储介质，该方法以主对象为中心构建主对象描述子，只需要两帧的主对象位置接近且能观测到同一场景中的一个或多个主对象即可，相比现有技术中以激光雷达为中心建立Scan Context描述子的方法，本发明提供的这种不依赖观测位置的描述子能解决Scan Context描述子在两帧位置不重合时无法进行回环检测的问题，扩大了回环检测的适用范围，提高了回环检测的鲁棒性。进一步地，由于最终计算出的当前关键帧和历史关键帧之间的相对位姿包含x、y方向的偏移和旋转角度，使得回环检测得到的结果发送给后端做全局目标地图优化时，可以得到更好的位姿优化结果，能够提高整个SLAM系统的鲁棒性和定位建图的精度。

Description

回环检测方法、点云地图构建方法、电子设备及存储介质

技术领域

本发明涉及地图构建技术领域，具体涉及一种回环检测方法、点云地图构建方法、电子设备及存储介质。

背景技术

SLAM(Simultaneous Localization And Mapping，同步定位与地图构建)技术在自动驾驶、无人机、机器人、VR(Virtual Reality，虚拟现实)和AR(Augmented Reality，增强现实)等领域有着广泛应用。SLAM是指设备在未知环境下从一个未知位置开始移动，在移动过程中根据传感器收集到的数据和构建出的环境地图进行自身定位，并在自身定位的基础上构建增量式地图的过程。

目前流行的三维SLAM框架主要是基于图进行优化的方法，包含前端、后端和回环检测三个模块。前端进行传感器数据的收集、数据的处理并输出里程计信息；后端主要构建一个以车辆位姿为节点、以相对位姿为边的图结构，用于进行全局的位姿优化。

回环检测是SLAM系统中非常重要的一环，在SLAM系统中，传感器数据噪声和算法本身都会导致前端估算的载具位姿存在一定的误差，该误差会随着时间不断积累积，当载具完成一个环形的运动之后，SLAM算法计算出的路径会由于累积误差而无法完全闭合，这时就要求SLAM系统的回环检测模块能够识别出回环并计算出准确的相对位姿，将此相对位姿发送给后端来进行载具位姿的优化。

Scan Context是目前流行的回环检测技术之一。Scan Context，从英文字面理解就是“扫描上下文”。类比于我们阅读的时候，需要理解上下文，才能明白其意，SLAM在进行回环检测的时候，也需要将“上下文”(之前的数据)进行比较，方才知道我们是不是又走到了之前的同一个地方(回环)。该技术以激光雷达为中心建立一种名为Scan Context的全局描述子，通过旋转匹配的方式进行回环检测，相比直接匹配的方法有较高的效率、准确率和召回率，但是仍然存在一些缺陷。

相关技术提出了一种基于3D点云的场景识别技术，该技术将3D点云投影到XY平面，以激光雷达为中心建立Scan Context作为关键帧的descriptor(描述子)，以ScanContext的环特征向量为节点建立多维树结构以便后续快速搜索。当设备回到历史某一时刻的坐标位置时，根据当前的Scan Context的环特征向量在多维树结构中快速搜索出多个候选帧，通过旋转的方式进行Scan Context之间的匹配，若最高的匹配相似度大于预设值，则认为检测到回环，最后将匹配的位姿信息发送给后端。该技术使用的以激光雷达为中心建立Scan Context的方法要求当前帧与历史帧的位置重合才能检测出回环，会造成许多回环的漏检；该技术通过旋转的方式进行Scan Context匹配只能计算出两帧之间设备的旋转角度，并不能计算出设备在x和y方向的偏移，在实际使用时有一定局限性。

相关技术还提出了一种基于图描述子的激光SLAM回环检测系统及方法。该方法利用了从点云数据中提取的语义信息形成了全图描述子和顶点描述子两种图描述子来进行点云帧和语义物体的表征。相比于传统算法从像素级别提取描述子，本发明可以避免繁重的法向量计算任务和避免视角大幅变化带来的问题，可以更加快速和鲁棒地检测回环。尽管该技术使用了语义信息构建了两种描述子，提高了场景识别的的鲁棒性，但是并不能准确地计算出当前帧与回环帧之间的相对位姿。

相关技术还提出了一种基于静态语义信息的点云地图创建与场景辨识方法，包括步骤1：激光点云与语义图像联合投影获得语义点云；步骤2：通过语义标签筛选鲁棒静态信息；步骤3：提取鲁棒静态信息的描述子并组合成当前场景的描述子；步骤4：语义点云与激光雷达的里程计相结合生成全局地图，并通过点云配准在全局地图上进行重定位。该技术提取鲁棒静态信息建立描述子，描述子直接匹配得出当前帧的位姿，但是描述子内容强依赖于激光雷达的位置，这容易造成描述子误匹配，并且直接匹配方式将消耗大量的时间，在实际使用时有一定局限性。

综上，Scan Context方法以及基于Scan Context的多种改进方法虽然能快速检索出回环候选帧并计算两帧之间的相似度，但要求两帧坐标基本重合，无法识别位置相差一定距离的两帧的场景；其次，此类方法只能计算出当前帧相对匹配帧的旋转角度，无法计算两帧之间的X和Y坐标的偏移，在实际使用时有一定局限性。

发明内容

有鉴于此，本发明的目的在于提供一种回环检测方法、点云地图构建方法、电子设备及存储介质，以解决现有技术中回环检测方法要求两帧坐标基本重合，无法识别位置相差一定距离的场景的问题。

根据本发明实施例的第一方面，提供一种回环检测方法，包括：

获取前端构建的关键帧，及，从所述关键帧中确定出的主对象；

对任一主对象，以所述主对象为中心构建主对象描述子；

将当前关键帧的主对象描述子和历史关键帧的主对象描述子两两匹配；

根据匹配结果，判断当前关键帧与历史关键帧是否处于同一场景，若是，判定当前关键帧和历史关键帧之间存在回环边，将当前关键帧与历史关键帧之间的相对位姿作为约束边，添加到后端构建的位姿图中，以使后端根据所述约束边对所述位姿图进行全局位姿优化。

优选地，所述前端通过将当前帧中的3D点云及其前四帧中的3D点云按照前端的匹配结果融合、滤波，构建关键帧，并将当前帧的位姿定义为所述关键帧的位姿，将关键帧中的点云定义为子地图；和/或，

所述前端通过如下方法从所述关键帧中确定出主对象，包括：

对任一子地图，获取该子地图中每一个点的语义标签；

根据所述语义标签，筛选出主对象点云；

对筛选出的主对象点云进行聚类，得到该子地图的所有主对象；

对任一主对象，根据该主对象内所有点的坐标值，计算该主对象的X坐标和Y坐标。

优选地，所述以所述主对象为中心构建主对象描述子，包括：

对任一主对象，以所述主对象为中心、以激光雷达到主对象的连线为极轴建立极坐标系，将极轴划分为N_r等分，将极角划分为N_s等分，以将主对象周围的环境划分为N_r×N_s个栅格，将子地图中的点向下投影对应到栅格中，计算每个栅格内所有点的平均高度；其中，N_r≥2，N_s≥2；

将每个栅格对应的平均高度值，组合在一起形成一个N_r×N_s的矩阵，将所述N_r×N_s的矩阵，确定为该主对象的主对象描述子。

优选地，所述方法，还包括：

删除所述栅格中高度低于高度阈值的点，所述高度阈值根据地面点的平均高度进行确定；和/或，忽略未投影到所述栅格中的点。

优选地，所述方法，还包括：

将主对象描述子按行求均值得到的列向量，定义为环特征向量；将主对象描述子按列求均值得到的行向量，定义为扇特征向量；

对任一子地图，将主对象位置列表、主对象描述子列表、扇特征向量列表按照按照关键帧序号保存到数据库中；

所述主对象位置列表中存储有所述子地图中各主对象的X坐标和Y坐标；

所述主对象描述子列表中存储有所述子地图中各主对象的主对象描述子；

所述特征向量列表中存储有所述子地图的各主对象描述子的扇特征向量。

优选地，所述将当前关键帧的主对象描述子和历史关键帧的主对象描述子两两匹配，包括：

根据历史关键帧的环特征向量，构建多维树结构；

创建候选关键帧集合；

遍历当前关键帧中所有主对象描述子的环特征向量，为每个环特征向量在多维树结构中搜索出预设数量个与其相似的节点，将相似节点对应的关键帧序号加入到候选关键帧集合中；

将所述候选关键帧集合中的关键帧序号所对应的历史关键帧作为候选关键帧，将当前关键帧的主对象描述子和候选关键帧的主对象描述子两两匹配。

优选地，所述将当前关键帧的主对象描述子和候选关键帧的主对象描述子两两匹配，包括：

获取候选关键帧的扇特征向量列表、主对象描述子列表；

根据所述扇特征向量列表中的扇特征向量，计算当前关键帧的主对象描述子和候选关键帧的主对象描述子对齐所需要的粗偏移量；

在以所述粗偏移量为中心的偏移窗口内构建优化方程，以计算当前关键帧的主对象描述子和候选关键帧的主对象描述子对齐所需要的精偏移量；

根据所述精偏移量，计算当前关键帧的主对象描述子和候选关键帧的主对象描述子的相似度，并将所述相似度存储在匹配结果中。

优选地，所述将当前关键帧的主对象描述子和候选关键帧的主对象描述子两两匹配，还包括：

获取候选关键帧的主对象位置列表；

根据当前关键帧中主对象在子地图中的X坐标和Y坐标，及，候选关键帧中主对象在子地图中的X坐标和Y坐标，计算当前关键帧的主对象和候选关键帧的主对象之间的相对位姿，并将所述相对位姿与所述相似度对应存储在匹配结果中；

所述相对位姿至少包括：当前关键帧的主对象相对于候选关键帧的主对象在X方向的偏移，Y方向的偏移，及，旋转角度。

优选地，所述根据所述匹配结果，判断当前关键帧与历史关键帧是否处于同一场景，包括：

剔除匹配结果中低于相似度阈值的相似度及对应的相对位姿；

在剩余的匹配结果中，对当前关键帧的每一个主对象描述子，在候选关键帧中找到与其相似度最高的主对象描述子，并将对应的相似度筛选出来；

将筛选出的相似度所对应的相对位姿看作是一组三维点，映射到三维空间中，聚类得到多个类；

若至少存在一个类包含的点数量大于数量阈值，则判定当前关键帧与历史关键帧是处于同一场景，将包含点数量最多的类中所有点的位姿平均值作为当前关键帧与历史关键帧之间的相对位姿；

若不存在一个类包含的点数量大于数量阈值，则判定当前关键帧与历史关键帧不是处于同一场景，当前关键帧和历史关键帧之间不存在回环边。

优选地，所述方法，还包括：

若判定当前关键帧和历史关键帧之间不存在回环边，将当前关键帧的所有环特征向量添加到所述多维树结构中。

根据本发明实施例的第二方面，提供一种点云地图构建方法，至少包括：

基于主对象特征的雷达里程计；

上述的回环检测方法。

优选地，所述基于主对象特征的雷达里程计，包括：

从每一帧3D点云中确定出主对象；

确定出主对象后，提取主对象特征，所述主对象特征至少包括：每个主对象的质心坐标，及，每个主对象的方向向量；

每获得一帧3D点云，都与前一帧3D点云进行主对象匹配，并控制整体匹配误差最小，计算得到每一帧相对前一帧的相对位姿；所述整体匹配误差和每一帧中所有主对象特征相关；

将计算出的相对位姿依次叠乘，得到每一帧在世界坐标系中的位姿。

根据本发明实施例的第三方面，提供一种电子设备，包括：

通信模块、处理器和存储器，其中，所述存储器中存储有程序指令；

所述处理器用于执行存储器中存储的程序指令，执行上述的回环检测方法，和/或，上述的点云地图构建方法。

根据本发明实施例的第四方面，提供一种计算机可读存储介质，其上存储有可擦写的计算机程序；

当所述计算机程序在计算机设备上运行时，使得所述计算机设备执行上述的回环检测方法，和/或，上述的点云地图构建方法。

本发明的实施例提供的技术方案可以包括以下有益效果：

本发明提供的技术方案，以主对象为中心构建主对象描述子，只需要两帧的主对象位置接近且能观测到同一场景中的一个或多个主对象即可，相比现有技术中以激光雷达为中心建立Scan Context描述子的方法，本发明提供的这种不依赖观测位置的描述子能解决Scan Context描述子在两帧位置不重合时无法进行回环检测的问题，扩大了回环检测的适用范围，提高了回环检测的鲁棒性。

进一步地，本发明提供的技术方案，通过对当前关键帧的主对象描述子和历史关键帧的主对象描述子两两匹配结果进行筛选，来避免误匹配的影响。最终计算出的当前关键帧和历史关键帧之间的相对位姿包含x、y方向的偏移和旋转角度，相比现有技术无法计算两帧之间的X和Y坐标的偏移，本发明提供的技术方案，回环检测得到的结果发送给后端做全局目标地图优化时，可以得到更好的位姿优化结果，能够提高整个SLAM系统的鲁棒性和定位建图的精度。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。

图1是根据一示例性实施例示出的一种回环检测方法的流程图；

图2是根据一示例性实施例示出的点云地图构建方法的信号流转图；

图3是根据一示例性实施例示出的主对象描述子的构建示意图；

图4是根据一示例性实施例示出的当前关键帧的主对象和候选关键帧的主对象之间的相对位姿示意图；

图5是根据一示例性实施例示出的当前关键帧的主对象描述子和候选关键帧的主对象描述子两两匹配结果示意图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本发明相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本发明的一些方面相一致的装置和方法的例子。

需要说明的是，本发明提供的技术方案，需要使用载具上的传感器，实现同步定位和建图，并使用回环检测来优化位姿。

“载具”包括但不限于：自动驾驶车辆、无人机、无人物流车、移动机器人等；“传感器”包括但不限于：激光雷达、结构光相机、毫米波雷达等所有能提供环境空间3D点云信息的传感器。

传感器的数据需要经过传感器相对载具中心的外参转换，将传感器数据转换到载具中心，本发明提供的技术方案默认传感器数据已经过外参转换。

现对本发明各实施例所提及的部分概念汇总如下：

帧：传感器的数据以帧的形式读取，每一帧为某一时刻下的一包完整的传感器数据，帧发送接收频率是由设备所使用的系统决定的；

主对象：是实现场景识别的基础，场景中带有某一些特定语义的物体。

主对象描述子：以主对象为中心，对周围点云类别和分布进行表示的一种形式。

坐标系：本发明使用了直角坐标系和极坐标系，直角坐标系使用通用的笛卡尔直角坐标系右手系(向前为X轴，向上为Z轴，向左为Y轴)；极坐标系定义从极轴开始逆时针旋转为极角的正方向。

位姿：为载具在2D空间的位置和姿态，位置为x方向和y方向的坐标，姿态为以x轴为正方向的方位角θ。

观测位置：以传感器中心为坐标原点、载具正方向为X轴、向左为Y轴建立直角坐标系，主对象在坐标系中的位置称为观测位置。

现对本发明各实施例的具体实现方式解释说明如下：

实施例一

图1是根据一示例性实施例示出的一种回环检测方法的流程图，如图1所示，该方法包括：

步骤S11、获取前端构建的关键帧，及，从所述关键帧中确定出的主对象；

步骤S12、对任一主对象，以所述主对象为中心构建主对象描述子；

步骤S13、将当前关键帧的主对象描述子和历史关键帧的主对象描述子两两匹配；

步骤S14、根据匹配结果，判断当前关键帧与历史关键帧是否处于同一场景，若是，判定当前关键帧和历史关键帧之间存在回环边，将当前关键帧与历史关键帧之间的相对位姿作为约束边，添加到后端构建的位姿图中，以使后端根据所述约束边对所述位姿图进行全局位姿优化。

需要说明的是，本实施例提供的技术方案，适用于SLAM技术领域，适用于对点云地图的构建。参见图2，点云地图的构建包括：

前端里程计，基于前端传感器(包括但不限于：激光雷达、相机、IMU、GNSS以及轮速编码器等传感器)输出的数据帧完成实时的位姿跟踪，对输入数据帧进行处理，计算姿态变化，得到传感器间的运动关系。基于传感器间的运动关系，初步完成地图的构建，这一步往往是传感器信息的堆积，但是随着时间的累计，误差会累积。

回环检测，利用数据帧之间的相似性，来判断是否到达先前的位置，以此来消除累积误差，得到全局一致性轨迹和地图。

后端优化及建图，根据回环检测的结果，优化前端里程计建立的位姿图，并根据优化后的位姿图建立全局目标地图(即点云地图)。

可以理解的是，本实施例提供的技术方案，是对点云地图构建过程中的回环检测部分进行了改进。在本实施例提供的这种回环检测方法的基础上，结合现有的前端里程计和后端优化方法，即可实现点云地图的构建。

在具体实践中，所述步骤S11中的“前端”通过将当前帧中的3D点云及其前四帧中的3D点云按照前端的匹配结果融合、滤波，构建关键帧；并将当前帧的位姿定义为所述关键帧的位姿，将关键帧中的点云定义为子地图；

对任一子地图，获取该子地图中每一个点的语义标签；

根据所述语义标签，筛选出主对象点云；

具体来说，每建立一个子地图时，使用训练好的点云语义分割网络RangeNet++(也可使用其他3D点云语义分割网络，包括但不限于：PointRCNN，PointNet++等)，或者，通过图像语义分割再投影到激光雷达坐标系的方式来获得3D点云中每一个点的语义标签，或者，通过相机收集图像，使用图像语义分割网络处理图像获得语义图像，然后通过外参转换将语义图像投影到3D点云上，最终获得3D点云语义。

筛选出主对象点云后，进行欧几里得聚类(也可采用K-Means聚类算法、DBSCAN聚类算法等其他点云聚类算法，还可以使用基于深度学习的点云聚类方法)获得多个主对象，计算每个主对象的X坐标和Y坐标。

优选地，对任一主对象，对该主对象内所有点的坐标值求平均值，得到该主对象的X坐标和Y坐标。

在具体实践中，每个关键帧中主对象的语义并不一定是特定的某一个或者某几个语义，不同的关键帧可以根据点云的具体分布确定不同的主对象。

一般而言，主对象的选择应遵循以下原则：

1、主对象不能是运动的物体，例如，运动的汽车、行人等，主对象不能是常见的可移动物体，例如，路障、静止的汽车等；

2、主对象在关键帧中应该有多个备选目标，并且组成每个备选目标的3D点的数目应超过一定值；

3、激光雷达到主对象的方向上不应该有其他物体，避免选中易被遮挡的备选目标。

在具体实践中，主对象通常选用路灯、树干这些很常见且不会被移动的物体。

在具体实践中，所述步骤S12中“以所述主对象为中心构建主对象描述子”，包括：

参见图3，对任一主对象(图3中的五角星代表主对象)，以所述主对象为中心、以激光雷达到主对象的连线为极轴建立极坐标系，将极轴划分为N_r等分，将极角划分为N_s等分，以将主对象周围的环境划分为N_r×N_s个栅格，将子地图中的点向下投影对应到栅格中，计算每个栅格内所有点的平均高度(忽略未投影到所述栅格中的点)；其中，N_r≥2，N_s≥2；

可以理解的是，由于地面点带有的高度信息很相似，可能会造成误匹配，所以在建立主对象描述子的时候不考虑地面点，过滤的方式是使用一高度阈值进行过滤。因此，可以删除所述栅格中高度低于高度阈值的点，所述高度阈值根据地面点的平均高度进行确定。

在具体实践中，所述方法，还包括：

在具体实践中，所述步骤S13中“将当前关键帧的主对象描述子和历史关键帧的主对象描述子两两匹配”，包括：

根据历史关键帧的环特征向量，构建多维树结构；

创建候选关键帧集合；

可以理解的是，由于一个子地图中会含有多个主对象描述子，为了提高搜索的效率和搜索结果的准确率，可以创建一个候选关键帧集合，遍历当前关键帧中所有主对象描述子的环特征向量，每个环特征向量在多维树结构中搜索出预设数量个与其相似的节点，将相似节点对应的关键帧序号加入到候选关键帧集合中。遍历结束，即得到与当前关键帧相似的所有历史关键帧。候选关键帧集合为非重复集合，防止重复操作同一关键帧。

其中，所述预设数量根据用户需要或者历史经验值或者实验数据进行设置，例如，设置为10个。

在具体实践中，所述将当前关键帧的主对象描述子和候选关键帧的主对象描述子两两匹配，包括：

获取候选关键帧的扇特征向量列表、主对象描述子列表；

根据所述扇特征向量列表中的扇特征向量，计算当前关键帧的主对象描述子和候选关键帧的主对象描述子对齐所需要的粗偏移量shift；

对当前关键帧的任一主对象描述子，遍历候选关键帧的主对象描述子，计算两者对齐所需要的粗偏移量shift(可以规定向左为正)，计算公式如下：

式中，S^q表示候选主对象描述子的扇特征向量，

表示当前主对象描述子的扇特征向量向左偏移shift个单位，||S||表示向量S的二范数。

在以所述粗偏移量为中心的偏移窗口内构建如下优化方程，以计算当前关键帧的主对象描述子和候选关键帧的主对象描述子对齐所需要的精偏移量n：

式中，shift为粗偏移量，[shift-k,shift+k]表示以shift为中心、以k为半径的偏移量窗口，在本实施例中k＝3，I^q表示候选主对象描述子，

表示当前主对象描述子向左偏移n个单位，d表示两个主对象苗子的距离，计算公式如下：

式中，

和

分别代表主对象描述子对齐后的从左往右第j个Sector，||c||表示向量c的二范数。

那么，相似度计算公式如下，相似度取值范围为0到1，越接近1说明越相似，计算公式如下：

将计算出的相似度存储在匹配结果中。

在具体实践中，所述将当前关键帧的主对象描述子和候选关键帧的主对象描述子两两匹配，还包括：

获取候选关键帧的主对象位置列表；

根据当前关键帧中主对象在子地图中的X坐标和Y坐标(x₁,y₁)，及，候选关键帧中主对象在子地图中的X坐标和Y坐标(x₂,y₂)。

需要说明的是，主对象在子地图中的X坐标和Y坐标，即为主对象在关键帧中的坐标。

参见图4，可以推导出当前关键帧的主对象和候选关键帧的主对象之间的相对位姿：

Δx＝x₁-x₂cosΔθ_o+y₂sinΔθ_o (6)

Δy＝y₁-x₂sinΔθ_o+y₂cosΔθ_o (7)

式中，γ为当前关键帧中的主对象描述子向左偏移n个单位对应的角度，n为上面步骤计算出的精偏移量：Δx为当前关键帧的主对象相对于候选关键帧的主对象在X方向的偏移；Δy为当前关键帧的主对象相对于候选关键帧的主对象在Y方向的偏移；Δθ₀为当前关键帧的主对象相对于候选关键帧的主对象的旋转角度，逆时针为正；其中，

将计算出的相对位姿与上面步骤计算出的相似度对应存储在匹配结果中。

可以理解的是，假设当前关键帧有M个主对象描述子，后续关键帧有N个主对象描述子，那么M*N组匹配共生成M*N个相似度和相对位姿；其中，M≥1，N≥1。

在具体实践中，所述步骤S14中“根据所述匹配结果，判断当前关键帧与历史关键帧是否处于同一场景”，包括：

在剩余的匹配结果中，对当前关键帧的每一个主对象描述子，在候选关键帧中找到与其相似度最高的主对象描述子，并将对应的相似度筛选出来。

具体为：首先挑选出最大相似度对应的匹配，假设此匹配的主对象描述子序号分别为p和q，然后在匹配结果中删除所有含有主对象描述子p和主对象描述子q参与的匹配；再挑选出剩余匹配结果中最大相似度对应的匹配，如此重复下去，直到对当前关键帧的每一个主对象描述子，在候选关键帧中找到与其相似度最高的主对象描述子。

每次挑选出的相似度最大值以及对应的相对位姿为最终的匹配结果。

参见图5所示，假设当前关键帧中有8个主对象描述子，候选关键帧中有7个主对象描述子，两两匹配产生56个匹配相似度。按照上述筛选策略，首先挑选出最大值0.864，然后删除表中的第4行和第8列(删除之后剩下的匹配结果仍然构成一个矩阵)；再挑选出剩余匹配结果中最大值0.862，然后删除表中第3行和第1列(删除之后剩下的匹配结果仍然构成一个矩阵)，如此循环。最终筛选出的相似度为：0.864，0.862，0.861，0.821，0756，0.724，0.684。

将筛选出的相似度所对应的相对位姿(Δx,Δy,Δθ_o)看作是一组三维点，映射到三维空间中，聚类得到多个类(例如，进行欧几里得聚类，得到多个类{C_i|i＝1,2,3…})；

若至少存在一个类包含的点数量大于数量阈值(假设其中第k个类C_k含有的点数量最多，若此点数量大于数量阈值2)，则判定当前关键帧与历史关键帧是处于同一场景，将包含点数量最多的类中所有点的位姿平均值作为当前关键帧与历史关键帧之间的相对位姿(计算C_k的位姿平均值作为最终的相对位姿)；

进一步地，若判定当前关键帧和历史关键帧之间不存在回环边，将当前关键帧的所有环特征向量添加到所述多维树结构中。

可以理解的是，若判定当前关键帧和历史关键帧之间不存在回环边，将当前关键帧的所有环特征向量添加到所述多维树结构中，能够保证下次如果回到当前场景时，在多维树结构中能搜索出候选关键帧。如果检测到回环边又把当前关键帧的环特征向量插入多维树结构中，在下次又回到当前场景时，会有回环边的冗余，没有必要。

可以理解的是，本实施例提供的技术方案，以主对象为中心构建主对象描述子，只需要两帧的主对象位置接近且能观测到同一场景中的一个或多个主对象即可，相比现有技术中以激光雷达为中心建立Scan Context描述子的方法，本实施例提供的这种不依赖观测位置的描述子能解决Scan Context描述子在两帧位置不重合时无法进行回环检测的问题，扩大了回环检测的适用范围，提高了回环检测的鲁棒性。

需要说明的是，本实施例提供的技术方案，不依赖观测位置的意思是，描述子以主对象为中心建立，激光雷达在其周围不同位置，描述子的内容是大致一样的(只需要旋转一下就能匹配上)，而现有技术中Scan Context方法的激光雷达位置不一样，那么其描述子就是完全不一样的。

进一步地，本实施例提供的技术方案，通过对当前关键帧的主对象描述子和历史关键帧的主对象描述子两两匹配结果进行筛选，来避免误匹配的影响。最终计算出的当前关键帧和历史关键帧之间的相对位姿包含x、y方向的偏移和旋转角度，相比现有技术无法计算两帧之间的X和Y坐标的偏移，本实施例提供的技术方案，回环检测得到的结果发送给后端做全局目标地图优化时，可以得到更好的位姿优化结果，能够提高整个SLAM系统的鲁棒性和定位建图的精度。

在具体实践中，后端以每一个关键帧的位姿作为位姿节点，关键帧之间的相对位姿作为约束边，构建位姿图。每当前端的位姿数据发送到后端时，后端通过使位姿图整体误差最小的方式对每个节点的位姿进行优化，用优化后的位姿更新关键帧。

当回环检测检测到回环边后，在当前关键帧和相应的历史关键帧节点之间添加一个约束边(即为本实施例计算出的相对位姿)，位姿图形成一个环，优化之后历史关键帧的位姿会有较大改变，在回环的场景中能明显看到位姿优化的过程。

具体地，后端基于本实施例提供的回环检测结果，进行全局位姿优化，包括：

用顶点表示优化变量，用约束边表示误差项。以载具的全局位姿为顶点，约束边则为两个位姿节点之间的相对位姿估计，该估计可以来自于特征点法或者直接法，也可以来自GPS或IMU积分。无论哪种手段，假设估计了位姿节点T_i和位姿节点T_j之间的一个运动ΔT_ij。该运动可以有若干种表达方式，取比较自然的一种：

也可表示为：

按照图优化的思路，实际中上式不会精确地成立，因此设立最小二乘误差，然后讨论误差关于优化变量的导数。这里把上式的T_ij移至等式右侧，构建误差e_ij：

上式中优化变量有两个ξ_i和ξ_j，因此求e_ij关于这两个变量的导数，按照李代数的求导方式，给ξ_i和ξ_j各一个左扰动：δξ_i和δξ_j。于是误差变为：

根据伴随性质exp((Ad(T)ξ)^)＝Texp(T^)T^-1可得：

exp(T^)T＝T exp((Ad(T)ξ)^) (14)

根据上式可推导出右乘形式的雅克比矩阵(挪到左边时形成左乘)：

至此，已经求出了误差关于两个位姿的雅克比矩阵。

关于T_i的雅克比矩阵：

关于T_j的雅克比矩阵：

雅克比求导之后，剩下的部分就是一个简单的最小二乘问题了，优化变量为各个顶点的位姿，边来自于位姿观测约束。

记ε为所有边的集合，那么总体目标函数为

使用高斯牛顿法(Gauss-Newton法)或者Levenberg-Marquardt法(LM法)求解此问题，在实际工程中使用g2o或Ceres进行求解。

具体地，后端全局位姿优化后，还要进行全局目标地图的构建和更新(即点云地图的构建和更新)，包括：

将所有的关键帧中的子地图按照关键帧中的位姿进行三维变换，将变换后的结果融合在一起，滤波即可得到全局的稀疏点云地图，调节滤波的参数即可改变地图的稀疏程度。每当位姿图中有新节点插入时，位姿图都会进行优化，并更新所有关键帧中的位姿，同时更新全局目标地图。

实施例二

根据一示例性实施例示出的一种点云地图构建方法，该方法至少包括：

基于主对象特征的雷达里程计；

实施例一所述的回环检测方法。

可以理解的是，由实施例一的阐述可知，点云地图的构建包括：前端里程计、回环检测、后端优化及建图。由于回环检测、后端优化及建图的实现方式在实施例一中已经详细阐述，本实施例不再赘述。现以前端里程计为雷达里程计为例，对基于主对象特征的雷达里程计的实现方式解释说明如下：

1、数据收集和预处理

收集激光雷达、相机、IMU、GNSS以及轮速编码器等传感器的数据。因为时钟源都有钟漂，而且每个传感器的时钟源钟漂不同，所以需要把各个传感器时间戳在初始时刻对齐，使用线性插值的方法进行时间同步，以激光雷达的时间戳为标准推算在此时刻的其余传感器数据。

点云运动畸变处理：

运动畸变产生的原因是一帧点云中的点不是在同一时刻采集的，在采集过程中，激光雷达随着载具在运动，但是激光雷达测量的是观测物和雷达之间的距离，所以不同激光点的坐标系就不一样了。点云运动畸变现象会随着载具速度的提高和激光雷达频率的下降而加重。点云运动畸变现象会严重影响点云匹配的结果，产生不准确的里程计信息。

为了消除点云运动畸变，用上一帧载具的速度和角速度预测当前帧载具的速度和角速度或者直接使用IMU等传感器获取当前帧的载具运动状态，结合激光雷达的扫描方式，推算出当前帧激光点云中每个点与第一个点同时刻时所处的位置，这样的过程称为点云运动畸变处理。点云运动畸变处理能够明显提高建图的精度。

2、3D点云语义分割并提取主对象特征

如何从关键帧中确定出主对象已经在实施例一中介绍，如何从每一帧中确定出主对象的方法同从关键帧中确定出主对象，本实施例不再赘述。

确定出主对象后，提取主对象特征包括：

计算每个主对象的质心坐标，并对每一个主对象进行主成分分析，取其最大的特征值对应的特征向量为该主对象的方向向量。

3、基于主对象特征的雷达里程计

每获得一帧3D点云，都与前一帧3D点云进行匹配，匹配的方法是一种基于主对象的匹配方法，包括：

对于当前帧中的每一个主对象，在上一帧中搜索一定空间范围内距其最近的主对象(若在一定范围内搜索不到带有同样语义的主对象，则忽略此主对象对应的误差项)两主对象进行匹配，匹配的误差为：

error＝e(c₁，c₂)+e(n₁，n₂) (19)

式中，e(c₁,c₂)为主对象质心坐标的匹配误差，c₁和c₂为主对象的质心坐标，e(n₁,n₂)为主对象方向向量的匹配误差，n₁和n₂为主对象的方向向量。

式中，R和T三维旋转变换矩阵和三维平移变换矩阵，组合在一起表示点的三维变换。构建如下优化方程：

式中，N表示当前帧点云中的主对象个数。整体匹配误差为每一个主对象的匹配误差之和，每一个主对象的匹配误差又包括主对象质心坐标匹配误差和主对象方向向量匹配误差。点云匹配本质上是一个优化问题，构建出如上的优化方程后，求解方法可选用高斯牛顿法(Gauss-Newton法)或者Levenberg-Marquardt法(LM法)，迭代计算出准确的匹配位姿。

计算出每一帧相对前一帧的相对位姿，依次叠乘即可得到每一帧在世界坐标系中的位姿，即为里程计信息。

可以理解的是，本实施例提供的技术方案，包含实施例一所述的回环检测方法，该回环检测方法以主对象为中心构建主对象描述子，只需要两帧的主对象位置接近且能观测到同一场景中的一个或多个主对象即可，相比现有技术中以激光雷达为中心建立ScanContext描述子的方法，本实施例提供的这种不依赖观测位置的描述子能解决ScanContext描述子在两帧位置不重合时无法进行回环检测的问题，扩大了回环检测的适用范围，提高了回环检测的鲁棒性。

实施例三

根据一示例性实施例示出的一种电子设备，包括：

所述处理器用于执行存储器中存储的程序指令，执行如实施例一所述的回环检测方法，和/或，实施例二所述的点云地图构建方法。

需要说明的是，所述电子设备包括但不限于：车载电脑和外接的计算机设备。通信模块包括但不限于：有线通信模块和无线通信模块，例如：WCDMA、GSM、CDMA和/或LTE通讯模块、ZigBee模块、蓝牙模块、Wi-Fi模块等。

处理器包括但不限于：CPU、单片机、PLC控制器、FPGA控制器等。

存储器可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(RAM)和/或高速缓存存储器；还可以包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。存储器可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明各实施例的功能。

实施例四

根据一示例性实施例示出的一种计算机可读存储介质，其上存储有可擦写的计算机程序；

当所述计算机程序在计算机设备上运行时，使得所述计算机设备执行如实施例一所述的回环检测方法，和/或，实施例二所述的点云地图构建方法。

本实施例公开的计算机可读存储介质包括但不限于：电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

可以理解的是，上述各实施例中相同或相似部分可以相互参考，在一些实施例中未详细说明的内容可以参见其他实施例中相同或相似的内容。

需要说明的是，在本发明的描述中，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性。此外，在本发明的描述中，除非另有说明，“多个”的含义是指至少两个。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

上述提到的存储介质可以是只读存储器，磁盘或光盘等。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims

1.一种回环检测方法，其特征在于，包括：

对任一主对象，以所述主对象为中心构建主对象描述子；

2.根据权利要求1所述的方法，其特征在于，

所述前端通过将当前帧中的3D点云及其前四帧中的3D点云按照前端的匹配结果融合、滤波，构建关键帧，并将当前帧的位姿定义为所述关键帧的位姿，将关键帧中的点云定义为子地图；和/或，

对任一子地图，获取该子地图中每一个点的语义标签；

根据所述语义标签，筛选出主对象点云；

3.根据权利要求2所述的方法，其特征在于，所述以所述主对象为中心构建主对象描述子，包括：

4.根据权利要求3所述的方法，其特征在于，还包括：

5.根据权利要求3所述的方法，其特征在于，还包括：

6.根据权利要求5所述的方法，其特征在于，所述将当前关键帧的主对象描述子和历史关键帧的主对象描述子两两匹配，包括：

根据历史关键帧的环特征向量，构建多维树结构；

创建候选关键帧集合；

7.根据权利要求6所述的方法，其特征在于，所述将当前关键帧的主对象描述子和候选关键帧的主对象描述子两两匹配，包括：

获取候选关键帧的扇特征向量列表、主对象描述子列表；

8.根据权利要求7所的方法，其特征在于，所述将当前关键帧的主对象描述子和候选关键帧的主对象描述子两两匹配，还包括：

获取候选关键帧的主对象位置列表；

9.根据权利要求8所述的方法，其特征在于，所述根据所述匹配结果，判断当前关键帧与历史关键帧是否处于同一场景，包括：

10.根据权利要求9所述的方法，其特征在于，还包括：

11.一种点云地图构建方法，其特征在于，至少包括：

基于主对象特征的雷达里程计；

权利要求1～10任一项所述的回环检测方法。

12.根据权利要求11所述的方法，其特征在于，所述基于主对象特征的雷达里程计，包括：

从每一帧3D点云中确定出主对象；

13.一种电子设备，其特征在于，包括：

所述处理器用于执行存储器中存储的程序指令，执行如权利要求1～10任一项所述的回环检测方法，和/或，权利要求11或12所述的点云地图构建方法。

14.一种计算机可读存储介质，其特征在于，其上存储有可擦写的计算机程序；

当所述计算机程序在计算机设备上运行时，使得所述计算机设备执行如权利要求1～10任一项所述的回环检测方法，和/或，权利要求11或12所述的点云地图构建方法。