一种SLAM中重复子图检测方法

一种slam中重复子图检测方法
技术领域
1.本发明属于地图重复度检测技术领域，更具体地，本发明涉及一种lam中重复子图检测方法。


背景技术：

2.在工业无人仓储物流中，移动机器人应用越来越广泛，其最主要的定位导航方式就是激光slam定位导航。激光slam实时执行前端匹配，非实时执行后端优化，依靠历史建立的子图集，通过激光帧和各个子图匹配建立约束，通过迭代优化实现精准定位。如果机器人重复走一条路，就会在一个位置附近建立很多子图，并且子图非常相似。当计算机cpu计算性能较差时，激光帧节点和各个子图之间的约束不能及时计算，就会进入排队优化状态，造成任务累积，影响机器人正常定位。因此机器人在建图时走回环或重复路线，检测和删除重复子图的操作就十分必要，纯定位时的计算量就会减少，降低机器人定位时可能出现的延时，提高定位的稳定性。
3.cn108710886b，专利名称：一种基于sift算法的重复图像匹配方法，该方案是首先通过sift算法提取待匹配图像的匹配特征点及其位置信息，然后以匹配点位置信息相似度最大和投影变换的幅值最小为目标，运用最小二乘法求解投影变换参数，最后通过优化的最终投影变换参数，计算匹配点位置信息相似度，通过与阈值的比较判断图像匹配情况；但是该方案存在的问题具体如下：
4.sift特征适用于彩色图像的特征提取和匹配，由于栅格地图是灰度地图，相对于彩色图像信息比较单一，导致特征相似度较高，进而致使特征提取的效果很差，匹配失败率高。


技术实现要素：

5.本发明提供一种lam中重复子图检测方法，旨在改善上述问题。
6.本发明是这样实现的，一种slam中重复子图检测方法，所述方法具体包括如下步骤：
7.s1、将在当前子图重叠距离阈值内的其他子图放入子图集中，并统计子图集中的子图总数目q；
8.s2、检测当前子图总数目q是否小于等于设置的最小保留重叠子图数目overlap_min，若检测结果为是，则不计算当前子图的重叠子图，执行步骤s1，若检测结果为否，则检测当前的重叠子图，对不是当前子图的重叠子图进行删除。
9.进一步的，当最小保留子图数目overlap_min＝1时，重叠子图的检测方法具体如下：
10.确定两个子图间的位置关系，内含、相交或相离，
11.若两者的位置关系是内含时，则被包含的子图为重叠子图；
12.若两者的位置关系为相离时，则不存在重叠子图；
13.若两者的位置关系为相交时，则计算两个子图的重叠度，重叠度大于重叠度阈值的子图则为重叠子图。
14.进一步的，当最小保留子图数目overlap_min≥2，且全局有k个子图时，重叠子图的检测方法具体如下：
15.获取k个子图的不重合占据点面积，将不重合占据点面积area_n小于预设的不重合占据点面积阈值area_k认定为重叠子图。
16.进一步的，子图i的不重合占据点面积area_i获取方法具体如下：
17.提取子图i的占据点云；
18.获取子图i的占据点云在其它子图的投影点，即子图i的重叠占据点；
19.确定子图i的不重叠占据点，并计算子图i的不重合占据点面积areai。
20.进一步的，子图i的占据点云occ_cloudi的获取方法具体如下：
21.读取子图i栅格的占据概率值mi(grid_x，grid_y)＝occ_value，如果占据概率值occ_value大于预设的占据阈值，认为该点是占据点，将占据点存入占据点云occ_cloudi，计算占据点云occ_cloudi中各占据点栅格中心的世界坐标p(px，py)，将占据点的世界坐标存入占据矩阵m
x
(i，i)的元素中。
22.进一步的，子图i的重叠占据点获取方法具体如下：
23.将占据点云occ_cloudi中的所有占据点p(px，py)投影到子图1，计算占据点在子图1中的栅格坐标(gr_x，gr_y)，判断占据点是否投影到了子图1的占据栅格中，若判断结果为是，则将其保存到m
x
(i，1)中的占据点云中；
24.基于上述方法再将子图i的剩余占据点依次投影到子图2至子图k中，获取保存到m
x
(i，2)～m
x
(i，k)的占据点云。
25.进一步的，子图的i不重合占据点面积areai计算公式具体如下：
26.area_i＝m
x
(i，k+1)*resolution227.m
x
(i，k+1)＝m_ii-m_i1-m_i2....-m_ik
28.其中，m_ii为m
x
(i，i)中存储的占据点云数，m_i1为m
x
(i，1)中存储的占据点云数，m_i2为m
x
(i，2)中存储的点云数，m_ik为m
x
(i，k)中存储的点云数，resolution为占据栅格地图的分辨率。
29.进一步的，占据点是否投影到子图1的占据栅格的判断方法具体如下：
30.占据点的栅格坐标为(gr_x，gr_y)，检测gr_x是否大于0并且小于1_min_x，且gr_y是否大于0并且小于1_miny，如果判断结果为是，则该栅格(gr_x，gr_y)位于子图1中；
31.获取栅格(gr_x，gr_y)在子图1中的占据概率值m1(gr_x，gr_y)，若占据概率值m1(gr_x，gr_y)大于占据阈值，判定该占据点投影到了子图1的占据栅格中。
32.进一步的，子图的重叠度ov计算公式具体如下：
33.s3＝overlap_width*overlap*length
34.ov＝s3/s；
35.overlap_width*overlap*length分别表示两个子图交叉区域的长度ovarlap_length和宽度overlap_width，s表示该子图的有效栅格地图面积。
36.本发明提出了一种针对栅格地图的slam建图过程中的重复子图检测方法，进而提高了重复子图的检测精度，减少了建图和定位过程中的重复约束，进而提高了建图和定位
的效率。
附图说明
37.图1为本发明实施例提供的lam中重复子图检测方法流程图；
38.图2为本发明实施例提供的当前子图中的子图集形成过程示意图。
具体实施方式
39.下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
40.图1为本发明实施例提供的lam中重复子图检测方法流程图，该方法具体包括如下步骤：
41.s1、将在当前子图重叠距离阈值内的其他子图放入子图集中，并统计子图集中的子图总数目q；
42.设置子图重叠距离阈值overlap_range，以当前子图的坐标原点为中心，检测原点落入当前子图的子图重叠距离阈值overlap_range内的其他子图，并放入子图集中，最后统计子图集中的子图总数目q，如图2所示，当前子图为子图4，子图2及子图3的坐标原点位于子图4的子图重叠距离阈值overlap_range内。
43.雷达扫描的连续若干帧即构成一个子图，以90帧为例进行说明，第1帧～第90帧构成子图1，第91帧～第180帧构成子图2，依次类推。
44.s2、检测当子图总数目q是否小于等于设置的最小保留重叠子图数目overlap_min，若检测结果为是，即q≤overlap_min，则不计算当前子图的重叠子图，执行步骤s1，若检测结果为否，即q＞overlap_min，则检测重叠子图，删除重叠子图。
45.当设定的最小保留子图数目overlap_min＝1时，构建二维矩阵ms，矩阵ms的元素表示如下：
46.ms(a，b)＝(s1，s2，s3，ov1，ov2)
47.其中，s1为子图a的有效栅格地图面积，s2为子图b的有效栅格地图的面积，s3为当前两子图的重叠面积，ov1、ov2分别表示子图a、子图b的重复度，子图a和子图b分别表示当前子图、位于当前子图的子图重叠距离阈值内的其他子图。
48.子图a的有效栅格地图的最小坐标为[a_min_x，a_min_y]，最大坐标为[a_max_x，a_max_y]；子图b的有效栅格地图最小坐标为[b_min_x，b_min_y]，最大坐标为[b_max_x，b_max_y]，a子图的有效面积s1的计算公式具体如下：
[0049]
s1＝(a_max_x-a_min_x)*(a_max_y-a_min_y)；
[0050]
同理，b子图的有效面积s2的计算公式具体如下：
[0051]
s2＝(b_max_x-b_min_x)*(b_max_y-b_min_y)。
[0052]
检测子图a与子图b间的位置关系，并基于两者的位置关系来确定两子图间的重叠度，其过程具体如下：
[0053]
(1)若min(a_max_x，b_max_x)≥max(a_min_x，b_min_x)，或者是min(a_max_y，b_max_y)≥max(a_min_y，b_min_y)时，则子图a与子图b相离，两者不重合；
[0054]
(2)若a_max_x≥b_max_x，且a_max_y≥b_max_y，同时a_min_x≤b_min_x，且a_min_y≤b_min_y，则a子图完全包含b子图，子图b即为可删除的重复子图。反之，b子图包含a子图，子图a为可删除的不是当前子图的重复子图；
[0055]
(3)在不满足上述(1)和(2)时，其余是两个子图交叉，计算得到两个子图交叉区域的长度ovarlap_length和宽度overlap_width：
[0056]
ovarlap_length＝min(a_max_x，b_max_x)-max(a_min_x，b_min_x)；
[0057]
overlap_width＝min(a_max_y，b_max_y)-max(a_min_y，b_min_y)；
[0058]
其中，min和max表示选择内部元素的较小和较大值。此时，计算子图的重叠面积s3和子图重叠度ov1、ov2，其计算公式具体如下：
[0059]
s3＝overlap_width*overlap*length
[0060]
ov1＝s3/s1，ov2＝s3/s2
[0061]
遍历二维矩阵ms，如果重叠度ov1大于预设阈值ov，认为子图a重叠度高，遍历矩阵ms元素，如果重叠度ov2大于预设阈值ov，认为子图b重叠度高，可以一次性将所有重复子图找出并删除不是当前子图的重叠子图。
[0062]
当最小保留子图数目overlap_min≥2时，当全局有k个子图时，分别获取k个子图的不重合占据点面积，其获取方法具体如下：
[0063]
1)提取各子图的占据点云；
[0064]
读取子图i栅格的占据概率值mi(grid_x，grid_y)＝occ_value，如果占据概率值occ_value大于预设的占据阈值，认为该点是占据点，将占据点存入占据点云occ_cloudi，计算占据点云occ_cloudi中各占据点栅格中心的世界坐标p(px，py)，其计算公式具体如下：
[0065]
px＝max_x-resolution*(grid_x+0.5)；
[0066]
py＝max_y-resolution*(grid_y+0.5)；
[0067]
其中，resolution表示占据栅格地图的分辨率，(max_x，max_y)表示栅格坐标(i_max_x，i_max_y)在世界坐标系下的投影坐标，(i_max_x，i_max_y)为子图i有效栅格地图的最大坐标，将占据点的世界坐标存入占据矩阵m
x
(i，i)的元素中，
[0068]
依次遍历所有的子图，形成各子图的占据点云，将占据点数存入对应的占据矩阵中。
[0069]
2)依次获取子图i在其它子图的投影点；
[0070]
将m
x
(i，i)中的占据点云投影到子图1中，将占据点p(px，py)投影到子图1，计算占据点在子图中1的栅格坐标(gr_x，gr_y)，其计算公式具体如下：
[0071]
gr_x＝(max_x-px)/resolution；
[0072]
gr_y＝(max_y-py)/resolution；
[0073]
子图1的有效栅格坐标的最大值为1_min_x、1_min_y，判断gr_x是否大于0并且小于1_min_x，且gr_y是否大于0并且小于1_min_y，如果判断结果为是，则该栅格(gr_x，gr_y)位于子图1中，再获取对应栅格(gr_x，gr_y)在子图1中的占据概率值m1(gr_x，gr_y)，如果占据概率值m1(gr_x，gr_y)大于占据阈值，判定该占据点投影到了子图1的占据栅格中，将其保存到m
x
(i，1)中的占据点云中，基于上述方法再将子图i的剩余点依次投影到子图2至子图k中，获取m
x
(i，2)～m
x
(i，k)中的占据点云。
[0074]
3)计算子图i不重叠占据点面积；
[0075]
记m
x
(i，i)中存储的占据点云数是m_ii，m
x
(i，1)中存储的占据点云数是m_i1，m
x
(i，2)中存储的点云数是m_i2，同理，m
x
(i，k)中存储的点云数是m_ik，计算子图i的不重叠点数m
x
(i，k+1)：
[0076]mx
(i，k+1)＝m_ii-m_i1-m_i2....-m_ik；
[0077]
则子图i的不重合占据点面积area_i计算公式具体如下：
[0078]
area_i＝m
x
(i，k+1)*resolution2。
[0079]
基于上述方法依次获取子图1至子图k的不重合占据点面积area_1～area_k，
[0080]
4)提取重复子图；
[0081]
对k个不重合占据点面积按照时间先后进行排序，排序最靠前的不重合占据点面积对应的子图n为子图k，为最新子图，该子图需要用于实时匹配和更新，不能删除，当n不等于k时，且不重合占据点面积area_n小于预设的不重合占据点面积阈值area_k，即认定为该子图为可删除的重复子图。
[0082]
本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。