CN118089746B - 一种用于室内场景的激光重定位方法、系统和存储介质 - Google Patents

Abstract

本发明公开了一种用于室内场景的激光重定位方法、系统和存储介质，通过将当前数据与预存储的室内地图各采样点的当前场景描述特征进行相似度对比，获取多个地图位置作为粗略位置，以及地图和当前帧点云墙体的地图线段集和当前帧线段集，计算地图线段集中各粗略位置和当前帧线段集的主结构方向，在对其主结构方向进行对齐变换后，搜索变换后的当前帧线段集的不同主结构方向上的线段在地图线段集中设定距离阈值内的平行线段，组成多个候选匹配对后计算对应各候选匹配对对齐的精确位姿及在该位姿下当前帧线段集与地图线段集的匹配度，将匹配度最高的候选匹配对应的精确位置和方向作为重定位位姿，从而有效提高室内机器人重定位的精确度。

Description

一种用于室内场景的激光重定位方法、系统和存储介质

技术领域

本发明设计室内定位技术领域，尤其涉及一种用于室内场景的激光重定位方法、系统和存储介质。

背景技术

当前室内移动机器人主要通过激光雷达持续不断的旋转扫描激光光束，形成一帧帧的三维点云，对周围环境几何结构进行采样。机器人预先扫描场景内全部区域，通过持续拼接时间上连续的点云帧重建整个场景结构，形成全局点云地图并预存储于系统内。当机器人初次运行或遭遇外力搬运、激光雷达被遮挡等定位丢失情形时，需凭借当前局部的激光观测与预存储的全局点云地图进行匹配，找到其在地图中的准确位置，这个过程通常称之为重定位。重定位成功后，之后的每一帧点云即可凭借上一次的位置、当前点云与地图的匹配及其他传感器持续地获取定位。然而目前，机器人重定位技术大多存在计算量大、精确度较低等问题，导致机器人响应失败或定位错误。

发明内容

本发明针对现有技术中的不足，提供了一种用于室内场景的激光重定位方法，包括：

S1，计算从设备当前位置采集到的当前帧点云或图像数据的当前场景描述特征，将所述当前场景描述特征与预存储的室内地图各采样点的场景描述特征进行相似度对比，获取具有高相似度的多个采样点场景描述特征所在地图位置作为设备当前所在的粗略位置；

S2，从室内地图3D点云中获取墙体投影在与地面平行的2D平面上的墙体线段，形成地图线段集；从当前帧点云中提取出墙体投影在与地面平行的2D平面上的墙体线段形成当前帧线段集；

S3，从地图线段集中检索获取各粗略位置邻域内的局部线段集，根据获得各局部线段集计算获取对应的各粗略位置在地图上的主结构方向，并计算获取当前帧线段集的主结构方向；

S4，分别将当前帧的主结构方向和检索的室内地图上各粗略位置的主要结构方向进行对齐变换后；对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含不少于两组当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段；

S5，分别计算对应各候选匹配对对齐的精确位姿及在该精确位姿下的当前帧线段集与地图线段集的匹配度，将匹配度最高的候选匹配对应的精确位置和方向作为重定位位姿。

优选的，所述步骤S1具体包括：

S11，获取存储的室内地图3D点云数据，在室内地图所在水平面空间进行均匀的网格式采样，在每个采样点位计算附近点云的场景描述特征；

S12，计算从设备当前位置采集到的局部点云的当前场景描述特征，将所述当前场景描述特征与存储的各采样点的场景描述特征进行比对并计算余弦相似度，将所述余弦相似度超过设定值的对应各采样点记录为设备当前所在的粗略位置。

优选的，所述步骤S2具体包括：

将室内地图3D点云或当前帧点云通过正射投影投影到水平面上形成2D点云，并在投影的栅格上统计投影点数量后归一化到图像像素值区间形成密度图；

在所述密度图上通过线段检测算法LSD得到带有梯度方向的线段；

获取每一线段设定距离范围内与其平行且梯度方向相反的线段，形成初始平行线段对和对应的支撑点集，所述支撑点集为位于两平行线段间的点；

对所述支撑点集进行线段拟合后获得替代所述初始平行线段对的拟合线段，将获得的各拟合线段中距离较近且共线的线段合并形成墙体线段。

优选的，所述步骤S3具体包括：

从地图线段集中检索获取各粗略位置邻域内的局部线段集，将各局部线段集内的每根线段离散化成数量和长度成正比的单位向量，并将这些单位向量看作数据空间中的点；

使用DBSCAN算法及角度相似度量进行聚类，计算单位向量两两之间的余弦绝对值，若所述余弦绝对值大于设定阈值则将视为两者之间连有一条边，将通过边连接的无向图上的各顶点进行标记；

在所述无向图中获取包含至少一个核心顶点的连通分量，所述核心顶点被设置为具有超过设定数量的临接顶点的顶点，将各连通分量子图作为一聚类后获取该聚类中单位向量和x轴夹角的平均角度作为对应的所述局部线段集的主结构方向。

优选的，所述步骤S4中包括：

对每个粗略位置的每个主结构方向和当前帧的每个主结构方向进行对齐变换，计算实现每一组主结构方向的对齐变换所需的对齐旋转角度，所述每组主结构方向为任一粗略位置的任一主要结构方向和当前帧的任一主结构方向；

对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含不少于两组当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段。

本发明还公开了一种用于室内场景的激光重定位系统，包括位置获取模块、线段提取模块、方向计算模块、对齐匹配模块和位姿定位模块，其中，位置获取模块，用于计算从设备当前位置采集到的当前帧点云或图像数据的当前场景描述特征，将所述当前场景描述特征与预存储的室内地图各采样点的场景描述特征进行相似度对比，获取具有高相似度的多个采样点场景描述特征所在地图位置作为设备当前所在的粗略位置；线段提取模块，用于从室内地图3D点云中获取墙体投影在与地面平行的2D平面上的墙体线段，形成地图线段集；从当前帧点云中提取出墙体投影在与地面平行的2D平面上的墙体线段形成当前帧线段集；方向计算模块，用于从地图线段集中检索获取各粗略位置邻域内的局部线段集，根据获得各局部线段集计算获取对应的各粗略位置在地图上的主结构方向，并计算获取当前帧线段集的主结构方向；对齐匹配模块，用于分别将当前帧的主结构方向和检索的室内地图上各粗略位置的主要结构方向进行对齐变换后；对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含不少于两组当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段；位姿定位模块，用于分别计算对应各候选匹配对对齐的精确位姿及在该精确位姿下的当前帧线段集与地图线段集的匹配度，将匹配度最高的候选匹配对应的精确位置和方向作为重定位位姿。

优选的，所述位置获取模块包括特征提取模块和对比定位模块，其中，特征提取模块，用于获取存储的室内地图3D点云数据，在室内地图所在水平面空间进行均匀的网格式采样，在每个采样点位计算附近点云的场景描述特征；对比定位模块，用于计算从设备当前位置采集到的局部点云的当前场景描述特征，将所述当前场景描述特征与存储的各采样点的场景描述特征进行比对并计算余弦相似度，将所述余弦相似度超过设定值的对应各采样点记录为设备当前所在的粗略位置。

优选的，所述线段提取模块包括转换归一模块、线段检测模块、线段定位模块和线段拟合模块，其中，转换归一模块，用于将室内地图3D点云或当前帧点云通过正射投影投影到水平面上形成2D点云，并在投影的栅格上统计投影点数量后归一化到图像像素值区间形成密度图；线段检测模块，用于在所述密度图上通过LSD 算法得到带有梯度方向的线段；线段定位模块，用于获取每一线段设定距离范围内与其平行且梯度方向相反的线段，形成初始平行线段对和对应的支撑点集，所述支撑点集为位于两平行线段间的点；线段拟合模块，用于对所述支撑点集进行线段拟合后获得替代所述初始平行线段对的拟合线段，将获得的各拟合线段中距离较近且共线的线段合并形成墙体线段。

本发明还公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述任一所述方法的步骤。

本发明还公开了一种计算机程序产品，包括计算机程序/指令，所述计算机程序/指令被处理器执行时实现如上述任一所述方法的步骤。

本发明公开的一种用于室内场景的激光重定位方法、系统和存储介质，首先将当前帧和全局地图的3D点云转换为2D平面上的线段，并根据场景描述特征获取当前帧在全局地图上所可能对应的多个粗略位置，进而根据计算得出的当前帧线段集和各粗略位置邻域内线段集的多个主结构方向，将当前帧与各粗略位置进行对齐转换后，获取当前帧在地图上的平行匹配线段组成多个候选匹配对，最后通过计算并比较各候选匹配对的匹配度找到机器人当前所处的位置和方向，在简化运算量的同时，进一步提高机器人重定位的精确度和鲁棒性。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明一实施例公开的用于室内场景的激光重定位方法的步骤示意图。

图2为本发明一实施例公开的步骤S1的具体步骤示意图。

图3为本发明一实施例公开的转换线段步骤的具体内容示意图。

图4为本发明另一实施例公开的转换线段步骤的具体内容示意图。

图5为本发明一实施例公开的步骤S3的具体内容示意图。

图6为本发明一实施例公开的步骤S4的具体内容示意图。

图7为本发明一实施例公开的当前帧线段对齐旋转的示意图。

图8为本发明另一实施例公开的用于室内场景的激光重定位方法的步骤示意图。

图9为本发明另一实施例公开的用于室内场景的激光重定位系统的结构框图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例的附图，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员在无需创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明中，除非另有明确的规定和限定，此处使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明专利申请说明书以及权利要求书中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。同样，“一个”或者“一”等类似词语也不表示数量限制，而是表示存在至少一个。

当前室内移动机器人主要通过激光雷达持续不断的旋转扫描激光光束，形成一帧帧的三维点云，对周围环境几何结构进行采样。机器人预先扫描场景内全部区域，通过持续拼接时间上连续的点云帧重建整个场景结构，形成全局点云地图并预存储于系统内，当机器人初次运行或遭遇外力搬运、激光雷达被遮挡等定位丢失情形时，需凭借当前局部的激光观测与预存储的全局点云地图进行匹配，找到其在地图中的准确位置，这个过程通常称之为重定位。重定位成功后，之后的每一帧点云即可凭借上一次的位置、当前点云与地图的匹配及其他传感器持续地获取定位。然而目前，机器人重定位技术大多存在计算量大、精确度较低或鲁棒性不足等问题，导致机器人响应失败或定位错误。因此，本实施例公开了一种用于室内场景的激光重定位方法，如附图1所示，包括如下步骤。

步骤S1，计算从设备当前位置采集到的当前帧点云或图像数据的当前场景描述特征，将所述当前场景描述特征与预存储的室内地图各采样点的场景描述特征进行相似度对比，获取具有高相似度的多个采样点场景描述特征所在地图位置作为设备当前所在的粗略位置。

具体的，存储器内预先存储有待检测室内场景的全局地图以供机器正常工作，该全局地图通过预先扫描和点云帧拼接重建待检测整个场景结构，其中，全局地图内预先设置有各采样点并存储有各采样点的场景描述特征，以便设备在需要时通过采集当前帧点云，与所存储的各采样点通过场景描述特征进行对比而进行初步重定，得到可能的设备当前位置。

其中，对于通过采用激光雷达进行定位的移动机器人，如附图2所示，所述步骤S1具体包括如下内容。

步骤S11，获取存储的室内地图3D点云数据，在室内地图所在水平面空间进行均匀的网格式采样，在每个采样点位计算附近点云的场景描述特征。

本实施例中，该场景描述特征被配置为通过Scan Context算法所获取的特征描述符。

步骤S12，计算从设备当前位置采集到的局部点云的当前场景描述特征，将所述当前场景描述特征与存储的各采样点的场景描述特征进行比对并计算余弦相似度，将所述余弦相似度超过设定值的对应各采样点记录为设备当前所在的粗略位置。

在另一实施例中，对于通过相机获取图像数据进行定位的机器人，所述步骤S1具体还包括如下内容。

步骤S101，获取存储的室内地图图像数据，在室内地图所在水平面空间进行均匀的网格式采样，在每个采样点位计算该处图像的场景描述特征。本实施例中，该场景描述特征被配置为通过RelocNet算法所获取的特征向量。

步骤S102，计算从设备当前位置采集到的图像数据的当前场景描述特征，将所述当前场景描述特征与存储的各采样点的场景描述特征进行比对并计算欧氏距离，将所述欧氏距离超过设定值的对应各采样点记录为设备当前所在的粗略位置。

在本实施例中，所选取粗略位置的数量还可根据当前机器人计算平台资源的丰富程度自动选取，具体包括如下内容。

步骤S201，确定重定位算法被分配的算力上限C,并估算个检索位置需要算力c。

步骤S102，所选取粗略位置的数量被配置为C/c。

步骤S2，从室内地图3D点云中获取墙体投影在与地面平行的2D平面上的墙体线段，形成地图线段集；从当前帧点云中提取出墙体投影在与地面平行的2D平面上的墙体线段形成当前帧线段集。

具体的，由于3D激光测量距离远、采集场景大等原因，其所获取3D点云包含数据量大、对机器人算力要求较高，本实施例通过将3D点云转化成二维平面线段，可有效降低计算复杂度和移动机器人响应速度，帮助快速计算出全局最优位置。其中，室内地图线段提取所得到的地图线段集预先存储于数据库中，当机器进行重定位时，实时采集当前帧点云形成当前帧线段集。

在本实施例中，如附图3所示，以室内地图为例，所述转换线段方法具体包括如下步骤。

步骤S21，将室内地图3D点云或当前帧点云通过正射投影投影到水平面上形成2D点云，并在投影的栅格上统计投影点数量后归一化到图像像素值区间形成密度图。

步骤S22，在所述密度图上通过LSD 算法得到带有梯度方向的线段。

步骤S23，获取每一线段设定距离范围内与其平行且梯度方向相反的线段，形成初始平行线段对和对应的支撑点集，所述支撑点集为位于两平行线段间的点。

步骤S24，对所述支撑点集进行线段拟合后获得替代所述初始平行线段对的拟合线段，将获得的各拟合线段中距离较近且共线的线段合并形成墙体线段。

具体的， 如附图4所示，本实施例将3D点云输入线段提取模块，通过正射投影投影到水平面上形成2D点云，并在投影的栅格上统计投影点数量后将其归一化到图像像素值区间（0-255）得到密度图。在密度图上进行 LSD 算法，可进一步得到带有梯度方向的线段。对于每一根线段，在其附近找到与其平行（通常0.1m内）且梯度方向相反的线段，两者是点云密集的同一墙面的两边，两根线段所包夹中间的点即为墙面上的点，称为它们的支撑点集。通过对这些2D支撑点进行RANSAC线段拟合，得到新的线段替代原始的平行线段对，最后在RANSAC拟合后的新线段集合中寻找距离较近且共线的线段进行合并，最终得到表示墙体结构的线段，输出主要墙体在 2D 平面上的投影线段。其中。当前帧点云与室内地图提取方法相同，所述支撑点集通过RANSAC参数估计方法进行拟合，并将得到的拟合线段用以代替初始平行线段，可使重复线段数量减少并进一步提高线段提取的精度和鲁棒性。

步骤S3，从地图线段集中检索获取各粗略位置邻域内的局部线段集，根据获得各局部线段集计算获取对应的各粗略位置在地图上的主结构方向，并计算获取当前帧线段集的主结构方向。

在本实施例中，如附图5所示，以各粗略位置在地图上主结构方向为例，该主结构方向计算方式具体包括如下内容。

步骤S31，从地图线段集中检索获取各粗略位置邻域内的局部线段集，将各局部线段集内的每根线段离散化成数量和长度成正比的单位向量，并将这些单位向量看作数据空间中的点。

步骤S32，使用DBSCAN算法及角度相似度量进行聚类，计算单位向量两两之间的余弦绝对值，若所述余弦绝对值大于设定阈值则将视为两者之间连有一条边，将通过边连接的无向图上的各顶点进行标记。

具体的，根据DBSCAN算法，无向图内的各顶点根据相邻顶点数量和类型被分别标记为核心顶点、边界点与噪声。其中核心顶点被配置为其邻接顶点超过预设数量，边界点被配置为与一个核心顶点相连，其余顶点被配置为噪声，在后续计算中被忽略，避免异常点影响整体计算。

步骤S33，在所述无向图中获取包含至少一个核心顶点的连通分量，所述核心顶点被设置为具有超过设定数量的临接顶点的顶点，将各连通分量子图作为一聚类后获取该聚类中单位向量和x轴夹角的平均角度作为对应的所述局部线段集的主结构方向。

具体的，当前帧线段集的主结构方向计算方式与各粗略位置在地图上主结构方向相同，同一局部线段集以及当前帧中包含多个主结构方向。

步骤S4，分别将当前帧的主结构方向和检索的室内地图上各粗略位置的主要结构方向进行对齐变换后；对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含不少于两组当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段。

如附图6所示，在本实施例中，所述步骤S4中包括如下步骤。

步骤S41，对每个粗略位置的每个主结构方向和当前帧的每个主结构方向进行对齐变换，计算实现每一组主结构方向的对齐变换所需的对齐旋转角度，所述每组主结构方向为任一粗略位置的任一主要结构方向和当前帧的任一主结构方向。

具体的，同一粗略位置预设邻域内的局部线段集和当前帧线段集中分别存在多个主结构方向，如附图7所示，图中各箭头所表示的方向即为该线段集中的一个主结构方向，对该两线段集内的各主结构方向进行依次遍历匹配，可组成不同组主结构方向，计算各组主结构方向之间的方向夹角即可得到各组所对应的对齐旋转角度，每个对齐转换角度对应一个粗略位姿变换。

步骤S42，对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含不少于两组当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段。

在本实施例中，所述步骤S42还包括如下步骤。

步骤S421，根据每个粗略位置中各组主结构方向所对应的对齐旋转角度，计算得到各组主结构方向所对应的第一位姿转换矩阵。

步骤S422，搭建每个粗略位置的Rtree树结构，并根据各组主结构方向所对应的第一位姿转换矩阵，将当前帧线段集中的所有线段转换到地图坐标系中，形成当前帧变换线段集，该Rtree树结构被配置为覆盖该粗略位置邻域内局部线段集内的所有线段。

步骤S423，在各组主结构方向所对应的当前帧变换线段集中，筛选预设数值个不同的主结构方向，并在所筛选的各主结构方向上分别选取一条线段作为基准线段。

步骤S424，通过Rtree数结构在地图线段集中搜索各基准线段预设阈值距离内的平行线段，组成各组主结构方向下的多个候选匹配对。

具体的，本实施例中该预设阈值被配置为3米，如附图7所示，在当前线段集中筛选2个不同的主结构方向，图中、分别为当前帧线段集中所选取的两个主结构方向的一个线段，将其按照某组主结构方向所对应的第一位姿转换矩阵转换至地图坐标系中后，在地图线段集中分别找到其3米内所对应的地图线段集中的平行线段、，即可组成一个候选匹配对（，）、（，）。同一第一位姿转换矩阵下，可通过对当前帧不同主结构方向以及主结构方向内不同线段和地图上不同平行线段进行组合，得到多个候选匹配对，各候选匹配对中可包含不同数量的平行线段组。

在本实施例中，通过每个粗略位置中各组主结构方向所对应的对齐旋转角度得到的第一位姿转换矩阵精确度较低，可在获取其所对应的各候选匹配对后，进一步计算得到更为精确的每个候选匹配对所对应的第二位姿转换矩阵，具体包括如下内容。

步骤S101，将第一位姿转换矩阵下所得到的各候选匹配对中各线段所在直线参数化表示为法向和原点到直线的距离，并根据第一优化函数确定当前帧旋转至地图对应方向的最佳旋转矩阵，该第一优化函数被配置为：

其中,表示一个候选匹配对中的一个当前帧和地图线段匹配，表示一个候选匹配对中的全部当前帧和地图线段匹配，表示一个候选匹配对中一个当前帧线段所在直线的法向，表示一个候选匹配对中一个地图线段所在直线的法向,U、V分别表示将相关矩阵H进行SVD分解后得到的正交矩阵，H计算公式被配置为：

步骤S102，根据第二优化函数求解各候选匹配对中当前帧线段上的点到地图线段所在直线的最短距离得到平移值，该第二优化函数被配置为：

其中，表示一个候选匹配对中的一个当前帧和地图线段的匹配，表示一个候选匹配对中的全部当前帧和地图线段的匹配，表示一个候选匹配对中一个当前帧线段上的一个点，表示一个候选匹配对中的一个当前帧线段，表示一个候选匹配对中一个地图线段所在直线的法向，为最佳旋转矩阵，表示原点到一个候选匹配对中一个地图线段所在直线的距离，表示根据当前帧线段的两个端点所构建的线性方程组中A的伪逆矩阵，其中，

，

步骤S103，根据最佳旋转矩阵和平移值得到各候选匹配对所对应的第二位姿转换矩阵=。

步骤S5，分别计算对应各候选匹配对对齐的精确位姿及在该精确位姿下的当前帧线段集与地图线段集的匹配度，将匹配度最高的候选匹配对应的精确位置和方向作为重定位位姿。

具体的，将当前帧线段集根据各候选匹配对所对应的第二位姿转换矩阵进行转换后，计算其与地图线段集的匹配度，匹配度最高的即为所需的重定位位姿，可根据此确定机器人当前位置和方向。

在本实施例中，所述步骤S5具体还包括如下内容。

步骤S51，对地图线段集和当前帧线段集中的线段分别按照预设步长进行均匀采样，并分别将其离散化为地图离散点集和当前帧离散点集，即

，其中表示地图线段集或当前帧线段集中的一个线段， 表示地图线段集或当前帧线段集，上标start表示线段的起点，上标direction表示线段的起点到终点的方向，上标length表示线段的起点到终点的长度，表示预设步长，表示地图离散点集或当前帧离散点集中的一个离散点，表示一个离散点。具体的，最终得到地图离散点集和前帧离散点集。

步骤S52，根据第二位姿转换矩阵将当前帧离散点集转换到地图坐标系中形成当前帧转换离散点集，即，，其中，表示当前帧转换离散点集，表示第二位姿转换矩阵，表示当前帧离散点集，表示当前帧离散点集中的一个离散点，表示当前帧转换离散点集中的一个离散点。

步骤S53，根据匹配公式统计各候选匹配对所对应的当前帧转换离散点集在预设步长内所包含地图离散点的比例，得到最终匹配分数，最终匹配分数最高的位姿即为重定位位姿，即，该匹配公式被配置为：，其中，表示最终匹配分数，表示当前帧转换离散点集，表示地图离散点集，表示当前帧转换离散点集中的一个离散点，表示地图离散点集中的一个离散点，表示预设步长。

如附图8所示，本实施例首先如图中左边上方所示，使用ScanContext或RelocNet等算法检索当前帧在地图中粗略的位置，计算每个位置的局部线段的主要方向。同时如图中右边上方所示，对当前帧点云以相同方式进行线段提取并计算主方向。进而在每个检索位置，将当前帧主方向和该位置主方向进行对齐，并在一个检索位置的一个对齐方向上，将当前帧线段变换到该位置和方向上。接下来如图中右边中间菱形所示，对变换后当前帧不同主方向上的线段分别搜索一定距离阈值内的平行线段。由于当前帧及检索位置有多个主方向，并且主方向上存在多个线段，则组成多个候选匹配对：{{((当前帧方向1线段，地图线段)，(当前帧方向2线段，地图线段))}}。最终，如图中右边下方所示，对于每个匹配对，分别结算精确的位姿，并计算分数，分数最高的位姿即为重定位位姿。

本实施例公开的一种用于室内场景的激光重定位方法，首先将当前帧和全局地图的3D点云转换为2D平面上的线段，并根据场景描述特征获取当前帧在全局地图上所可能对应的多个粗略位置，进而根据计算得出的当前帧线段集和各粗略位置邻域内线段集的多个主结构方向，将当前帧与各粗略位置进行对齐转换后，获取当前帧在地图上的平行匹配线段组成多个候选匹配对，最后通过计算并比较各候选匹配对的匹配度找到机器人当前所处的位置和方向，在简化运算量的同时，进一步提高机器人重定位的精确度和鲁棒性。

在另一实施例中，如附图9所示，还公开了一种用于室内场景的激光重定位系统，包括位置获取模块1、线段提取模块2、方向计算模块3、对齐匹配模块4和位姿定位模块5。其中，位置获取模块，用于计算从设备当前位置采集到的当前帧点云或图像数据的当前场景描述特征，将所述当前场景描述特征与预存储的室内地图各采样点的场景描述特征进行相似度对比，获取具有高相似度的多个采样点场景描述特征所在地图位置作为设备当前所在的粗略位置。线段提取模块，用于从室内地图3D点云中获取墙体投影在与地面平行的2D平面上的墙体线段，形成地图线段集；从当前帧点云中提取出墙体投影在与地面平行的2D平面上的墙体线段形成当前帧线段集。方向计算模块，用于从地图线段集中检索获取各粗略位置邻域内的局部线段集，根据获得各局部线段集计算获取对应的各粗略位置在地图上的主结构方向，并计算获取当前帧线段集的主结构方向。对齐匹配模块，用于分别将当前帧的主结构方向和检索的室内地图上各粗略位置的主要结构方向进行对齐变换后；对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段。位姿定位模块，用于分别计算对应各候选匹配对对齐的精确位姿及在该精确位姿下的当前帧线段集与地图线段集的匹配度，将匹配度最高的候选匹配对应的精确位置和方向作为重定位位姿。

在本实施例中，所述位置获取模块还包括特征提取模块和对比定位模块。其中，特征提取模块，用于获取存储的室内地图3D点云数据，在室内地图所在水平面空间进行均匀的网格式采样，在每个采样点位计算附近点云的场景描述特征。对比定位模块，用于计算从设备当前位置采集到的局部点云的当前场景描述特征，将所述当前场景描述特征与存储的各采样点的场景描述特征进行比对并计算余弦相似度，将所述余弦相似度超过设定值的对应各采样点记录为设备当前所在的粗略位置。

在本实施例中，所述线段提取模块还包括转换归一模块、线段检测模块、线段定位模块和线段拟合模块。其中，转换归一模块，用于将室内地图3D点云或当前帧点云通过正射投影投影到水平面上形成2D点云，并在投影的栅格上统计投影点数量后归一化到图像像素值区间形成密度图。线段检测模块，用于在所述密度图上通过LSD 算法得到带有梯度方向的线段。

线段定位模块，用于获取每一线段设定距离范围内与其平行且梯度方向相反的线段，形成初始平行线段对和对应的支撑点集，所述支撑点集为位于两平行线段间的点。线段拟合模块，用于对所述支撑点集进行线段拟合后获得替代所述初始平行线段对的拟合线段，将获得的各拟合线段中距离较近且共线的线段合并形成墙体线段。

上述用于室内场景的激光重定位系统的具体功能与前面实施例所公开的用于室内场景的激光重定位方法一一对应，故在此不再详细展开描述，具体可参考前面公开的用于室内场景的激光重定位方法的各实施例。需要说明的是，本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

所述用于室内场景的激光重定位方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器、随机存取存储器、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

总之，以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所作的均等变化与修饰，皆应属本发明专利的涵盖范围。

Claims (9)

1.一种用于室内场景的激光重定位方法，其特征在于，包括如下步骤：

计算从设备当前位置采集到的当前帧点云或图像数据的当前场景描述特征，将所述当前场景描述特征与预存储的室内地图各采样点的场景描述特征进行相似度对比，获取具有高相似度的多个采样点场景描述特征所在地图位置作为设备当前所在的粗略位置；

从室内地图3D点云中获取墙体投影在与地面平行的2D平面上的墙体线段，形成地图线段集；从当前帧点云中提取出墙体投影在与地面平行的2D平面上的墙体线段形成当前帧线段集；

从地图线段集中检索获取各粗略位置邻域内的局部线段集，根据获得各局部线段集计算获取对应的各粗略位置在地图上的主结构方向，并计算获取当前帧线段集的主结构方向，具体包括：

从地图线段集中检索获取各粗略位置邻域内的局部线段集，将各局部线段集内的每根线段离散化成数量和长度成正比的单位向量，并将这些单位向量看作数据空间中的点；

使用DBSCAN算法及角度相似度量进行聚类，计算单位向量两两之间的余弦绝对值，若所述余弦绝对值大于设定阈值则将视为两者之间连有一条边，将通过边连接的无向图上的各顶点进行标记；

在所述无向图中获取包含至少一个核心顶点的连通分量，所述核心顶点被设置为具有超过设定数量的邻接顶点的顶点，将各连通分量子图作为一聚类后获取该聚类中单位向量和x轴夹角的平均角度作为对应的所述局部线段集的主结构方向；当前帧线段集的主结构方向计算方式与各粗略位置在地图上主结构方向计算方式相同，同一局部线段集以及当前帧线段集中包含多个主结构方向；

分别将当前帧的主结构方向和检索的室内地图上各粗略位置的主结构方向进行对齐变换后；对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含不少于两组当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段；

分别计算对应各候选匹配对对齐的精确位姿及在该精确位姿下的当前帧线段集与地图线段集的匹配度，将匹配度最高的候选匹配对对应的精确位置和方向作为重定位位姿。

2.根据权利要求1所述的用于室内场景的激光重定位方法，其特征在于，计算从设备当前位置采集到的当前帧点云或图像数据的当前场景描述特征，将所述当前场景描述特征与预存储的室内地图各采样点的场景描述特征进行相似度对比，获取具有高相似度的多个采样点场景描述特征所在地图位置作为设备当前所在的粗略位置，具体包括：

获取存储的室内地图3D点云数据，在室内地图所在水平面空间进行均匀的网格式采样，在每个采样点位计算附近点云的场景描述特征；

计算从设备当前位置采集到的局部点云的当前场景描述特征，将所述当前场景描述特征与存储的各采样点的场景描述特征进行比对并计算余弦相似度，将所述余弦相似度超过设定值的对应各采样点记录为设备当前所在的粗略位置。

3.根据权利要求1所述的用于室内场景的激光重定位方法，其特征在于，从室内地图3D点云中获取墙体投影在与地面平行的2D平面上的墙体线段，具体包括：

将室内地图3D点云通过正射投影投影到水平面上形成2D点云，并在投影的栅格上统计投影点数量后归一化到图像像素值区间形成密度图；

在所述密度图上通过线段检测算法得到带有梯度方向的线段；

获取每一线段设定距离范围内与其平行且梯度方向相反的线段，形成初始平行线段对和对应的支撑点集，所述支撑点集为位于两平行线段间的点；

对所述支撑点集进行线段拟合后获得替代所述初始平行线段对的拟合线段，将获得的各拟合线段中距离较近且共线的线段合并形成墙体线段。

4.根据权利要求3所述的用于室内场景的激光重定位方法，其特征在于，分别将当前帧的主结构方向和检索的室内地图上各粗略位置的主结构方向进行对齐变换后，对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段组成了多个候选匹配对，具体包括：

对每个粗略位置的每个主结构方向和当前帧的每个主结构方向进行对齐变换，计算实现每组主结构方向的对齐变换所需的对齐旋转角度，所述每组主结构方向为任一粗略位置的任一主结构方向和当前帧的任一主结构方向；

对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含不少于两组当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段。

5.一种用于室内场景的激光重定位系统，其特征在于，包括：

位置获取模块，用于计算从设备当前位置采集到的当前帧点云或图像数据的当前场景描述特征，将所述当前场景描述特征与预存储的室内地图各采样点的场景描述特征进行相似度对比，获取具有高相似度的多个采样点场景描述特征所在地图位置作为设备当前所在的粗略位置；

线段提取模块，用于从室内地图3D点云中获取墙体投影在与地面平行的2D平面上的墙体线段，形成地图线段集；从当前帧点云中提取出墙体投影在与地面平行的2D平面上的墙体线段形成当前帧线段集；

方向计算模块，用于从地图线段集中检索获取各粗略位置邻域内的局部线段集，根据获得各局部线段集计算获取对应的各粗略位置在地图上的主结构方向，并计算获取当前帧线段集的主结构方向；从地图线段集中检索获取各粗略位置邻域内的局部线段集，将各局部线段集内的每根线段离散化成数量和长度成正比的单位向量，并将这些单位向量看作数据空间中的点；使用DBSCAN算法及角度相似度量进行聚类，计算单位向量两两之间的余弦绝对值，若所述余弦绝对值大于设定阈值则将视为两者之间连有一条边，将通过边连接的无向图上的各顶点进行标记；在所述无向图中获取包含至少一个核心顶点的连通分量，所述核心顶点被设置为具有超过设定数量的邻接顶点的顶点，将各连通分量子图作为一聚类后获取该聚类中单位向量和x轴夹角的平均角度作为对应的所述局部线段集的主结构方向；且当前帧线段集的主结构方向计算方式与各粗略位置在地图上主结构方向计算方式相同，同一局部线段集以及当前帧线段集中包含多个主结构方向；

对齐匹配模块，用于分别将当前帧的主结构方向和检索的室内地图上各粗略位置的主结构方向进行对齐变换后；对变换后当前帧线段集的不同主结构方向上的线段分别在地图线段集中搜索在设定距离阈值内的平行线段，组成了多个候选匹配对，所述候选匹配对包含不少于两组当前帧线段集一主结构方向上的线段和对应的地图线段集中的平行线段；

位姿定位模块，用于分别计算对应各候选匹配对对齐的精确位姿及在该精确位姿下的当前帧线段集与地图线段集的匹配度，将匹配度最高的候选匹配对对应的精确位置和方向作为重定位位姿。

6.根据权利要求5所述的用于室内场景的激光重定位系统，其特征在于，所述位置获取模块包括：

特征提取模块，用于获取存储的室内地图3D点云数据，在室内地图所在水平面空间进行均匀的网格式采样，在每个采样点位计算附近点云的场景描述特征；

对比定位模块，用于计算从设备当前位置采集到的局部点云的当前场景描述特征，将所述当前场景描述特征与存储的各采样点的场景描述特征进行比对并计算余弦相似度，将所述余弦相似度超过设定值的对应各采样点记录为设备当前所在的粗略位置。

7.根据权利要求6所述的用于室内场景的激光重定位系统，其特征在于，所述线段提取模块包括：

转换归一模块，用于将室内地图3D点云通过正射投影投影到水平面上形成2D点云，并在投影的栅格上统计投影点数量后归一化到图像像素值区间形成密度图；

线段检测模块，用于在所述密度图上通过线段检测算法得到带有梯度方向的线段；

线段定位模块，用于获取每一线段设定距离范围内与其平行且梯度方向相反的线段，形成初始平行线段对和对应的支撑点集，所述支撑点集为位于两平行线段间的点；

线段拟合模块，用于对所述支撑点集进行线段拟合后获得替代所述初始平行线段对的拟合线段，将获得的各拟合线段中距离较近且共线的线段合并形成墙体线段。

8.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1-4任一所述方法的步骤。

9.一种计算机程序产品，包括计算机程序/指令，其特征在于，该计算机程序/指令被处理器执行时实现权利要求1-4任一所述方法的步骤。