CN113492411A - 机器人抓取路径规划方法、装置、电子设备及存储介质 - Google Patents

Abstract

本申请属于机器人控制技术领域，公开了一种机器人抓取路径规划方法、装置、电子设备及存储介质，通过获取多个待抓取物体的位置坐标和放置点的位置坐标；获取机器人TCP点的起始点的位置坐标；根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，把多个待抓取物体分为两组；根据两组待抓取物体的位置坐标和放置点的位置坐标，分别建立两组待抓取物体的距离矩阵；根据两个距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；把两条初步最优路径整合为一条全局最优抓取路径；与直接对全部待抓取物体进行全局最优抓取路径规划的方式相比，耗费的规划时间更小，可更快速规划出全局最优抓取路径。

Description

机器人抓取路径规划方法、装置、电子设备及存储介质

技术领域

本申请涉及机器人控制技术领域，具体而言，涉及一种机器人抓取路径规划方法、装置、电子设备及存储介质。

背景技术

现有的一些工业产线中，使用机器人进行物件的移载，由机器人把物件从输送皮带或其它运输装置中抓取并放置在预设的放置工位上。在产线生产过程中，有时会存在分散的多个物件需要抓取的情况，传统抓取方法中抓取物件的先后顺序是人为预先设定的，其合理性与人的经验有关，无法保证其抓取顺序的合理性，无法保证机器人抓取所有物体所走过的全局路径较短。目前，也有通过蚁群算法来直接规划全局最优抓取路径，从而保证机器人抓取所有物体所走过的全局路径较短，但是，当待抓取的物件数量较多时，规划效率较低。

发明内容

本申请的目的在于提供一种机器人抓取路径规划方法、装置、电子设备及存储介质，可快速规划出全局最优抓取路径。

第一方面，本申请提供了一种机器人抓取路径规划方法，用于规划机器人抓取多个分散的物体到同一个放置点的抓取路径，包括以下步骤：

A1.获取多个待抓取物体的位置坐标和放置点的位置坐标；

A2.获取机器人TCP点的起始点的位置坐标；

A3.根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，把多个所述待抓取物体分为两组；

A4.根据两组所述待抓取物体的位置坐标和所述放置点的位置坐标，分别建立两组所述待抓取物体的距离矩阵；所述距离矩阵包含对应一组所述待抓取物体中任意两个所述待抓取物体与所述放置点的距离之和；

A5.根据两个所述距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；

A6.把两条所述初步最优路径整合为一条全局最优抓取路径。

本申请的机器人抓取路径规划方法，根据各待抓取物体的位置坐标和机器人TCP点的起始点的位置坐标，把待抓取物体分为两组，根据蚁群算法并行地针对两组待抓取物体进行初步最优路径的规划，最后把两条初步最优路径整合为一条全局最优抓取路径；由于把待抓取物体分为两组且并行地进行初步最优路径的规划，规划初步最优路径所需的时间较短，与直接对全部待抓取物体进行全局最优抓取路径规划的方式相比，需要耗费的规划时间更小，从而可快速规划出全局最优抓取路径。

优选地，步骤A3包括：

A301.根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，计算各所述待抓取物体与所述机器人TCP点的起始点的距离；

A302.根据各所述待抓取物体与所述机器人TCP点的起始点的距离确定最近物体、最远物体和中间物体；所述最近物体是指离所述机器人TCP点的起始点最近的所述待抓取物体，所述最远物体是指离所述机器人TCP点的起始点最远的所述待抓取物体，所述中间物体是指与所述最近物体的距离最接近所述最远物体与所述最近物体的距离的一半的所述待抓取物体；

A303.把与所述最近物体的距离小于或等于所述最远物体与所述最近物体的距离的一半的所述待抓取物体划分为第一组，把其它所述待抓取物体划分为第二组，并在不包含所述中间物体的一组待抓取物体中加入所述中间物体。

通过这种方式来对待抓取物体进行分组，可大致地把待抓取物体均分为两组，从而使两条初步最优路径的规划时间接近，有利于进一步降低路径规划的总耗时。

优选地，步骤A4包括针对两组所述待抓取物体，分别执行以下步骤：

计算各所述待抓取物体与所述放置点的距离；

依次以各所述待抓取物体为第一物体，以同组的其它所述待抓取物体为第二物体，分别计算各所述第二物体与所述放置点的距离和所述第一物体与所述放置点的距离之和；

以每个所述距离之和为一个元素，创建对应一组所述待抓取物体的距离矩阵。

优选地，步骤A5包括：

A501.以其中一组所述待抓取物体中的所述中间物体的位置点为终点，规划第一条初步最优路径；

A502.并行地，以另一组所述待抓取物体中的所述中间物体的位置点为起点，规划第二条初步最优路径；

步骤A6包括：

在所述中间物体的位置点处把所述第一条初步最优路径和所述第二条初步最优路径拼合为一条全局最优抓取路径。

在规划两条初步最优路径时，把其中一条的终点和另一条的起点均设置在中间物体的位置点处，从而在整合成一条全局最优抓取路径时，直接在中间物体的位置点处拼合即可，方便快捷。

优选地，步骤A501包括：

B1.对蚁群规模、信息素因子、启发函数因子、信息素挥发因子、信息素常数和最大迭代次数进行参数初始化；

B2.随机把蚂蚁放在不同的待抓取物体的位置点处作为出发点，对每个蚂蚁依次计算下一个应到达的所述待抓取物体的位置点，直到所有蚂蚁均访问完同一组内的所有所述待抓取物体的位置点；

B3. 根据对应的所述距离矩阵计算各蚂蚁经过的路径的长度，以终点为所述中间物体的位置点的所述路径中的最短路径为本次迭代的最优路径，对所述最优路径上的信息素浓度进行更新；

B4.判断当前迭代此时是否达到最大迭代次数，若否，则返回步骤B2进行下一次迭代，若是，则结束迭代并以最后得到的所述最优路径为所述第一条初步最优路径。

优选地，步骤A502包括：

C1.对蚁群规模、信息素因子、启发函数因子、信息素挥发因子、信息素常数和最大迭代次数进行参数初始化；

C2.把蚂蚁放在所述中间物体的位置点处作为出发点，对每个蚂蚁依次计算下一个应到达的所述待抓取物体的位置点，直到所有蚂蚁均访问完同一组内的所有所述待抓取物体的位置点；

C3. 根据对应的所述距离矩阵计算各蚂蚁经过的路径的长度，以长度最短的所述路径为本次迭代的最优路径，对所述最优路径上的信息素浓度进行更新；

C4.判断当前迭代此时是否达到最大迭代次数，若否，则返回步骤C2进行下一次迭代，若是，则结束迭代并以最后得到的所述最优路径为所述第二条初步最优路径。

第二方面，本申请提供了一种机器人抓取路径规划装置，用于规划机器人抓取多个分散的物体到同一个放置点的抓取路径，包括：

第一获取模块，用于获取多个待抓取物体的位置坐标和放置点的位置坐标；

第二获取模块，用于获取机器人TCP点的起始点的位置坐标；

分组模块，用于根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，把多个所述待抓取物体分为两组；

第一执行模块，用于根据两组所述待抓取物体的位置坐标和所述放置点的位置坐标，分别建立两组所述待抓取物体的距离矩阵；所述距离矩阵包含对应一组所述待抓取物体中任意两个所述待抓取物体与所述放置点的距离之和；

规划模块，用于根据两个所述距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；

整合模块，用于把两条所述初步最优路径整合为一条全局最优抓取路径。

本申请的机器人抓取路径规划装置，根据各待抓取物体的位置坐标和机器人TCP点的起始点的位置坐标，把待抓取物体分为两组，根据蚁群算法并行地针对两组待抓取物体进行初步最优路径的规划，最后把两条初步最优路径整合为一条全局最优抓取路径；由于把待抓取物体分为两组且并行地进行初步最优路径的规划，规划初步最优路径所需的时间较短，与直接对全部待抓取物体进行全局最优抓取路径规划的方式相比，需要耗费的规划时间更小，从而可快速规划出全局最优抓取路径。

优选地，所述分组模块用于在根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，把多个所述待抓取物体分为两组的时候：

根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，计算各所述待抓取物体与所述机器人TCP点的起始点的距离；

根据各所述待抓取物体与所述机器人TCP点的起始点的距离确定最近物体、最远物体和中间物体；所述最近物体是指离所述机器人TCP点的起始点最近的所述待抓取物体，所述最远物体是指离所述机器人TCP点的起始点最远的所述待抓取物体，所述中间物体是指与所述最近物体的距离最接近所述最远物体与所述最近物体的距离的一半的所述待抓取物体；

把与所述最近物体的距离小于或等于所述最远物体与所述最近物体的距离的一半的所述待抓取物体划分为第一组，把其它所述待抓取物体以及所述中间物体划分为第二组。

第三方面，本申请提供了一种电子设备，包括处理器以及存储器，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，运行如前文所述机器人抓取路径规划方法中的步骤。

第四方面，本申请提供了一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述机器人抓取路径规划方法中的步骤。

有益效果：

本申请提供的机器人抓取路径规划方法、装置、电子设备及存储介质，通过获取多个待抓取物体的位置坐标和放置点的位置坐标；获取机器人TCP点的起始点的位置坐标；根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，把多个所述待抓取物体分为两组；根据两组所述待抓取物体的位置坐标和所述放置点的位置坐标，分别建立两组所述待抓取物体的距离矩阵；所述距离矩阵包含对应一组所述待抓取物体中任意两个所述待抓取物体与所述放置点的距离之和；根据两个所述距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；把两条所述初步最优路径整合为一条全局最优抓取路径；由于把待抓取物体分为两组且并行地进行初步最优路径的规划，规划初步最优路径所需的时间较短，与直接对全部待抓取物体进行全局最优抓取路径规划的方式相比，需要耗费的规划时间更小，从而可快速规划出全局最优抓取路径。

本申请的其他特征和优点将在随后的说明书阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请了解。本申请的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

图1为本申请实施例提供的机器人抓取路径规划方法的一种流程图。

图2为本申请实施例提供的机器人抓取路径规划装置的第一种结构示意图。

图3为本申请实施例提供的电子设备的结构示意图。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参照图1，图1是本申请一些实施例中的机器人抓取路径规划方法，用于规划机器人抓取多个分散的物体到同一个放置点的抓取路径，包括以下步骤：

A1.获取多个待抓取物体的位置坐标和放置点的位置坐标；

A2.获取机器人TCP点的起始点的位置坐标；

A3.根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，把多个待抓取物体分为两组；

A4.根据两组待抓取物体的位置坐标和放置点的位置坐标，分别建立两组待抓取物体的距离矩阵；距离矩阵包含对应一组待抓取物体中任意两个待抓取物体与放置点的距离之和；

A5.根据两个距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；

A6.把两条初步最优路径整合为一条全局最优抓取路径。

该机器人抓取路径规划方法，根据各待抓取物体的位置坐标、机器人TCP点的起始点的位置坐标和放置点的位置坐标，把待抓取物体分为两组，根据蚁群算法并行地针对两组待抓取物体进行初步最优路径的规划，最后把两条初步最优路径整合为一条全局最优抓取路径；由于把待抓取物体分为两组且并行地进行初步最优路径的规划，规划初步最优路径所需的时间较短，与直接对全部待抓取物体进行全局最优抓取路径规划的方式相比，需要耗费的规划时间更小，从而可快速规划出全局最优抓取路径。在实际应用中，当机器人按照该全局最优抓取路径移动时，每当移动到一个待抓取物体的位置点（即该待抓取物体的位置坐标对应的点）时就会执行抓取动作以抓取对应的待抓取物体，然后移动到放置点处放下该待抓取物体，接着移动到下一个待抓取物体的位置点处，重复上述过程，直到完成所有待抓取物体的抓取。

其中，机器人在启动后，其TCP点（末端工具坐标系原点）会先移动到一个预设的位置点，该位置点即为机器人TCP点的起始点。其中，上述位置坐标是三维空间位置坐标，包括x、y、z三个轴坐标值。

其中，待抓取物体的位置坐标、放置点的位置坐标和机器人TCP点的起始点的位置坐标可以由人工输入、或者通过三维相机获取、或者通过读取预设的位置坐标记录文件得到，但不限于此。

其中，把多个待抓取物体分为两组时，可采用任意方式进行分组，例如，对各待抓取物体进行编号，然后根据编号顺序进行划分；或者随机分组。

在一些优选实施方式中，步骤A3包括：

A301.根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，计算各待抓取物体与机器人TCP点的起始点的距离；

A302.根据各待抓取物体与机器人TCP点的起始点的距离确定最近物体、最远物体和中间物体；最近物体是指离机器人TCP点的起始点最近的待抓取物体，最远物体是指离机器人TCP点的起始点最远的待抓取物体，中间物体是指与最近物体的距离（可把该距离称为距离1）最接近最远物体与最近物体的距离（可把该距离称为距离2）的一半的一个待抓取物体（即距离1最接近距离2的一半的一个待抓取物体就是中间物体）；

A303.把与最近物体的距离小于或等于最远物体与最近物体的距离的一半的待抓取物体划分为第一组，把其它待抓取物体划分为第二组，并在不包含中间物体的一组待抓取物体中加入中间物体（即第一组和第二组均包含该中间物体）。

其中，步骤A302包括：

按各待抓取物体与机器人TCP点的起始点的距离的大小对各待抓取物体进行升序排序；

以排序第一的待抓取物体为最近物体A，以排序最后的待抓取物体为最远物体B；

计算最近物体A与最远物体B之间的距离L；

计算各待抓取物体与最近物体A的距离

（

表示第i个待抓取物体与最近物体A之间的距离，包括最近物体A与自身的距离，该距离为0）；

计算各待抓取物体与最近物体A的距离

和L/2的偏差

（为绝对值）；

按该偏差

的大小对各待抓取物体进行升序排序；

以排序第一的（按偏差

排序第一的）待抓取物体为中间物体C。

其中，若与机器人TCP点的起始点距离最小的待抓取物体有两个（即有两个待抓取物体与机器人TCP点的起始点的距离相等且均为最小距离）时，则随机选择其中一个作为最近物体A，若与机器人TCP点的起始点距离最小的待抓取物体有三个以上（即有三个以上的待抓取物体与机器人TCP点的起始点的距离相等且均为最小距离，以下把该三个以上的待抓取物体称为第一物体），则先计算这些第一物体的分布中心点的位置坐标（分别计算这些第一物体的三个轴坐标值的均值，即得到第一物体的分布中心点的位置坐标的三个轴坐标值），然后以离该分布中心点最近的第一物体为最近物体A（若离该分布中心点最近的第一物体有多个，则随机选取一个为最近物体A）；

其中，若与机器人TCP点的起始点距离最大的待抓取物体有两个（即有两个待抓取物体与机器人TCP点的起始点的距离相等且均为最大距离）时，则随机选择其中一个作为最远物体B，若与机器人TCP点的起始点距离最大的待抓取物体有三个以上（即有三个以上的待抓取物体与机器人TCP点的起始点的距离相等且均为最大距离，以下把该三个以上的待抓取物体称为第二物体），则先计算这些第二物体的分布中心点的位置坐标（分别计算这些第二物体的三个轴坐标值的均值，即得到第二物体的分布中心点的位置坐标的三个轴坐标值），然后以离该分布中心点最近的第二物体为最远物体B（若离该分布中心点最近的第二物体有多个，则随机选取一个为最远物体B）；

其中，若偏差

最小的待抓取物体有多个（即有多个待抓取物体对应的偏差

相等且均为最小值），则可随机选择其中一个作为中间物体C，或者选择其中离最近物体A的位置点与最远物体B的位置点的连线AB最近的待抓取物体为中间物体C。

其中，步骤A303包括：

依次判断各待抓取物体（包括最近物体A本身）与最近物体A的距离

是否不大于L/2，若是，则把该待抓取物体划分到第一组，否则把该物体划分到第二组；

把中间物体C加入不包含中间物体的一组待抓取物体中（例如，中间物体C的距离

小于L/2，则会被直接划分入第一组，从而第二组不包含中间物体C，此时需要把中间物体C也添加到第二组）。

从而第一组包含最近物体A和中间物体C，第二组包含中间物体C和最远物体B，在后续进行初步最优路径规划时，可以令一条最优路径的终点和另一条最优路径的起点为该中间物体C的位置点，从而在进行两条最优路径的整合时，可直接在该中间物体C的位置点处拼合，方便快捷。

通过这种方式来对待抓取物体进行分组，可大致地把待抓取物体均分为两组，使两组待抓取物体的数量接近，从而使两条初步最优路径的规划时间接近（蚁群算法迭代过程所需时间接近），有利于进一步降低路径规划的总耗时。尤其是当待抓取物体的分布位置比较规则的时候（例如按照矩阵排布的时候），效果更佳。

优选地，步骤A4包括针对两组待抓取物体，分别执行以下步骤：

计算各待抓取物体与放置点的距离；

依次以各待抓取物体为第一物体，以同组的其它待抓取物体为第二物体，分别计算各第二物体与放置点的距离和第一物体与放置点的距离之和；

以每个距离之和为一个元素，创建对应一组待抓取物体的距离矩阵。

例如，假设放置点D的坐标为(Xd，Yd，Zd)，以第i个待抓取物体为第一物体时该第一物体的位置坐标为(X1, Y1，Z1)，以第j（j不等于i）个待抓取物体为第二物体时该第二物体的位置坐标为(X2, Y2，Z2)，则从第i个待抓取物体（第一物体）处到放置点D的距离

，从放置点D到第j个待抓取物体（第二物体）的距离

，从而，从第i个待抓取物体处移动到放置点D处再移动到第j个待抓取物体处的移动距离（即第二物体与放置点的距离和第一物体与放置点的距离之和）Dij= D1d+D2d，进而得到距离矩阵的一个元素。以此遍历同一组的所有待抓取物体，得到的所有i、j组合的元素，这些元素组成完整的距离矩阵。

优选地，步骤A5包括：

A501.以其中一组待抓取物体中的中间物体的位置点为终点，规划第一条初步最优路径；

A502.并行地，以另一组待抓取物体中的中间物体的位置点为起点，规划第二条初步最优路径；

步骤A6包括：

在中间物体的位置点处把第一条初步最优路径和第二条初步最优路径拼合为一条全局最优抓取路径。

在规划两条初步最优路径时，把其中一条的终点和另一条的起点均设置在中间物体的位置点处，从而在整合成一条全局最优抓取路径时，直接在中间物体的位置点处拼合即可，方便快捷。

其中，步骤A501包括：

B1.对蚁群规模、信息素因子、启发函数因子、信息素挥发因子、信息素常数和最大迭代次数进行参数初始化；

B2.随机把蚂蚁放在不同的待抓取物体的位置点处作为出发点，对每个蚂蚁依次计算下一个应到达的待抓取物体的位置点，直到所有蚂蚁均访问完同一组内的所有待抓取物体的位置点；

B3.根据对应的距离矩阵计算各蚂蚁经过的路径的长度，以终点为中间物体的位置点的路径中的最短路径为本次迭代的最优路径，对最优路径上的信息素浓度进行更新；

B4.判断当前迭代此时是否达到最大迭代次数，若否，则返回步骤B2进行下一次迭代，若是，则结束迭代并以最后得到的最优路径为第一条初步最优路径。

其中，步骤B3中，根据对应的距离矩阵计算各蚂蚁经过的路径的长度时，蚂蚁从一个待抓取物体的位置点移动到下一个待抓取物体的位置点时是先移动到放置点再移动到下一个待抓取物体的位置点的（而不是直接从一个待抓取物体的位置点移动到下一个待抓取物体的位置点），其移动距离为该距离矩阵中对应的元素值，根据蚂蚁经过各待抓取物体的位置点的顺序把该距离矩阵中对应的元素值相加即得到该蚂蚁经过的路径的长度。例如，若蚂蚁从第i个待抓取物体的位置点移动到第j个待抓取物体的位置点处，则其移动距离为该距离矩阵的元素Dij，而不是第i个待抓取物体的位置点与第j个待抓取物体的位置点之间的距离。

由于在每次迭代时，以终点为中间物体C的位置点的路径中的最短路径为本次迭代的最优路径，保证了最终得到的第一条初步最优路径的终点为中间物体C的位置点。

其中，步骤A502包括：

C1.对蚁群规模、信息素因子、启发函数因子、信息素挥发因子、信息素常数和最大迭代次数进行参数初始化；

C2.把蚂蚁放在中间物体的位置点处作为出发点，对每个蚂蚁依次计算下一个应到达的待抓取物体的位置点，直到所有蚂蚁均访问完同一组内的所有待抓取物体的位置点；

C3. 根据对应的距离矩阵计算各蚂蚁经过的路径的长度，以长度最短的路径为本次迭代的最优路径，对最优路径上的信息素浓度进行更新；

C4.判断当前迭代此时是否达到最大迭代次数，若否，则返回步骤C2进行下一次迭代，若是，则结束迭代并以最后得到的最优路径为第二条初步最优路径。

其中，步骤C3中，根据对应的距离矩阵计算各蚂蚁经过的路径的长度时，蚂蚁从一个待抓取物体的位置点移动到下一个待抓取物体的位置点时是先移动到放置点再移动到下一个待抓取物体的位置点的（而不是直接从一个待抓取物体的位置点移动到下一个待抓取物体的位置点），其移动距离为该距离矩阵中对应的元素值，根据蚂蚁经过各待抓取物体的位置点的顺序把该距离矩阵中对应的元素值相加即得到该蚂蚁经过的路径的长度。例如，若蚂蚁从第i个待抓取物体的位置点移动到第j个待抓取物体的位置点处，则其移动距离为该距离矩阵的元素Dij，而不是第i个待抓取物体的位置点与第j个待抓取物体的位置点之间的距离。

由于在每次迭代时，以中间物体C的位置点为出发点，保证了最终得到的第二条初步最优路径的起点为中间物体C的位置点。

由上可知，该机器人抓取路径规划方法，通过获取多个待抓取物体的位置坐标和放置点的位置坐标；获取机器人TCP点的起始点的位置坐标；根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，把多个待抓取物体分为两组；根据两组待抓取物体的位置坐标和放置点的位置坐标，分别建立两组待抓取物体的距离矩阵；距离矩阵包含对应一组待抓取物体中任意两个待抓取物体与放置点的距离之和；根据两个距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；把两条初步最优路径整合为一条全局最优抓取路径；由于把待抓取物体分为两组且并行地进行初步最优路径的规划，规划初步最优路径所需的时间较短，与直接对全部待抓取物体进行全局最优抓取路径规划的方式相比，需要耗费的规划时间更小，从而可快速规划出全局最优抓取路径。

请参考图2，本申请提供了一种机器人抓取路径规划装置，用于规划机器人抓取多个分散的物体到同一个放置点的抓取路径，包括：

第一获取模块1，用于获取多个待抓取物体的位置坐标和放置点的位置坐标；

第二获取模块2，用于获取机器人TCP点的起始点的位置坐标；

分组模块3，用于根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，把多个待抓取物体分为两组；

第一执行模块4，用于根据两组待抓取物体的位置坐标和放置点的位置坐标，分别建立两组待抓取物体的距离矩阵；距离矩阵包含对应一组待抓取物体中任意两个待抓取物体与放置点的距离之和；

规划模块5，用于根据两个距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；

整合模块6，用于把两条初步最优路径整合为一条全局最优抓取路径。

该机器人抓取路径规划装置，根据各待抓取物体的位置坐标和机器人TCP点的起始点的位置坐标，把待抓取物体分为两组，根据蚁群算法并行地针对两组待抓取物体进行初步最优路径的规划，最后把两条初步最优路径整合为一条全局最优抓取路径；由于把待抓取物体分为两组且并行地进行初步最优路径的规划，规划初步最优路径所需的时间较短，与直接对全部待抓取物体进行全局最优抓取路径规划的方式相比，需要耗费的规划时间更小，从而可快速规划出全局最优抓取路径。

其中，机器人在启动后，其机器人TCP点（末端工具坐标系原点）会先移动到一个预设的位置点，该位置点即为机器人TCP点的起始点。其中，上述位置坐标是三维空间位置坐标，包括x、y、z三个轴坐标值。

其中，待抓取物体的位置坐标、放置点的位置坐标和机器人TCP点的起始点的位置坐标可以由人工输入、或者通过三维相机获取、或者通过读取预设的位置坐标记录文件得到，但不限于此。

其中，分组模块3把多个待抓取物体分为两组时，可采用任意方式进行分组，例如，对各待抓取物体进行编号，然后根据编号顺序进行划分；或者随机分组。

在一些优选实施方式中，分组模块3用于在根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，把多个待抓取物体分为两组的时候：

根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，计算各待抓取物体与TCP点的起始点的距离；

根据各待抓取物体与机器人TCP点的起始点的距离确定最近物体、最远物体和中间物体；最近物体是指离机器人TCP点的起始点最近的待抓取物体，最远物体是指离机器人TCP点的起始点最远的待抓取物体，中间物体是指与最近物体的距离最接近最远物体与最近物体的距离的一半的待抓取物体；

把与最近物体的距离小于或等于最远物体与最近物体的距离的一半的待抓取物体划分为第一组，把其它待抓取物体划分为第二组，并在不包含中间物体的一组待抓取物体中加入中间物体。

其中，分组模块3在根据各待抓取物体与机器人TCP点的起始点的距离确定最近物体、最远物体和中间物体的时候：

按各待抓取物体与机器人TCP点的起始点的距离大小对各待抓取物体进行升序排序；

以排序第一的待抓取物体为最近物体A，以排序最后的待抓取物体为最远物体B；

计算最近物体A与最远物体B之间的距离L；

计算各待抓取物体与最近物体A的距离

（

表示第i个待抓取物体与最近物体A之间的距离，包括最近物体A与自身的距离，该距离为0）；

计算各待抓取物体与最近物体A的距离

和L/2的偏差

（为绝对值）；

按该偏差

的大小对各待抓取物体进行升序排序；

以排序第一的（按偏差

排序第一的）待抓取物体为中间物体C。

其中，若与机器人TCP点的起始点距离最小的待抓取物体有两个（即有两个待抓取物体与机器人TCP点的起始点的距离相等且均为最小距离）时，则随机选择其中一个作为最近物体A，若与机器人TCP点的起始点距离最小的待抓取物体有三个以上（即有三个以上的待抓取物体与机器人TCP点的起始点的距离相等且均为最小距离，以下把该三个以上的待抓取物体称为第一物体），则先计算这些第一物体的分布中心点的位置坐标（分别计算这些第一物体的三个轴坐标值的均值，即得到第一物体的分布中心点的位置坐标的三个轴坐标值），然后以离该分布中心点最近的第一物体为最近物体A（若离该分布中心点最近的第一物体有多个，则随机选取一个为最近物体A）；

其中，若与机器人TCP点的起始点距离最大的待抓取物体有两个（即有两个待抓取物体与机器人TCP点的起始点的距离相等且均为最大距离）时，则随机选择其中一个作为最远物体B，若与机器人TCP点的起始点距离最大的待抓取物体有三个以上（即有三个以上的待抓取物体与机器人TCP点的起始点的距离相等且均为最大距离，以下把该三个以上的待抓取物体称为第二物体），则先计算这些第二物体的分布中心点的位置坐标（分别计算这些第二物体的三个轴坐标值的均值，即得到第二物体的分布中心点的位置坐标的三个轴坐标值），然后以离该分布中心点最近的第二物体为最远物体B（若离该分布中心点最近的第二物体有多个，则随机选取一个为最远物体B）；

其中，若偏差

最小的待抓取物体有多个（即有多个待抓取物体对应的偏差

相等且均为最小值），则可随机选择其中一个作为中间物体C，或者选择其中离最近物体A的位置点与最远物体B的位置点的连线AB最近的待抓取物体为中间物体C。

其中，分组模块3在把与最近物体的距离小于或等于最远物体与最近物体的距离的一半的待抓取物体划分为第一组，把其它待抓取物体以及中间物体划分为第二组的时候：

依次判断各待抓取物体（包括最近物体A本身）与最近物体A的距离

是否不大于L/2，若是，则把该待抓取物体划分到第一组，否则把该物体划分到第二组；

把中间物体C加入不包含中间物体的一组待抓取物体中。

从而第一组包含最近物体A和中间物体C，第二组包含中间物体C和最远物体B，在后续进行初步最优路径规划时，可以令一条最优路径的终点和另一条最优路径的起点为该中间物体C的位置点，从而在进行两条最优路径的整合时，可直接在该中间物体C的位置点处拼合，方便快捷。

通过这种方式来对待抓取物体进行分组，可大致地把待抓取物体均分为两组，使两组待抓取物体的数量接近，从而使两条初步最优路径的规划时间接近（蚁群算法迭代过程所需时间接近），有利于进一步降低路径规划的总耗时。尤其是当待抓取物体的分布位置比较规则的时候（例如按照矩阵排布的时候），效果更佳。

优选地，第一执行模块4在分别建立两组待抓取物体的距离矩阵的时候，针对两组待抓取物体，分别执行以下步骤：

计算各待抓取物体与放置点的距离；

依次以各待抓取物体为第一物体，以同组的其它待抓取物体为第二物体，分别计算各第二物体与放置点的距离和第一物体与放置点的距离之和；

以每个距离之和为一个元素，创建对应一组待抓取物体的距离矩阵。

例如，假设放置点D的坐标为(Xd，Yd，Zd)，以第i个待抓取物体为第一物体时该第一物体的位置坐标为(X1, Y1，Z1)，以第j（j不等于i）个待抓取物体为第二物体时该第二物体的位置坐标为(X2, Y2，Z2)，则从第i个待抓取物体（第一物体）处到放置点D的距离

，从放置点D到第j个待抓取物体（第二物体）的距离

，从而，从第i个待抓取物体处移动到放置点D处再移动到第j个待抓取物体处的移动距离（即第二物体与放置点的距离和第一物体与放置点的距离之和）Dij= D1d+D2d，进而得到距离矩阵的一个元素。以此遍历同一组的所有待抓取物体，得到的所有i、j组合的元素，这些元素组成完整的距离矩阵。

优选地，规划模块5在根据两个距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径的时候：

以其中一组待抓取物体中的中间物体的位置点为终点，规划第一条初步最优路径；

并行地，以另一组待抓取物体中的中间物体的位置点为起点，规划第二条初步最优路径；

从而，整合模块6在把两条初步最优路径整合为一条全局最优抓取路径的时候：

在中间物体的位置点处把第一条初步最优路径和第二条初步最优路径拼合为一条全局最优抓取路径。

在规划两条初步最优路径时，把其中一条的终点和另一条的起点均设置在中间物体的位置点处，从而在整合成一条全局最优抓取路径时，直接在中间物体的位置点处拼合即可，方便快捷。

其中，规划模块5在以其中一组待抓取物体中的中间物体的位置为终点，规划第一条初步最优路径的时候执行以下步骤：

B1.对蚁群规模、信息素因子、启发函数因子、信息素挥发因子、信息素常数和最大迭代次数进行参数初始化；

B2.随机把蚂蚁放在不同的待抓取物体的位置点处作为出发点，对每个蚂蚁依次计算下一个应到达的待抓取物体的位置点，直到所有蚂蚁均访问完同一组内的所有待抓取物体的位置点；

B3. 根据对应的距离矩阵计算各蚂蚁经过的路径的长度，以终点为中间物体的位置点的路径中的最短路径为本次迭代的最优路径，对最优路径上的信息素浓度进行更新；

B4.判断当前迭代此时是否达到最大迭代次数，若否，则返回步骤B2进行下一次迭代，若是，则结束迭代并以最后得到的最优路径为第一条初步最优路径。

其中，步骤B3中，根据对应的距离矩阵计算各蚂蚁经过的路径的长度时，蚂蚁从一个待抓取物体的位置点移动到下一个待抓取物体的位置点时是先移动到放置点再移动到下一个待抓取物体的位置点的（而不是直接从一个待抓取物体的位置点移动到下一个待抓取物体的位置点），其移动距离为该距离矩阵中对应的元素值，根据蚂蚁经过各待抓取物体的位置点的顺序把该距离矩阵中对应的元素值相加即得到该蚂蚁经过的路径的长度。例如，若蚂蚁从第i个待抓取物体的位置点移动到第j个待抓取物体的位置点处，则其移动距离为该距离矩阵的元素Dij，而不是第i个待抓取物体的位置点与第j个待抓取物体的位置点之间的距离。

由于在每次迭代时，以终点为中间物体C的位置点的路径中的最短路径为本次迭代的最优路径，保证了最终得到的第一条初步最优路径的终点为中间物体C的位置点。

其中，规划模块5在以另一组待抓取物体中的中间物体的位置为起点，规划第二条初步最优路径的时候执行以下步骤：

C1.对蚁群规模、信息素因子、启发函数因子、信息素挥发因子、信息素常数和最大迭代次数进行参数初始化；

C2.把蚂蚁放在中间物体的位置点处作为出发点，对每个蚂蚁依次计算下一个应到达的待抓取物体的位置点，直到所有蚂蚁均访问完同一组内的所有待抓取物体的位置点；

C3. 根据对应的距离矩阵计算各蚂蚁经过的路径的长度，以长度最短的路径为本次迭代的最优路径，对最优路径上的信息素浓度进行更新；

C4.判断当前迭代此时是否达到最大迭代次数，若否，则返回步骤C2进行下一次迭代，若是，则结束迭代并以最后得到的最优路径为第二条初步最优路径。

其中，步骤C3中，根据对应的距离矩阵计算各蚂蚁经过的路径的长度时，蚂蚁从一个待抓取物体的位置点移动到下一个待抓取物体的位置点时是先移动到放置点再移动到下一个待抓取物体的位置点的（而不是直接从一个待抓取物体的位置点移动到下一个待抓取物体的位置点），其移动距离为该距离矩阵中对应的元素值，根据蚂蚁经过各待抓取物体的位置点的顺序把该距离矩阵中对应的元素值相加即得到该蚂蚁经过的路径的长度。例如，若蚂蚁从第i个待抓取物体的位置点移动到第j个待抓取物体的位置点处，则其移动距离为该距离矩阵的元素Dij，而不是第i个待抓取物体的位置点与第j个待抓取物体的位置点之间的距离。

由于在每次迭代时，以中间物体C的位置点为出发点，保证了最终得到的第二条初步最优路径的起点为中间物体C的位置点。

由上可知，该机器人抓取路径规划装置，通过获取多个待抓取物体的位置坐标和放置点的位置坐标；获取机器人TCP点的起始点的位置坐标；根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，把多个待抓取物体分为两组；根据两组待抓取物体的位置坐标和放置点的位置坐标，分别建立两组待抓取物体的距离矩阵；距离矩阵包含对应一组待抓取物体中任意两个待抓取物体与放置点的距离之和；根据两个距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；把两条初步最优路径整合为一条全局最优抓取路径；由于把待抓取物体分为两组且并行地进行初步最优路径的规划，规划初步最优路径所需的时间较短，与直接对全部待抓取物体进行全局最优抓取路径规划的方式相比，需要耗费的规划时间更小，从而可快速规划出全局最优抓取路径。

请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备300，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当计算设备运行时，处理器301执行该计算机程序，以执行上述实施例的任一可选的实现方式中的机器人抓取路径规划方法，以实现以下功能：获取多个待抓取物体的位置坐标和放置点的位置坐标；获取机器人TCP点的起始点的位置坐标；根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，把多个待抓取物体分为两组；根据两组待抓取物体的位置坐标和放置点的位置坐标，分别建立两组待抓取物体的距离矩阵；距离矩阵包含对应一组待抓取物体中任意两个待抓取物体与放置点的距离之和；根据两个距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；把两条初步最优路径整合为一条全局最优抓取路径。

本申请实施例提供一种存储介质，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的机器人抓取路径规划方法，以实现以下功能：获取多个待抓取物体的位置坐标和放置点的位置坐标；获取机器人TCP点的起始点的位置坐标；根据机器人TCP点的起始点的位置坐标和多个待抓取物体的位置坐标，把多个待抓取物体分为两组；根据两组待抓取物体的位置坐标和放置点的位置坐标，分别建立两组待抓取物体的距离矩阵；距离矩阵包含对应一组待抓取物体中任意两个待抓取物体与放置点的距离之和；根据两个距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；把两条所述初步最优路径整合为一条全局最优抓取路径。其中，存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random Access Memory, 简称SRAM），电可擦除可编程只读存储器（Electrically Erasable Programmable Read-OnlyMemory, 简称EEPROM），可擦除可编程只读存储器（Erasable Programmable Read OnlyMemory, 简称EPROM），可编程只读存储器（Programmable Red-Only Memory, 简称PROM），只读存储器（Read-Only Memory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。

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

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

再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (10)

1.一种机器人抓取路径规划方法，用于规划机器人抓取多个分散的物体到同一个放置点的抓取路径，其特征在于，包括以下步骤：

A1.获取多个待抓取物体的位置坐标和放置点的位置坐标；

A2.获取机器人TCP点的起始点的位置坐标；

A3.根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，把多个所述待抓取物体分为两组；

A4.根据两组所述待抓取物体的位置坐标和所述放置点的位置坐标，分别建立两组所述待抓取物体的距离矩阵；所述距离矩阵包含对应一组所述待抓取物体中任意两个所述待抓取物体与所述放置点的距离之和；

A5.根据两个所述距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；

A6.把两条所述初步最优路径整合为一条全局最优抓取路径。

2.根据权利要求1所述的机器人抓取路径规划方法，其特征在于，步骤A3包括：

A301.根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，计算各所述待抓取物体与所述机器人TCP点的起始点的距离；

A302.根据各所述待抓取物体与所述机器人TCP点的起始点的距离确定最近物体、最远物体和中间物体；所述最近物体是指离所述机器人TCP点的起始点最近的一个所述待抓取物体，所述最远物体是指离所述机器人TCP点的起始点最远的一个所述待抓取物体，所述中间物体是指与所述最近物体的距离最接近所述最远物体与所述最近物体的距离的一半的一个所述待抓取物体；

A303.把与所述最近物体的距离小于或等于所述最远物体与所述最近物体的距离的一半的所述待抓取物体划分为第一组，把其它所述待抓取物体划分为第二组，并在不包含所述中间物体的一组待抓取物体中加入所述中间物体。

3.根据权利要求1所述的机器人抓取路径规划方法，其特征在于，步骤A4包括针对两组所述待抓取物体，分别执行以下步骤：

计算各所述待抓取物体与所述放置点的距离；

依次以各所述待抓取物体为第一物体，以同组的其它所述待抓取物体为第二物体，分别计算各所述第二物体与所述放置点的距离和所述第一物体与所述放置点的距离之和；

以每个所述距离之和为一个元素，创建对应一组所述待抓取物体的距离矩阵。

4.根据权利要求2所述的机器人抓取路径规划方法，其特征在于，步骤A5包括：

A501.以其中一组所述待抓取物体中的所述中间物体的位置点为终点，规划第一条初步最优路径；

A502.并行地，以另一组所述待抓取物体中的所述中间物体的位置点为起点，规划第二条初步最优路径；

步骤A6包括：

在所述中间物体的位置点处把所述第一条初步最优路径和所述第二条初步最优路径拼合为一条全局最优抓取路径。

5.根据权利要求4所述的机器人抓取路径规划方法，其特征在于，步骤A501包括：

B1.对蚁群规模、信息素因子、启发函数因子、信息素挥发因子、信息素常数和最大迭代次数进行参数初始化；

B2.随机把蚂蚁放在不同的待抓取物体的位置点处作为出发点，对每个蚂蚁依次计算下一个应到达的所述待抓取物体的位置点，直到所有蚂蚁均访问完同一组内的所有所述待抓取物体的位置点；

B3.根据对应的所述距离矩阵计算各蚂蚁经过的路径的长度，以终点为所述中间物体的位置点的所述路径中的最短路径为本次迭代的最优路径，对所述最优路径上的信息素浓度进行更新；

B4.判断当前迭代此时是否达到最大迭代次数，若否，则返回步骤B2进行下一次迭代，若是，则结束迭代并以最后得到的所述最优路径为所述第一条初步最优路径。

6.根据权利要求4所述的机器人抓取路径规划方法，其特征在于，步骤A502包括：

C1.对蚁群规模、信息素因子、启发函数因子、信息素挥发因子、信息素常数和最大迭代次数进行参数初始化；

C2.把蚂蚁放在所述中间物体的位置点处作为出发点，对每个蚂蚁依次计算下一个应到达的所述待抓取物体的位置点，直到所有蚂蚁均访问完同一组内的所有所述待抓取物体的位置点；

C3.根据对应的所述距离矩阵计算各蚂蚁经过的路径的长度，以长度最短的所述路径为本次迭代的最优路径，对所述最优路径上的信息素浓度进行更新；

C4.判断当前迭代此时是否达到最大迭代次数，若否，则返回步骤C2进行下一次迭代，若是，则结束迭代并以最后得到的所述最优路径为所述第二条初步最优路径。

7.一种机器人抓取路径规划装置，用于规划机器人抓取多个分散的物体到同一个放置点的抓取路径，其特征在于，包括：

第一获取模块，用于获取多个待抓取物体的位置坐标和放置点的位置坐标；

第二获取模块，用于获取机器人TCP点的起始点的位置坐标；

分组模块，用于根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，把多个所述待抓取物体分为两组；

第一执行模块，用于根据两组所述待抓取物体的位置坐标和所述放置点的位置坐标，分别建立两组所述待抓取物体的距离矩阵；所述距离矩阵包含对应一组所述待抓取物体中任意两个所述待抓取物体与所述放置点的距离之和；

规划模块，用于根据两个所述距离矩阵，并行地采用蚁群算法分别规划一条初步最优路径；

整合模块，用于把两条所述初步最优路径整合为一条全局最优抓取路径。

8.根据权利要求7所述的机器人抓取路径规划装置，其特征在于，所述分组模块用于在根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，把多个所述待抓取物体分为两组的时候：

根据所述机器人TCP点的起始点的位置坐标和多个所述待抓取物体的位置坐标，计算各所述待抓取物体与所述机器人TCP点的起始点的距离；

根据各所述待抓取物体与所述机器人TCP点的起始点的距离确定最近物体、最远物体和中间物体；所述最近物体是指离所述机器人TCP点的起始点最近的所述待抓取物体，所述最远物体是指离所述机器人TCP点的起始点最远的所述待抓取物体，所述中间物体是指与所述最近物体的距离最接近所述最远物体与所述最近物体的距离的一半的所述待抓取物体；

把与所述最近物体的距离小于或等于所述最远物体与所述最近物体的距离的一半的所述待抓取物体划分为第一组，把其它所述待抓取物体以及所述中间物体划分为第二组。

9.一种电子设备，其特征在于，包括处理器以及存储器，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，运行如权利要求1-6任一项所述机器人抓取路径规划方法中的步骤。

10.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时运行如权利要求1-6任一项所述机器人抓取路径规划方法中的步骤。

Images