CN110969649A - 一种激光点云与地图的匹配评价方法、介质、终端和装置 - Google Patents

Abstract

本发明公开一种激光点云与地图的匹配评价方法、介质、终端和装置，方法包括以下步骤：根据激光点云在当前时刻的先验位姿将激光点云在环境地图上栅格化；计算每个激光点在环境地图上分别对应的最近点集；计算最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成激光点的匹配得分；对所有激光点的匹配得分进行求和生成激光点云与环境地图的匹配总分。本发明对于误差较大点具有更强的适应性，能更准确反应出点云与地图匹配的实际效果，不容易陷入局部极小值，提高了匹配精度与容错性，同时在保证运算速度的情况下，能更快地收敛到最优解，在实际应用中具有重要意义。

Description

一种激光点云与地图的匹配评价方法、介质、终端和装置

【技术领域】

本发明涉及导航定位领域，尤其涉及一种激光点云与地图的匹配评价方法、介质、终端和装置。

【背景技术】

激光slam(simultaneous localization and mapping)是移动机器人定位导航的核心技术，全称为同时定位与建图。当机器人处于未知环境中的未知位置时，基于自身携带的传感器通过激光slam技术可以实现机器人边移动边估计出机器人实时位姿，然后不断扩展与更新地图，逐渐构建环境的完整地图。目前激光slam的两大主流的方向是粒子滤波与图优化的方法，两种方法都涉及点云与栅格地图的匹配，是激光slam的核心组成部分。匹配的精度对定位建图精度有较大的影响，同时匹配的速度影响了定位建图的实时性，进一步影响机器人导航控制的响应速度与平滑性。因此提高点云与地图的匹配精度与速度是机器人高精度高效运行的关键技术。已知正在构建的环境地图，根据点云原点在地图上的先验位姿将点云栅格化，计算得到点云的每个点所在地图的栅格，将栅格化的点云与地图进行匹配得到点云原点在地图的后验位姿，从而可以确定所携带该激光的机器人在地图中的精确后验位姿。理想情况下点云所在的栅格都为占用状态，即表示环境中的物体或障碍点。点云与地图的匹配评价标准采用残差或者转化为得分的方式。根据一些搜索方法确定点云在栅格地图中最近点，计算点云与最近点的距离称为残差，或者根据该距离计算正相关的得分，点云匹配的总体残差越小或者得分越高说明匹配效果越好。良好的匹配效果评价方法，能反映实际的匹配效果，而且受误差较大点的影响较小，容错性强。点云与地图匹配是沿着残差更小或得分更高的方向变换，因此更好的匹配评价方法不仅得到更好的点云地图匹配精度，而且更快收敛到最优值，避免陷入局部极小值。当前主要采用点到点距离的评价方法，该方法使用激光点云周围最近点信息较少，且地图上有些最近点误差较大，导致该点的评价出现偏差，不能反应实际环境的信息。再比如有些点是移动物体在地图上生成的障碍点，那么随着物体的移动，利用当前障碍点再用于匹配的评价，容易导致评价指标不够准确，无法准确反应点云与地图匹配的实际效果。点云与地图的匹配方法是沿着评价得分较高或残差较小的方向移动，并估计出激光点云在地图的后验位姿，如果匹配的评价指标不准确，容易导致较低的匹配精度，有可能错过匹配的最优解，甚至陷入局部极小值。其它方法采用双二次线性插值，选取点云附近的几个距离较近的点及其对应概率值，计算激光点所在栅格的概率值。占用栅格地图的离散性限制了该方法可以实现的精度，也不允许直接计算插值值或导数，因此只能近似计算，导致较大的计算量，同时近似结果受栅格地图分辨率影响。

【发明内容】

本发明提供了一种激光点云与地图的匹配评价方法、介质、终端和装置，解决了以上所述的技术问题。

本发明解决上述技术问题的技术方案如下：一种激光点云与地图的匹配评价方法，包括以下步骤：

步骤1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

步骤2，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；

步骤3，计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；

步骤4，对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。

在一个优选实施方式中，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集具体包括以下步骤：

S201，获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；

S202，获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为最近点，并保存所述最近点的世界坐标形成所述激光点对应的最近点集。

在一个优选实施方式中，所述步骤3和步骤4之间还包括以下步骤：当所述最近点集中没有最近点时，则将所述障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，计算所述激光点与对应的目标障碍点的第一欧拉距离，并根据第一预设公式计算所述激光点的匹配得分，所述第一预设公式为：

其中d_i为激光点i与目标障碍点的第一欧拉距离，r为最近点圆的预设半径；

当目标障碍点为0时，所述激光点的匹配得分为0。

在一个优选实施方式中，所述步骤3中，当最近点集中包括一个最近点时，所述最近点即为均值点；当最近点集中包括两个以上最近点时，根据最近点集中每个最近点的权重计算所有最近点的均值点，具体包括以下步骤：

计算所述激光点与每个最近点的第二欧拉距离，并将第二欧拉距离的倒数作为每个最近点的权重；

对所有权重进行归一化处理，并采用归一化处理后的权重求取所有最近点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有最近点的均值点坐标。

在一个优选实施方式中，所述步骤3中，根据激光点与对应均值点的距离生成所述激光点云中每个激光点的匹配得分具体为：

计算所述激光点与对应均值点的第三欧拉距离，并带入第二预设公式生成所述激光点的匹配得分，所述第二预设公式为：

其中f_i为激光点i与对应均值点的第三欧拉距离。

本发明实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的激光点云与地图的匹配评价方法。

本发明实施例的第三方面提供了一种激光点云与地图的匹配评价终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述激光点云与地图的匹配评价方法的步骤。

本发明实施例的第四方面提供了一种激光点云与地图的匹配评价装置，包括栅格化模块、最近点集获取模块、计算模块和求和模块，

所述栅格化模块用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

所述最近点集获取模块用于计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；

所述计算模块用于计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；

所述求和模块用于对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。

在一个优选实施方式中，所述最近点集获取模块具体包括：

最近点圆获取单元，用于获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；

最近点集获取单元，用于获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为最近点，并保存所述最近点的世界坐标形成所述激光点对应的最近点集。

在一个优选实施方式中，还包括第二计算模块，所述第二计算模块具体用于当所述最近点集中没有最近点时，则将所述障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，计算所述激光点与对应的目标障碍点的第一欧拉距离，并根据第一预设公式计算所述激光点的匹配得分，所述第一预设公式为：

其中d_i为激光点i与目标障碍点的第一欧拉距离，r为最近点圆的预设半径；当目标障碍点为0时，计算激光点的匹配得分为0。

本发明提供了一种激光点云与地图的匹配评价方法、介质、终端和装置，首先计算激光点云每个点所在栅格的周围一定范围内的所有最近点，根据每个最近点到点云对应点的欧拉距离得到每个点的权重，然后对权重归一化处理，将最近点集的障碍点坐标进行加权得到均值点，以均值点代表整个最近点集，然后计算均值点到点云对应点的欧拉距离，以该距离作为点云评价的标准。这样每个最近点对点云的匹配得分都具有贡献值，权重越大那么对点云匹配得分的影响越大，但匹配得分的计算不完全依赖于地图中单个点，而是依赖于一定范围内最近点集加权得到的均值点，因此对于某些误差较大点，具有更强的适应性，对于匹配评价更准确，更能准确反应出点云与地图匹配的实际效果，不容易陷入局部极小值，从而更符合实际匹配情况，提高了匹配精度与容错性，同时在保证运算速度的情况下，能更快地收敛到最优解，在实际应用中具有重要意义。

为使发明的上述目的、特征和优点能更明显易懂，下文特举本发明较佳实施例，并配合所附附图，作详细说明如下。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是实施例1提供的激光点云与地图的匹配评价方法的流程示意图；

图2是实施例2提供的激光点云与地图的匹配评价装置的结构示意图；

图3是实施例3提供的激光点云与地图的匹配评价终端的结构示意图。

【具体实施方式】

为了使本发明的目的、技术方案和有益技术效果更加清晰明白，以下结合附图和具体实施方式，对本发明进行进一步详细说明。应当理解的是，本说明书中描述的具体实施方式仅仅是为了解释本发明，并不是为了限定本发明。

图1是本发明实施例1提供的一种激光点云与地图的匹配评价方法的流程示意图，如图1所示，包括以下步骤：

步骤1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

步骤2，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；

步骤3，计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；

步骤4，对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。

以下对上述实施例的步骤进行详细说明。

已知用于匹配的环境地图M，将用于匹配的激光点云根据t-1时刻的后验位姿及由t-1到t时刻的变化量，得到t时刻的先验位姿，然后通过点云与所述环境地图的匹配即可得到t时刻的后验位姿，即精确匹配得到修正后的位姿。首先将激光点云基于t时刻的先验位姿，在环境地图上栅格化，得到激光点云每个点所在的栅格，然后在激光点云每个点周围半径为r的圆的范围内，搜索标记状态为占用的栅格。本实施例预设r大小为1-2个栅格分辨率大小，优选为1.5个栅格分辨率大小，且圆心为激光点云的各个激光点所在栅格的中心，这样形成的圆为激光点云每个点的最近点圆。

设激光点云中任意激光点为p_i，获取每个激光点在预设搜索范围内的所有障碍点，即搜索激光点p_i所在栅格m_i的周围预设搜索范围内所有栅格的状态，本优选实施例中可以搜索周围6-10个栅格的状态，为保证了匹配精度和匹配速度，优选搜索周围8个栅格的状态，当栅格为占用状态时即为障碍点，从而获取每个激光点在预设搜索范围内的所有障碍点，然后去除不在激光点p_i对应最近点圆范围内的障碍点，而在最近点圆范围内的障碍点作为最近点，将其索引并保存障碍点的世界坐标形成所述激光点对应的最近点集。为了提高匹配效率，优选实施例中可以将激光点的p_i最近点集按照到点p_i坐标的欧拉距离进行升序排列，从而方便后续过去进行加权计算。

然后按照如下方法，计算最近点集中每个最近点的权重，再将权重进行归一化，权重反应了每个最近点靠近点云中对应激光点的距离大小，且权重大的点对评价得分的结果影响越大。为了保证良好匹配评价的前提下，提高匹配得分的计算效率，本优选实施例对最近点集中每个最近点的权重以及每个最近点的障碍点坐标进行加权计算得到所有最近点的均值点，然后计算均值点与激光点云中对应激光点的欧拉距离，以该欧拉距离计算每个激光点的匹配得分，然后将点云中每个激光点的匹配得分求和。

根据最近点集包含点的数量的不同，得到不同的匹配效果得分计算方法：

(1)当所述最近点集中没有最近点时，则将所述障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，比如查找在最近点圆范围外、在周围8个栅格内且距离对应激光点最近的障碍点q_i，计算p_i与q_i的第一欧拉距离d_i，得到p_i点的匹配得分为

其中r为最近点圆的预设半径。如果未找到目标障碍点，即没有在激光点的预设搜索范围内找到任何障碍点，那么p_i点得分为0。

(2)当所述最近点集中包含1个最近点q_i，则该最近点即为均值点h_i，计算该激光点p_i与均值点h_i的第三欧拉距离f_i，得到p_i点的匹配得分为

(3)当所述最近点集中包含2个最近点，设为最近点q_i1，最近点q_i2，计算最近点q_i1，最近点q_i2分别与点p_i的第二欧拉距离分别为d_i1，d_i2，可计算出两个最近点的权重分别为

权重归一化得到最近点的权重分别为

因此加权得到两个最近点的均值点h_i坐标为h_i＝ω_i1q_i1+ω_i2q_i2，计算该激光点p_i与均值点h_i的第三欧拉距离f_i，得到p_i点的匹配得分为

(4)当所述最近点集中包含n个最近点，为{q_i1,...,q_in}，计算所有最近点与点p_i的第二欧拉距离记为d_ij＝{d_i1,...d_in}，其中j∈(1，n)，从而得出每个最近点的权重分别为

权重归一化得到最近点的权重分别为

因此加权得到n个最近点的均值点h_i坐标为

计算点p_i与均值点h_i的第三欧拉距离f_i，得到p_i点的匹配得分为

最后，对激光点云中所有激光点的匹配得分进行求和，得到激光点云与环境地图匹配迭代一次的总得分。

上述优选实施例提供了一种激光点云与地图的匹配评价方法、介质、终端和装置，首先计算激光点云每个点所在栅格的周围一定范围内的所有最近点，根据每个最近点到点云对应点的欧拉距离得到每个点的权重，然后对权重归一化处理，将最近点集的障碍点坐标进行加权得到均值点，以均值点代表整个最近点集，然后计算均值点到点云对应点的欧拉距离，以该距离作为点云评价的标准。这样每个最近点对点云的匹配得分都具有贡献值，权重越大那么对点云匹配得分的影响越大，但匹配得分的计算不完全依赖于地图中单个点，而是依赖于一定范围内最近点集加权得到的均值点，因此对于某些误差较大点，具有更强的适应性，对于匹配评价更准确，更能准确反应出点云与地图匹配的实际效果，不容易陷入局部极小值，从而更符合实际匹配情况，提高了匹配精度与容错性，同时在保证运算速度的情况下，能更快地收敛到最优解，在实际应用中具有重要意义。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

本发明实施例还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的激光点云与地图的匹配评价方法。

图2是本发明实施例2提供的一种激光点云与地图的匹配评价装置的结构示意图，如图2所示，包括栅格化模块100、最近点集获取模块200、计算模块300和求和模块400，

所述栅格化模块100用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

所述最近点集获取模块200用于计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；

所述计算模块300用于计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；

所述求和模块400用于对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。

优选实施例中，所述最近点集获取模块200具体包括：

最近点圆获取单元201，用于获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；

最近点集获取单元202，用于获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为最近点，并保存所述最近点的世界坐标形成所述激光点对应的最近点集。

另一优选实施例中，还包括第二计算模块500，所述第二计算模块500具体用于当所述最近点集中没有最近点时，则将所述障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，计算所述激光点与对应的目标障碍点的第一欧拉距离，并根据第一预设公式计算所述激光点的匹配得分，所述第一预设公式为：

其中d_i为激光点i与目标障碍点的第一欧拉距离，r为最近点圆的预设半径；当目标障碍点为0时，计算激光点的匹配得分为0。

另一优选实施例中，所述计算模块300包括：

均值点坐标计算单元301，用于当最近点集中包括一个最近点时，将最近点作为均值点；当最近点集中包括两个以上最近点时，计算所述激光点与每个最近点的第二欧拉距离，并将第二欧拉距离的倒数作为每个最近点的权重，对所有权重进行归一化处理，并采用归一化处理后的权重求取所有最近点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有最近点的均值点坐标；

匹配得分计算单元，用于计算所述激光点与对应均值点的第三欧拉距离，并带入第二预设公式生成所述激光点的匹配得分，所述第二预设公式为：

其中f_i为激光点i与对应均值点的第三欧拉距离。

本发明实施例还提供了一种激光点云与地图的匹配评价终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述激光点云与地图的匹配评价方法的步骤。图3是本发明实施例3提供的激光点云与地图的匹配评价终端的结构示意图，如图3所示，该实施例的激光点云与地图的匹配评价终端8包括：处理器80、可读存储介质81以及存储在所述可读存储介质81中并可在所述处理器80上运行的计算机程序82。所述处理器80执行所述计算机程序82时实现上述各个方法实施例中的步骤，例如图1所示的步骤1至步骤4。或者，所述处理器80执行所述计算机程序82时实现上述各装置实施例中各模块的功能，例如图2所示模块100至400的功能。

示例性的，所述计算机程序82可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述可读存储介质81中，并由所述处理器80执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序82在所述激光点云与地图的匹配评价终端8中的执行过程。

所述激光点云与地图的匹配评价终端8可包括，但不仅限于，处理器80、可读存储介质81。本领域技术人员可以理解，图3仅仅是激光点云与地图的匹配评价终端8的示例，并不构成对激光点云与地图的匹配评价终端8的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述激光点云与地图的匹配评价终端还可以包括电源管理模块、运算处理模块、输入输出设备、网络接入设备、总线等。

所称处理器80可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述可读存储介质81可以是所述激光点云与地图的匹配评价终端8的内部存储单元，例如激光点云与地图的匹配评价终端8的硬盘或内存。所述可读存储介质81也可以是所述激光点云与地图的匹配评价终端8的外部存储设备，例如所述激光点云与地图的匹配评价终端8上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(SecureDigital，SD)卡，闪存卡(Flash Card)等。进一步地，所述可读存储介质81还可以既包括所述激光点云与地图的匹配评价终端8的内部存储单元也包括外部存储设备。所述可读存储介质81用于存储所述计算机程序以及所述激光点云与地图的匹配评价终端所需的其他程序和数据。所述可读存储介质81还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述***中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及方法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

本发明并不仅仅限于说明书和实施方式中所描述，因此对于熟悉领域的人员而言可容易地实现另外的优点和修改，故在不背离权利要求及等同范围所限定的一般概念的精神和范围的情况下，本发明并不限于特定的细节、代表性的设备和这里示出与描述的图示示例。

Claims (10)

1.一种激光点云与地图的匹配评价方法，其特征在于，包括以下步骤：

步骤1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

步骤2，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；

步骤3，计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；

步骤4，对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。

2.根据权利要求1所述激光点云与地图的匹配评价方法，其特征在于，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集具体包括以下步骤：

S201，获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；

S202，获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为最近点，并保存所述最近点的世界坐标形成所述激光点对应的最近点集。

3.根据权利要求2所述激光点云与地图的匹配评价方法，其特征在于，所述步骤3和所述步骤4之间还包括以下步骤：当所述最近点集中没有最近点时，则将所述障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，计算所述激光点与对应的目标障碍点的第一欧拉距离，并根据第一预设公式计算所述激光点的匹配得分，所述第一预设公式为：

其中d_i为激光点i与目标障碍点的第一欧拉距离，r为最近点圆的预设半径；

当目标障碍点为0时，所述激光点的匹配得分为0。

4.根据权利要求1-3任一所述激光点云与地图的匹配评价方法，其特征在于，所述步骤3中，当最近点集中包括一个最近点时，所述最近点即为均值点；当最近点集中包括两个以上最近点时，根据最近点集中每个最近点的权重计算所有最近点的均值点，具体包括以下步骤：

计算所述激光点与每个最近点的第二欧拉距离，并将第二欧拉距离的倒数作为每个最近点的权重；

对所有权重进行归一化处理，并采用归一化处理后的权重求取所有最近点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有最近点的均值点坐标。

5.根据权利要求4所述激光点云与地图的匹配评价方法，其特征在于，所述步骤3中，根据激光点与对应均值点的距离生成所述激光点云中每个激光点的匹配得分具体为：

计算所述激光点与对应均值点的第三欧拉距离，并带入第二预设公式生成所述激光点的匹配得分，所述第二预设公式为：

其中f_i为激光点i与对应均值点的第三欧拉距离。

6.一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序被处理器执行时，实现权利要求1-5任一项所述的激光点云与地图的匹配评价方法。

7.一种激光点云与地图的匹配评价终端，其特征在于，包括权利要求6所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现如权利要求1-5任一项所述激光点云与地图的匹配评价方法的步骤。

8.一种激光点云与地图的匹配评价装置，其特征在于，包括栅格化模块、最近点集获取模块、计算模块和求和模块，

所述栅格化模块用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

所述最近点集获取模块用于计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；

所述计算模块用于计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；

所述求和模块用于对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。

9.根据权利要求8所述激光点云与地图的匹配评价装置，其特征在于，所述最近点集获取模块具体包括：

最近点圆获取单元，用于获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；

最近点集获取单元，用于获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为最近点，并保存所述最近点的世界坐标形成所述激光点对应的最近点集。

10.根据权利要求9所述激光点云与地图的匹配评价装置，其特征在于，还包括第二计算模块，所述第二计算模块具体用于当所述最近点集中没有最近点时，则将所述障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，计算所述激光点与对应的目标障碍点的第一欧拉距离，并根据第一预设公式计算所述激光点的匹配得分，所述第一预设公式为：

其中d_i为激光点i与目标障碍点的第一欧拉距离，r为最近点圆的预设半径；当目标障碍点为0时，计算激光点的匹配得分为0。

Images