CN111380510A - 重定位方法及装置、机器人 - Google Patents

Abstract

本发明公开了一种重定位方法，该方法包括：获取多个子图边界点；将多个子图边界点绘制为第一子图；从第一子图中裁剪出第二子图；利用第二子图对已知地图进行模板匹配获取多个第一匹配结果；根据第二子图在第一子图中的位置关系，分别获取每个第一匹配结果对应的第二匹配结果；分别将第二匹配结果中的边界点与子图边界点进行配准以从多个第二匹配结果中筛选出第三匹配结果；利用定位条件从第三匹配结果中选出重定位结果。本发明还公开了一种重定位装置、机器人和可读存储介质。通过上述方式，本发明能够在出现环境变化的情况下实现准确率高、精确率高、错误率低的重定位。

Description

重定位方法及装置、机器人

技术领域

本发明涉及定位领域，特别是涉及一种重定位方法及装置、机器人、可读存储介质。

背景技术

机器人、无人飞行器等具有自主运动能力的载体，在工作过程中可以对其上安装的传感器数据进行采集，结合已有的地图数据，生成对载体自身位置姿态(简称位姿)的定位，从而进行自主导航。

导航中，经常需要感知周围环境，根据已知地图确认载体在改地图中的当前位置，例如在初始化时、导航过程中定位错误时，这一过程也可以被称为重定位。

常用的重定位方法为自适应蒙特卡洛定位(adaptive Monte CarloLocalization，AMCL)方法，该方法对计算资源要求低，在环境不变且特征明显的情况下，定位精确率较高，耗时短；但在环境变化情况下，精确率低，错误率高，耗时长。实际应用中，环境变化难以控制，使用AMCL方法进行重定位可能出错，影响导航的安全。

发明内容

本发明主要解决的技术问题是提供一种重定位方法及装置、机器人、可读存储介质，能够解决现有技术中环境变化的情况下重定位错误率高的问题。

为了解决上述技术问题，本发明提供了一种重定位方法，该方法包括：获取多个子图边界点；将多个子图边界点绘制为第一子图；从第一子图中裁剪出第二子图，第二子图的自由空间特征值与第一子图的自由空间特征值的比值大于或等于预设阈值，预设阈值小于1；利用第二子图对已知地图进行模板匹配获取多个第一匹配结果；根据第二子图在第一子图中的位置关系，分别获取每个第一匹配结果对应的第二匹配结果，第一匹配结果在其对应的第二匹配结果中的位置关系与第二子图在第一子图中的位置关系相同；分别将每个第二匹配结果中的边界点与子图边界点进行配准以从多个第二匹配结果中筛选出第三匹配结果；利用定位条件从第三匹配结果中选出重定位结果。

为了解决上述技术问题，本发明提供了一种重定位装置，该装置包括至少一个处理器，单独或协同工作，处理器用于执行指令以实现前述的重定位方法。

为了解决上述技术问题，本发明提供了一种机器人，该机器人包括处理器和距离传感器，处理器连接距离传感器，处理器用于执行指令以实现前述的重定位方法。

为了解决上述技术问题，本发明提供了一种可读存储介质，存储有指令，指令被执行时实现前述的重定位方法。

本发明的有益效果是：在重定位过程中，对第一子图进行裁剪获取第二子图，利用第二子图对已知地图进行模板匹配获取第一匹配结果。第二子图的获取利用了第一子图的自由空间信息，可以代表第一子图中的主要自由空间特征，利用第二子图的重定位更适应环境变化，配合后续的对第一匹配结果对应的第二匹配结果进行的边界点配准和基于定位条件的筛选，在出现环境变化的情况下能够实现准确率高、精确率高、错误率低的重定位。

附图说明

图1是本发明重定位方法一实施例的流程示意图；

图2是图1中S4的具体流程示意图；

图3是图1中S6的具体流程示意图；

图4是本发明重定位方法一具体实施例的流程示意图；

图5是本发明重定位装置第一实施例的结构示意图；

图6是本发明机器人第一实施例的结构示意图；

图7是本发明可读存储介质第一实施例的结构示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细说明。以下各实施例中不冲突的可以相互结合。

如图1所示，本发明重定位方法一实施例包括：

S1：获取多个子图边界点。

为便于说明，以下以机器人为例说明载体在导航过程中重定位的过程。实际应用中，载体也可以为其他能够自主运动和导航的设备，例如无人飞行器等。

机器人定位常用的传感器为距离传感器，例如激光雷达等。机器人可以利用距离传感器扫描周围环境以对周围的物体进行测距得到多个子图边界点。每个子图边界点具有角度和距离信息，提供了该角度上的地图边界(例如障碍物)的信息。距离传感器可以为激光雷达、超声波测距传感器、红外测距传感器等。

S2：将多个子图边界点绘制为第一子图。

可以以机器人为中心，根据每个子图边界点的角度和距离信息绘制出所有子图边界点形成第一子图。在第一子图中，机器人和子图边界点之间的区域是机器人可以自由通行的区域，即自由空间。子图边界点之外的区域情况未知或者在障碍物之后，机器人不可自由通行，可称为非自由空间。自由空间和非自由空间之间由子图边界点分隔。第一子图中可以使用不同的像素值来区分自由空间、非自由空间和子图边界点。

S3：从第一子图中裁剪出第二子图。

第二子图的自由空间特征值与第一子图的自由空间特征值的比值大于或等于预设阈值，预设阈值大于0且小于1。

自由空间特征值可以为自由空间的总和，例如第一子图的自由空间特征值可以为第一子图中属于自由空间的像素点的总数。或者，自由空间特征值可以为自由空间的加权和，例如第一子图的自由空间特征值可以为属于自由空间的像素点的置信权重的总和。自由空间的权重可以为根据数据误差模型计算得到的置信权重。数据误差模型可以为距离传感器测距得到的距离数据误差模型，例如采用激光雷达作为距离传感器时，数据误差模型可以为激光数据误差模型。

在本发明一具体实施例中，第二子图为第一子图中以载体(例如机器人)位置为中心，且自由空间特征值等于第一子图的自由空间特征值与预设阈值的乘积的最小矩形。在其他实施例中，第二子图也可以为梯形、平行四边形、三角形等等其他形状。

S4：利用第二子图对已知地图进行模板匹配获取多个第一匹配结果。

第二子图可以代表第一子图中的主要自由空间特征，利用第二子图而不是第一子图来进行模板匹配可以更适应环境变化，即在环境变化(例如出现临时障碍物)的情况下，利用第二子图进行模板匹配的成功率比利用第一子图进行模板匹配的成功率更高。

模板匹配是一种常用的匹配方法，通过将模板叠放在被搜索图像上，计算模板与被搜索图像上被模板覆盖的区域(也可以被称为子图)之间的用于评价二者相似程度的指标，计算完成后平移模板重复上述过程，最后根据所有计算得到的指标从所有子图中寻找与模板相似程度最高的一个或者更多个子图作为模板匹配的结果。

由于模板匹配中只涉及平移操作，无法处理旋转，而第一子图的方向不一定与已知地图的方向一致，可能需要旋转。为减少遗漏匹配结果的可能性，可以将第二子图/已知地图旋转多个不同角度之后进行模板匹配。

在本发明一具体实施例中，如图2所示，S4包括：

S41：将第二子图旋转多个不同角度。

S42：分别将多个旋转结果和第二子图作为模板对已知地图进行模板匹配获取多个第一匹配结果。

当然，在其他具体实施例中，可以选择将已知地图旋转多个不同角度，将第二子图作为模板分别对已知地图和已知地图的旋转结果进行模板匹配来获取多个第一匹配结果。

由于模板匹配的计算量较大，被搜索图像尺寸较大的情况下计算量问题尤为突出，为减少重定位的耗时，可以选择使用图像金字塔对模板匹配进行加速。

模板匹配得到的模板匹配结果数量可能不止一个，其中可能存在冗余，为精简结果，减少后续的计算量，可以对模板匹配的结果利用非极大值抑制选择得到多个第一匹配结果。当然也可以直接将模板匹配的结果作为第一匹配结果。

S5：根据第二子图在第一子图中的位置关系，分别获取每个第一匹配结果对应的第二匹配结果。

第一匹配结果的尺寸和形状与第二子图一致，第二子图是第一子图中的一部分，不包括完整的子图边界点信息。为便于后续的点云配准，可以分别对每个第一匹配结果进行S3中裁剪的反操作得到对应的第二匹配结果。这一过程也可以理解为，根据每个第一匹配结果在已知地图中的位置，将第一子图叠放在已知地图上，使得其中的第二子图与该第一匹配结果完全重合，此时已知地图中被第一子图覆盖的部分，或者说与第一子图重合的部分，即为该第一匹配结果对应的第二匹配结果。第一匹配结果在其对应的第二匹配结果中的位置关系与第二子图在第一子图中的位置关系相同。

S6：分别将每个第二匹配结果中的边界点与子图边界点进行配准以从多个第二匹配结果中筛选出第三匹配结果。

配准可以是指寻找变换(可以包括旋转、平移和缩放)参数，使得两个点集中的一个(可以被称为源点集)在按照找到的变换参数进行变换之后，与另一个(可以被称为目标点集)尽量重合。如果成功找到变换参数且变换之后的源点集与目标点集之间的重合度满足要求，则可以认为配准成功。可以将第二匹配结果中的边界点作为源点集，子图边界点作为目标点集；或者将子图边界点作为源点集，第二匹配结果中的边界点作为目标点集。

第三匹配结果可以为配准成功的第二匹配结果。配准可以算出变换参数，变换参数中的旋转角度与对应的第一匹配结果在模板匹配中使用的角度不一定相同。

如图3所示，S6可以具体包括：

S61：分别将每个第二匹配结果中的边界点与子图边界点进行最近点迭代配准。

最近点迭代(Iterative Closest Point，ICP)配准是一种常用的配准方法。主要包括(1)计算最近点集、(2)计算变换矩阵、(3)应用变换矩阵和(4)目标函数计算与收敛判断，满足收敛条件时，结束迭代，否则返回(1)。

ICP配准不成功的情况可以包括迭代次数达到设置的最大值时仍不满足收敛条件；或者满足收敛条件结束迭代后，对源点集应用变换矩阵之后得到的变换结果与目标点集之间的重合度不满足要求。

若ICP配准成功，则跳转到S62；若ICP配准不成功，则跳转到S63。

S62：将第二匹配结果作为第三匹配结果。

S63：不将第二匹配结果作为第三匹配结果。

S7：选择最优的第三匹配结果作为重定位结果。

若第三匹配结果的数量为1，则可以直接将第三匹配结果作为重定位结果。若第三匹配结果的数量大于1，则可以利用定位条件从第三匹配结果中选出最优者作为重定位结果。具体的，可以利用定位条件对每个第三匹配结果进行评价以获取定位评价值；按照定位评价值选择从第三匹配结果中选择最优者作为重定位结果。可选的，定位评价值越高，满足定位条件的程度越高，对应的第三匹配结果越适合作为重定位结果，最优的第三匹配结果的定位评价值最高。

定位条件包括重定位位置在已知地图上的坐标位于自由空间内，场景变化率在预设范围内，测距工具不可穿过已知地图上的障碍物中的至少一种。具体的，重定位位置是指根据第三匹配结果确定的机器人位置，正常的情况下，机器人在已知地图上的位置应该是在自由空间内。场景变化率是指按照配准所得的变换参数进行变换之后的源点集和目标点集之间的误差率，正常情况下，即使考虑到场景变换(例如临时障碍物)，这个误差率应该在一预设范围内而不会很大。测距工具是指距离传感器用于测量距离所用的工具，例如激光雷达的测距工具为激光，超声波测距传感器的测距工具为超声波，红外测距传感器的测距工具为红外线等等。

通过本实施例的实施，在重定位过程中，对第一子图进行裁剪获取第二子图，利用第二子图对已知地图进行模板匹配获取第一匹配结果。第二子图的获取利用了第一子图的自由空间信息，可以代表第一子图中的主要自由空间特征，利用第二子图的重定位更适应环境变化，配合后续的对第一匹配结果对应的第二匹配结果进行的边界点配准和基于定位条件的筛选，在出现环境变化的情况下能够实现准确率高、精确率高、错误率低的重定位

下面结合附图具体说明重定位的具体过程。

如图4所示，在本发明一具体实施例中，机器人采用激光雷达进行重定位。

首先，获取第二子图。具体的，机器人在当前位置利用激光雷达扫描周围环境获得多个子图边界点，这些子图边界点也可以被称为激光子图点云或者激光子图点集。接着，机器人将这些子图边界点绘制成第一子图，根据激光数据误差模型，计算第一子图上每一个属于自由空间的即像素点的置信权重，然后计算第一子图中所有属于自由空间的像素点的置信权重总和，记为TW。然后，在第一子图中裁剪出以当前位置为中心，包含a*TW的最小矩形作为第二子图，a为预设阈值。第二子图可以代表第一子图中的主要自由空间特征

然后，利用第二子图对已知地图进行模板匹配获取多个第一匹配结果，这一过程也可以被称为粗定位。具体的，可以将第二子图旋转多个角度得到多个旋转结果。这些角度可以在0至360度的范围内等差排列。角度数量越多，计算量越大，但遗漏匹配结果的可能性越小。然后将旋转结果和第二子图作为模板，分别对已知地图进行模板匹配，得到多个模板匹配结果，模板匹配使用了图像金字塔进行加速。接着对模板匹配结果利用非极大值抑制选择得到多个第一匹配结果(也可以被称为粗定位结果)。

接着，利用ICP配准获取第三匹配结果，这一过程也可以被称为精确定位，是对粗定位结果的进一步筛选。具体的，机器人可以根据第二子图在第一子图中的位置关系，对每个第一匹配结果进行裁剪的反操作，得到对应的第二匹配结果。机器人分别每个第二匹配结果中的边界点与子图边界点进行ICP配准，若成功，则将该第二匹配结果作为第三匹配结果，否则不将该第二匹配结果作为第三匹配结果，从而筛选出第三匹配结果(也可以被称为精确定位结果)。

最后，选出最优的第三匹配结果作为重定位结果。具体的，若只有一个第三匹配结果，则可以直接将该第三匹配结果作为重定位结果；若第三匹配结果的数量大于1，则可以利用定位条件对每个第三匹配结果进行评价以获取定位评价值；选择定位评价值最高的第三匹配结果作为重定位结果。

如图5所示，本发明重定位装置第一实施例包括：处理器110。图中只画出了一个处理器110，实际数量可以更多。处理器110可以单独或者协同工作。

处理器110控制重定位装置的操作，处理器110还可以称为CPU(CentralProcessing Unit，中央处理单元)。处理器110可能是一种集成电路芯片，具有信号序列的处理能力。处理器110还可以是通用处理器、数字信号序列处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

处理器110用于执行指令以实现本发明重定位方法任一实施例以及不冲突的组合所提供的方法。

如图6所示，本发明机器人第一实施例包括：处理器210和距离传感器220。

处理器210控制机器人的操作，处理器210还可以称为CPU(Central ProcessingUnit，中央处理单元)。处理器210可能是一种集成电路芯片，具有信号序列的处理能力。处理器210还可以是通用处理器、数字信号序列处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

距离传感器220能够获取测量范围内的障碍物与距离传感器220之间的距离信息。距离传感器可以为激光雷达、超声波测距传感器、红外测距传感器等。

处理器210用于执行指令以实现本发明重定位方法任一实施例以及不冲突的组合所提供的方法。

如图7所示，本发明可读存储介质第一实施例包括存储器310，存储器310存储有指令，该指令被执行时实现本发明重定位方法任一实施例以及任意不冲突的组合所提供的方法。

存储器310可以包括只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、闪存(Flash Memory)、硬盘、光盘等。

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

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

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

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

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

Claims (13)

1.一种重定位方法，其特征在于，包括：

获取多个子图边界点；

将所述多个子图边界点绘制为第一子图；

从所述第一子图中裁剪出第二子图，所述第二子图的自由空间特征值与所述第一子图的自由空间特征值的比值大于或等于预设阈值，所述预设阈值小于1；

利用所述第二子图对已知地图进行模板匹配获取多个第一匹配结果；

根据所述第二子图在所述第一子图中的位置关系，分别获取每个所述第一匹配结果对应的第二匹配结果，所述第一匹配结果在其对应的所述第二匹配结果中的位置关系与所述第二子图在所述第一子图中的位置关系相同；

分别将每个所述第二匹配结果中的边界点与所述子图边界点进行配准以从多个所述第二匹配结果中筛选出第三匹配结果；

选择最优的所述第三匹配结果作为重定位结果。

2.根据权利要求1所述的方法，其特征在于，

所述自由空间特征值为自由空间的总和；或所述自由空间特征值为所述自由空间的加权和，所述自由空间的权重为根据数据误差模型计算得到的置信权重。

3.根据权利要求2所述的方法，其特征在于，

所述第二子图为所述第一子图中以载体位置为中心，且所述自由空间特征值等于所述第一子图的自由空间特征值与所述预设阈值的乘积的最小矩形。

4.根据权利要求1所述的方法，其特征在于，

利用所述第二子图对已知地图进行模板匹配获取多个第一匹配结果包括：

将所述第二子图旋转多个不同角度；

分别将多个旋转结果和所述第二子图作为模板对所述已知地图进行所述模板匹配获取多个所述第一匹配结果。

5.根据权利要求1所述的方法，其特征在于，

所述模板匹配使用图像金字塔进行加速。

6.根据权利要求1所述的方法，其特征在于，

多个所述第一匹配结果是对所述模板匹配的结果利用非极大值抑制选择得到的。

7.根据权利要求1所述的方法，其特征在于，

所述分别将每个所述第二匹配结果中的边界点与所述子图边界点进行配准以从多个所述第二匹配结果中筛选出第三匹配结果包括：

分别将每个所述第二匹配结果中的边界点与所述子图边界点进行最近点迭代配准；

若所述最近点迭代配准成功，则将所述第二匹配结果作为所述第三匹配结果，否则不将所述第二匹配结果作为所述第三匹配结果。

8.根据权利要求1所述的方法，其特征在于，

所述选择最优的所述第三匹配结果作为重定位结果包括：

若所述第三匹配结果的数量为1，则将所述第三匹配结果作为所述重定位结果；若所述第三匹配结果的数量大于1，则利用定位条件从所述第三匹配结果中选出最优者作为所述重定位结果。

9.根据权利要求8所述的方法，其特征在于，

所述定位条件包括重定位位置在所述已知地图上的坐标位于自由空间内，场景变化率在预设范围内，测距工具不可穿过所述已知地图上的障碍物中的至少一种。

10.一种重定位装置，其特征在于，包括至少一个处理器，单独或协同工作，所述处理器用于执行指令以实现如权利要求1-9中任一项所述的方法。

11.一种机器人，其特征在于，包括处理器和距离传感器，所述处理器连接所述距离传感器，所述处理器用于执行指令以实现如权利要求1-9中任一项所述的方法。

12.根据权利要求11所述的机器人，其特征在于，所述距离传感器为激光雷达。

13.一种可读存储介质，存储有指令，其特征在于，所述指令被执行时实现如权利要求1-9中任一项所述的方法。

Images