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

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

Info

Publication number
CN111707279A
CN111707279A CN202010425583.8A CN202010425583A CN111707279A CN 111707279 A CN111707279 A CN 111707279A CN 202010425583 A CN202010425583 A CN 202010425583A CN 111707279 A CN111707279 A CN 111707279A
Authority
CN
China
Prior art keywords
point
laser
laser point
points
obstacle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010425583.8A
Other languages
English (en)
Other versions
CN111707279B (zh
Inventor
李国飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Yogo Robot Co Ltd
Original Assignee
Shanghai Yogo Robot Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Yogo Robot Co Ltd filed Critical Shanghai Yogo Robot Co Ltd
Priority to CN202010425583.8A priority Critical patent/CN111707279B/zh
Publication of CN111707279A publication Critical patent/CN111707279A/zh
Application granted granted Critical
Publication of CN111707279B publication Critical patent/CN111707279B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开一种激光点云与地图的匹配评价方法、介质、终端和装置，方法包括以下步骤：根据激光点云在当前时刻的先验位姿将激光点云在环境地图上栅格化；获取激光点云中每个激光点在所述环境地图上分别对应的目标障碍点，并采用预设公式计算激光点与对应目标障碍点的距离；采用预设公式计算每个激光点的匹配得分；对激光点云中所有激光点的匹配得分进行求和，生成激光点云与环境地图的匹配总分。本发明不仅保证了运算速度，同时提高了激光点云与栅格地图匹配精度，增强了定位的容错性与偏差修正能力，在机器人定位导航中具有重要意义。

Description

一种激光点云与地图的匹配评价方法、介质、终端和装置
【技术领域】
本发明涉及导航定位领域，尤其涉及一种激光点云与地图的匹配评价方法、介质、终端和装置。
【背景技术】
激光slam(simultaneous localization and mapping)是移动机器人定位导航的核心技术，全称为同时定位与建图。当机器人处于未知环境中的未知位置时，基于自身携带的传感器通过激光slam技术可以实现机器人边移动边估计出机器人实时位姿，然后不断扩展与更新地图，逐渐构建环境的完整地图。目前激光slam的两大主流的方向是粒子滤波与图优化的方法，两种方法都涉及点云与栅格地图的匹配，是激光 slam的核心组成部分。匹配的精度对定位建图精度有较大的影响，同时匹配的速度影响了定位建图的实时性，进一步影响机器人导航控制的响应速度与平滑性。因此提高点云与地图的匹配精度与速度是机器人高精度高效运行的关键技术。
点云与地图的匹配评价标准通常采用残差或者转化为得分的方式。首先根据一些搜索方法确定点云在栅格地图中最近障碍点，计算点云与最近障碍点的距离称为残差，再通过指数函数得到每个点与地图匹配的得分，将所有点与地图匹配得分求和得到当前激光与地图匹配的总得分。该评分函数对最近点距离的梯度比较小，在距离变化较大的情况下，得分变化不明显，不能精确评价点云与地图匹配的效果，因此在匹配过程中导致匹配精度不高，容易陷入局部最优值，尤其在初始位姿误差较大情况下，难以进行位姿的准确实时修正。
【发明内容】
本发明提供了一种激光点云与地图的匹配评价方法、介质、终端和装置，解决了以上所述的技术问题。
本发明解决上述技术问题的技术方案如下：一种激光点云与地图的匹配评价方法，包括以下步骤：
步骤1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；
步骤2，获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点，并计算所述激光点与对应目标障碍点的距离dmin；
步骤3，计算每个激光点的匹配得分；
步骤4，对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。
在一个优选实施方式中，所述获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点具体包括以下步骤：
S201，获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；
S202，获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为候选障碍点，并保存所述候选障碍点的世界坐标形成所述激光点对应的最近点集；
S203，将所述最近点集中所有候选障碍点的均值点作为所述目标障碍点。
在一个优选实施方式中，当所述最近点集中包括一个候选障碍点时，所述候选障碍点即为均值点；当最近点集中包括两个以上候选障碍点时，根据最近点集中每个候选障碍点的权重计算所有候选障碍点的均值点，具体包括以下步骤：
S2031，计算所述激光点与每个候选障碍点的欧拉距离，并将所述欧拉距离的倒数作为每个候选障碍点的权重；
S2032，对所有权重进行归一化处理，并采用归一化处理后的权重求取所有候选障碍点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有候选障碍点的均值点坐标。
在一个优选实施方式中，所述获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点还包括以下步骤：当不存在候选障碍点形成最近点集时，将所有障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点。
本发明实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的激光点云与地图的匹配评价方法。
本发明实施例的第三方面提供了一种激光点云与地图的匹配评价终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述激光点云与地图的匹配评价方法的步骤。
本发明实施例的第四方面提供了一种激光点云与地图的匹配评价装置，包括栅格化模块、距离计算模块、评分模块和求和模块，
所述栅格化模块用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；
所述距离计算模块用于获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点，并计算所述激光点与对应目标障碍点的距离dmin；
所述评分模块用于计算每个激光点的匹配得分；
所述求和模块用于对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。
在一个优选实施方式中，所述距离计算模块具体包括：
最近点圆获取单元，用于获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；
最近点集获取单元，用于获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为候选障碍点，并保存所述候选障碍点的世界坐标形成所述激光点对应的最近点集；
目标障碍点获取单元，用于将所述最近点集中所有候选障碍点的均值点作为所述目标障碍点；
距离计算单元，用于计算所述激光点与对应目标障碍点的距离dmin。
在一个优选实施方式中，所述目标障碍点获取单元用于当所述最近点集中包括一个候选障碍点时，将所述候选障碍点作为均值点；以及用于当最近点集中包括两个以上候选障碍点时，计算所述激光点与每个候选障碍点的欧拉距离，并将所述欧拉距离的倒数作为每个候选障碍点的权重，且对所有权重进行归一化处理，采用归一化处理后的权重求取所有候选障碍点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有候选障碍点的均值点坐标。
在一个优选实施方式中，所述目标障碍点获取单元还用于当不存在候选障碍点形成最近点集时，将所有障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点。
本发明提供了一种激光与地图匹配的评分方法，首先计算激光点云的每个点在栅格地图中击中的单元格，确定离该单元格最近的目标障碍点的距离，即最近点距离，然后采用本发明提供的最近点距离平方倒数的评分函数计算每个点与地图匹配得分，将所有点与地图匹配得分求和得到激光点云与地图匹配的得分，不仅保证了运算速度，同时提高了激光点云与栅格地图匹配精度，增强了定位的容错性与偏差修正能力，在机器人定位导航中具有重要意义。
为使发明的上述目的、特征和优点能更明显易懂，下文特举本发明较佳实施例，并配合所附附图，作详细说明如下。
【附图说明】
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1是实施例1提供的激光点云与地图的匹配评价方法的流程示意图；
图2是实施例2提供的激光点云与地图的匹配评价装置的结构示意图；
图3是实施例3提供的激光点云与地图的匹配评价终端的结构示意图。
【具体实施方式】
为了使本发明的目的、技术方案和有益技术效果更加清晰明白，以下结合附图和具体实施方式，对本发明进行进一步详细说明。应当理解的是，本说明书中描述的具体实施方式仅仅是为了解释本发明，并不是为了限定本发明。
图1是本发明实施例1提供的一种激光点云与地图的匹配评价方法的流程示意图，如图1所示，包括以下步骤：
步骤1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化。具体来说，已知用于匹配的环境地图M，将用于匹配的激光点云根据t-1时刻的后验位姿及由t-1到t时刻的变化量，得到t时刻的先验位姿，然后将激光点云基于t时刻的先验位姿，在环境地图上栅格化，即可得到激光点云每个点所在的栅格。
步骤2，获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点，并计算所述激光点与对应目标障碍点的距离dmin。一个实施例中，可以遍历每个激光点在栅格地图中所击中的单元格，从而查找离该单元格最近的目标障碍点的距离。
另一优选实施例中，获取激光点云中每个激光点在环境地图上分别对应的目标障碍点具体包括以下步骤：
S201，获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值。具体来说，在激光点云每个点周围半径为r的圆的范围内，搜索标记状态为占用的栅格。本实施例预设r大小为1-2个栅格分辨率大小，优选为1.5个栅格分辨率大小，且圆心为激光点云的各个激光点所在栅格的中心，这样形成的圆为激光点云每个点的最近点圆。
S202，然后获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为候选障碍点，并保存所述候选障碍点的世界坐标形成所述激光点对应的最近点集。具体来说，设激光点云中任意激光点为pi，获取每个激光点在预设搜索范围内的所有障碍点，即搜索激光点pi所在栅格mi的周围预设搜索范围内所有栅格的状态，本优选实施例中可以搜索周围6-10个栅格的状态，为保证了匹配精度和匹配速度，优选搜索周围8个栅格的状态，当栅格为占用状态时即为障碍点，从而获取每个激光点在预设搜索范围内的所有障碍点，然后去除不在激光点pi对应最近点圆范围内的障碍点，并将最近点圆范围内的障碍点作为候选障碍点，将其索引并保存候选障碍点的世界坐标形成所述激光点对应的最近点集。为了提高匹配效率，优选实施例中可以将激光点的pi最近点集按照到点pi坐标的欧拉距离进行升序排列，从而方便后续过去进行加权计算。
S203，然后将所述最近点集中所有候选障碍点的均值点作为所述目标障碍点。具体可以采用权重的方法，即计算最近点集中每个候选障碍点的权重，再将权重进行归一化，权重反应了每个候选障碍点靠近点云中对应激光点的距离大小，且权重大的点对评价得分的结果影响越大。为了保证良好匹配评价的前提下，提高匹配得分的计算效率，本优选实施例对最近点集中每个候选障碍点的权重以及每个候选障碍点的障碍点坐标进行加权计算得到所有候选障碍点的均值点，然后计算均值点与激光点云中对应激光点的欧拉距离，以该欧拉距离计算每个激光点的匹配得分，然后将点云中每个激光点的匹配得分求和。
根据最近点集包含点的数量的不同，得到不同的匹配效果得分计算方法：
(1)当所述最近点集中没有候选障碍点时，则将搜索出的所有障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点，比如查找在最近点圆范围外、在周围8个栅格内且距离对应激光点最近的障碍点qi，计算pi与qi的欧拉距离di即为dmin。如果未找到目标障碍点，即没有在激光点的预设搜索范围内找到任何障碍点，那么pi点得分为0。
(2)当所述最近点集中包含1个候选障碍点qi，则该候选障碍点即为均值点hi，计算该激光点pi与均值点hi的欧拉距离从而得到dmin。
(3)当所述最近点集中包含2个目标障碍点，设为目标障碍点qi1，目标障碍点qi2，计算目标障碍点qi1，目标障碍点qi2分别与点pi的欧拉距离分别为di1，di2，可计算出两个目标障碍点的权重分别为
Figure RE-GDA0002600113300000091
Figure RE-GDA0002600113300000092
权重归一化得到目标障碍点的权重分别为
Figure RE-GDA0002600113300000093
Figure RE-GDA0002600113300000094
因此加权得到两个目标障碍点的均值点hi坐标为hi＝ωi1qi1+ωi2qi2，然后计算该激光点pi与均值点hi的距离从而得到dmin。
(4)当所述最近点集中包含n个目标障碍点，为{qi1,...,qin}，计算所有目标障碍点与点pi的欧拉距离记为dij＝{di1,...din}，其中j∈(1，n)，从而得出每个目标障碍点的权重分别为
Figure RE-GDA0002600113300000095
权重归一化得到目标障碍点的权重分别为
Figure RE-GDA0002600113300000096
因此加权得到n个目标障碍点的均值点hi坐标为
Figure RE-GDA0002600113300000097
计算点pi与均值点hi的欧拉距离从而得到dmin。
然后执行步骤3，计算每个激光点的匹配得分。
步骤4，最后对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。
上述实施例提供了一种激光与地图匹配的评分方法，首先计算激光点云的每个点在栅格地图中击中的单元格，确定离该单元格最近的目标障碍点的距离，即最近点距离，然后采用本发明提供的最近点距离平方倒数的评分函数计算每个点与地图匹配得分，将所有点与地图匹配得分求和得到激光点云与地图匹配的得分，不仅保证了运算速度，同时提高了激光点云与栅格地图匹配精度，增强了定位的容错性与偏差修正能力，在机器人定位导航中具有重要意义。同时，获取目标障碍点时不完全依赖于地图中单个点，而是依赖于一定范围内最近点集加权得到的均值点，因此对于某些误差较大点，具有更强的适应性，对于匹配评价更准确，更能准确反应出点云与地图匹配的实际效果，不容易陷入局部极小值，更符合实际匹配情况。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
本发明实施例还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的激光点云与地图的匹配评价方法。
图2是本发明实施例2提供的一种激光点云与地图的匹配评价装置的结构示意图，如图2所示，包括栅格化模块100、距离计算模块200、评分模块300和求和模块400，
所述栅格化模块100用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；
所述距离计算模块200用于获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点，并计算所述激光点与对应目标障碍点的距离dmin；
所述评分模块300用于计算每个激光点的匹配得分；
所述求和模块400用于对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。
优选实施例中，所述距离计算模块200具体包括：
最近点圆获取单元201，用于获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；
最近点集获取单元202，用于获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为候选障碍点，并保存所述候选障碍点的世界坐标形成所述激光点对应的最近点集；
目标障碍点获取单元203，用于将所述最近点集中所有候选障碍点的均值点作为所述目标障碍点；
距离计算单元204，用于计算所述激光点与对应目标障碍点的距离 dmin。
优选实施例中，所述目标障碍点获取单元203用于当所述最近点集中包括一个候选障碍点时，将所述候选障碍点作为均值点；以及用于当最近点集中包括两个以上候选障碍点时，计算所述激光点与每个候选障碍点的欧拉距离，并将所述欧拉距离的倒数作为每个候选障碍点的权重，且对所有权重进行归一化处理，采用归一化处理后的权重求取所有候选障碍点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有候选障碍点的均值点坐标。
另一优选实施例中，所述目标障碍点获取单元203还用于当不存在候选障碍点形成最近点集时，将所有障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点。
本发明实施例还提供了一种激光点云与地图的匹配评价终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述激光点云与地图的匹配评价方法的步骤。图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，获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点，并计算所述激光点与对应目标障碍点的距离dmin；
步骤3，计算每个激光点的匹配得分；
步骤4，对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。
2.根据权利要求1所述激光点云与地图的匹配评价方法，其特征在于，所述获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点具体包括以下步骤：
S201，获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；
S202，获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为候选障碍点，并保存所述候选障碍点的世界坐标形成所述激光点对应的最近点集；
S203，将所述最近点集中所有候选障碍点的均值点作为所述目标障碍点。
3.根据权利要求2所述激光点云与地图的匹配评价方法，其特征在于，当所述最近点集中包括一个候选障碍点时，所述候选障碍点即为均值点；当最近点集中包括两个以上候选障碍点时，根据最近点集中每个候选障碍点的权重计算所有候选障碍点的均值点，具体包括以下步骤：
S2031，计算所述激光点与每个候选障碍点的欧拉距离，并将所述欧拉距离的倒数作为每个候选障碍点的权重；
S2032，对所有权重进行归一化处理，并采用归一化处理后的权重求取所有候选障碍点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有候选障碍点的均值点坐标。
4.根据权利要求2或3所述激光点云与地图的匹配评价方法，其特征在于，所述获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点还包括以下步骤：当不存在候选障碍点形成最近点集时，将所有障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点。
5.一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序被处理器执行时，实现权利要求1-4任一项所述的激光点云与地图的匹配评价方法。
6.一种激光点云与地图的匹配评价终端，其特征在于，包括权利要求5所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现如权利要求1-4任一项所述激光点云与地图的匹配评价方法的步骤。
7.一种激光点云与地图的匹配评价装置，其特征在于，包括栅格化模块、距离计算模块、评分模块和求和模块，
所述栅格化模块用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；
所述距离计算模块用于获取所述激光点云中每个激光点在所述环境地图上分别对应的目标障碍点，并计算所述激光点与对应目标障碍点的距离dmin；
所述评分模块用于计算每个激光点的匹配得分；
所述求和模块用于对所述激光点云中所有激光点的匹配得分进行求和，生成所述激光点云与所述环境地图的匹配总分。
8.根据权利要求7所述激光点云与地图的匹配评价装置，其特征在于，所述距离计算模块具体包括：
最近点圆获取单元，用于获取所述激光点云中每个激光点在所述环境地图上的最近点圆，所述最近点圆的圆心为所述激光点所在栅格的中心，半径为预设值；
最近点集获取单元，用于获取每个激光点在预设搜索范围内的所有障碍点，将处于对应的最近点圆范围内的障碍点作为候选障碍点，并保存所述候选障碍点的世界坐标形成所述激光点对应的最近点集；
目标障碍点获取单元，用于将所述最近点集中所有候选障碍点的均值点作为所述目标障碍点；
距离计算单元，用于计算所述激光点与对应目标障碍点的距离dmin。
9.根据权利要求8所述激光点云与地图的匹配评价装置，其特征在于，所述目标障碍点获取单元用于当所述最近点集中包括一个候选障碍点时，将所述候选障碍点作为均值点；以及用于当最近点集中包括两个以上候选障碍点时，计算所述激光点与每个候选障碍点的欧拉距离，并将所述欧拉距离的倒数作为每个候选障碍点的权重，且对所有权重进行归一化处理，采用归一化处理后的权重求取所有候选障碍点分别对应的障碍点世界坐标的加权平均值，所述加权平均值即为所有候选障碍点的均值点坐标。
10.根据权利要求8或9所述激光点云与地图的匹配评价装置，其特征在于，所述目标障碍点获取单元还用于当不存在候选障碍点形成最近点集时，将所有障碍点中处于所述最近点圆范围外且与所述激光点距离最近的障碍点作为目标障碍点。
CN202010425583.8A 2020-05-19 2020-05-19 一种激光点云与地图的匹配评价方法、介质、终端和装置 Active CN111707279B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010425583.8A CN111707279B (zh) 2020-05-19 2020-05-19 一种激光点云与地图的匹配评价方法、介质、终端和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010425583.8A CN111707279B (zh) 2020-05-19 2020-05-19 一种激光点云与地图的匹配评价方法、介质、终端和装置

Publications (2)

Publication Number Publication Date
CN111707279A true CN111707279A (zh) 2020-09-25
CN111707279B CN111707279B (zh) 2023-09-08

Family

ID=72537304

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010425583.8A Active CN111707279B (zh) 2020-05-19 2020-05-19 一种激光点云与地图的匹配评价方法、介质、终端和装置

Country Status (1)

Country Link
CN (1) CN111707279B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112306061A (zh) * 2020-10-28 2021-02-02 深圳优地科技有限公司 一种机器人控制方法以及机器人
CN112904367A (zh) * 2021-03-19 2021-06-04 北京小狗吸尘器集团股份有限公司 应用于扫地机重定位时的位置打分方法、装置和电子设备
CN113313765A (zh) * 2021-05-28 2021-08-27 上海高仙自动化科技发展有限公司 定位方法、装置、电子设备和存储介质
CN113436098A (zh) * 2021-06-25 2021-09-24 浙江合信地理信息技术有限公司 一种使用照相技术完善激光点影像测绘算法
WO2024093511A1 (zh) * 2022-10-31 2024-05-10 华为云计算技术有限公司 一种点云地图的评价方法以及装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180299273A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for positioning vehicle
CN110471422A (zh) * 2019-08-29 2019-11-19 南京理工大学 智能轮椅的障碍物检测与自主避障方法
CN110609290A (zh) * 2019-09-19 2019-12-24 北京智行者科技有限公司 激光雷达匹配定位方法及装置
US20200081119A1 (en) * 2018-09-07 2020-03-12 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for determining relative pose, device and medium
CN110969649A (zh) * 2019-11-29 2020-04-07 上海有个机器人有限公司 一种激光点云与地图的匹配评价方法、介质、终端和装置
CN111079801A (zh) * 2019-11-29 2020-04-28 上海有个机器人有限公司 基于点云匹配快速搜索最近点的方法、介质、终端和装置

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180299273A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for positioning vehicle
US20200081119A1 (en) * 2018-09-07 2020-03-12 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for determining relative pose, device and medium
CN110471422A (zh) * 2019-08-29 2019-11-19 南京理工大学 智能轮椅的障碍物检测与自主避障方法
CN110609290A (zh) * 2019-09-19 2019-12-24 北京智行者科技有限公司 激光雷达匹配定位方法及装置
CN110969649A (zh) * 2019-11-29 2020-04-07 上海有个机器人有限公司 一种激光点云与地图的匹配评价方法、介质、终端和装置
CN111079801A (zh) * 2019-11-29 2020-04-28 上海有个机器人有限公司 基于点云匹配快速搜索最近点的方法、介质、终端和装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李鑫;仲训昱;彭侠夫;龚赵慧;仲训杲;: "基于多分辨率搜索与多点云密度匹配的快速ICP-SLAM方法", no. 05 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112306061A (zh) * 2020-10-28 2021-02-02 深圳优地科技有限公司 一种机器人控制方法以及机器人
CN112904367A (zh) * 2021-03-19 2021-06-04 北京小狗吸尘器集团股份有限公司 应用于扫地机重定位时的位置打分方法、装置和电子设备
CN112904367B (zh) * 2021-03-19 2023-03-14 北京小狗吸尘器集团股份有限公司 应用于扫地机重定位时的位置打分方法、装置和电子设备
CN113313765A (zh) * 2021-05-28 2021-08-27 上海高仙自动化科技发展有限公司 定位方法、装置、电子设备和存储介质
CN113313765B (zh) * 2021-05-28 2023-12-01 上海高仙自动化科技发展有限公司 定位方法、装置、电子设备和存储介质
CN113436098A (zh) * 2021-06-25 2021-09-24 浙江合信地理信息技术有限公司 一种使用照相技术完善激光点影像测绘算法
WO2024093511A1 (zh) * 2022-10-31 2024-05-10 华为云计算技术有限公司 一种点云地图的评价方法以及装置

Also Published As

Publication number Publication date
CN111707279B (zh) 2023-09-08

Similar Documents

Publication Publication Date Title
CN110969649B (zh) 一种激光点云与地图的匹配评价方法、介质、终端和装置
CN111707279B (zh) 一种激光点云与地图的匹配评价方法、介质、终端和装置
CN108871353B (zh) 路网地图生成方法、***、设备及存储介质
CN111079801B (zh) 基于点云匹配快速搜索最近点的方法、介质、终端和装置
CN108645411B (zh) 基于粒子群算法的机器人路径规划方法、装置及终端设备
CN110927740A (zh) 一种移动机器人定位方法
CN112328715B (zh) 视觉定位方法及相关模型的训练方法及相关装置、设备
CN111707262B (zh) 基于最近点向量投影的点云匹配方法、介质、终端和装置
CN112595323A (zh) 机器人及其建图方法和装置
CN114565916B (zh) 目标检测模型训练方法、目标检测方法以及电子设备
CN110887493A (zh) 基于局部地图匹配的轨迹推算方法、介质、终端和装置
CN112154429A (zh) 高精度地图定位方法、***、平台及计算机可读存储介质
CN112819199A (zh) 降水量的预测方法、装置、设备和存储介质
CN115346183A (zh) 一种车道线检测方法、终端及存储介质
CN110887490B (zh) 一种激光定位导航的关键帧选取方法、介质、终端和装置
CN111598941A (zh) 一种杆塔倾斜度测量方法、装置、设备及存储介质
CN114674328B (zh) 地图生成方法、装置、电子设备、存储介质、及车辆
CN115952248A (zh) 终端设备的位姿处理方法、装置、设备、介质及产品
CN114910892A (zh) 一种激光雷达的标定方法、装置、电子设备及存储介质
CN114777757A (zh) 信标地图构建方法、装置、设备及存储介质
CN113723405A (zh) 区域轮廓的确定方法、装置和电子设备
CN117933519B (zh) 一种路网优化方法和相关装置
CN117788529B (zh) 三维平面点云粗配准方法、***、介质及设备
CN117075171B (zh) 激光雷达的位姿信息确定方法、装置、设备及存储介质
CN114674307B (zh) 一种重定位方法及电子设备

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant