一种基于点云语义图描述符和位置信息的回环检测方法

1.本发明涉及地图构建技术领域，尤其是涉及一种基于点云语义图描述符和位置信息的回环检测方法。


背景技术：

2.随着计算机水平和传感器技术的不断进步，移动机器人也在飞速发展，定位是移动机器人技术体系中的重要部分，其精度和实时性直接决定了整个移动机器人的运行。
3.同时定位与建图(simultaneous localization and mapping，slam)是移动机器人定位的关键技术，激光slam利用三维激光雷达和机器人的自身状态来完成移动机器人的定位。回环检测目的是找到机器人历史经过的地方，在slam系统中加入回环检测模块是为了降低slam系统的累计误差，它是slam中的关键模块。
4.目前大多数的slam中使用的回环检测方法是通过判断当前帧和历史帧之间的位姿距离是否在阈值范围内来检测回环，该类方法非常耗时，需要对所有符合距离条件的点云数据进行匹配并判断回环，回环检测效率低，从而导致移动机器人定位精度降低，甚至会导致试验失败。
5.中国专利申请号cn202211042843.9提供了一种基于点云语义激光slam回环检测方法，s1，任意两帧点云扫描数据，记为第一帧，第二帧；s2，将所述第一帧，第二帧输入到语义分割网络中，进行点云语义分割，并对其中语义块添加对应标签；s3，将所述第一帧，第二帧分别按照点的水平扫描距离分成m数量份，并分别使用第一帧语义向量、第二帧语义向量分别描述所述语义信息；s4，获取所述第一帧语义向量、第二帧语义向量1-范数差小于一预设值的帧，作为回环候选帧；s5，将所述回环候选帧作为参数输入配准算法中，用于构建回环约束。
6.上述申请通过获取点云数据的语义信息并进行等分，使用范数筛选回环候选帧，以减少对于两帧图像中的计算量。但是，上述申请使用范数进行筛选会丢失三维空间中的相对位置信息，导致回环匹配正确率不理想。


技术实现要素：

7.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于点云语义图描述符和位置信息的回环检测方法，以提高回环匹配成功率的同时，降低计算量。
8.本发明的目的可以通过以下技术方案来实现：
9.本发明提供了一种基于点云语义图描述符和位置信息的回环检测方法，包括如下步骤：
10.获取当前帧的激光点云数据和卫星定位数据，针对所述激光点云数据进行语义分割处理，并转化为预设的点云语义图描述符形式，通过距离匹配和节点匹配，从历史帧数据中筛选得到回环候选帧；
11.将所述当前帧和所述回环候选帧转换为圆环图形式，通过相似度验证和几何一致
性验证获取目标回环帧。
12.作为优选的技术方案，针对所述激光点云数据进行语义分割处理的过程包括如下步骤：
13.针对预处理后的激光点云数据进行语义分割，提取环境中的物体，使用无向图构建点云语义描述信息，在所述无向图中，使用节点表示物体的位置，使用边表示物体间的距离。
14.作为优选的技术方案，所述的环境中的物体包括车辆、树干和杆。
15.作为优选的技术方案，所述的距离匹配具体包括如下步骤：
16.基于当前帧的卫星定位数据，筛选出历史帧中定位位置与当前帧的定位位置间距大于/小于预设阈值的历史帧。
17.作为优选的技术方案，所述的节点匹配具体包括如下步骤：
18.针对经过距离匹配筛选得到的历史帧，基于当前帧点云语义图描述符形式的激光点云数据，筛选出历史帧中环境物体数量与当前帧中环境物体数量接近的历史帧，作为所述回环候选帧。
19.作为优选的技术方案，针对所述当前帧和所述回环候选帧中的任一帧，圆环图的生成过程包括如下步骤：
20.构建多个同心圆环，所述同心圆环的数量与语义分割得到的无向图中边的种类数量相同；
21.以同一条直线作为起点，针对各个同心圆环，分割为多个均匀扇形，计算环境中同种边的欧氏距离并排序，根据欧氏距离的大小分配对应的扇形数量，实现圆环图的构建。
22.作为优选的技术方案，所述的相似度验证的过程包括如下步骤：
23.基于圆环图形式的当前帧和回环候选帧，计算当前帧和各个回环候选帧间的圆环相似度，判断回环候选帧的圆环相似度是否大于/小于预设阈值，若是则保留，若否则丢弃。
24.作为优选的技术方案，所述的几何一致性验证的过程包括如下步骤：
25.使用下式计算经相似度匹配筛选后的回环候选帧以及当前帧间的回环检测损失值，判断回环候选帧的回环检测损失值是否大于/小于预设阈值若是则作为所述目标回环帧，若否则丢弃，
[0026][0027]
其中，loss为所述回环检测损失值，p
l
为当前帧点云集合，ph为该回环候选帧点云集合，n表示当前数据帧点云中3d点的个数，t表示变换矩阵，p
il
和p
ih
分别表示当前帧和回环候选帧的第i个3d点。
[0028]
作为优选的技术方案，所述的回环检测方法还包括如下步骤：
[0029]
判断是否成功匹配到目标回环帧，若是，则变换矩阵t即为slam系统中的六自由度初始位姿，若否，将当前帧的卫星定位数据和点云语义图描述符形式的激光点云数据储存到预设的点云语义图容器中。
[0030]
作为优选的技术方案，通过三维激光雷达获取所述的激光点云数据，通过gnss天
线和gnss接收机获取所述的卫星定位数据。
[0031]
与现有技术相比，本发明具有以下优点：
[0032]
(1)回环匹配成功率高，计算量小：不同于现有的使用范数对候选回环帧进行筛选的方案，本发明首先获取当前帧的激光点云数据和卫星定位数据，并针对激光点云数据进行语义分割处理，通过距离匹配和节点匹配，从历史帧数据中筛选得到回环候选帧，完成第一阶段的匹配，此后，将当前帧和回环候选帧转换为圆环图形式，尝试通过相似度匹配和几何一致性匹配获取目标回环帧，实现第二阶段的匹配。本方法综合欧氏距离、节点数量、圆环图相似性和回环检测损失对历史帧进行有效筛选，充分利用了特征的高级约束，通过识别物体在三维空间中的相对位置，从宏观的角度来理解整个场景，本发明可以进行快速、高效、准确地进行回环检测，能够有效地提高slam系统的定位精度和系统的鲁棒性。以室外场景中常见稳定的物体作为语义分割的目标对象以构建云语义图描述符，提高了点云描述符的代表性。
[0033]
(2)兼容性高，成本低：本发明仅使用三维激光雷达和gnss系统即可实现回环检测功能，能够用于目前主流的slam系统中，设备成本低且操作简单易上手。
附图说明
[0034]
图1为实施例中基于点云语义图描述符和位置信息的回环检测方法的流程图；
[0035]
图2为点云语义图描述符示意图；
[0036]
图3为点云语义图描述符构建方法示意图；
[0037]
图4为圆环图的示意图；
[0038]
图5为圆环图构建方法的示意图。
具体实施方式
[0039]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
[0040]
实施例1
[0041]
为了解决上述现有技术的问题，本发明的目的在于克服已有技术存在的不足，提供一种基于点云语义图描述符的回环检测方法，同时在回环检测的过程中加入全球导航卫星系统(global navigation satellite system，gnss)得到的位置信息，用于解决移动机器人slam系统中回环检测难、回环匹配计算量大、回环匹配正确率低的问题，提高移动机器人的定位精度和系统鲁棒性。
[0042]
如图1所述，本实施例提供了一种基于点云语义图描述符和位置信息的回环检测方法，包括如下步骤：
[0043]
步骤s1，安装设备与采集三维激光点云和gnss数据：
[0044]
将velodyne vlp-16三维激光雷达、gnss天线、gnss接收机均安装在汽车的顶部，该三维激光扫描范围为100米，在30
°
的视角范围内有16条三维激光雷达，垂直方向的分辨率为2
°
，当扫描频率为10hz时，其水平分辨率为0.2
°
。可以使用实时动态载波相位差分技术
(real-time kinematic，rtk)来获得厘米级的位置信息，三维激光雷达和gnss接收机将采集到的点云数据和gnss-rtk数据传输到车载工控机中，车载工控机和自动驾驶汽车通过机器人操作系统(robot operating system)进行数据传输和指令发送，以此来获得三维激光点云数据和gnss-rtk数据的输入；
[0045]
步骤s2，构建点云语义图描述符容器：
[0046]
首先对采集到的三维点云数据进行预处理，去除点云的运动畸变，并对点云数据进行下采样，降低点云数据的规模；对于雷达点云数据帧转换得到的点语义图描述符，其生成的具体方法分为如下步骤：
[0047]
使用rangenet++对点云数据进行语义分割，提取环境中物体构建点云语义图描述符。在构建点云描述符时，为了避免目标对象随机性较大，导致点云描述符不具有代表性，在室外场景中选择常见稳定的物体作为语义分割的目标对象，假设选择的目标对象为：车辆、树干和杆。
[0048]
对于每一个点云数据候选帧，在使用rangenet++对其点云进行语义分割出目标对象后，使用一个无向图g＝＜v,e＞来构建点云描述符，节点v是无向图g中的目标对象的位置(使用目标对象的质心来表示其在空间中的位置)，e是节点v之间的边，边e
ij
＝＜vi,vj＞为vi,vj两个节点之间的欧式距离。
[0049]
在无向图g中，目标对象两两组合，可以得到3种顶点和6种边，v中有三种顶点，分别为：车辆、树干、杆；e中有六种边分别为：杆-杆、树干-树干、车辆-车辆、车辆-树干、树干-杆、杆-车辆。
[0050]
如图2所述为点云语义图描述符示意图，如图3所述为点云语义图描述符构建方法示意图，同时将该关键帧对应的描述符和位置信息以键值对的形式一同存储在点云语义图描述符容器中，容器使用c++中的vector容器存储，后续在进行回环匹配时，容器中的所有描述符被用来构造kd-tree，使用标准的k近邻搜索算法，快速搜索回环候选帧。
[0051]
步骤s3，距离匹配：
[0052]
当进行回环检测时，首先根据当前帧对应的gnss位置信息，在步骤s3得到的点云语义图描述符容器中搜索与当前帧点云描述符位置距离最近的历史关键帧，将满足距离条件的历史关键帧记为回环候选帧，距离阈值设置为0.25m，本发明设置得到10个回环候选帧；
[0053]
步骤s4，节点匹配：
[0054]
通过在所述步骤s3中得到的回环候选帧后，再对回环候选帧对应的点云语义图描述符中的三类目标对象节点数目进行初步匹配，对三类目标对象总和在预定阈值范围内的回环候选帧，将其列入候选对象；设当前的对象节点数为v，待匹配的回环候选帧的对象节点数目为v
total
，对三类目标对象总和在预定阈值范围λ内的回环候选帧，我们就将其列入候选对象，λ取15，v
total
应该满足下式：
[0055]vtotal
＝[v-15,v+15]
[0056]
步骤s5，回环的第一重验证—数据帧相似度匹配：
[0057]
通过在所述步骤s4中得到的候选对象后，将其转换成点云语义图描述符并同时生成圆环图，采用回环候选帧和当前帧的圆环图对两个数据帧进行相似度得分计算，从而判断回环候选帧和当前帧是否构成回环帧。
[0058]
如图5所述，圆环图生成的具体方法分为如下步骤：
[0059]
步骤s51-s52，圆环图中的圆环数量和无向图g中的e的种类数目相同，本文中的圆环图呈6圆环状，分别对应于e中的六种边类型。
[0060]
本发明只介绍e中6种边类型中的1种边类型—e
trunk-pole
，构造其对应的圆环图中一个圆环的过程，其他5种边类型的圆环生成方法均相同。
[0061]
步骤s53，假设点云语义图描述符中有m个e
trunk-pole
边，这m个e
trunk-pole
边组成一个边集合s
trunk-pole
，s
trunk-pole
中的边的欧式距离范围为：
[0062][0063]
上式中s
min
是s
trunk-pole
中边欧式距离最小值，s
max
是s
trunk-pole
中边欧式距离最大值。步骤s54，对s
trunk-pole
的每个边按照欧式距离进行升序排序，得到有序边集合s
trunk-pole
′
为：
[0064][0065]
步骤s55，将s
trunk-pole'
对应的圆环等分成ms
max
个扇形，每一个扇形对应的角度为则有序边集合s
trunk-pole'
中每个边对应的扇形个数ni为：
[0066][0067]
将s
trunk-pole'
中每条边对应的扇形个数依次排列在圆环中，使用1个扇形将各个边对应的扇形进行隔开，以水平右线为起始线，逆时针对每条边进行排列。
[0068]
这样就将点云描述符中s
trunk-pole'
类型的边用圆环图中的1个圆环进行表示，我们将s
trunk-pole'
对应的这个圆环记作c
trunk-pole
，则圆环c
trunk-pole
其在圆环图中的扇形集合为：
[0069]ctrunk-pole
＝{n0,n1...,n
m-1
}
[0070]
然后再将点云描述符中其余的5种类型边在圆环图中用圆环表示，就构成了一个完整的圆环图c
graph
，生成的圆环图如图4所述。
[0071]cgraph
＝(c
trunk-trunk
,...c
pole-vehicle
)
[0072]
步骤s56，这样一个点云语义图描述符对应的圆环图就是c
graph
，我们可以使用回环候选帧和当前帧的圆环图进行相似度匹配，用于快速筛选回环候选帧，从而得到回环帧。
[0073]
在进行圆环图匹配时，首先将回环候选帧和当前帧的圆环图按圆心和水平右线进行对齐，然后通过匹配圆环图中的每个圆环—不同欧式距离的边的相似性，圆环相似度匹配计算公式为：
[0074][0075]
上式中，表示当前帧对应的圆环图，表示某个回环候选帧对应的圆环图，m为圆环图中的圆环个数(即点云语义图描述符中边的种类)。
[0076]
根据阈值α来判断当前帧和回环候选帧之间是否可能存在回环：
[0077]
[0078]
如果相似度得分小于设定的阈值α，本实施例阈值α取0.85，则表示没有匹配到回环帧，将当前帧生成的描述符和其对应的gnss-rtk位置一起存储到点云语义图描述符容器中；反之，则认为当前帧和该回环候选帧之间可能存在回环，在完成回环的第一重验证后，后续还需进行回环的第二重验证—几何验证。
[0079]
步骤s6，回环的第二重验证—几何验证：
[0080]
通过在所述步骤s5中得到的当前帧和某几个回环候选帧之间可能存在回环后，还需要再进一步进行几何验证，该步骤的重点是为当前帧寻找几何上的一致性对应，从而确定回环帧，并得到六自由度初始位姿。
[0081]
本发明使用下式来评估回环候选帧和当前帧的环路回环检测的损失loss：
[0082][0083]
上式中，p
l
为当前帧点云集合，ph为该回环候选帧点云集合，n表示当前数据帧点云个数，t表示变换矩阵，p
il
和p
ih
分别表示当前帧和回环候选帧的第i个3d点。
[0084][0085]
如果某个回环候选帧的损失值loss大于阈值β，本实施例阈值β取0.3，则表示没有匹配到回环帧，将当前帧生成的描述符和其对应的gnss-rtk位置一起存储到点云语义图描述符容器中；反之，则判断这两个地方存在一个回环，当该回环被确认存在后，将变换矩阵t作为六自由度初始位姿。
[0086]
在得到回环帧之后，即可与当前帧构建回环约束因子，并加入到slam系统的全局因子图模型中，同时回环检测过程中得到的六自由度初始位姿有利于前端的点云匹配，能够有效降低slam系统的累计误差，实现移动机器人实时高精度定位。
[0087]
本发明与现有技术相比较，具有如下显而易见的突出实质性特点和显著优点：
[0088]
1.本发明解决了目前回环检测难、回环匹配计算量大、回环匹配正确率低的问题，相比于使用当前帧和历史帧之间的位姿距离是否在阈值范围内的传统方法，本发明充分利用了特征的高级约束，通过识别物体在三维空间中的相对位置，从宏观的角度来理解整个场景，本发明可以进行快速、高效、准确地进行回环检测，能够有效地提高slam系统的定位精度和系统的鲁棒性。
[0089]
2.本发明仅使用三维激光雷达和gnss系统即可实现回环检测功能，能够用于目前主流的slam系统中，设备成本低且操作简单易上手；
[0090]
3.本发明方法简单易行，成本低，适合推广使用。
[0091]
实施例2
[0092]
与实施例1相比，本实施例在步骤s2中，在去除点云运动畸变时，可以通过惯性测量单云(inertial measurement unit，imu)的观测信息，使用高频imu观测传播模型来恢复激光雷达的运动状态，能够更好地校正点云的运动畸变，具体步骤为：
[0093]
步骤1，安装设备与采集三维激光点云和gnss数据：
[0094]
本实施例使用自动驾驶汽车作为移动机器人的应用对象，将三维激光雷达和gnss
天线安装在汽车顶部，gnss接收机装载汽车后备箱中，三维激光雷达和gnss会在汽车行驶的过程中采集周围环境的点云数据并获得位置信息，将得到的三维点云数据和位置信息传输到计算机中并保存用于后续算法处理；
[0095]
步骤2，构建点云语义图描述符容器：
[0096]
首先对采集到的三维点云数据进行预处理，使用高频imu观测传播模型来恢复激光雷达的运动状态，去除点云的运动畸变，并对点云数据进行下采样，降低点云数据的规模；对于雷达点云数据帧转换得到的点语义图描述符，同时将该关键帧对应的描述符和位置信息以键值对的形式一同存储在点云语义图描述符容器中；
[0097]
步骤3，距离匹配：
[0098]
当进行回环检测时，首先根据当前帧对应的gnss位置信息，在步骤3得到的点云语义图描述符容器中搜索与当前帧点云描述符位置距离最近的历史关键帧，将满足距离条件的历史关键帧记为回环候选帧；
[0099]
步骤4，节点匹配：
[0100]
通过在所述步骤3中得到的回环候选帧后，再对回环候选帧对应的点云语义图描述符中的三类目标对象节点数目进行初步匹配，对三类目标对象总和在预定阈值范围内的回环候选帧，将其列入候选对象；
[0101]
步骤5，回环的第一重验证—数据帧相似度匹配：
[0102]
通过在所述步骤4中得到的候选对象后，将其转换成点云语义图描述符并同时生成圆环图，采用回环候选帧和当前帧的圆环图对两个数据帧进行相似度得分计算，从而判断回环候选帧和当前帧是否构成回环帧；
[0103]
步骤6，回环的第二重验证—几何验证：
[0104]
通过在所述步骤5中得到的当前帧和某几个回环候选帧之间可能存在回环后，还需要再进一步进行几何验证，该步骤的重点是为当前帧寻找几何上的一致性对应，从而确定回环帧，并得到六自由度初始位姿。
[0105]
实施例3
[0106]
本实施例提供了一种电子设备，包括：一个或多个处理器以及存储器，所述存储器内储存有一个或多个程序，所述一个或多个程序包括用于执行如实施例1或实施例2任一所述基于点云语义图描述符和位置信息的回环检测方法的指令。
[0107]
实施例4
[0108]
本实施例提供了一种计算机可读存储介质，包括供电子设备的一个或多个处理器执行的一个或多个程序，所述一个或多个程序包括用于执行如实施例1或实施例2任一所述基于点云语义图描述符和位置信息的回环检测方法的指令。
[0109]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

技术特征：
1.一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，包括如下步骤：获取当前帧的激光点云数据和卫星定位数据，针对所述激光点云数据进行语义分割处理，并转化为预设的点云语义图描述符形式，通过距离匹配和节点匹配，从历史帧数据中筛选得到回环候选帧；将所述当前帧和所述回环候选帧转换为圆环图形式，通过相似度验证和几何一致性验证获取目标回环帧。2.根据权利要求1所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，针对所述激光点云数据进行语义分割处理的过程包括如下步骤：针对预处理后的激光点云数据进行语义分割，提取环境中的物体，使用无向图构建点云语义描述信息，在所述无向图中，使用节点表示物体的位置，使用边表示物体间的距离。3.根据权利要求2所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，所述的环境中的物体包括车辆、树干和杆。4.根据权利要求1所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，所述的距离匹配具体包括如下步骤：基于当前帧的卫星定位数据，筛选出历史帧中定位位置与当前帧的定位位置间距大于/小于预设阈值的历史帧。5.根据权利要求1所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，所述的节点匹配具体包括如下步骤：针对经过距离匹配筛选得到的历史帧，基于当前帧点云语义图描述符形式的激光点云数据，筛选出历史帧中环境物体数量与当前帧中环境物体数量接近的历史帧，作为所述回环候选帧。6.根据权利要求1所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，针对所述当前帧和所述回环候选帧中的任一帧，圆环图的生成过程包括如下步骤：构建多个同心圆环，所述同心圆环的数量与语义分割得到的无向图中边的种类数量相同；以同一条直线作为起点，针对各个同心圆环，分割为多个均匀扇形，计算环境中同种边的欧氏距离并排序，根据欧氏距离的大小分配对应的扇形数量，实现圆环图的构建。7.根据权利要求1所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，所述的相似度验证的过程包括如下步骤：基于圆环图形式的当前帧和回环候选帧，计算当前帧和各个回环候选帧间的圆环相似度，判断回环候选帧的圆环相似度是否大于/小于预设阈值，若是则保留，若否则丢弃。8.根据权利要求1所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，所述的几何一致性验证的过程包括如下步骤：使用下式计算经相似度匹配筛选后的回环候选帧以及当前帧间的回环检测损失值，判断回环候选帧的回环检测损失值是否大于/小于预设阈值若是则作为所述目标回环帧，若否则丢弃，
其中，loss为所述回环检测损失值，p
l
为当前帧点云集合，p
h
为该回环候选帧点云集合，n表示当前数据帧点云中3d点的个数，t表示变换矩阵，p
il
和p
ih
分别表示当前帧和回环候选帧的第i个3d点。9.根据权利要求8所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，所述的回环检测方法还包括如下步骤：判断是否成功匹配到目标回环帧，若是，则变换矩阵t即为slam系统中的六自由度初始位姿，若否，将当前帧的卫星定位数据和点云语义图描述符形式的激光点云数据储存到预设的点云语义图容器中。10.根据权利要求1所述的一种基于点云语义图描述符和位置信息的回环检测方法，其特征在于，通过三维激光雷达获取所述的激光点云数据，通过gnss天线和gnss接收机获取所述的卫星定位数据。

技术总结
本发明涉及一种基于点云语义图描述符和位置信息的回环检测方法，包括如下步骤：获取当前帧的激光点云数据和卫星定位数据，针对所述激光点云数据进行语义分割处理，转化为预设的点云语义图描述符形式，通过距离匹配和节点匹配，从历史帧数据中筛选得到回环候选帧；将所述当前帧和所述回环候选帧转换为圆环图形式，尝试通过相似度验证和几何一致性验证获取目标回环帧。与现有技术相比，本发明解决了移动机器人SLAM系统中回环检测难、回环匹配计算量大、回环匹配正确率低的问题，提高移动机器人的定位精度和系统鲁棒性。人的定位精度和系统鲁棒性。人的定位精度和系统鲁棒性。


技术研发人员：田应仲 刘峰 杨晓东 倪雨嘉 王洁羽 李龙 金滔 林杨乔 张泉
受保护的技术使用者：上海大学
技术研发日：2023.04.27
技术公布日：2023/8/1