CN114066989B - 机器人定位方法、装置、设备及可读存储介质 - Google Patents

Abstract

本发明公开了一种机器人定位方法、装置、设备以及计算机可读存储介质，该方法包括预先获得栅格化的点云地图中并根据每个栅格的栅格中心和最近目标点之间的距离确定栅格的梯度值；根据坐标值、栅格的梯度值，利用ICP算法以待配准点云中每个待配准点所在栅格对应的最近目标点为待配准点的配准点，将待配准点云和点云地图中目标点云进行配准，获得待配准点云配准到点云地图中的目标点云的位置转换关系；根据位置转换关系和粗略坐标值，确定机器人在点云地图中的精准坐标值。本申请中预先确点云地图中各个栅格的最近目标点和梯度值，以作为待配准点配准过程中的最近目标点和梯度值，简化了配准运算过程，提高机器人定位效率。

Description

机器人定位方法、装置、设备及可读存储介质

技术领域

本发明涉及定位技术领域，特别是涉及一种机器人定位方法、装置、设备以及计算机可读存储介质。

背景技术

随着智能控制技术的发展和应用，扫地机器人、货物分拣机器人等等智能化机器人的应用也应运而生。而在完成各种工作任务中自动实现对智能机器人本身移动过程中的精准定位，对智能机器人高效准确的完成工作有重要意义。

目前在智能机器人执行工作任务过程中对智能机器人的位置姿态进行定位的方法有多种，但往往不是定位精度不足就是定位运算过程复杂，增加机器人的使用成本。

发明内容

本发明的目的是提供一种机器人定位方法、装置、设备以及计算机可读存储介质，能够在一定程度上降低机器人定位运算难度，降低机器人的使用成本。

为解决上述技术问题，本发明提供一种机器人定位方法，包括：

预先获得机器人所在环境的栅格化的点云地图，并根据每个栅格的栅格中心和最近目标点之间的距离确定所述栅格的梯度值；其中，每个栅格的最近目标点为距离栅格中心最近的目标点，每个栅格的梯度值为所述栅格中心到所述最近目标点的梯度大小；

根据机器人当前在所述点云地图中的粗略坐标值，以及待配准点云在机器人坐标系中的相对坐标值，获得所述待配准点云在所述点云地图中的初始坐标值；

根据所述初始坐标值、所述栅格的所述梯度值，利用ICP算法以所述待配准点云中每个待配准点所在栅格对应的最近目标点为所述待配准点的配准点，将所述待配准点云和所述点云地图中目标点云进行配准，获得所述待配准点云配准到所述点云地图中的目标点云的位置转换关系；

根据所述位置转换关系和所述粗略坐标值，确定所述机器人在所述点云地图中的精准坐标值。

可选地，预先确定每个栅格对应的最近目标点的过程包括：

根据所述栅格中心在所述点云地图中的中心坐标值、和以所述栅格为中心的预设 区域内的各个目标点的目标点坐标值，结合得分函数

，依次确 定所述栅格相对于所述预设区域范围内的各个所述目标点的得分值；

其中，

，

，

，

，

为 所述目标点坐标值；

为所述中心坐标值；

分别为所述预设区域分别 在x轴、y轴、z轴方向上的尺寸大小；

选取所述得分值最高的目标点作为最近目标点。

可选地，预先确定所述栅格的梯度值的过程包括：

根据梯度公式

和所述栅格的中心坐标值 以及对应的最近目标点的所述目标点坐标值，确定所述栅格对应的梯度值。

可选地，获得所述待配准点云配准到所述点云地图中的目标点云的位置转换关系的过程，包括：

根据配准过程中所述待配准点平移时所在栅格的梯度值，确定所述位置转换关系中的平移梯度，并根据配准过程中所述待配准点依次平移的步长确定所述位置转换关系中的平移步长；

其中，所述平移梯度为

，且

，

；

根据配准过程中各个所述待配准点平移时所在栅格的梯度值，和旋转变换梯度公 式

确定所述位置转换关系中的 旋转变换梯度；并根据根据配准过程中所述待配准点依次旋转变换的步长确定所述位置转 换关系中的旋转步长；

根据所述位置转换关系和所述粗略坐标值，确定所述机器人在所述点云地图中的精准坐标值，包括：

根据所述平移梯度、所述平移步长、所述旋转变换梯度、所述旋转步长和所述粗略 坐标值，获得所述机器人的精准坐标值：其中，

为所述粗略坐标值。

可选地，获得所述机器人的粗略坐标值的过程包括：

根据所述机器人的驱动装置驱动所述机器人运动的驱动数据确定所述机器人在所述点云地图的坐标系中的所述粗略坐标值。

可选地，获得待配准点云在机器人坐标系中的相对坐标值的过程包括：

通过和所述机器人相对固定设置的激光扫描仪扫描获得所述机器人周围环境中的待配准点云和所述待配准点云对应的相对坐标值。

一种机器人定位装置，包括

梯度值模块，用于预先获得机器人所在环境的栅格化的点云地图，并根据每个栅格的栅格中心和最近目标点之间的距离确定所述栅格的梯度值；其中，每个栅格的最近目标点为距离栅格中心最近的目标点，每个栅格的梯度值为所述栅格中心到所述最近目标点的梯度大小；

初始坐标模块，用于根据机器人当前在所述点云地图中的粗略坐标值，以及待配准点云在机器人坐标系中的相对坐标值，获得所述待配准点云在所述点云地图中的初始坐标值；

配准运算模块，用于根据所述初始坐标值、所述栅格的所述梯度值，利用ICP算法以所述待配准点云中每个待配准点所在栅格对应的最近目标点为所述待配准点的配准点，将所述待配准点云和所述点云地图中目标点云进行配准，获得所述待配准点云配准到所述点云地图中的目标点云的位置转换关系；

精准坐标模块，用于根据所述位置转换关系和所述粗略坐标值，确定所述机器人在所述点云地图中的精准坐标值。

一种机器人定位设备，包括：

扫描装置，用于扫描获得所述机器人所在环境中的待配准点，以及所述机器人的相对位置关系；

存储器，用于存储计算机程序；

处理器，用于根据所述待配准点对应的相对位置关系确定各个所述待配准点在所述机器人的坐标系中的相对坐标值，并根据所述相对坐标值执行所述计算机程序，以实现如上任一项所述的机器人定位方法的步骤。

可选地，所述扫描装置为固定在所述机器人上的激光扫描仪。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被执行以实现如上任一项所述的机器人定位方法的步骤。

本发明所提供的机器人定位方法，该方法包括预先获得机器人所在环境的栅格化的点云地图，并根据每个栅格的栅格中心和最近目标点之间的距离确定栅格的梯度值；其中，每个栅格的最近目标点为距离栅格中心最近的目标点，每个栅格的梯度值为栅格中心到最近目标点的梯度大小；根据机器人当前在点云地图中的粗略坐标值，以及待配准点云在机器人坐标系中的相对坐标值，获得待配准点云在点云地图中的初始坐标值；根据初始坐标值、栅格的梯度值，利用ICP算法以待配准点云中每个待配准点所在栅格对应的最近目标点为待配准点的配准点，将待配准点云和点云地图中目标点云进行配准，获得待配准点云配准到点云地图中的目标点云的位置转换关系；根据位置转换关系和粗略坐标值，确定机器人在点云地图中的精准坐标值。

本申请中利用ICP算法将和机器人相对位置关系确定的待配准点云配准到点云地图中的目标点，从而确定待配准点在点云地图中位置坐标，再利用各个待配准点和机器人之间的相对位置关系确定出机器人的准确坐标位置。在此基础上，为了减少待配准点向目标点配准过程中的运算量，还预先对点云地图进行栅格化，并确定出每个栅格的栅格中心最近的目标点对应的梯度值；在每次迭代配准的过程中，以待配准点所在栅格对应的最近目标点作为对应配准点，以所在栅格对应梯度值作为待配准点配准到最近目标点的平移梯度，在进行配准平移方向梯度的运算时，即可直接利用该梯度值进行运算，在很大程度上降低了待配准点配准到目标点的运算量，也就降低了机器人定位的运算量并提高运算速度，在提高机器人定位效率的基础上降低了机器人的处理器成本，也就降低了机器人的使用成本。

本申请中还提供了一种机器人定位装置、设备以及计算机可读存储介质。

附图说明

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

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

图2为本发明实施例提供的机器人定位装置的结构框图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

ICP算法（Iterative Closest Point，最近迭代算法）是最常用的数据精确配准方法，在每次迭代的过程中，对数据点云的每一点，在模型点云中寻找最近点作为对应配准点，最终确定出数据点云配准重合到对应的配准点需要进行的旋转和平移的参数，也即实现了数据电源向模型点云中点进行配准的过程。而在这一配准过程中，确定各个数据点在模型点云整的最近点往往需要进行大量的运算，进而导致配准过程的运算量较大。

以对机器人定位的过程为例，将和机器人之间相对位置关系的待配准点，利用点云配准的方式确定配准到地图中准确的目标点，该目标点的位置也即是待配准点在地图中准确的位置，在此基础上利用待配准点和机器人之间的相对位置关系，显然即可准确确定机器人在地图中准确的位置信息。但是因为待配准点向地图中的目标点进行配准时，需要进行大量的运算寻找确定最近目标点，这也就使得待配准点向目标点配准过程中运算量大，从而导致机器人每次定位的运算量较大，机器人定位运算效率也就相对低；并且在机器人执行工作任务时，往往需要频繁的甚至实时的定位，使得在对完成机器人定位运算的处理器要求较高，也即增加了处理器成本。

为此，本申请中提供了一种机器人定位的技术方案，能够在一定程度上降低定位运算的数据运算量，从而降低机器人的使用成本。

如图1所示，图1为本申请实施例提供的一种机器人定位方法的流程示意图，该方法可以包括：

S11：预先获得机器人所在环境的栅格化的点云地图，并根据每个栅格的栅格中心和最近目标点之间的距离确定栅格的梯度值。

其中，每个栅格的最近目标点为距离栅格中心最近的目标点，每个栅格的梯度值为栅格中心到最近目标点的梯度大小。

可以先将机器人执行工作任务时所活动的环境空间通过扫描装置进行扫描，从而获得环境中各个位置物点信息。例如，可以采用激光器对环境进行360度的扫描，当激光器向其周围空间中发射激光光线，该激光光线照射到距离激光器一定距离范围内的物体表面时，即可反射回来，并被激光器所接收，激光器即可确定该物体表面反射激光的点相对于激光器的距离，在基于激光器本身发射激光的角度方向，即可确定该物体表面反射激光的点相对于激光器的相对位置信息，以此类推，即可获得大量点云的位置信息，由此构建点云地图，该点云地图中包含由大量位置坐标确定的目标点。

当然，在实际应用中也并不排除采用广角相机进行多角度拍摄，并选取周围物景中某些特定点形成点云地图中的目标点，例如环境中物体的轮廓点、表面中心点等等；对此本申请中不一一列举。

在确定出大量目标点并形成点云地图的基础上，可以进一步地对该点云地图进行栅格化。在进行栅格化过程中，可以先对点云地图按照大栅格进行栅格划分，完成大栅格划分后，再对存在目标点的大栅格进行进一步的小栅格划分，显然大栅格的栅格尺寸大于小栅格的栅格尺寸，通过两次栅格划分，避免了对没有目标点的区域进行无用的栅格划分。

在完成栅格划分之后，即可对每个栅格进行梯度值计算。本实施例中每个栅格对应的梯度值是指该栅格的栅格中心到距离该栅格的栅格中心最近的目标点的梯度值。

本实施例以当前进行梯度值计算的栅格为当前栅格进行说明，要确定当前栅格梯度，首先需要确定当前栅格的最近目标点；显然该最近目标点正常情况下，不会距离该当前栅格过远；因此可以以距离当前栅格的栅格中心一定距离范围作为查找最近目标点的区域范围。

在确定最近目标点的搜索区间后，即可在搜索区间中选取最近目标点。可选地，确定这一最近目标点的过程可以包括：

根据栅格中心在点云地图中的中心坐标值、和以栅格为中心的预设区域内的各个 目标点的目标点坐标值，结合得分函数

，依次确 定栅格相对于预设区域范围内的各个目标点的得分值；选取得分值最高的目标点作为最近 目标点。

其中，

；

，

，

，

为目标点坐标值；

为中心坐标值；

分别为预设区域分别 在x轴、y轴、z轴方向上的尺寸大小。

设目标点的位置为

、

、

，设定当前栅格的栅格中心对应的中心坐标值为

、

、

；在以

分别为长宽高以目标点为中心围成的立方体空间范围内，以 当前栅格的得分值表征当前栅格的栅格中心到目标点的距离远近。设该目标点周围的立方 体空间对其邻近的各个栅格的得分值符合XYZ三个方向上独立的联合高斯分布，将高斯分 布的联合概率密度值作为该点的得分函数。

根据目标点满足的高斯分布的联合概率密度函数

及

；另

，

，

，则得分函数可改写为：

；

将各个目标点的坐标值和当前栅格的栅格中心对应的中心坐标值，代入上述得分函数，即可确定当前栅格到各个目标点的得分值，且目标点距离当前栅格的栅格中心越近，该得分值的大小越大，由此，即可选出得分最高的目标点作为该当前栅格对应的最近目标点。

需要说明的是，上述确定最近目标点的方式是以各个栅格为中心，通过确定各个栅格周围一定区域范围的各个目标点中得分最高的目标点为最近目标点。

但是在实际应用过程中，也可以以目标点为中心，确定该目标点周围以

分别为长宽高以目标点为中心围成的立方体空间范围内各个栅格到该目标点 的得分值，由此依次确定各个目标点周围范围内各个栅格的得分值；而对于每个栅格而言， 每计算出一个目标点对该栅格的得分值，就将该得分值和上一个目标点对应的得分值进行 比较，若大于上一目标点对应的得分值，就对该栅格的得分值进行刷新，若小于上一目标点 对应的得分值，则不对栅格的得分值进行刷新，进而使得每个栅格最终保留的得分值为最 高得分值，而最高得分值对应的目标点，也即是该栅格对应的最近目标点。

在确定每个栅格对应的最近目标点之后，即可对栅格到对应的最近目标点的梯度 值进行运算。在本申请的一种可选地实施例中，还可以进一步地利用上述得分函数确定每 个栅格和对应的最近目标点之间满足的梯度公式；可以直接对上述得分函数进行X、Y、Z三 个方向求偏导，从而获得栅格的梯度公式为

；结合

，

，

，将栅格中心的中心坐标值和最近目标点的目标点坐 标值代入上述梯度公式，即可确定栅格对应的梯度值。

需要说明的是，本申请中在确定最近目标点和栅格到最近目标点的梯度值时，利用的是高斯分布的联合概率密度函数进行最近目标点的选取和对应的梯度值运算。在实际应用中也不排除采用其他方式确定最近目标点，例如之间计算栅格周围范围内各个目标点和栅格中心之间的欧式距离，以欧式距离的大小作为评价各个目标点和栅格中心距离远近的标准，而梯度值则直接根据目标点和栅格中心点对应的坐标相量想减即可确定对应的梯度方向，也即梯度值。还可以采用其他确定栅格对应的最近目标点以及梯度值的方式，在此本实施例中不一一列举。

S12：根据机器人当前在点云地图中的粗略坐标值，以及待配准点云在机器人坐标系中的相对坐标值，获得待配准点云在点云地图中的初始坐标值。

需要说明的是，在机器人执行工作任务时，是通过其驱动装置来实现移动的，显然，基于机器人的驱动装置的驱动数据，可以大致确定出机器人当前所在位置。但是显然驱动装置的驱动精度有限，导致基于驱动数据确定出的机器人的实际位置存在一定的偏差，为此可以基于驱动装置中的驱动数据大致确定出机器人的粗略坐标值。

当然，在实际应用中，对机器人位置的粗略定位并不仅限于驱动装置驱动过程中产生的驱动数据，例如对于室外获得的机器人而言，也可以考虑参照GPS导航系统确定出的粗略位置，或者基于环境中的摄像头拍摄的图像大致确定出机器人的粗略位置等等，进而确定粗略坐标值，都不影响本实施例的实现。

可以理解的是，本实施例中所指的待配准点云机器人周围的点云，应当是距离机器人相对较近的一定范围内的点云，同样可以采用扫描装置扫描获得，显然该扫描装置可以是和上述扫描获得点云地图中目标点的设备采用同一种设备，例如激光扫描仪、摄像机等等。

为了更精准的获得机器人和待配准点之间的相对位置关系，可以直接将该扫描装置安装固定在机器人上随着机器人同步移动，那么基于扫描获得各个待配准点相对于扫描装置的相对位置，也即可以确定各个待配准点和机器人之间的相对位置关系。当然，扫描装置也并必然设置在机器人上，例如，可以是设置在其他移动或固定不动的设备上的激光器或摄像头扫描确定多个待配准点和机器人之间的相对位置关系即可。

在确定待配准点和机器人之间的相对位置关系之后，显然即可确定各个待配准点在以机器人为原点的坐标系中的相对坐标值。而该待配准点也同样是点云地图中的点，基于各个待配准点的相对坐标值和机器人的粗略坐标值，显然可以确定各个待配准点在点云地图的坐标系中的不准确的坐标值，以该不准确的坐标值作为待配准点的初始坐标值。因为待配准点也同样是点云地图中的点，在点云地图中应当存在和其一一对应的目标点，该待配准点在点云地图中对应的目标点的坐标值显然才是待配准点在点云地图中的准确坐标值。

由此本实施例中以点云配准的方式将待配准点向点云地图中的目标点进行配准，也即是将待配准点的不准确位置坐标向准确位置坐标配准的过程，当待配准点配准到和对应的目标点重合，可确定待配准点在点云地图中的准确位置，相应地，也就可以确定机器人在点云地图中的准确位置。

S13：根据初始坐标值、栅格的梯度值，利用ICP算法以待配准点云中每个待配准点所在栅格对应的最近目标点为待配准点的配准点，将待配准点云和点云地图中目标点云进行配准，获得待配准点云配准到点云地图中的目标点云的位置转换关系。

设定待配准点云为

，目标点云为

。ICP算法的基本原理是：在目标点云

中 找到待配准点云

中待配准点

的最近目标点

，然后基于n对（

，

）的坐标值计算出最 优匹配参数R和t，使得误差函数最小。误差函数为E（R，t）为：

其中，R为旋转矩阵，t为平移向量。

ICP算法过程如下：

（1）在待配准点云中取n个待配准点

∈

；

（2）找出目标点

中的对应的最近目标点

∈Q，使得

最小；

（3）计算旋转参数R和平移参数t，使得误差函数最小；

（4）对旋转矩阵R和平移矩阵t进行旋转和平移变换的到新的对应点

，

；

（5）计算

与对应点

的平均距离，也即是误差函数的计算结果；

（6）如果误差函数的计算结果小于某一给定的阈值或者大于预设的最大迭代次数（即满足迭代终止条件），则停止迭代计算，否则根据待配准点旋转平移后的坐标位置返回第（2）步，直到满足迭代终止条件为止。

而在实际运算中，如何确定目标点云

中对应于待配准点

的最近点，则需要 进行较为复杂的运算，需要将每个待配准点

和其周围临近的目标点

逐一进行距离运 算；并且在一次迭代完成，各个待配准点通过配准确定的旋转参数和平移参数确定出新的 坐标位置后，在进行下一次迭代配准是需要在基于各个待配准点新的坐标位置重新在目标 点云中确定新的最邻近点。如此反复，导致配准过程中运算量较大。

为此，本实施例中考虑到对于待配准点的初始坐标值而言，其本身就是不准确的，由此在确定最邻近的目标点时，可以不直接根据待配准点的位置坐标确定最邻近的目标点，而是以待配准点所在栅格的栅格中心点最邻近的目标点作为该待配准点对应的最近目标点，与此同时，以该栅格中心到该最近目标点方向的梯度值作为该待配准点向最近目标点移动的梯度；而在迭代配准过程中，无论经过多少次迭代运算，待配准点的坐标值经过多少次移动变换，其每次都必然会落在某一栅格内，而每个栅格对应的最近目标点和梯度值均是提前确定好的无需重复计算，从而在很大程度上提升了确定最邻近目标点的效率，并减少确定待配准点对应的最近目标点的运算量，由此加快了待配准点配准到目标点的配准速度。

当配准完成时，即可确定待配准点的初始坐标值变换到配准的目标点的坐标值的位置变换关系。

可选地，确定位置转换关系的过程可以包括：

根据配准过程中待配准点平移时所在栅格的梯度值，确定位置转换关系中的平移梯度，并根据配准过程中待配准点依次平移的步长确定位置转换关系中的平移步长；

其中，平移梯度为

，且

，

；

根据配准过程中各个待配准点平移时所在栅格的梯度值，和旋转变换梯度公式

确定所述位置转换关 系中的旋转变换梯度；并根据根据配准过程中所述待配准点依次旋转变换的步长确定所述 位置转换关系中的旋转步长；

在3D空间内，待配准点向目标点配准而进行位置变换时，存在6个自由度（x，y，z，yaw，pitch，roll），因此优化ICP算法需要优化6个变量。

1）首先优化XYZ三个方向的平移量。该平移量可以视为平移梯度和平移步长的乘 积，平移梯度在XYZ三个坐标轴方向的分量即为

；其中，

，由此获得

；基于预 先确定的各个待配准点所在栅格的梯度值，即可分别确定

，

，

；在基于配准过程 中，每次待配准点向最近目标点配准平移的步长，即可确定平移量，可以理解的是每次配准 的平移步长，是由待配准点向最近目标点平移的距离决定。

2）再优化yaw，pitch，roll三个变量，即为三个方向的旋转变量，和上述确定平移量类似，三个旋转变量同样可以视为旋转变换梯度和旋转变换步长的乘积获得。而三个旋转变量可通过旋转矩阵定量描述，可以通过对旋转矩阵求导确定配准点云的旋转变换梯度。

假设仅仅只需要优化yaw，pitch，roll三个变量，即对一个点云仅仅只需要进行旋 转就可配准为目标点云，则ICP的误差函数公式即可改写为

。

基于改写后的ICP公式，计算旋转之后的坐标对于旋转的导数，也即是对

进行求导相当于对

求偏导，记为

。

对于优化损失函数，需要优化旋转矩阵，设R对应的李代数为

，则上式改写为：

；其中^表示将向量转化为反对称矩阵操作。

对R进行一次左扰动△R，设左扰动的李代数为

，然后对

求导：

考虑到，在R->0时，R初值应为I，所以上式改写为：

。

基于上述论述，可以确定

是待配准点移动变换时所在栅格的得分值，

和待配准点移动到目标点的距离成反比；而F为

的求和，也即是说，F 就是和所有待配准点移动到最近目标点的距离总和成反比；而

则表示待配准点和最近目标点之间距离总和；由此，对E求偏 导也即可转换为对F进行求偏导。

因为最终需要确定机器人的位置变换结果，因此以机器人的粗略坐标值作为递推初值向旋转梯度方向旋转，这个旋转变换方向可以用向量叉乘表示：

； 其中机器人的粗略坐标值为

。

将旋转变换方向结果带入求导的结果中，即可获得旋转变换梯度为：

；

确定上述旋转变换梯度，并基于每次变换的变换步长，即可确定旋转变量。

S14：根据位置转换关系和粗略坐标值，确定机器人在点云地图中的精准坐标值。

根据位置转换关系中的根据平移梯度、平移步长、旋转变换梯度、旋转步长和粗略坐标值，即可获得机器人的精准坐标值。

根据位置变换公式：

，即可确定机器人的精准坐标，其中R表述旋转变量，该旋转变量等于旋转变换梯度和旋转步长的乘积，T表述平移量，该平移量等于平移梯度和平移步长的乘积，而

表示机器人的粗略坐标，而

则表示机器人的精准坐标值。

综上所述，本申请在对机器人的当前位置进行实时定位时，利用了ICP算法将机器人所在环境的待配准点和点云地图中的目标点相互配准的方式，确定待配准点的坐标向目标点坐标转换的关系，进而实现机器人不准确的粗略坐标值向精准坐标值转换；在此基础上考虑到ICP算法过程中待配准点向目标点配准的运算量过大，预先确定待配准点所在栅格的最近目标点，以及栅格中心点到该最近目标点的梯度值，使得在配准过程中直接利用待配准点所在栅格的最近目标点和梯度值作为该待配准点的最近目标点和配准移动的梯度，完成配准过程，在很大程度上减少了配准过程的运算量，也就提高了机器人定位的工作效率，并降低对机器人定位运算的处理器的要求，进而降低机器人的定位成本，有利于机器人的广泛应用。

下面对本发明实施例提供的机器人定位装置进行介绍，下文描述的机器人定位装置与上文描述的机器人定位方法可相互对应参照。

图2为本发明实施例提供的机器人定位装置的结构框图，参照图2的机器人定位装置可以包括：

梯度值模块100，用于预先获得机器人所在环境的栅格化的点云地图，并根据每个栅格的栅格中心和最近目标点之间的距离确定所述栅格的梯度值；其中，每个栅格的最近目标点为距离栅格中心最近的目标点，每个栅格的梯度值为所述栅格中心到所述最近目标点的梯度大小；

初始坐标模块200，用于根据机器人当前在所述点云地图中的粗略坐标值，以及待配准点云在机器人坐标系中的相对坐标值，获得所述待配准点云在所述点云地图中的初始坐标值；

配准运算模块300，用于根据所述初始坐标值、所述栅格的所述梯度值，利用ICP算法以所述待配准点云中每个待配准点所在栅格对应的最近目标点为所述待配准点的配准点，将所述待配准点云和所述点云地图中目标点云进行配准，获得所述待配准点云配准到所述点云地图中的目标点云的位置转换关系；

精准坐标模块400，用于根据所述位置转换关系和所述粗略坐标值，确定所述机器人在所述点云地图中的精准坐标值。

在本申请的一种可选地实施例中，梯度值模块100用于根据所述栅格中心在所述 点云地图中的中心坐标值、和以所述栅格为中心的预设区域内的各个目标点的目标点坐标 值，结合得分函数

，依次确定 所述栅格相对于所述预设区域范围内的各个所述目标点的得分值；其中，

；

，

，

，

为所述目标点坐标值；

为所述中心坐标值；

分别为所述预设区域分别在x轴、y轴、z轴方向 上的尺寸大小；选取所述得分值最高的目标点作为最近目标点。

在本申请的一种可选地实施例中，梯度值模块100用于根据梯度公式

和所述栅格的中心坐标值以及对应的最近目标点 的所述目标点坐标值，确定所述栅格对应的梯度值。

在本申请的一种可选地实施例中，配准运算模块300，用于根据配准过程中各个所述待配准点平移时所在栅格的梯度值，确定所述位置转换关系中的平移梯度，并根据配准过程中所述待配准点依次平移的步长确定所述位置转换关系中的平移步长；

其中，所述平移梯度为

，且

，

；

根据配准过程中各个所述待配准点平移时所在栅格的梯度值，和旋转变换梯度公 式

确定所述位置转换关系中的 旋转变换梯度；并根据根据配准过程中所述待配准点依次旋转变换的步长确定所述位置转 换关系中的旋转步长；

相应地，精准坐标模块400具体用于根据所述平移梯度、所述平移步长、所述旋转 变换梯度、所述旋转步长和所述粗略坐标值，获得所述机器人的精准坐标值：其中，

为所述粗略坐标值。

在本申请的一种可选地实施例中，还包括参数获取模块，用于根据所述机器人的驱动装置驱动所述机器人运动的驱动数据确定所述机器人在所述点云地图的坐标系中的所述粗略坐标值。

在本申请的一种可选地实施例中，参数获取模块用于通过和所述机器人相对固定设置的激光扫描仪扫描获得所述机器人周围环境中的待配准点云和所述待配准点云对应的相对坐标值。

本实施例的机器人定位装置用于实现前述的机器人定位方法，因此机器人定位装置中的具体实施方式可见前文中的机器人定位方法的实施例部分，在此不再赘述。

本申请还提供了一种机器人定位设备的实施例，该设备可以包括：

扫描装置，用于扫描获得所述机器人所在环境中的待配准点，以及所述机器人的相对位置关系；

存储器，用于存储计算机程序；

处理器，用于根据所述待配准点对应的相对位置关系确定各个所述待配准点在所述机器人的坐标系中的相对坐标值，并根据所述相对坐标值执行所述计算机程序，以实现如上任一项所述的机器人定位方法的步骤。

可选地，该扫描装置可以为激光扫描仪、摄像头等设备，且该扫描装置可以是固定在机器人上的，或者是和机器人相对位置固定的。

本申请还提供了一种计算机可读存储介质的实施例，该计算机可读存储介质存储有计算机程序，所述计算机程序被执行以实现如上任一项所述的机器人定位方法的步骤。

该计算机可读存储介质可以包括随机存储器（RAM）、内存、只读存储器（ROM）、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、 “包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。另外，本申请实施例提供的上述技术方案中与现有技术中对应技术方案实现原理一致的部分并未详细说明，以免过多赘述。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (9)

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

预先获得机器人所在环境的栅格化的点云地图，并根据每个栅格的栅格中心和最近目标点之间的距离确定所述栅格的梯度值；其中，每个栅格的最近目标点为距离栅格中心最近的目标点，每个栅格的梯度值为所述栅格中心到所述最近目标点的梯度大小；

根据机器人当前在所述点云地图中的粗略坐标值，以及待配准点云在机器人坐标系中的相对坐标值，获得所述待配准点云在所述点云地图中的初始坐标值；其中，所述相对坐标值为根据所述待配准点云和所述机器人之间的相对位置关系确定的；

根据所述初始坐标值、所述栅格的所述梯度值，利用ICP算法以所述待配准点云中每个待配准点所在栅格对应的最近目标点为所述待配准点的配准点，将所述待配准点云和所述点云地图中目标点云进行配准，获得所述待配准点云配准到所述点云地图中的目标点云的位置转换关系；

根据所述位置转换关系和所述粗略坐标值，确定所述机器人在所述点云地图中的精准坐标值；

预先确定每个栅格对应的最近目标点的过程包括：

根据所述栅格中心在所述点云地图中的中心坐标值、和以所述栅格为中心的预设区域 内的各个目标点的目标点坐标值，结合得分函数

，依次确定所 述栅格相对于所述预设区域内的各个所述目标点的得分值；

其中，

，

，

，

，

为所述 目标点坐标值；

为所述中心坐标值；

分别为所述预设区域分别在x 轴、y轴、z轴方向上的尺寸大小；

选取所述得分值最高的目标点作为最近目标点。

2.如权利要求1所述的机器人定位方法，其特征在于，预先确定所述栅格的梯度值的过程包括：

根据梯度公式

和所述栅格的中心坐标值以及 对应的最近目标点的所述目标点坐标值，确定所述栅格对应的梯度值。

3.如权利要求1所述的机器人定位方法，其特征在于，获得所述待配准点云配准到所述点云地图中的目标点云的位置转换关系的过程，包括：

根据配准过程中各个所述待配准点平移时所在栅格的梯度值，确定所述位置转换关系中的平移梯度，并根据配准过程中所述待配准点依次平移的步长确定所述位置转换关系中的平移步长；

其中，所述平移梯度为

，且

，

；

根据配准过程中各个所述待配准点平移时所在栅格的梯度值，和旋转变换梯度公式

确定所述位置转换关系中的旋 转变换梯度；并根据配准过程中所述待配准点依次旋转变换的步长确定所述位置转换关系 中的旋转步长；

根据所述位置转换关系和所述粗略坐标值，确定所述机器人在所述点云地图中的精准坐标值，包括：

根据所述平移梯度、所述平移步长、所述旋转变换梯度、所述旋转步长和所述粗略坐标 值，获得所述机器人的精准坐标值：其中，

为所述粗略坐标值。

4.如权利要求1所述的机器人定位方法，其特征在于，获得所述机器人的粗略坐标值的过程包括：

根据所述机器人的驱动装置驱动所述机器人运动的驱动数据确定所述机器人在所述点云地图的坐标系中的所述粗略坐标值。

5.如权利要求1所述的机器人定位方法，其特征在于，获得待配准点云的过程包括：

通过和所述机器人相对固定设置的激光扫描仪扫描获得所述机器人周围环境中的待配准点云。

6.一种机器人定位装置，其特征在于，包括

梯度值模块，用于预先获得机器人所在环境的栅格化的点云地图，并根据每个栅格的栅格中心和最近目标点之间的距离确定所述栅格的梯度值；其中，每个栅格的最近目标点为距离栅格中心最近的目标点，每个栅格的梯度值为所述栅格中心到所述最近目标点的梯度大小；

初始坐标模块，用于根据机器人当前在所述点云地图中的粗略坐标值，以及待配准点云在机器人坐标系中的相对坐标值，获得所述待配准点云在所述点云地图中的初始坐标值；其中，所述相对坐标值为根据所述待配准点云和所述机器人之间的相对位置关系确定；

配准运算模块，用于根据所述初始坐标值、所述栅格的所述梯度值，利用ICP算法以所述待配准点云中每个待配准点所在栅格对应的最近目标点为所述待配准点的配准点，将所述待配准点云和所述点云地图中目标点云进行配准，获得所述待配准点云配准到所述点云地图中的目标点云的位置转换关系；

精准坐标模块，用于根据所述位置转换关系和所述粗略坐标值，确定所述机器人在所述点云地图中的精准坐标值；

所述梯度值模块，用于根据所述栅格中心在所述点云地图中的中心坐标值、和以所述 栅格为中心的预设区域内的各个目标点的目标点坐标值，结合得分函数

，依次确定所 述栅格相对于所述预设区域内的各个所述目标点的得分值；其中，

，

，

，

，

为所述目标点坐标值；

为所述中心坐标值；

分别为所述预设区域分别在x轴、y轴、z轴方向上的尺寸大 小；选取所述得分值最高的目标点作为最近目标点。

7.一种机器人定位设备，其特征在于，包括：

扫描装置，用于扫描获得所述机器人所在环境中的待配准点云，以及所述待配准点云和所述机器人的相对位置关系；

存储器，用于存储计算机程序；

处理器，用于根据所述待配准点云对应的相对位置关系确定所述待配准点云中各个待配准点在所述机器人的坐标系中的相对坐标值，并根据所述相对坐标值执行所述计算机程序，以实现如权利要求1至5任一项所述的机器人定位方法的步骤。

8.如权利要求7所述的机器人定位设备，其特征在于，所述扫描装置为固定在所述机器人上的激光扫描仪。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被执行以实现如权利要求1至5任一项所述的机器人定位方法的步骤。

Images