应用于二维激光的地图构建回环检测方法及设备与流程

本发明涉及计算机领域，尤其涉及一种应用于二维激光的地图构建回环检测方法及设备。

背景技术：

回环检测模块是slam（地图构建）系统中不可或缺的一部分，其主要负责判断机器人是否到达先前的位置，并将检测到的回环信息提供给后端进行处理。slam系统引入回环检测可以有效地消除slam过程中的累计里程误差，构建高精度的环境地图。

目前主流的回环检测采用图像检索的方式，通过提取图像的特征点，将当前帧的特征点和历史关键帧提取的特征点进行配准，如果特征点的匹配对数大于一定的数量，则认为出现回环。该方法受到环境和人工特征点选取的影响，存在误匹配高，计算量大，实时性差，且仅适用于回环时视角不改变的情况。

技术实现要素：

本发明的一个目的是提供一种应用于二维激光的地图构建回环检测方法及设备。

根据本发明的一个方面，提供了一种应用于二维激光的地图构建回环检测方法，该方法包括：

步骤s1，确定机器人的激光雷达的当前激光帧l的主方向，将当前激光的主方向对齐到激光坐标系的x轴；

步骤s2，按照所述激光坐标系中激光测量值的不同，将所述当前激光帧l分为不同的子类，表示所述子类的个数；

步骤s3，计算所述子类对应的平面描述；

步骤s4，合并所述平面描述生成该当前激光帧的直方图f，其中，表示每个平面描述向量的长度：

；

步骤s5，利用矩阵范数，对当前激光帧的直方图和激光地图中历史关键帧的直方图进行相似度测量，若测量得到的相似度大于预设阈值，则判定发生回环。

进一步的，上述方法中，若测量得到的相似度大于预设阈值，则判定发生回环，包括：

当距离小于设定的阈值时，则判定发生回环，其中，平面描述向量和表示直方图和的列向量，即激光帧l每个子类的描述向量：

。

进一步的，上述方法中，确定机器人的激光雷达的当前激光帧的主方向，将当前激光的主方向对齐到激光坐标系的轴，包括：

获取机器人的激光雷达的当前激光帧l，并且判定当前激光帧l是否为关键帧，若是关键帧，确定激光雷达的当前激光帧l的主方向，将当前激光帧l的主方向对齐到激光坐标系的轴，然后结束本步骤跳转到步骤s2；若不是关键帧，重复本步骤。

进一步的，上述方法中，获取机器人的激光雷达的当前激光帧l，并且判定当前激光帧l是否为关键帧，若是关键帧，确定激光雷达的当前激光帧l的主方向，将当前激光帧l的主方向对齐到激光坐标系的x轴，然后结束本步骤跳转到步骤s2；若不是关键帧，重复本步骤，包括：

步骤s111，获取当前激光帧l，根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧，若是关键帧，跳转到步骤s112；否则，重复本步骤；

步骤s112，为当前激光帧l分配一个方向描述向量[0,0,0,0]和直线穿过当前激光帧的原点；

步骤s113，计算当前激光帧l中每个激光束的欧式空间坐标，计算公式如下：

，

其中，表示当前激光帧l中第个激光束的测量值，表示当前激光帧l中第个观测值相对于激光坐标系的角度值；

步骤s114，计算当前激光帧l每个激光束最近的直线，更新所述方向描述向量，其中，计算过程如下所示：

，

；

步骤s115，根据所述方向描述向量d中的最大值确定当前激光帧l的主方向，根据激光的主方向旋转当前激光帧l到激光坐标系的x轴，然后结束本步骤跳转到步骤s2，其中，计算过程如下，和分别表示直线的斜率和截距：

，

；

；

。

进一步的，上述方法中，按照所述激光坐标系中激光测量值的不同，将所述当前激光帧l分为不同的子类，包括：

将当前激光帧l表示为，其中，表示当前激光帧l中第个激光束的测量值，表示当前激光帧l中第个观测值相对于激光坐标系的角度值；

对当前激光帧l中的每个激光束按照其测量值的大小归到不同的子类。

进一步的，上述方法中，计算所述子类对应的平面描述向量，包括：

为所述子类初始化一个平面描述向量和分配一个网格状的结构体；

对于所述子类的每个成员计算其在欧式坐标系中的位置:

；

计算每个点在中的网格，并将其存储在中；

根据中的存储的点集合计算的协方差及其特征向量和特征值，其中，，

，

，

；

分配m=4条直线穿过的均值，根据上一步计算出的特征值和特征向量，计算所属的子类index：

，

，

其中，表示特征向量和直线之间的距离;

根据所述index，更新的值：

。

根据本发明的另一面，还提供一种应用于二维激光的地图构建回环检测设备，该设备包括：

第一装置，用于确定机器人的激光雷达的当前激光帧l的主方向，将当前激光的主方向对齐到激光坐标系的x轴；

第二装置，用于按照所述激光坐标系中激光测量值的不同，将所述当前激光帧l分为不同的子类，表示所述子类的个数；

第三装置，用于计算所述子类对应的平面描述向量；

第四装置，用于合并所述平面描述向量生成该当前激光帧的直方图f，其中，表示每个平面描述向量的长度：

；

第五装置，用于利用矩阵范数，对当前激光帧的直方图和激光地图中历史关键帧的直方图进行相似度测量，若测量得到的相似度大于预设阈值，则判定发生回环。

进一步的，上述设备中，所述第五装置，用于当距离小于设定的阈值时，则判定发生回环，其中，平面描述向量和表示直方图和的列向量，即激光帧l每个子类的描述向量：

。

进一步的，上述设备中，所述第一装置，用于获取机器人的激光雷达的当前激光帧l，并且判定当前激光帧l是否为关键帧，若是关键帧，确定激光雷达的当前激光帧l的主方向，将当前激光帧l的主方向对齐到激光坐标系的轴，然后结束所述第一装置的执行跳转到所述第二装置执行；若不是关键帧，重复执行所述第一装置。

进一步的，上述设备中，所述第一装置，包括

第一一装置，用于获取当前激光帧l，根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧，若是关键帧，跳转到第一二装置；否则，重复本第一一装置的执行；

第一二装置，用于为当前激光帧l分配一个方向描述向量[0,0,0,0]和直线穿过当前激光帧的原点；

第一三装置，用于计算当前激光帧l中每个激光束的欧式空间坐标，计算公式如下：

，

其中，表示当前激光帧l中第个激光束的测量值，表示当前激光帧l中第个观测值相对于激光坐标系的角度值；

第一四装置，用于计算当前激光帧l每个激光束最近的直线，更新所述方向描述向量d，其中，计算过程如下所示：

，

；

第一五装置，用于根据所述方向描述向量d中的最大值确定当前激光帧l的主方向，根据激光的主方向旋转当前激光帧l到激光坐标系的x轴，然后结束本第一五装置第执行跳转到所述第二装置执行，其中，计算过程如下，和分别表示直线的斜率和截距：

，

；

；

。

进一步的，上述设备中，，所述第二装置，用于将当前激光帧l表示为，其中，表示当前激光帧l中第个激光束的测量值，表示当前激光帧l中第个观测值相对于激光坐标系的角度值；对当前激光帧l中的每个激光束按照其测量值的大小归到不同的子类。

进一步的，上述设备中，第三装置，用于为所述子类初始化一个平面描述向量和分配一个网格状的结构体；对于所述子类的每个成员计算其在欧式坐标系中的位置:

；

计算每个点在中的网格，并将其存储在中；根据中的存储的点集合计算的协方差及其特征向量和特征值，其中，，

，

，

；

分配m=4条直线穿过的均值，根据上一步计算出的特征值和特征向量，计算所属的子类index：

，

，

其中，表示特征向量和直线之间的距离;

根据所述index，更新的值：

。

根据本发明的另一面，还提供一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行以实现上述任一项所述的方法。

根据本发明的另一面，还提供一种用于在网络设备端信息处理的设备，该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该设备执行上述任一项所述的方法。

与现有技术相比，本发明针对背景技术中所涉及的传统回环检测方法存在误匹配率高，计算量大，实时性差等缺陷，提供一种基于二维激光的回环检测方法，该方法直接利用二维激光实现回环检测。本发明相比于基于图像的回环检测方法的进步效果在于：本方法不仅计算量小，实时性高，而且在回环检测时具有旋转不变性。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1示出本发明一实施例的激光雷达划分子类示意图；

图2示出本发明一实施例的旋转激光主方向到x轴示意图；

图3示出本发明一实施例的应用于二维激光的地图构建回环检测方法流程图。

附图中相同或相似的附图标记代表相同或相似的部件。

具体实施方式

下面结合附图对本发明作进一步详细描述。

在本申请一个典型的配置中，终端、服务网络的设备和可信方均包括一个或多个处理器(cpu)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(ram)和/或非易失性内存等形式，如只读存储器(rom)或闪存(flashram)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(pram)、静态随机存取存储器(sram)、动态随机存取存储器(dram)、其他类型的随机存取存储器(ram)、只读存储器(rom)、电可擦除可编程只读存储器(eeprom)、快闪记忆体或其他内存技术、只读光盘只读存储器(cd-rom)、数字多功能光盘(dvd)或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括非暂存电脑可读媒体(transitorymedia)，如调制的数据信号和载波。

如图3所示，本发明提供一种应用于二维激光的地图构建回环检测方法，所述方法包括：

步骤s1，如图2所示，确定机器人的激光雷达的当前激光帧l的主方向，将当前激光的主方向对齐到激光坐标系的x轴；

步骤s2，如图1所示，按照所述激光坐标系中激光测量值的不同，将所述当前激光帧l分为不同的子类，表示所述子类的个数；

步骤s3，计算所述子类对应的平面描述向量；

步骤s4，合并所述平面描述向量生成该当前激光帧的直方图f，其中，表示子类的个数，表示每个平面描述向量的长度；

；

步骤s5，利用矩阵范数，对当前激光帧的直方图和激光地图中历史关键帧的直方图进行相似度测量，若测量得到的相似度大于预设阈值，则判定发生回环。

在此，本发明针对背景技术中所涉及的传统回环检测方法存在误匹配率高，计算量大，实时性差等缺陷，提供一种基于二维激光的回环检测方法，该方法直接利用二维激光实现回环检测。本发明相比于基于图像的回环检测方法的进步效果在于：本方法不仅计算量小，实时性高，而且在回环检测时具有旋转不变性。

本发明的应用于二维激光的地图构建回环检测方法一实施例中，步骤s5中，若测量得到的相似度大于预设阈值，则判定发生回环，包括：

当距离小于设定的阈值时，则判定发生回环，其中，平面描述向量和表示直方图和的列向量，即激光帧l每个子类的描述向量：

。

在此，若距离小于设定的阈值则slam尝试进行回环；否则跳转到步骤s1。

本实施例通过判断距离是否小于设定的阈值，可以跟高效、可靠的判断是否发生回环。

本发明的应用于二维激光的地图构建回环检测方法一实施例中，步骤s1，确定机器人的激光雷达的当前激光帧的主方向，将当前激光的主方向对齐到激光坐标系的轴，包括：

步骤s11，获取机器人的激光雷达的当前激光帧l，并且判定当前激光帧l是否为关键帧，若是关键帧，确定激光雷达的当前激光帧l的主方向，将当前激光帧l的主方向对齐到激光坐标系的轴，然后结束本步骤跳转到步骤s2；若不是关键帧，重复本步骤，以继续获取当前激光帧l。

在此，通过选取为关键帧的当前激光帧l，可以保证后续回环检测的可靠性。

本发明的应用于二维激光的地图构建回环检测方法一实施例中，步骤s11，获取机器人的激光雷达的当前激光帧l，并且判定当前激光帧l是否为关键帧，若是关键帧，确定激光雷达的当前激光帧l的主方向，将当前激光帧l的主方向对齐到激光坐标系的x轴，然后结束本步骤跳转到步骤s2；若不是关键帧，重复本步骤，包括：

步骤s111，获取当前激光帧l，根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧，若是关键帧，跳转到步骤s112；否则，重复本步骤；

步骤s112，为当前激光帧l分配一个方向描述向量[0,0,0,0]和直线穿过当前激光帧的原点；

步骤s113，计算当前激光帧l中每个激光束的欧式空间坐标，计算公式如下：

，

其中，表示当前激光帧l中第个激光束的测量值，表示当前激光帧l中第个观测值相对于激光坐标系的角度值；

步骤s114，计算当前激光帧l每个激光束最近的直线，更新所述方向描述向量，其中，计算过程如下所示：

，

；

步骤s115，根据所述描述向量d中的最大值确定当前激光帧l的主方向，根据激光的主方向旋转当前激光帧l到激光坐标系的x轴，然后结束本步骤跳转到步骤s2，如图2所示，其中，计算过程如下，其中，和分别表示直线的斜率和截距：

，

；

；

。

本发明的应用于二维激光的地图构建回环检测方法一实施例中，步骤s2，按照所述激光坐标系中激光测量值的不同，将所述当前激光帧l分为不同的子类，其中，的取值为9，包括：

步骤s21，将当前激光帧l表示为，其中，表示当前激光帧l中第个激光束的测量值，表示当前激光帧l中第个观测值相对于激光坐标系的角度值；

步骤s22，对当前激光帧l中的每个激光束按照其测量值的大小归到不同的子类，如图1所示，从而实现准确的归类。

本发明的应用于二维激光的地图构建回环检测方法一实施例中，步骤s3，计算所述子类对应的平面描述向量，包括：

步骤s31，为所述子类初始化一个平面描述向量和分配一个网格状的结构体；

步骤s32，对于所述子类的每个成员计算其在欧式坐标系中的位置:

，

步骤s33，计算每个点在中的网格，并将其存储在中；

步骤s34，根据中的存储的点集合计算的协方差及其特征向量和特征值，其中，，

，

，

；

步骤s35，分配m=4条直线穿过的均值，根据上一步计算出的特征值和特征向量，计算所属的子类index：

，

，

其中，表示特征向量和直线之间的距离;

步骤s36，根据上一步的结果index，更新的值：

。

根据本发明的另一面，还提供一种应用于二维激光的地图构建回环检测设备，该设备包括：

第一装置，用于确定机器人的激光雷达的当前激光帧l的主方向，将当前激光的主方向对齐到激光坐标系的x轴；

第二装置，用于按照所述激光坐标系中激光测量值的不同，将所述当前激光帧l分为不同的子类，表示所述子类的个数；

第三装置，用于计算所述子类对应的平面描述向量；

第四装置，用于合并所述平面描述向量生成该当前激光帧的直方图f，其中，表示每个平面描述向量的长度：

；

第五装置，用于利用矩阵范数，对当前激光帧的直方图和激光地图中历史关键帧的直方图进行相似度测量，若测量得到的相似度大于预设阈值，则判定发生回环。

进一步的，上述设备中，所述第五装置，用于当距离小于设定的阈值时，则判定发生回环，其中，和表示直方图和的列向量,即激光帧l每个子类的描述向量：

。

进一步的，上述设备中，所述第一装置，用于获取机器人的激光雷达的当前激光帧l，并且判定当前激光帧l是否为关键帧，若是关键帧，确定激光雷达的当前激光帧l的主方向，将当前激光帧l的主方向对齐到激光坐标系的轴，然后结束所述第一装置的执行跳转到所述第二装置执行；若不是关键帧，重复执行所述第一装置。

进一步的，上述设备中，所述第一装置，包括

第一一装置，用于获取当前激光帧l，根据机器人的当前时刻和上一个时刻之间的里程距离判断当前激光帧是否为关键帧，若是关键帧，跳转到第一二装置；否则，重复本第一一装置的执行；

第一二装置，用于为当前激光帧l分配一个方向描述向量[0,0,0,0]和直线穿过当前激光帧的原点；

第一三装置，用于计算当前激光帧l中每个激光束的欧式空间坐标，计算公式如下：

，

其中，表示当前激光帧l中第个激光束的测量值，表示当前激光帧l中第个观测值相对于激光坐标系的角度值；

第一四装置，用于计算当前激光帧l每个激光束最近的直线，更新所述方向描述向量，其中，计算过程如下所示：

，

；

第一五装置，用于根据所述方向描述向量d中的最大值确定当前激光帧l的主方向，根据激光的主方向旋转当前激光帧l到激光坐标系的x轴，然后结束本第一五装置第执行跳转到所述第二装置执行，其中，计算过程如下，和分别表示直线的斜率和截距：

，

；

；

。

进一步的，上述设备中，所述第二装置，用于将当前激光帧l表示为，其中，表示当前激光帧l中第个激光束的测量值，表示当前激光帧l中第个观测值相对于激光坐标系的角度值；对当前激光帧l中的每个激光束按照其测量值的大小归到不同的子类。

进一步的，上述设备中，第三装置，用于为所述子类初始化一个平面描述向量和分配一个网格状的结构体；对于所述子类的每个成员计算其在欧式坐标系中的位置:

；

计算每个点在中的网格，并将其存储在中；根据中的存储的点集合计算的协方差及其特征向量和特征值，其中，，

，

，

；

分配m=4条直线穿过的均值，根据上一步计算出的特征值和特征向量，计算所属的子类index：

，

，

其中，表示特征向量和直线之间的距离;

根据所述index，更新的值：

。

根据本发明的另一面，还提供一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行以实现上述任一项所述的方法。

根据本发明的另一面，还提供一种用于在网络设备端信息处理的设备，该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该设备执行上述任一项所述的方法。

本发明的各设备和存储介质实施例的详细内容，具体可参见各方法实施例的对应部分，在此，不再赘述。

与现有技术相比，本发明针对背景技术中所涉及的传统回环检测方法存在误匹配率高，计算量大，实时性差等缺陷，提供一种基于二维激光的回环检测方法，该方法直接利用二维激光实现回环检测。本发明相比于基于图像的回环检测方法的进步效果在于：本方法不仅计算量小，实时性高，而且在回环检测时具有旋转不变性。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

需要注意的是，本发明可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路（asic）、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中，本发明的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地，本发明的软件程序（包括相关的数据结构）可以被存储到计算机可读记录介质中，例如，ram存储器，磁或光驱动器或软磁盘及类似设备。另外，本发明的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。

另外，本发明的一部分可被应用为计算机程序产品，例如计算机程序指令，当其被计算机执行时，通过该计算机的操作，可以调用或提供根据本发明的方法和/或技术方案。而调用本发明的方法的程序指令，可能被存储在固定的或可移动的记录介质中，和/或通过广播或其他信号承载媒体中的数据流而被传输，和/或被存储在根据所述程序指令运行的计算机设备的工作存储器中。在此，根据本发明的一个实施例包括一个装置，该装置包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该装置运行基于前述根据本发明的多个实施例的方法和/或技术方案。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。