一种检测2D激光定位丢失的方法及系统、存储介质与流程

一种检测2d激光定位丢失的方法及系统、存储介质
技术领域
1.本发明涉及激光定位技术，尤其涉及一种检测2d激光定位丢失的方法及系统、存储介质。


背景技术：

2.工业机器人在自动化生产中起着重要作用，准确的定位技术对于机器人的精确操作和高效生产至关重要。目前，室内环境通常采用2d单线激光进行建图、定位和导航。尽管基于单线激光的建图技术已成熟，但在特殊复杂环境下，如大范围环境变化、人流量较多、相似障碍物增多等环境条件下，2d激光定位技术容易出现定位丢失的风险。
3.而工业环境中一旦发生定位丢失，会带来严重问题。首先，产品损坏是一个重要风险，机器人定位丢失可能导致错位或姿态不正确，从而无法精确到达指定位置。其次，定位丢失会使机器人运动路径偏离预期，需要额外时间和操作来重新调整定位，导致生产效率下降，延长生产周期，甚至引发生产线停机等中断问题。
4.此外，定位丢失还存在安全事故的风险，机器人无法准确识别工作区域边界和障碍物，增加了碰撞、意外接触或夹伤的风险，可能导致人身伤害、设备损坏或其他安全事故，会降低用户对机器人系统的信心，影响产品市场竞争力和企业声誉。
5.因此，通过何种手段来准确、可靠地检测出工业机器人2d激光定位是否丢失，是本领域技术人员亟待解决的问题。


技术实现要素：

6.为此本发明的主要目的在于提供一种检测2d激光定位丢失的方法及系统、存储介质，以供在复杂环境下，准确的判断出2d激光定位是否发生定位丢失。
7.为了实现上述目的，根据本发明的第一个方面，提供了一种检测2d激光定位丢失的方法，步骤包括：
8.步骤s100将地图点进行高斯分布，生成似然高斯概率栅格地图，计算各激光点所处栅格与其最近地图点的匹配程度g，并计算匹配程度均值g；
9.步骤s200当判断存在激光穿透地图时，记录各穿透点位置处的地图点及激光点，以计算两者间的距离比例得分l；
10.步骤s300统计栅格地图整体穿透比例n，计算所有激光点与地图点的平均距离m；
11.步骤s400当检测到匹配程度均值g、距离比例得分l、穿透比例n满足丢失阈值判断条件时给出定位丢失判断结果，否则再判断穿透比例n、平均距离m是否超出反比阈值判断条件，以得出定位丢失判断结果。
12.在可能的优选实施方式中，其中步骤s100中计算匹配程度均值g的步骤包括：
13.步骤s110计算
14.；
15.其中为当前激光点所处栅格与其最近地图点的距离；
16.步骤s120将所有计算的匹配程度g相加，并除以激光点所处栅格总数，获取匹配程度均值g。
17.在可能的优选实施方式中，其中步骤s200中判断激光是否穿透地图的步骤包括：
18.步骤s210将地图坐标系按照斜率等分为数个区域，以当前所处位置为原点，各激光点为终点，采用bresenham划线算法构建激光射线；
19.步骤s220遍历各区域内激光射线上所有的纵、横坐标位置，判断是否存在与地图点重合的坐标，如存在则视为穿透地图。
20.在可能的优选实施方式中，其中步骤s220中遍历各区域内激光射线上所有的纵、横坐标位置的步骤包括：
21.步骤s221判断当前激光点与原点坐标之间的斜率，以确定激光射线所处地图坐标系的区域；
22.步骤s222根据激光射线所处地图坐标系的区域，进行坐标转换以变换到主区域中；
23.步骤s223设置比较表达式的增量及比较表达式的初始值，循环遍历激光射线上每一个横坐标，求出对应的纵的坐标。
24.在可能的优选实施方式中，其中计算穿透点位置处的地图点及激光点的距离比例得分l的步骤包括：
25.步骤s230设穿透点位置处的地图点坐标为（、），对应激光点的坐标为（），计算两者之间的距离
26.；
27.步骤s340计算所有穿透点位置的dist之和，并将其除以穿透点的总数，得到距离比例得分l
[0028] 。
[0029]
在可能的优选实施方式中，其中步骤s400中所述丢失阈值判断条件包括：
[0030]
当匹配程度均值g》0.7时，判断穿透比例n》0.25，距离比例得分l》0.3；
[0031]
当匹配程度均值g《=0.7时，判断穿透比例n》0.18，距离比例得分l》0.2；
[0032]
符合任一条件时判定当前定位丢失。
[0033]
在可能的优选实施方式中，其中步骤s400中所述反比阈值判断条件包括：
[0034]
当穿透比例n＜0.17，而平均距离m＞0.3时，判定当前定位丢失。
[0035]
在可能的优选实施方式中，所述的检测2d激光定位丢失的方法中，步骤还包括：
[0036]
步骤s500当判断激光信息和定位信息之间的时间戳不同步时，停止定位丢失判断。
[0037]
为了实现上述目的，对应上述方法，根据本发明的第二个方面，还提供了一种检测2d激光定位丢失的系统，其包括：
[0038]
存储单元，用于存储包括如上任一所述检测2d激光定位丢失的方法步骤的程序，以供处理单元，导航单元适时调取执行；
[0039]
导航单元，用于将地图点进行高斯分布，生成似然高斯概率栅格地图，并获取激光
雷达扫描帧，确定激光点在栅格地图上的位置，及当前所处定位位置；
[0040]
处理单元，用于计算各激光点所处栅格与其最近地图点的匹配程度g，并计算匹配程度均值g；并当判断存在激光穿透地图时，记录各穿透点位置处的地图点及激光点，以计算两者间的距离比例得分l；统计栅格地图整体穿透比例n，计算所有激光点与地图点的平均距离m；之后当检测到匹配程度均值g、距离比例得分l、穿透比例n满足丢失阈值判断条件时给出定位丢失判断结果，否则再判断穿透比例n、平均距离m是否超出反比阈值判断条件，以得出定位丢失判断结果。
[0041]
为了实现上述目的，对应上述方法，根据本发明的第三个方面，还提供了一种计算机可读存储介质，其上存储有计算机程序，其中所述计算机程序被处理器执行时，实现如上任一项所述检测2d激光定位丢失的方法的步骤。
[0042]
通过本发明提供的该检测2d激光定位丢失的方法及系统、存储介质，能够巧妙的计算出激光和地图的匹配程度，并判断是否存在激光穿透地图的现象，同时综合该些特征对应的数值来综合评价当前定位是否丢失，从而可准确、可靠地实时检测出工业机器人2d激光定位是否存在丢失问题，籍此降低移动机器人的安全事故风险，提升用户对机器人自动化生产的信心。
附图说明
[0043]
构成本技术的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
[0044]
图1为本发明的检测2d激光定位丢失方法的步骤示意图；
[0045]
图2为定位正常状态下激光扫描帧与地图的对应示意图；
[0046]
图3为本发明的检测2d激光定位丢失方法中，在判断激光是否穿透地图时，为降低计算资源，将坐标系等分为8个区域的构思示意图；
[0047]
图4为本发明的检测2d激光定位丢失方法中，在判断激光是否穿透地图时，在主区域计算激光射线上，所有纵、横坐标位置的示意图；
[0048]
图5为本发明的检测2d激光定位丢失方法中，判断激光是否穿透地图时，根据激光射线上是否存在与地图重合的位置，以判断穿透的示意图；
[0049]
图6为本发明的检测2d激光定位丢失方法中，判断定位是否丢失的逻辑示意图；
[0050]
图7为本发明的检测2d激光定位丢失方法中，示例满足反比阈值判断条件的示意图；
[0051]
图8为本发明的检测2d激光定位丢失系统的结构示意图。
具体实施方式
[0052]
为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的具体技术方案进行清楚、完整地描述，以助于本领域的技术人员进一步理解本发明。显然，本案所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思及相互不冲突的前提下，本技术中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的
揭露及保护范围。
[0053]
此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“s100”、“s200”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。除非另有明确的规定和限定，术语“设置”、“布设”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本案中的具体含义。
[0054]
为了准确判断机器人定位是否丢失，本发明构思上综合考虑了几个指标。首先，根据当前激光和地图的匹配程度，可判断定位的准确性。即较高的匹配程度表示定位良好，较低的匹配程度可能意味着定位丢失。
[0055]
其次，通过检查激光是否穿透原始构建的地图来判断定位是否丢失。如果激光大部分穿透地图或者每个激光点到其最近地图的距离较大，可能表明定位丢失的可能性较高。
[0056]
此外，考虑到由于导航地图是高斯似然地图，存在定位丢失但激光仍保持在地图附近但未穿透地图的情况。为此在这种情况下，发明人考虑可以计算整体激光点到地图的平均距离，这样较小的平均距离可能意味着定位较好，而较大的平均距离可能暗示定位丢失。
[0057]
因此基于上述发明构思，本发明提出了一种检测2d激光定位丢失的方法，以供在复杂环境下，准确的判断出2d激光定位是否发生定位丢失，其中如图1至图6所示，该方法步骤包括：
[0058]
步骤s100将地图点进行高斯分布，生成似然高斯概率栅格地图，计算各激光点所处栅格与其最近地图点的匹配程度g，并计算匹配程度均值g。
[0059]
具体的，为了提高判断计算效率，可以先对栅格地图上激光点可能扫描到的栅格进行匹配程度g分值的计算。如构建高斯概率珊格地图，把接收到的地图点划分为离散的网格单元，并在每个网格单元中，存储有关该位置的信息，之后当激光点落在该网格中时，便可对应的知晓其匹配程度g及当前网格到其最近地图点的距离。
[0060]
其示例构建步骤为：首先确定珊格地图的大小和每个网格的尺寸，创建空白的珊格地图，并初始化网格中的值。然后根据接收到的原始地图点，在相对应的珊格位置设置初始匹配程度分值为1。最后，以每一个地图点栅格为中心，检查其周围的4个栅格与地图点栅格的距离。如果距离小于阈值，则计算膨胀出的4个栅格所在位置的匹配程度g。然后，以每个新栅格为中心，再次膨胀出4个栅格，并计算这些栅格到地图栅格的距离。如果距离大于阈值，则该栅格退出循环；如果距离小于阈值，则进行循环迭代，计算每个栅格的匹配程度g。
[0061]
其中该匹配程度g的计算步骤包括：
[0062]
步骤s110计算
[0063]
；
[0064]
其中为当前激光点所处栅格与其最近地图点的距离；籍此通过上述步骤，可以知晓每个地图点附近的每个网格都有不同的得分值，可以更加全面的分析当前激光和地图的匹配得分。
[0065]
之后每当激光扫描一帧后，便可计算匹配程度均值g，其步骤包括：
[0066]
步骤s120将当前激光扫描帧中所有计算的激光点所落栅格与地图点的匹配程度g相加，并除以激光点所处栅格总数，便可获取匹配程度均值g。
[0067]
步骤s200当判断存在激光穿透地图时，记录各穿透点位置处的地图点及激光点，以计算两者间的距离比例得分l。
[0068]
其中，本示例中该步骤s200中判断激光是否穿透地图的示例步骤包括：
[0069]
步骤s210将地图坐标系按照斜率等分为数个区域，以当前所处位置为原点，各激光点为终点，采用bresenham划线算法构建激光射线。
[0070]
步骤s220遍历各区域内激光射线上所有的纵、横坐标位置，判断是否存在与地图点重合的坐标，如存在则视为穿透地图。
[0071]
具体来说，请参阅图3至图5所示，为判断当前激光点是否穿透过地图，本示例优选采用bresenham划线算法的思想来模拟激光射线，其中由于需要计算大量的激光点，所以必须要保证计算效率，为此，本示例中优选只使用加减法和比较操作来实现直线绘制，从而大大降低了所需的计算资源。如图3所示，其整体思路是将坐标系按照斜率等分成8个部分，分别计算每个区域内的激光射线上的纵横坐标位置。
[0072]
例如，设机器人原点坐标为（），终点激光点的位置坐标为（）,以机器人原点为起始点，激光点的位置为终点，绘制出一条直线。其思想是遍历所有的横坐标，根据直线方程找出每个横坐标对应的纵坐标（如图4所示，其中每个格子代表一个像素）。
[0073]
由图4可知，对应的纵坐标为，使用四舍五入将纵坐标值取整。以为例如果 则等价于，如果 则等价于。将两边同时乘以2，变成,其中等于，等式变成求解，（，），因此可以将替换成，作为表达式的初始值，只需判断与0的比较表达式，即可求出相对应的y值，此时， 均为整数，完全避免了浮点类型的复杂计算。
[0074]
同时，根据图3可知，在区域1（主区域）中，斜率slope是大于0小于1的，所以相邻的横坐标对应的纵坐标要么保持不变，要么最多加1。由此可以推断出，如果满足，则说明当前的是由上一时刻的东方衍生出来的，其值保持不变。如果不满足改条件，则说明当前时刻的值是由上一时刻的东北方衍生出来的，其值直接加1。
[0075]
接着考虑对应的纵坐标是，如果前一个像素是来自东北方衍生，则需要比较（因为在处，其y值范围是到，其分水岭是1.5），等价于，也等价于，其增量为。
[0076]
如果前一个像素是来自东北方衍生，则需要比较，等价于也等价于,其增量为。
[0077]
同理可以一直推导到线段的末尾，找出规律，如果前一个位置是由东方衍生出来，则增量为，如果前一个位置是由东北方衍生出来，则增量为：。
[0078]
基于该构思，实际上可以发现，不同区域的激光射线，都可以转换到主区域（区域1）中，再通过上述方案进行计算，从而避免浮点类型的复杂计算。
[0079]
为此，本示例的步骤s220中，遍历各区域内激光射线上所有的纵、横坐标位置的优选步骤包括：
[0080]
步骤s221判断当前激光点与原点坐标之间的斜率，以确定激光射线所处地图坐标系的区域。
[0081]
步骤s222根据激光射线所处地图坐标系的区域，进行坐标转换以变换到主区域中。
[0082]
步骤s223设置比较表达式的增量及比较表达式的初始值，循环遍历激光射线上每一个横坐标，求出对应的纵的坐标。
[0083]
举例来说，首先判断当前激光点和机器人原点坐标之间的斜率slope。
[0084]
其中，，；
[0085]
如果slope小于1，则说明当前激光点在区域1中，无需变换。
[0086]
如果slope大于1，说明当前激光点在区域2中，交换横纵坐标即可，变换到区域1中。
[0087]
如果slope 小于0，说明当前激光点在区域7和8中，对轴做镜像变换即可，即，变换到区域1中。
[0088]
依据上述构思，其他情况说明当前激光点在3、4、5、6区域中，则交换线性的起点和终点，即可变换到y轴的右侧区域，即先根据斜率判断当前点在哪个区域内，根据不同区域的变换准则，均变换到区域1中。
[0089]
之后，设置比较表达式的增量，如果前一个位置是由东方衍生出来，则增量为：
[0090]
；
[0091]
如果前一个位置是由东北方衍生出来，则增量为：
[0092]
；
[0093]
设置比较表达式的初始值：
[0094]
；
[0095]
循环遍历每一个横坐标，求出对应的y的坐标：
[0096]
如果 表示是由东方衍生出的，y值不变，加上相应的增量；
[0097]
否则，表示是由东北方衍生出的，y值加一，加上相应的增量
[0098]
籍此便可计算出激光射线上所有的纵、横坐标位置，此时只要遍历各区域内激光射线上所有的纵、横坐标位置，判断是否存在与地图点重合的坐标，如存在则视为穿透地图，从而完成激光点是否穿透地图点的判断
[0099]
进一步的，当完成地图穿透的判断后，便可计算穿透点位置处的地图点及激光点
的距离比例得分l，其示例步骤包括：
[0100]
步骤s230设穿透点位置处的地图点坐标为（、），对应激光点的坐标为（），计算两者之间的距离
[0101]
；
[0102]
步骤s340计算所有穿透点位置的dist之和，并将其除以穿透点的总数，得到距离比例得分l
[0103] 。
[0104]
步骤s300统计栅格地图整体穿透比例n，计算所有激光点与地图点的平均距离m。
[0105]
具体的，当完成上述地图穿透判断后，便可知晓一帧激光雷达扫描帧中有多少个穿透点，统计所有激光点数量与穿透点数量的比值，便可计算出地图整体的穿透比例n，而所有激光点与地图点的平均距离m的计算示例为：
[0106]
；
[0107]
其中代表每个激光点的位置，代表每个激光点相对应的最近地图点的位，n代表激光点的个数，即对每个激光点到其最近的地图点距离求和后，除以一帧激光雷达扫描帧中所有的激光点总数。
[0108]
步骤s400当检测到匹配程度均值g、距离比例得分l、穿透比例n满足丢失阈值判断条件时给出定位丢失判断结果，否则再判断穿透比例n、平均距离m是否超出反比阈值判断条件，以得出定位丢失判断结果。
[0109]
具体来说，整体的判断流程如下：
[0110]
首先，判断当前机器人是否在非法点（地图外或障碍物上）上，若是，则默认定位丢失。
[0111]
其次，若机器人未在非法点上，则如图6所示，进行步骤s400中的所述丢失阈值判断包括：
[0112]
判断条件1：当匹配程度均值g》0.7时，判断是否穿透比例n》0.25，距离比例得分l》0.3；
[0113]
判断条件2：当匹配程度均值g《=0.7时，判断是否穿透比例n》0.18，距离比例得分l》0.2；
[0114]
其中当符合上述任一判断条件时，判定当前定位丢失。否则认为定位并未丢失。籍此通过设置两组不同的阈值判断调节，可根据不同情况调整其严格程度，从而可以更准确地检测定位丢失的情况。
[0115]
另一方面，考虑到实践中也存在穿透比例较小，但是激光距离地图的平均距离较大的情况，此时根据经验来说，可能会出现定位丢失的情况。
[0116]
例如图7所示，其中仅有约8%左右的激光穿透了地图，但此时定位已丢失。所以根据此类经验，还需要计算所有激光点与地图的平均距离，以确定定位是否丢失。例如平均距离大于2米且小于5米，则认定为定位丢失；但如果平均距离大于5米，则认为定位正常。这是因为当车辆被多人包围时，大部分激光会被完全遮挡，导致激光到地图的距离变得很大。
[0117]
为此步骤s400中所述反比阈值判断条件包括：
[0118]
当穿透比例n＜0.17，而平均距离m＞0.3时，判定当前定位丢失。
[0119]
此外本示例中上述的各个阈值判断条件参数仅为示例，本领域技术人员也可以根据实际情况进行调整，因此本发明示例并未进行限制。
[0120]
进一步的，为了避免误报问题产生，在预选示例中，还引入新的判断条件：当连续数帧（如5帧）都检测到定位丢失时，才认为当前定位已经丢失，并上报错误码。此举相比传统的单帧判断方式，连续帧判断能够降低环境噪声和瞬时偏差的影响，提高判断的稳定性和准确性，从而提升定位丢失判断的精准性。
[0121]
进一步的，为了降低发生误报率和提升检测精度以及控制每个数据的时间戳同步，所述的检测2d激光定位丢失的方法中，示例步骤还包括：
[0122]
步骤s500当判断激光信息和定位信息之间的时间戳不同步时，停止定位丢失判断。即当激光信息和定位信息之间的时间戳同步时，才执行上述示例的判断步骤。
[0123]
另一方面，请参阅图8所示，对应上述方法示例，本发明还提供了一种检测2d激光定位丢失的系统，其包括：
[0124]
存储单元，用于存储包括如上任一所述检测2d激光定位丢失的方法步骤的程序，以供处理单元，导航单元适时调取执行；
[0125]
导航单元，用于将地图点进行高斯分布，生成似然高斯概率栅格地图，并获取激光雷达扫描帧，确定激光点在栅格地图上的位置，及当前所处定位位置；
[0126]
处理单元，用于计算各激光点所处栅格与其最近地图点的匹配程度g，并计算匹配程度均值g；并当判断存在激光穿透地图时，记录各穿透点位置处的地图点及激光点，以计算两者间的距离比例得分l；统计栅格地图整体穿透比例n，计算所有激光点与地图点的平均距离m；之后当检测到匹配程度均值g、距离比例得分l、穿透比例n满足丢失阈值判断条件时给出定位丢失判断结果，否则再判断穿透比例n、平均距离m是否超出反比阈值判断条件，以得出定位丢失判断结果。
[0127]
另一方面，对应上述方法示例，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，其中所述计算机程序被处理器执行时，实现如上任一项所述检测2d激光定位丢失的方法的步骤。
[0128]
综上所述，通过本发明提供的该检测2d激光定位丢失的方法及系统、存储介质，能够巧妙的计算出激光和地图的匹配程度，并判断是否存在激光穿透地图的现象，同时综合该些特征对应的数值来综合评价当前定位是否丢失，从而可准确、可靠地实时检测出工业机器人2d激光定位是否存在丢失问题，籍此降低移动机器人的安全事故风险，提升用户对机器人自动化生产的信心。
[0129]
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
[0130]
本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供
的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0131]
此外实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器（rom，read-only memory）、随机存取存储器（ram，random access memory）、磁碟或者光盘等各种可以存储程序代码的介质。
[0132]
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

技术特征：
1.一种检测2d激光定位丢失的方法，步骤包括：步骤s100将地图点进行高斯分布，生成似然高斯概率栅格地图，计算各激光点所处栅格与其最近地图点的匹配程度g，并计算匹配程度均值g；步骤s200当判断存在激光穿透地图时，记录各穿透点位置处的地图点及激光点，以计算两者间的距离比例得分l；步骤s300统计栅格地图整体穿透比例n，计算所有激光点与地图点的平均距离m；步骤s400当检测到匹配程度均值g、距离比例得分l、穿透比例n满足丢失阈值判断条件时给出定位丢失判断结果，否则再判断穿透比例n、平均距离m是否超出反比阈值判断条件，以得出定位丢失判断结果。2.根据权要求1所述的检测2d激光定位丢失的方法，其中步骤s100中计算匹配程度均值g的步骤包括：步骤s110计算；其中为当前激光点所处栅格与其最近地图点的距离；步骤s120将所有计算的匹配程度g相加，并除以激光点所处栅格总数，获取匹配程度均值g。3.根据权要求1所述的检测2d激光定位丢失的方法，其中步骤s200中判断激光是否穿透地图的步骤包括：步骤s210将地图坐标系按照斜率等分为数个区域，以当前所处位置为原点，各激光点为终点，采用bresenham划线算法构建激光射线；步骤s220遍历各区域内激光射线上所有的纵、横坐标位置，判断是否存在与地图点重合的坐标，如存在则视为穿透地图。4.根据权要求3所述的检测2d激光定位丢失的方法，其中步骤s220中遍历各区域内激光射线上所有的纵、横坐标位置的步骤包括：步骤s221判断当前激光点与原点坐标之间的斜率，以确定激光射线所处地图坐标系的区域；步骤s222根据激光射线所处地图坐标系的区域，进行坐标转换以变换到主区域中；步骤s223设置比较表达式的增量及比较表达式的初始值，循环遍历激光射线上每一个横坐标，求出对应的纵的坐标。5.根据权要求1所述的检测2d激光定位丢失的方法，其中计算穿透点位置处的地图点及激光点的距离比例得分l的步骤包括：步骤s230设穿透点位置处的地图点坐标为（、），对应激光点的坐标为（），计算两者之间的距离；步骤s340计算所有穿透点位置的dist之和，并将其除以穿透点的总数，得到距离比例得分l 。
6.根据权要求1所述的检测2d激光定位丢失的方法，其中步骤s400中所述丢失阈值判断条件包括：当匹配程度均值g>0.7时，判断穿透比例n>0.25，距离比例得分l>0.3；当匹配程度均值g<=0.7时，判断穿透比例n>0.18，距离比例得分l>0.2；符合任一条件时判定当前定位丢失。7.根据权要求1所述的检测2d激光定位丢失的方法，其中步骤s400中所述反比阈值判断条件包括：当穿透比例n＜0.17，而平均距离m＞0.3时，判定当前定位丢失。8.根据权要求1所述的检测2d激光定位丢失的方法，其中步骤还包括：步骤s500当判断激光信息和定位信息之间的时间戳不同步时，停止定位丢失判断。9.一种检测2d激光定位丢失的系统，其包括：存储单元，用于存储包括如权利要求1至8中任一所述检测2d激光定位丢失的方法步骤的程序，以供处理单元，导航单元适时调取执行；导航单元，用于将地图点进行高斯分布，生成似然高斯概率栅格地图，并获取激光雷达扫描帧，确定激光点在栅格地图上的位置，及当前所处定位位置；处理单元，用于计算各激光点所处栅格与其最近地图点的匹配程度g，并计算匹配程度均值g；并当判断存在激光穿透地图时，记录各穿透点位置处的地图点及激光点，以计算两者间的距离比例得分l；统计栅格地图整体穿透比例n，计算所有激光点与地图点的平均距离m；之后当检测到匹配程度均值g、距离比例得分l、穿透比例n满足丢失阈值判断条件时给出定位丢失判断结果，否则再判断穿透比例n、平均距离m是否超出反比阈值判断条件，以得出定位丢失判断结果。10.一种计算机可读存储介质，其上存储有计算机程序，其中所述计算机程序被处理器执行时，实现如权利要求1至8中任一项所述检测2d激光定位丢失的方法的步骤。

技术总结
本发明提供了一种检测2D激光定位丢失的方法及系统、存储介质，其中方法步骤包括：将地图点进行高斯分布，生成似然高斯概率栅格地图，计算各激光点所处栅格与其最近地图点的匹配程度g，并计算匹配程度均值G；当判断存在激光穿透地图时，记录各穿透点位置处的地图点及激光点，以计算两者间的距离比例得分L；统计栅格地图整体穿透比例N，计算所有激光点与地图点的平均距离M；当检测到匹配程度均值G、距离比例得分L、穿透比例N满足丢失阈值判断条件时给出定位丢失判断结果，否则再判断穿透比例N、平均距离M是否超出反比阈值判断条件，以得出定位丢失判断结果。籍此以供在复杂环境下，准确的判断出2D激光定位是否发生定位丢失。确的判断出2D激光定位是否发生定位丢失。确的判断出2D激光定位是否发生定位丢失。


技术研发人员：孙天放 赵越
受保护的技术使用者：上海仙工智能科技有限公司
技术研发日：2023.08.29
技术公布日：2023/10/8