CN115375869B - 一种机器人重定位方法、机器人和计算机可读存储介质 - Google Patents

Abstract

本申请公开了一种机器人重定位方法、机器人和计算机可读存储介质，该方法应用于目标场景中的机器人，机器人包括传感器，该方法包括：获取与目标场景对应的第一地图以及传感器采集的第一点云数据；基于第一地图，构建多分辨率地图，多分辨率地图包括第一地图与至少一个第二地图，边界阈值集合包括与第二地图对应的评分阈值；基于边界阈值集合中与第二地图对应的评分阈值，对第一地图与第二地图中的候选点进行筛选，得到匹配候选点；基于匹配候选点，生成机器人的重定位结果；第二地图的数量小于多分辨率地图的最大分层数量。通过上述方式，本申请能够提升重定位的效率。

Description

一种机器人重定位方法、机器人和计算机可读存储介质

技术领域

本申请涉及机器人技术领域，具体涉及一种机器人重定位方法、机器人和计算机可读存储介质。

背景技术

当前机器人的自主导航技术为机器人领域的研究热点，大多自主导航方案通过匹配激光扫描和全局地图，来实现机器人的位姿推算，该方案的使用依赖于能够成功获取机器人在全局地图中的初始位姿，即重定位，但该方案重定位的效率较低，导致导航效果不佳，因此如何提升重定位的效率成为亟待解决的问题。

发明内容

本申请提供一种机器人重定位方法、机器人和计算机可读存储介质，能够提升重定位的效率。

为解决上述技术问题，本申请采用的技术方案是：提供一种机器人重定位方法，该方法应用于目标场景中的机器人，机器人包括传感器，该方法包括：获取与目标场景对应的第一地图以及传感器采集的第一点云数据；基于第一地图，构建多分辨率地图，多分辨率地图包括第一地图与至少一个第二地图，第一地图的分辨率大于所有第二地图的分辨率，边界阈值集合包括与第二地图对应的评分阈值；基于边界阈值集合中与第二地图对应的评分阈值，对第一地图与第二地图中的候选点进行筛选，得到匹配候选点；基于匹配候选点，生成机器人的重定位结果；其中，第二地图的数量小于多分辨率地图的最大分层数量。

为解决上述技术问题，本申请采用的另一技术方案是：提供一种机器人，该机器人包括互相连接的存储器和处理器，其中，存储器用于存储计算机程序，计算机程序在被处理器执行时，用于实现上述技术方案中的机器人重定位方法。

为解决上述技术问题，本申请采用的另一技术方案是：提供一种计算机可读存储介质，该计算机可读存储介质用于存储计算机程序，计算机程序在被处理器执行时，用于实现上述技术方案中的机器人重定位方法。

通过上述方案，本申请的有益效果是：先获取与目标场景对应的第一地图以及传感器采集的第一点云数据；然后利用第一地图构建出多分辨率地图，该多分辨率地图包括第一地图与至少一个第二地图，第一地图的分辨率大于所有第二地图的分辨率，第二地图的数量小于多分辨率地图的最大分层数量，最大分层数量通过第一地图的分辨率与最低分辨率计算得到，最低分辨率为所有第二地图的分辨率中的最小值；然后基于边界阈值集合中第二地图对应的评分阈值，对第一地图与第二地图中的候选点进行筛选，得到匹配候选点；然后对匹配候选点进行处理，生成机器人的重定位结果；本方案通过将多分辨率地图的层数设置得较少，减少了候选点的数量，并通过边界阈值集合中的评分阈值对各层地图中的候选点进行删减，避免访问无效的候选点，保证了算法在计算资源受限的平台仍能有较好的表现，且能够降低对候选点进行计算花费的时间，有助于提高机器人重定位的效率。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。其中：

图1是本申请提供的机器人重定位方法一实施例的流程示意图；

图2是本申请提供的机器人重定位方法另一实施例的流程示意图；

图3是本申请提供的多分辨率地图中候选点的连接方式的示意图；

图4是图2所示的实施例中S35的流程示意图；

图5是图4所示的实施例中S56的流程示意图；

图6是图5所示的实施例中S64的流程示意图；

图7是图6所示的实施例中S72的流程示意图；

图8是本申请提供的预测粒子与量测粒子的示意图；

图9是本申请提供的预测粒子与量测粒子的另一示意图；

图10是本申请提供的机器人一实施例的结构示意图；

图11是本申请提供的计算机可读存储介质一实施例的结构示意图。

具体实施方式

下面结合附图和实施例，对本申请作进一步的详细描述。特别指出的是，以下实施例仅用于说明本申请，但不对本申请的范围进行限定。同样的，以下实施例仅为本申请的部分实施例而非全部实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。

在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

需要说明的是，本申请中的术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或者隐含地包括至少一个该特征。本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

请参阅图1，图1是本申请提供的机器人重定位方法一实施例的流程示意图，该方法应用于目标场景中的机器人，即本实施例的执行主体为机器人，该方法包括：

S11：获取与目标场景对应的第一地图以及传感器采集的第一点云数据。

机器人包括传感器，该传感器可以为激光雷达，但不限于此，采用该激光雷达能够对目标场景进行扫描，得到第一点云数据。

进一步地，机器人还可包括存储设备，可预先获取目标场景的全局地图（即第一地图），然后将第一地图存储在存储设备中，该第一地图为栅格地图，即第一地图包括多个尺寸相同的栅格。

S12：基于第一地图，构建多分辨率地图。

在获取到第一地图后，对第一地图进行处理，以生成多分辨率地图。具体地，该多分辨率地图包括多层地图，多层地图包括第一地图与至少一个第二地图，第一地图的分辨率大于所有第二地图的分辨率，第二地图的数量小于多分辨率地图的最大分层数量。具体地，先计算第一地图的分辨率与最低分辨率的比值，然后对该比值进行取对数运算，得到最大分层数量。例如，可设置第二地图的最大层号为n，第二地图的最小层号为m，即m＜n，则第二地图的数量为n-m+1，最大分层数量为n。

进一步地，重定位算法通常以环境扫描和地图的匹配为导向，对机器人在全局地图中的可能位姿进行搜索；传统基于遍历或基于粒子滤波的重定位方案计算量较大，无法满足一些应用需求；基于此，本实施例提出了一种基于轻量化分支定界的重定位候选点搜索方法，该方案以分支定界算法为基础，通过减少多分辨率地图的层数或基于边界阈值集合减少对一些候选点的访问等优化操作进行算法加速，实现对重定位候选点的快速搜索。

分支定界算法通过“分支”拆解问题，构建求解框架，并通过“定界”进行剪支，实现算法加速。在重定位问题中，多分辨地图是构建分支的基础，因此以第一地图为底图，生成多层（例如，3-7层）的多分辨率地图，多分辨率地图为第一地图的膨胀地图，下面对多分辨率地图进行详细说明。

假设第一地图中栅格的尺寸为r×r，则第一地图的分辨率为1/r，多分辨率地图的设定如下所示：

A1）设定第一地图的层号为0，第二地图的层号分别为m、m+1、…、n。

A2）第i层地图的分辨率为1/(2ⁱ×r)，第i层地图中候选点的选取间隔为2ⁱ×r，i∈{0,m,m+1,…,n}。

A3）设定第m层地图为所有第二地图中分辨率最高的地图（简称为底层地图），第n层地图为所有第二地图中分辨率最低的地图（简称为顶层地图）。

其中，m的数值设定与环境复杂度相关，环境越复杂，m越小，反之越大；n的数值设定与第一地图的尺寸相关，例如，如果第一地图的尺寸为h×w，则n为大于log₂(max(h,w))的最小整数。

通常情况下，在一定范围内调整m对算法的结果没有影响，因此可提高m的数值，从而降低多分辨率地图的层数，进而在后续分支定界算法的执行过程中减少分支数量，提高算法的计算效率。例如，在m=2、n=6对应的多分辨率地图中，n=6，m为0、1或2对重定位结果的影响较小，因此可以设定m=2。

S13：基于边界阈值集合中与第二地图对应的评分阈值，对第一地图与第二地图中的候选点进行筛选，得到匹配候选点。

对于多分辨率地图来说，可在对多分辨率地图的各层地图进行访问的过程中，执行多阈值剪枝操作，即基于边界阈值集合，确定是否访问多分辨率地图中的某些候选点，如果确定不访问该候选点，则说明该候选点与当前机器人所处的位置较远，此时可不访问该候选点；通过对第一地图与所有第二地图进行搜索匹配，实现了从多分辨率地图的所有候选点中选出某些候选点，得到匹配候选点，这些匹配候选点与当前机器人所处位置的相关性较高。

进一步地，该边界阈值集合包括至少两个评分阈值，该边界阈值集合中评分阈值可被更新；例如，在初始时刻，根据经验或应用需要，设置边界阈值集合中的评分阈值的具体数值；在后续执行多分辨率地图的搜索匹配的过程中，更新边界阈值集合中的评分阈值。

S14：基于匹配候选点，生成机器人的重定位结果。

在通过对多分辨率地图进行处理生成匹配候选点后，可对该匹配候选点进行处理，得到当前机器人的重定位结果。

本实施例采用了一种轻量化分支定界的重定位候选点搜索方案，该方案以分支定界算法为基础，先通过降低多分辨率地图的层数，减少了分支数量；然后，采用基于多阈值剪枝的定界方法，加速了剪枝效率，避免了对无效节点的访问，保证了算法在计算资源受限平台仍能有较好的表现，而且，由于执行了剪枝操作，实现避免访问一些候选点，降低了计算这些候选点所需的时间，有助于缩短时间，能够提高机器人全局重定位的效率。

请参阅图2，图2是本申请提供的机器人重定位方法另一实施例的流程示意图，该方法应用于目标场景中的机器人，该机器人包括传感器，该方法包括：

S31：获取与目标场景对应的第一地图以及传感器采集的第一点云数据。

S32：基于第一地图，构建多分辨率地图。

多分辨率地图包括第一地图与至少一个第二地图，边界阈值集合包括与第二地图对应的评分阈值。

其中，S31-S32分别与上述实施例中的S11-S12相同，在此不再赘述。

S33：基于边界阈值集合中与第二地图对应的评分阈值，对第一地图与第二地图中的候选点进行筛选，得到粗匹配候选点与精匹配候选点。

按照如下方案对多分辨率地图进行处理：

（1）从多分辨率地图中选出当前层地图。

沿着从顶层地图向底层地图的顺序，从多分辨率地图中选出当前层地图。

（2）从当前层地图中取出当前候选点，计算当前候选点的评分。

由于分支搜索需要以候选点的评分为导向，因此从当前层地图的候选点中取出当前候选点，计算当前候选点的评分。具体地，为了计算出当前候选点的评分，可以基于当前候选点，对第一点云数据进行变换处理，得到第二点云数据，即相当于将第一点云数据变换到当前候选点所处的位姿处；将第二点云数据与当前层地图进行匹配，得到当前候选点的评分。

进一步地，利用当前候选点与第一点云数据生成第二点云数据的方法可参考相关技术，在此不再赘述。当前候选点的评分可以为第二点云数据和当前层地图的匹配率。

（3）基于当前候选点的评分与当前层地图的评分阈值，判断是否访问下一层候选点。

判断当前候选点的评分是否大于或等于边界阈值集合中当前层地图的评分阈值；若当前候选点的评分大于或等于当前层地图的评分阈值，则确定访问下一层候选点，该下一层候选点为下一层地图中与当前候选点对应的候选点。例如，如图3所示，对于第i+1层地图中的候选点B来说，其与第i层地图中的候选点C11、C12、C21、C22对应。

进一步地，在基于当前候选点的评分与当前层地图的评分阈值，判断是否访问下一层候选点之前，可判断当前层地图的层号是否大于第一预设值；若当前层地图的层号大于第一预设值，则当前层地图的评分阈值为第一最高评分；若当前层地图的层号小于或等于第一预设值，则当前层地图的评分阈值为第二最高评分。具体地，第一预设值可以为m+1，第一最高评分为第一地图中已访问过的候选点中评分的最大值，第二最高评分为参考层中已访问过的候选点中评分的最大值，参考层的层号与当前层的层号的差值为第二预设值，且参考层的层号小于当前层的层号，第二预设值可以为2。

如果当前候选点的评分小于当前层地图的评分阈值，则表明无需访问当前候选点对应的下一层候选点，此时将当前候选点更新为下一候选点，下一候选点为当前层地图中当前候选点的下一个候选点；返回基于当前候选点的评分与当前层地图的评分阈值，判断是否访问下一层候选点的步骤，直至对多分辨率地图遍历完毕，以生成匹配候选点。

（4）若判定出访问下一层候选点，则计算下一层候选点的评分。

如果判定出需要访问下一层候选点，则基于下一层候选点对第一点云数据进行变换处理，得到第二点云数据；将第二点云数据与下一层地图进行匹配，得到下一层候选点的评分。然后，将当前候选点更新为下一层候选点，将当前层地图更新为下一层地图，并返回基于当前候选点的评分与当前层地图的评分阈值，判断是否访问下一层候选点的步骤，直至对多分辨率地图遍历完毕，以生成匹配候选点。

可以理解地，如果判断出不存在下一层候选点，则访问当前层地图中的剩余候选点。

在一实施方式中，分支定界算法从顶层地图开始搜索，计算顶层地图中候选点的评分，以深度优先搜索算法为搜索逻辑，逐步向底层地图延伸搜索，该过程遵循以下规则进行候选点搜索：

若在第i层地图中访问候选点{(k₁r_i,k₂r_i)|r_i=2ⁱ×r}，则优先向下一层地图延伸搜索，访问下一层地图中与该候选点对应的候选点（即下一层候选点），在第i-1层地图中访问的候选点如下所示：

{(k₁r_i,k₂r_i),(k₁r_i+r_i-1,k₂r_i),(k₁r_i,k₂r_i+r_i-1),(k₁r_i+r_i-1,k₂r_i+r_i-1)|r_i=2ⁱ×r}（1）

其中，k₁、k₂为第i层地图中的候选点的索引，例如，如图3所示，候选点B对应的k₁为1、k₂为0。

通过上述方法可以构建候选点的搜索框架，但是该框架的搜索效率较低，因此引入多阈值剪枝进行算法加速。具体地，在分支搜索过程中，通过维护边界阈值集合的方式对候选点进行剪枝，设当前访问第i层地图中的候选点c_i(i=m+1,m+2,…,n)，若候选点c_i的评分小于thr_i，则执行剪枝操作，即当前候选点无需再向下一层地图中的相应候选点（即下一层候选点）延伸搜索；其中，{thr_i|i=m+1,m+2,…,n}为分支搜索过程中利用已访问的候选点的评分持续维护的边界阈值集合，且thr_i的定义如下所示：

当i=m+1时，thr_i=ms₀（2）

当i>m+1时，thr_i=ms_i-2（3）

其中，ms_i表示第i层地图中已访问的候选点的最高评分。

通过执行上述方案可以实现对多分辨率地图中的各层地图进行搜索匹配，得到每层地图中候选点的评分；本方案通过多阈值剪枝的方式，减少了不必要的候选点的评分计算，实现了算法加速，提升了算法的速度。

当第一地图出现信息稀疏或点云/地图误差等情况时，低分辨率的地图依旧能够获得较高的评分，因此低分辨率的地图有较高的抗干扰能力，但是在小场景下候选点的匹配评分区分度较低，结果可靠性不足；高分辨率的地图中候选点的匹配评分更加精准，但也使得评分对误差更加敏感，抗干扰能力较差。基于此，本实施例将两者的优势互补，提出一种结合精/粗匹配的重定位候选点提取及综合筛选方法，以生成匹配候选点，该匹配候选点包括第一匹配候选点与第二匹配候选点，下面进行具体描述。

值得说明的是，搜索匹配的具体逻辑与相关技术中类似，在此不予详述，仅作简要说明，即先对第n层地图中的候选点（记作P1）进行访问，计算候选点P1的评分，判断候选点P1的评分是否小于第n层地图的评分阈值，若候选点P1的评分小于第n层地图的评分阈值，则访问第n层地图中的另一候选点（记作P2）；若候选点P1的评分大于或等于第n层地图的评分阈值，则判断第n-1层地图中是否存在与候选点P1连接的候选点；若第n-1层地图中不存在与候选点P1连接的候选点，则访问第n层地图中的剩余候选点（记作P3）；若第n-1层地图中存在与候选点P1连接的候选点，则访问第n-1层地图中与候选点P1连接的候选点（记作P4），计算候选点P4的评分，判断候选点P4的评分是否小于第n-1层地图的评分阈值；若候选点P4的评分大于或等于第n-1层地图的评分阈值，则判断第n-2层地图中是否存在与候选点P3连接的候选点，若是，则访问第n-2层地图中与候选点P4连接的候选点（记作P5），若否，则访问第n-1层地图中与候选点P1连接的另一候选点（记作P6）；按照该逻辑，实现对多分辨率地图的访问。

S34：分别对粗匹配候选点与精匹配候选点进行筛选处理，得到第一匹配候选点与第二匹配候选点。

在当前层地图的层号为第三预设值时，判断当前候选点的评分是否大于第一评分阈值；若当前候选点的评分大于第一评分阈值，则将当前候选点确定为粗匹配候选点并存储。具体地，第三预设值可以为m，即当分支定界算法访问到第m层地图时，对评分满足第一评分阈值的候选点进行存储，将其作为粗匹配候选点。

在当前层地图的层号为第四预设值时，判断当前候选点的评分是否大于第二评分阈值；若当前候选点的评分大于第二评分阈值，则将当前候选点确定为精匹配候选点并存储。具体地，第四预设值可以为0，即当分支定界算法访问到第0层地图时，对评分满足第二评分阈值的候选点进行存储，将其作为精匹配候选点。

可以理解地，可以在获取到每层地图中候选点的评分后，基于候选点的评分，对边界阈值集合进行更新，即边界阈值集合通过第一地图中的候选点的评分与第二地图中的候选点的评分更新。

在获取到粗匹配候选点与精匹配候选点之后，分别对粗匹配候选点与精匹配候选点进行筛选处理，得到第一匹配候选点与第二匹配候选点。

例如，设精匹配候选点与粗匹配候选点的最高评分分别为ms_fine与ms_course，第一评分阈值为h×ms_fine，第二评分阈值为h×ms_fine，h为调整系数，0<h<1；判断精匹配候选点的评分是否大于h×ms_fine，若是，则将该精匹配候选点记作第一匹配候选点；判断粗匹配候选点的评分是否大于h×ms_course，若是，则将该粗匹配候选点记作第二匹配候选点。

S35：基于第一匹配候选点与第二匹配候选点，生成重定位结果。

采用图4所示的方案来生成重定位结果：

S51：对第一匹配候选点与第二匹配候选点进行合并处理，得到合并结果。

将第一匹配候选点与第二匹配候选点合并，并剔除重复候选点，得到合并结果，此时合并结果中的候选点即为当前重定位点的有效候选点，将其称作第一有效候选点。

S52：判断合并结果中是否为空。

重定位结果的第一个判定条件为判断第一有效候选点的数量是否为0个。

S53：在合并结果中为空时，判定重定位失败。

如果第一有效候选点的数量为0，则判定重定位失败。

S54：在合并结果不为空时，判定合并结果中第一有效候选点的数量是否为第一预设数量。

第一预设数量可以为1，即如果第一有效候选点的数量大于0，则判断第一有效候选点的数量是否为1。

S55：若合并结果中第一有效候选点的数量为第一预设数量，则将第一有效候选点的位姿确定为重定位结果。

重定位结果包括重定位位姿，如果第一有效候选点的数量为1，则判定重定位成功，第一有效候选点的位姿即为重定位位姿。

S56：若合并结果中第一有效候选点的数量不为第一预设数量，则对合并结果进行处理，得到重定位结果。

如果第一有效候选点的数量大于1，则表明机器人处于复杂或相似干扰环境中，无法确定正确候选点，此时需执行后续逻辑作进一步判断。具体地，驱动机器人前往下一重定位点，再次执行上述步骤；为了进一步提高重定位结果的置信度，本实施例提出一种基于粒子滤波的重定位候选点约束关联方法，其为可融合多传感器的重定位候选点约束关联方案，对不同重定位点的第一有效候选点进行关联校验，该方案以粒子滤波框架为基础，获取机器人转移到下一重定位点的过程中其他传感器信息（如：里程计或视觉传感器等采集的信息），实现对机器人的重定位结果及其他传感器信息的综合判定，以保证重定位结果的正确性，只需设定少量阈值，即可实现多传感器信息的综合校验，提高了算法的可拓展性及调试优化的便捷性，下面结合图5进行具体说明。

S61：获取机器人所处的当前重定位点的编号，并判断当前重定位点的编号是否为第五预设值。

第五预设值可以为1，即读取机器人当前所处的重定位点的编号rn，若rn=1，则执行S62；否则，执行S64。

S62：若当前重定位点的编号为第五预设值，则基于当前重定位点的第一有效候选点生成量测粒子。

在当前重定位点的编号为1时，表明该当前重定位点为第一个重定位点，此时可以执行粒子滤波算法：先初始化，然后根据第一个重定位点的第一有效候选点

生成量测粒子{mp_k}，其中，

表示第一个重定位点的第k个第一有效候选点，mp_k表示第k个候选点对应生成的量测粒子，量测粒子的位姿为第一有效候选点的位姿，量测粒子的权重为第一有效候选点的评分，然后执行S63。

S63：将当前重定位点的编号与第六预设值叠加，以对编号进行更新，将当前重定位点更新为下一重定位点。

第六预设值可以为1，驱动机器人前往下一重定位点，将rn加一，获取当前重定位点的第一有效候选点，并返回获取机器人所处的当前重定位点的编号的步骤，即返回执行S61。

S64：若当前重定位点的编号不为第五预设值，则获取历史量测粒子，基于获取历史量测粒子，生成重定位结果。

在当前重定位点的编号不为1时，可以采用图6所示的方案：

S71：基于其他传感器信息与历史量测粒子，生成第一预测粒子。

获取历史量测粒子{mp_k}，并以历史量测粒子为起点，根据其他传感器信息生成预测粒子

，

表示第k个历史量测粒子mp_k生成的第i个预测粒子，将其记作第一预测粒子。

进一步地，可根据传感器的不同进行适配，例如：如果其他传感器为未与激光雷达标定外参的里程计/视觉传感器等，则只能够获取两个重定位点的距离信息和航向信息，此时可生成图8所示的预测粒子。如果其他传感器为已与激光雷达标定外参的里程计/视觉传感器或激光雷达自身等，则能够获取两个重定位点的相对位姿变化，此时可生成图9所示的预测粒子。

S72：对第一预测粒子进行筛选，得到第二预测粒子。

第一预测粒子的筛选方案如图7所示，包括以下步骤：

S81：计算第一预测粒子的权重，并判断第一预测粒子的权重是否大于或等于第一权重阈值。

获取当前重定位点的所有第一有效候选点中与第一预测粒子的距离最近的第一有效候选点，得到第二有效候选点；基于第二有效候选点与第一预测粒子，计算出第一预测粒子的权重。

S82：若第一预测粒子的权重大于或等于第一权重阈值，则将第一预测粒子确定为第二预测粒子。

S83：若第一预测粒子的权重小于第一权重阈值，则将第一预测粒子删除。

从所有第一预测粒子中删除权重小于第一权重阈值的第一预测粒子，得到第二预测粒子。具体地，第一权重阈值可以根据经验或应用需要进行设置；对于每个第一预测粒子来说，在当前重定位点的第一有效候选点{vc^rn}中搜索与第一预测粒子距离最近的第一有效候选点，通过下式计算该第一预测粒子的权重

：

（4）

其中，

表示由历史量测粒子mp_h及其他传感器信息生成的第一预测粒子，

为第二有效候选点，

；Σ表示正态分布协方差，Σ的数值与传感器的性能有关，通常为定值，Σ∈R^3×3。

S73：对第二预测粒子进行聚簇，以生成预测粒子簇；基于预测粒子簇，生成质心粒子；对质心粒子与对应的历史量测粒子进行合并，生成第一量测粒子。

以欧氏距离为量度对第二预测粒子进行聚簇，在聚簇过程中保证同一粒子簇中的第二预测粒子均由同一历史量测粒子生成；然后，根据预测粒子簇生成质心粒子，质心粒子的位姿为预测粒子簇的质心，质心粒子的权重为预测粒子簇的最大粒子权重。

进一步地，由于质心粒子与第二预测粒子均由同一历史量测粒子产生，因此可将该历史量测粒子和质心粒子合并，生成第一量测粒子。具体地，对质心粒子的权重以及与质心粒子对应的历史量测粒子的权重进行叠加，以更新质心粒子的权重；将历史量测粒子删除，并将质心粒子确定为第一量测粒子。

S74：计算第一量测粒子的最大权重，并基于最大权重，对第一量测粒子进行筛选，得到第二量测粒子。

判断第一量测粒子的权重是否大于第二权重阈值，第二权重阈值与最大权重以及编号相关；若第一量测粒子的权重大于第二权重阈值，则将第一量测粒子确定为第二量测粒子。例如，所有第一量测粒子的最大权重记作ms_particle，第二权重阈值为h^rn×ms_particle，提取出所有权重大于h^rn×ms_particle的第一量测粒子，生成第二量测粒子，h的数值与上述计算匹配候选点（包括第一匹配候选点与第二匹配候选点）时h的数值相同。

S75：基于第二量测粒子，生成重定位结果。

判断第二量测粒子的数量是否为第二预设数量；若第二量测粒子的数量为第二预设数量，则将最大权重对应的第二量测粒子的位姿确定为重定位结果。具体地，第二预设数量可以为1，重定位结果的第二个判定条件为判断第二量测粒子的数量是否为1个，若第二量测粒子的数量为1，则判定重定位成功，最大权重对应的第二量测粒子的位姿即为重定位位姿。

S65：判断当前重定位点的编号是否达到重定位次数阈值。

如果当前重定位点的编号小于重定位次数阈值，则执行S63。

S66：若当前重定位点的编号达到重定位次数阈值，则确定重定位失败。

如果当前重定位点的编号等于预设的重定位次数阈值，则表明此次重定位失败。

本实施例方案为了解决复杂或相似干扰环境下，重定位正确率低的问题，以粒子滤波框架为基础，提出了一套可融合多传感器的重定位候选点约束关联框架，该候选点约束关联框架只需对重定位候选点进行粒子化处理，避免了大量随机样本涌入带来的计算资源的浪费，提高了资源利用率；通过设定重定位结果的外部判定条件，避免了传统粒子滤波框架中算法长时间无法收敛造成的冗余计算，保证了算法的灵活性，相比传统的粒子滤波框架，该框架具有可扩展性好、调试便捷、轻量化以及灵活性高等优点。

本申请提供了一种可融合多传感器的机器人快速重定位方法，以分支定界算法为基础，通过地图降层、多阈值剪枝等操作对分支定界算法进行优化，保证了候选点全局搜索的计算效率；而且，采用精匹配和粗匹配结合的方式计算匹配候选点，提高了算法对底图和点云噪声的抗干扰能力，同时降低了正确的重定位候选点丢失的可能性。此外，考虑到相似场景干扰下正确候选点易丢失的情况，本方案提出了一种重定位候选点的综合筛选方法，结合基于粒子滤波的重定位候选点约束关联方法，保证了重定位结果的正确性。综上，本实施例通过基于轻量化分支定界的重定位候选点搜索方法、重定位候选点提取及综合筛选方法以及基于粒子滤波的重定位候选点约束关联方法的结合，保证了重定位结果的正确，实现了提升重定位算法的精准度、鲁棒性和实现效率。

请参阅图10，图10是本申请提供的机器人一实施例的结构示意图，机器人110包括互相连接的存储器111和处理器112，存储器111用于存储计算机程序，计算机程序在被处理器112执行时，用于实现上述实施例中的机器人重定位方法。

本实施例为了提高机器人全局重定位的鲁棒性，提出了一套重定位候选点保留及综合筛选方案，先在分支定界执行过程中，采用精匹配和粗匹配配合计算的方式提取候选点，能够在保证候选点区分度的前提下，提高了算法对底图质量和点云噪声的抗干扰能力，减小了有效候选点丢失的可能性。而且，在候选点的处理过程中，通过综合筛选能够对相似场景进行辨识，降低了阈值设定对算法效果的影响，提高了算法对复杂或相似场景的适应性。

请参阅图11，图11是本申请提供的计算机可读存储介质一实施例的结构示意图，计算机可读存储介质120用于存储计算机程序121，计算机程序121在被处理器执行时，用于实现上述实施例中的机器人重定位方法。

计算机可读存储介质120可以是服务端、U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

在本申请所提供的几个实施方式中，应该理解到，所揭露的方法以及设备，可以通过其它的方式实现。例如，以上所描述的设备实施方式仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。

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

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

若本申请技术方案涉及个人信息，应用本申请技术方案的产品在处理个人信息前，已明确告知个人信息处理规则，并取得个人自主同意。若本申请技术方案涉及敏感个人信息，应用本申请技术方案的产品在处理敏感个人信息前，已取得个人单独同意，并且同时满足“明示同意”的要求。例如，在摄像头等个人信息采集装置处，设置明确显著的标识告知已进入个人信息采集范围，将会对个人信息进行采集，若个人自愿进入采集范围即视为同意对其个人信息进行采集；或者在个人信息处理的装置上，利用明显的标识/信息告知个人信息处理规则的情况下，通过弹窗信息或请个人自行上传其个人信息等方式获得个人授权；其中，个人信息处理规则可包括个人信息处理者、个人信息处理目的、处理方式以及处理的个人信息种类等信息。

以上仅为本申请的实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (16)

1.一种机器人重定位方法，其特征在于，应用于目标场景中的机器人，所述机器人包括传感器，所述方法包括：

获取与所述目标场景对应的第一地图以及所述传感器采集的第一点云数据；

基于所述第一地图，构建多分辨率地图，所述多分辨率地图包括第一地图与至少一个第二地图，所述第一地图的分辨率大于所有所述第二地图的分辨率；

从所述多分辨率地图中选出当前层地图；

从所述当前层地图中取出当前候选点，计算当前候选点的评分；

在所述当前层地图的层号为第三预设值时，判断所述当前候选点的评分是否大于第一评分阈值；若是，则将所述当前候选点确定为粗匹配候选点并存储；

在所述当前层地图的层号为第四预设值时，判断所述当前候选点的评分是否大于第二评分阈值；若是，则将所述当前候选点确定为精匹配候选点并存储；

分别对所述粗匹配候选点与所述精匹配候选点进行筛选处理，得到第一匹配候选点与第二匹配候选点；

基于所述第一匹配候选点与所述第二匹配候选点，生成重定位结果；

其中，所述第二地图的数量小于所述多分辨率地图的最大分层数量。

2.根据权利要求1所述的机器人重定位方法，其特征在于，所述方法还包括：

基于所述当前候选点的评分与所述当前层地图的评分阈值，判断是否访问下一层候选点，所述下一层候选点为下一层地图中与所述当前候选点对应的候选点；

若是，则计算所述下一层候选点的评分；

将所述当前候选点更新为所述下一层候选点，将所述当前层地图更新为所述下一层地图，并返回所述基于所述当前候选点的评分与所述当前层地图的评分阈值，判断是否访问下一层候选点的步骤，直至对所述多分辨率地图遍历完毕，以生成匹配候选点。

3.根据权利要求2所述的机器人重定位方法，其特征在于，所述计算当前候选点的评分的步骤，包括：

基于所述当前候选点，对所述第一点云数据进行变换处理，得到第二点云数据；

将所述第二点云数据与所述当前层地图进行匹配，得到所述当前候选点的评分。

4.根据权利要求2所述的机器人重定位方法，其特征在于，所述基于所述当前候选点的评分与所述当前层地图的评分阈值，判断是否访问下一层候选点的步骤，包括：

判断所述当前候选点的评分是否大于或等于所述当前层地图的评分阈值；

若是，则确定访问所述下一层候选点。

5.根据权利要求2所述的机器人重定位方法，其特征在于，所述基于所述当前候选点的评分与所述当前层地图的评分阈值，判断是否访问下一层候选点的步骤之前，包括：

判断所述当前层地图的层号是否大于第一预设值；

若是，则所述当前层地图的评分阈值为第一最高评分；

若否，则所述当前层地图的评分阈值为第二最高评分；

其中，所述第一最高评分为所述第一地图中已访问过的候选点中评分的最大值，所述第二最高评分为参考层中已访问过的候选点中评分的最大值，所述参考层的层号与所述当前层的层号的差值为第二预设值，且所述参考层的层号小于所述当前层的层号。

6.根据权利要求1所述的机器人重定位方法，其特征在于，所述基于所述第一匹配候选点与所述第二匹配候选点，生成所述重定位结果的步骤，包括：

对所述第一匹配候选点与所述第二匹配候选点进行合并处理，得到合并结果；

判断所述合并结果中是否为空；

在所述合并结果中为空时，判定重定位失败；

在所述合并结果不为空时，判定所述合并结果中第一有效候选点的数量是否为第一预设数量；

若是，则将所述第一有效候选点的位姿确定为所述重定位结果；

若否，则对所述合并结果进行处理，得到所述重定位结果。

7.根据权利要求6所述的机器人重定位方法，其特征在于，所述对所述合并结果进行处理，得到所述重定位结果的步骤，包括：

获取所述机器人所处的当前重定位点的编号，并判断所述编号是否为第五预设值；

若否，则获取历史量测粒子，基于所述获取历史量测粒子，生成所述重定位结果。

8.根据权利要求7所述的机器人重定位方法，其特征在于，所述基于所述获取历史量测粒子，生成所述重定位结果的步骤，包括：

基于其他传感器信息与所述历史量测粒子，生成第一预测粒子；

对所述第一预测粒子进行筛选，得到第二预测粒子；

对所述第二预测粒子进行聚簇，以生成预测粒子簇；

基于所述预测粒子簇，生成质心粒子；

对所述质心粒子与对应的历史量测粒子进行合并，生成第一量测粒子；

计算所述第一量测粒子的最大权重，并基于所述最大权重，对所述第一量测粒子进行筛选，得到第二量测粒子；

基于所述第二量测粒子，生成所述重定位结果。

9.根据权利要求8所述的机器人重定位方法，其特征在于，所述对所述质心粒子与对应的历史量测粒子进行合并，生成第一量测粒子的步骤，包括：

对所述质心粒子的权重以及与所述质心粒子对应的历史量测粒子的权重进行叠加，以更新所述质心粒子的权重；

将所述历史量测粒子删除，并将所述质心粒子确定为所述第一量测粒子。

10.根据权利要求8所述的机器人重定位方法，其特征在于，所述基于所述最大权重，对所述第一量测粒子进行筛选，得到第二量测粒子的步骤，包括：

判断所述第一量测粒子的权重是否大于第二权重阈值；

若是，则将所述第一量测粒子确定为所述第二量测粒子；

所述基于所述第二量测粒子，生成所述重定位结果的步骤，包括：

判断所述第二量测粒子的数量是否为第二预设数量；

若是，则将所述最大权重对应的第二量测粒子的位姿确定为所述重定位结果。

11.根据权利要求8所述的机器人重定位方法，其特征在于，所述对所述第一预测粒子进行筛选，得到第二预测粒子的步骤，包括：

计算所述第一预测粒子的权重，并判断所述第一预测粒子的权重是否大于或等于第一权重阈值；

若是，则将所述第一预测粒子确定为所述第二预测粒子；

若否，则将所述第一预测粒子删除。

12.根据权利要求11所述的机器人重定位方法，其特征在于，所述计算所述第一预测粒子的权重的步骤，包括：

获取所述当前重定位点的所有第一有效候选点中与所述第一预测粒子的距离最近的第一有效候选点，得到第二有效候选点；

基于所述第二有效候选点与所述第一预测粒子，计算出所述第一预测粒子的权重。

13.根据权利要求7所述的机器人重定位方法，其特征在于，所述方法还包括：

判断所述编号是否达到重定位次数阈值；

若是，则确定重定位失败；

若否，将所述编号与第六预设值叠加，以对所述编号进行更新，将所述当前重定位点更新为下一重定位点，并返回所述获取所述机器人所处的当前重定位点的编号的步骤。

14.根据权利要求2所述的机器人重定位方法，其特征在于，所述方法还包括：

基于所述候选点的评分，对边界阈值集合进行更新。

15.一种机器人，其特征在于，包括互相连接的存储器和处理器，其中，所述存储器用于存储计算机程序，所述计算机程序在被所述处理器执行时，用于实现权利要求1-14中任一项所述的机器人重定位方法。

16.一种计算机可读存储介质，用于存储计算机程序，其特征在于，所述计算机程序在被处理器执行时，用于实现权利要求1-14中任一项所述的机器人重定位方法。

Images