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

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

A matching evaluation method, medium, terminal and device of laser point cloud and map

【技术实现步骤摘要】
一种激光点云与地图的匹配评价方法、介质、终端和装置
本专利技术涉及导航定位领域，尤其涉及一种激光点云与地图的匹配评价方法、介质、终端和装置。
技术介绍
激光slam(simultaneouslocalizationandmapping)是移动机器人定位导航的核心技术，全称为同时定位与建图。当机器人处于未知环境中的未知位置时，基于自身携带的传感器通过激光slam技术可以实现机器人边移动边估计出机器人实时位姿，然后不断扩展与更新地图，逐渐构建环境的完整地图。目前激光slam的两大主流的方向是粒子滤波与图优化的方法，两种方法都涉及点云与栅格地图的匹配，是激光slam的核心组成部分。匹配的精度对定位建图精度有较大的影响，同时匹配的速度影响了定位建图的实时性，进一步影响机器人导航控制的响应速度与平滑性。因此提高点云与地图的匹配精度与速度是机器人高精度高效运行的关键技术。已知正在构建的环境地图，根据点云原点在地图上的先验位姿将点云栅格化，计算得到点云的每个点所在地图的栅格，将栅格化的点云与地图进行匹配得到点云原点在地图的后验位姿，从而可以确定所携带该激光的机器人在地图中的精确后验位姿。理想情况下点云所在的栅格都为占用状态，即表示环境中的物体或障碍点。点云与地图的匹配评价标准采用残差或者转化为得分的方式。根据一些搜索方法确定点云在栅格地图中最近点，计算点云与最近点的距离称为残差，或者根据该距离计算正相关的得分，点云匹配的总体残差越小或者得分越高说明匹配效果越好。良好的匹配效果评价方法，能反映实际的匹配效果，而且受误差较大点的影响较小，容错性强。点云与地图匹配是沿着残差更小或得分更高的方向变换，因此更好的匹配评价方法不仅得到更好的点云地图匹配精度，而且更快收敛到最优值，避免陷入局部极小值。当前主要采用点到点距离的评价方法，该方法使用激光点云周围最近点信息较少，且地图上有些最近点误差较大，导致该点的评价出现偏差，不能反应实际环境的信息。再比如有些点是移动物体在地图上生成的障碍点，那么随着物体的移动，利用当前障碍点再用于匹配的评价，容易导致评价指标不够准确，无法准确反应点云与地图匹配的实际效果。点云与地图的匹配方法是沿着评价得分较高或残差较小的方向移动，并估计出激光点云在地图的后验位姿，如果匹配的评价指标不准确，容易导致较低的匹配精度，有可能错过匹配的最优解，甚至陷入局部极小值。其它方法采用双二次线性插值，选取点云附近的几个距离较近的点及其对应概率值，计算激光点所在栅格的概率值。占用栅格地图的离散性限制了该方法可以实现的精度，也不允许直接计算插值值或导数，因此只能近似计算，导致较大的计算量，同时近似结果受栅格地图分辨率影响。
技术实现思路
本专利技术提供了一种激光点云与地图的匹配评价方法、介质、终端和装置，解决了以上所述的技术问题。本专利技术解决上述技术问题的技术方案如下：一种激光点云与地图的匹配评价方法，包括以下步骤：步骤1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；步骤2，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；步骤3，计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；步骤4，对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。在一个优选实施方式中，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集具体包括以下步骤：S201，获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；S202，获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为最近点，并保存所述最近点的世界坐标形成所述激光点对应的最近点集。在一个优选实施方式中，所述步骤3和步骤4之间还包括以下步骤：当所述最近点集中没有最近点时，则将所述障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，计算所述激光点与对应的目标障碍点的第一欧拉距离，并根据第一预设公式计算所述激光点的匹配得分，所述第一预设公式为：其中di为激光点i与目标障碍点的第一欧拉距离，r为最近点圆的预设半径；当目标障碍点为0时，所述激光点的匹配得分为0。在一个优选实施方式中，所述步骤3中，当最近点集中包括一个最近点时，所述最近点即为均值点；当最近点集中包括两个以上最近点时，根据最近点集中每个最近点的权重计算所有最近点的均值点，具体包括以下步骤：计算所述激光点与每个最近点的第二欧拉距离，并将第二欧拉距离的倒数作为每个最近点的权重；对所有权重进行归一化处理，并采用归一化处理后的权重求取所有最近点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有最近点的均值点坐标。在一个优选实施方式中，所述步骤3中，根据激光点与对应均值点的距离生成所述激光点云中每个激光点的匹配得分具体为：计算所述激光点与对应均值点的第三欧拉距离，并带入第二预设公式生成所述激光点的匹配得分，所述第二预设公式为：其中fi为激光点i与对应均值点的第三欧拉距离。本专利技术实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的激光点云与地图的匹配评价方法。本专利技术实施例的第三方面提供了一种激光点云与地图的匹配评价终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述激光点云与地图的匹配评价方法的步骤。本专利技术实施例的第四方面提供了一种激光点云与地图的匹配评价装置，包括栅格化模块、最近点集获取模块、计算模块和求和模块，所述栅格化模块用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；所述最近点集获取模块用于计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；所述计算模块用于计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；所述求和模块用于对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。在一个优选实施方式中，所述最近点集获取模块具体包括：最近点圆获取单元，用于获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；最近点集获取单元，用于获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为最近点，并保存所述最近点的世界坐标形成所述激光点对应的最近点集。在一个优选实施方式中，还包括第二计算模块，所述第二计算模块具体用于当所述最近点集中没有最近点时，则将所述障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，计算所述激光点与对应的目标障碍点的第一欧拉距离，并根据第一预设公本文档来自技高网...

【技术保护点】
1.一种激光点云与地图的匹配评价方法，其特征在于，包括以下步骤：/n步骤1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；/n步骤2，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；/n步骤3，计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；/n步骤4，对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。/n

【技术特征摘要】
1.一种激光点云与地图的匹配评价方法，其特征在于，包括以下步骤：
步骤1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；
步骤2，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集；
步骤3，计算所述最近点集中所有最近点的均值点，并根据激光点与对应均值点的距离生成所述激光点的匹配得分；
步骤4，对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。


2.根据权利要求1所述激光点云与地图的匹配评价方法，其特征在于，计算所述激光点云中每个激光点在所述环境地图上分别对应的最近点集具体包括以下步骤：
S201，获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；
S202，获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为最近点，并保存所述最近点的世界坐标形成所述激光点对应的最近点集。


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



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


4.根据权利要求1-3任一所述激光点云与地图的匹配评价方法，其特征在于，所述步骤3中，当最近点集中包括一个最近点时，所述最近点即为均值点；当最近点集中包括两个以上最近点时，根据最近点集中每个最近点的权重计算所有最近点的均值点，具体包括以下步骤：
计算所述激光点与每个最近点的第二欧拉距离，并将第二欧拉距离的倒数作为每个最近点的权重；
对所有权重进行归一化处理，并采用归一化处理后的权重求取所有最近点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有最近点的均值点坐标。


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

【专利技术属性】
技术研发人员：李国飞，
申请(专利权)人：上海有个机器人有限公司，
类型：发明
国别省市：上海;31