CN112238452B - 机械臂路径规划方法、装置、工控设备及存储介质 - Google Patents

Abstract

本发明实施例公开了一种机械臂路径规划方法、装置、工控设备及存储介质。该机械臂路径规划方法包括：在机械臂的机械臂末端承装待分拣的第一物品时，获取机械臂末端的起始位置信息及存放第一物品的格口位置信息；确定机械臂末端的最大可达范围空间；将最大可达范围空间划分为网格空间；根据起始位置信息和格口位置信息，在网格空间规划机械臂末端的运动路径。本发明实施例中先确定机械臂末端的最大可达范围空间，再划分网格空间，进而在网格空间规划机械臂末端的运动路径，由于网格空间的划分，使得机械臂末端路径的规划更加细化，路径规划更加细致，避免了现有路径规划的随意性，提高了路径规划的灵活性和效率，进而提高了物品分拣效率。

Description

机械臂路径规划方法、装置、工控设备及存储介质

技术领域

本发明涉及物流技术领域，具体涉及一种机械臂路径规划方法、装置、工控设备及存储介质。

背景技术

在快递配送中，快递分拣是其非常重要的环节，如何低成本高效的分拣至关重要。现大规模应用的分拣模式主要有人工分拣、交叉带分拣等。

其中，人工分拣成本高效率低，但分拣模式灵活，对二维码或地址信息模糊的包裹也能正确分拣。而交叉带分拣效率高，但存在占地面积大，分拣流向少的缺点，占地面积100平方米的交叉带大约具有60-80个流向，适合流量大流向小的一级中转场。对于机械臂分拣模式，单台机械臂占地面积约20平方，流向约70-80个，适合接近用户的中转场。

然而机械臂分拣的局限性在于目前机械臂进行分拣效率较低，使得机械臂分拣还无法完全取代人工分拣和交叉带分拣。为提高机械臂分拣效率，路径规划必不可少，通过最优路径进行分拣可极大提高分拣效率，现有的机械臂路径规划方法包括人工标定方法、快速扩展随机树(Rapidly exploring Random Tree， RRT)算法、路线图算法(probabilisticroadmap method，PRM)等随机的规划方法，然而随机规划方法计算量过大，且路径规划随意性大，规划路径慢又难以达到最优。

发明内容

本发明实施例提供一种机械臂路径规划方法、装置、工控设备及存储介质，使得机械臂末端路径的规划更加细化，路径规划更加细致，避免了现有路径规划的随意性，提高了路径规划的灵活性和效率，进而提高了物品分拣效率。

第一方面，本申请提供一种机械臂路径规划方法，应用于工控设备，所述机械臂路径规划方法，包括：

在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息；

确定所述机械臂末端的最大可达范围空间；

将所述最大可达范围空间划分为网格空间；

根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径。

在本申请一些实施例中，所述根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径，包括：

在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的最短时间路径；

将所述最短时间路径，作为所述机械臂末端的运动路径。

在本申请一些实施例中，所述在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的最短时间路径，包括：

在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的所有可达规划路径；

确定所述所有可达规划路径中的最短时间路径。

在本申请一些实施例中，所述确定所述所有可达规划路径中的最短时间路径，包括：

计算所述机械臂末端通过所述网格空间中每一条边长的最小时间；

根据所述网格空间中每一条边长的最小时间，确定所述所有可达规划路径中用时最短的路径；

对所述用时最短的路径进行样条拟合，得到所述最短时间路径。

在本申请一些实施例中，所述确定机械臂的机械臂末端的最大可达范围空间，包括：

采用D-H矩阵建立机器人运动学模型；

根据所述机器人运动学模型，确定所述机械臂末端的最大可达范围空间。

在本申请一些实施例中，所述方法还包括：

将所述运动路径发送给所述机械臂，以使得所述机械臂按照所述运动路径驱动所述机械臂末端运动。

在本申请一些实施例中，所述方法还包括：

保存每个已分拣的物品对应的机械臂末端起始位置信息、对应的存放格口位置信息和对应的最短时间路径；

在所述机械臂末端承装待分拣的第二物品时，获取所述机械臂末端当前起始位置信息及存放所述第二物品的格口位置信息；

若所述当前起始位置信息与已分拣目标物品对应的机械臂末端起始位置信息匹配，且所述第二物品的格口位置信息与所述目标物品对应的存放格口位置信息匹配，则获取所述目标物品对应的最短时间路径；

将所述目标物品对应的最短时间路径，作为所述机械臂末端分拣所述第二物品的运动路径。

第二方面，本申请提供一种机械臂路径规划装置，应用于工控设备，所述机械臂路径规划装置，包括：

获取单元，用于在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息；

确定单元，用于确定所述机械臂末端的最大可达范围空间；

划分单元，用于将所述最大可达范围空间划分为网格空间；

规划单元，用于根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径。

在本申请一些实施例中，所述规划单元具体用于：

在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的最短时间路径；

将所述最短时间路径，作为所述机械臂末端的运动路径。

在本申请一些实施例中，所述规划单元具体用于：

在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的所有可达规划路径；

确定所述所有可达规划路径中的最短时间路径。

在本申请一些实施例中，所述规划单元具体用于：

计算所述机械臂末端通过所述网格空间中每一条边长的最小时间；

根据所述网格空间中每一条边长的最小时间，确定所述所有可达规划路径中用时最短的路径；

对所述用时最短的路径进行样条拟合，得到所述最短时间路径。

在本申请一些实施例中，所述确定单元具体用于：

采用D-H矩阵建立机器人运动学模型；

根据所述机器人运动学模型，确定所述机械臂末端的最大可达范围空间。

在本申请一些实施例中，所述机械臂路径规划装置还包括发送单元，所述发送单元用于：

将所述运动路径发送给所述机械臂，以使得所述机械臂按照所述运动路径驱动所述机械臂末端运动。

在本申请一些实施例中，所述规划单元具体还用于：

保存每个已分拣的物品对应的机械臂末端起始位置信息、对应的存放格口位置信息和对应的最短时间路径；

在所述机械臂末端承装待分拣的第二物品时，获取所述机械臂末端当前起始位置信息及存放所述第二物品的格口位置信息；

若所述当前起始位置信息与已分拣目标物品对应的机械臂末端起始位置信息匹配，且所述第二物品的格口位置信息与所述目标物品对应的存放格口位置信息匹配，则获取所述目标物品对应的最短时间路径；

将所述目标物品对应的最短时间路径，作为所述机械臂末端分拣所述第二物品的运动路径。

第三方面，本申请提供一种工控设备，所述工控设备包括：

一个或多个处理器；

存储器；以及

一个或多个应用程序，其中所述一个或多个应用程序被存储于所述存储器中，并配置为由所述处理器执行以实现第一方面中任一项所述的机械臂路径规划方法。

第四方面，本申请还提供一种存储介质，其上存储有计算机程序，所述计算机程序被处理器进行加载，以执行第一方面中任一项所述的机械臂路径规划方法中的步骤。

本发明实施例在机械臂的机械臂末端承装待分拣的第一物品时，获取机械臂末端的起始位置信息及存放第一物品的格口位置信息；确定机械臂末端的最大可达范围空间；将最大可达范围空间划分为网格空间；根据起始位置信息和格口位置信息，在网格空间规划机械臂末端的运动路径。本发明实施例中在现有随机为机械臂规划路径的基础上，先确定机械臂末端的最大可达范围空间，在此最大可达范围空间内划分网格空间，进而在网格空间规划机械臂末端的运动路径，由于网格空间的划分，使得机械臂末端路径的规划更加细化，路径规划更加细致，避免了现有路径规划的随意性，提高了路径规划的灵活性和效率，进而提高了物品分拣效率。

附图说明

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

图1是本发明实施例中提供的物品分拣系统的一个实施例场景示意图；

图2是本发明实施例中提供的物品分拣系统中机械臂与分拣系统的分布场景示意图；

图3是本本发明实施例中提供的机械臂路径规划方法的一个实施例流程示意图；

图4是本发明实施例中提供的网格空间的一个实施例场景示意图；

图5是本发明实施例中网格空间中一个网格的实施例结构示意图；

图6是本发明实施例中在网格空间中进行路径规划的一个场景示意图；

图7是本发明实施例中对用时最短的路径进行三次样条拟合，得到拟合曲线的场景示意图；

图8是本发明实施例中机械臂路径规划装置的一个实施例结构示意图；

图9是本发明实施例中工控设备的一个实施例结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个所述特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

在本申请中，“示例性”一词用来表示“用作例子、例证或说明”。本申请中被描述为“示例性”的任何实施例不一定被解释为比其它实施例更优选或更具优势。为了使本领域任何技术人员能够实现和使用本发明，给出了以下描述。在以下描述中，为了解释的目的而列出了细节。应当明白的是，本领域普通技术人员可以认识到，在不使用这些特定细节的情况下也可以实现本发明。在其它实例中，不会对公知的结构和过程进行详细阐述，以避免不必要的细节使本发明的描述变得晦涩。因此，本发明并非旨在限于所示的实施例，而是与符合本申请所公开的原理和特征的最广范围相一致。

本发明实施例提供一种机械臂路径规划方法、装置、工控设备及存储介质，以下分别进行详细说明。

请参阅图1，图1为本发明实施例所提供的物品分拣系统的场景示意图，该物品分拣系统可以包括供件传送带101、工控设备102、至少一个分拣柜103和与工控设备通信连接的至少一个机械臂104。

其中，供件传送带101用于传输待分拣物品105，供件传送带101设置有扫码系统1011和光栅1012，该扫码系统与工控设备102通信连接，该扫码系统1011 用于扫码获取供件传送带101上待分拣物品105的物品信息(如物品名称，物品编码等)，并将该物品信息传输给工控设备1025，工控设备102可以根据待分拣物品的物品信息，为待分拣物品105在至少一个分拣柜103分配空闲的格口，以存放该待分拣物品105。光栅1012的作用是让传送带启动和停止用的，假如新的待分拣物品105来到指定位置，机械臂末端还没有到达起始位置，就停止供件传送带101，防止待分拣物品105掉地上，机器臂末端到达起始位置后再启动供件传送带101，至少一个机械臂104用于将供件传送带101上到达指定位置的待分拣物品分拣存放到至少一个分拣柜103中分配的格口中。

本发明实施例中，至少一个分拣柜103可以包括多个，优选的，如图1所示，每个机械臂104都可以对应设置为3个分拣柜103，分别设置在机械臂104的三面，以便于机械臂104对物品的分拣。

图1中，自动供件的供件传送带101以相等时间间隔匀速供件，供件传送带 101和机械臂104具有独立的控制系统，两个控制系统都与工控设备102建立通信连接，待分拣物品经过扫描系统时，自动计算该物品的流向，即对应的分拣格口，并将分拣该待分拣物品的格口信息发给工控设备102，工控设备102进行路径规划，路径规划完以后将规划路径发给机械臂控制装置，机械臂控制装置控制机械臂末端完成物品分拣，分拣完成后机械臂末端回到机械臂末端的固定起始位置，完成物品分拣。

机械臂是拟人手臂、手腕和手功能的机械电子装置，每个机械臂104中都设置有可驱动的机械臂末端以及驱动机械臂末端的机械臂控制装置，机械臂控制装置可控制机械臂末端按空间位姿(位置和姿态)的时变要求进行移动，从而完成某一工业生产的作业要求。工控设备101用于为机械臂规划运动路径，并将规划好的运动路径发送给机械臂104，使得机械臂104中的机械臂控制装置可以按照接收的运动路径驱动机械臂末端进行物品分拣。

如图2所示，为机械臂进行物品(如快递)分拣的一个实施例分拣示意图，分拣柜具有多个格口，每个格口表示一个流向，物品经过上游设备(对待分拣物品初步分拣处理的设备)处理后，以固定时间间隔将待分拣物品提供给供件传送带，供件传送带将待分拣物品掉入机械臂末端中，机械臂将机械臂末端内的待分拣物品投放到相格口中，完成分拣。

需要说明的是，本发明实施例中机械臂末端为可以承装物品的结构，例如吸盘或铲子等，由于传统机械臂末端多为吸盘，吸盘难以吸起重量较大物体，本发明实施例中主要用于物流领域的物品分拣，且物流领域的物品表面不平整也难以吸起，本发明实施例中机械臂末端优选为为铲子，物品可以直接掉入铲子中进行分拣，能够满足多种待分拣物品的类型，例如各种不同规格的盒子，包装袋或文件夹等。

由于现有的机械臂路径规划方法包括人工标定方法、快速扩展随机树 (Rapidlyexploring Random Tree，RRT)、路线图算法(probabilistic roadmap method，PRM)等随机的规划方法，其中，PRM算法是基于启发式节点增强策略的一种路径规划方法，很好的解决了在高维空间中构造出有效路径图的困难。该算法通过在构形空间中进行采样、对采样点进行碰撞检测、测试相邻采样点是否能够连接来表示路径图的连通性。此方法的一个巨大优点是，其复杂度主要依赖于寻找路径的难度，跟整个规划场景的大小和构形空间的维数关系不大。然而当规划的路径需要通过密集的障碍物或者需要经过狭窄的通道时， PRM方法的效率变的低下。RRT路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点(如机械臂末端的起始位置点)到目标点(如存放第一物品的格口位置点)的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似，该方法是概率完备且不最优的。

从上可知，现有的路径规划算法计算量过大，且路径规划随意性大，规划路径慢又难以达到最优。

因此，本发明实施例中首先基于工控设备101提供一种机械臂路径规划方法，该机械臂路径规划方法包括：在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息；确定所述机械臂末端的最大可达范围空间；将所述最大可达范围空间划分为网格空间；根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径。

如图3所示，为本发明实施例中机械臂路径规划方法的一个实施例流程示意图，该机械臂路径规划方法应用于工控设备，所述机械臂路径规划方法包括：

301、在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息。

如图1中所示，当供件传送带101传输待分拣的第一物品时，供件传送带101 可以扫码获取第一物品的物品信息(如物品名称，物品编码等)，并发送给工控设备102，工控设备102可以根据第一物品的物品信息，为第一物品在至少一个分拣柜103分配空闲的格口，以存放该第一物品，此时，工控设备102即可确定该用于存放第一物品的格口的格口位置信息。

本发明实施例中，可以在机械臂末端设置一个或多个与工控设备102通信连接的位置传感器，该位置传感器可以协助工控设备102实时获取机械臂末端当前的起始位置信息，当供件传送带101传输待分拣的第一物品到指定位置(如供件传送带101靠近机械臂一侧的最右端)时，机械臂的机械臂末端可承装待分拣的第一物品，此时工控设备102可根据上面描述的方式获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息。

另外，本发明实施例中所描述的物品，如第一物品，第二物品等，可以物流领域的物品，如快递，包裹，文件袋等，具体此处不做限定。

302、确定所述机械臂末端的最大可达范围空间。

本发实施例中，机械臂本身有一个运动范围，最大可达范围空间应该是机械臂的运动范围内的可达范围(如去掉机械臂运动范围内的一些障碍物)。

具体的，所述确定机械臂的机械臂末端的最大可达范围空间的步骤可以包括：采用D-H矩阵建立机器人运动学模型；根据所述机器人运动学模型，确定所述机械臂末端的最大可达范围空间。

D-H矩阵全称Denavit-Hartenberg Matrix，是Denavit和Hartenberg在1955 年提出一种通用的方法，这种方法在机械臂的每个连杆上都固定一个坐标系，然后用4×4的齐次变换矩阵来描述相邻两连杆的空间关系。通过依次变换可最终推导出末端执行器相对于基坐标系的位姿，从而建立机械臂对应的机器人运动学模型。

具体的，采用D-H矩阵建立机器人运动学模型如下：

其中，n_x，n_y，n_z，o_x，o_y，o_z，α_x，α_y，α_z表示机械臂末端姿态信息中的参数，p_x，p_y，p_z表示机械臂末端在点p的位置信息，即分别代表p点的x轴， y轴和z轴的坐标信息，根据不同机械臂关节对应的坐标信息，可确认不同类型机械臂各关节运动范围，具体的，根据不同类型机械臂各关节运动范围，具体可采用蒙特卡洛算法，可以得出机械臂运动范围，即机械臂末端的最大可达范围空间，如图4所示，S_min表示机械臂末端最小可达范围，S_max表示机械臂最大可达范围，S_min与S_max之间区域表示机械臂末端的最大可达范围空间。

303、将所述最大可达范围空间划分为网格空间。

在步骤302中确定机械臂末端的最大可达范围空间之后，即可将所述最大可达范围空间划分为网格空间，在一个具体实施方式中，将所述最大可达范围空间划分为网格空间：设定机械臂基坐标系，在机械臂基坐标系空间中，以预设网格边长对所述最大可达范围空间划分网格，具体可以是如图4所示，机械臂基坐标系位于点O，将所述最大可达范围空间划分为网格空间，网格边长为 d(即预设网格边长)，网格交点坐标为p(x，y，z)，如图4中的p(x1，y2， z2)、p(x1，y1，z1)和p(x2，y1，z1)等。

304、根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径。

具体的，在本申请一些实施例中，所述根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径的步骤可以进一步包括：在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的最短时间路径；将所述最短时间路径，作为所述机械臂末端的运动路径。

其中，所述在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的最短时间路径的步骤包括：在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的所有可达规划路径；确定所述所有可达规划路径中的最短时间路径。

如图4所示，当工控设备知晓机械臂末端的起始位置和存放第一物品的格口位置时，即可确定网格空间中所述机械臂末端从所述起始位置到所述格口位置之间的所有可达规划路径。

具体的，所述确定所述所有可达规划路径中的最短时间路径的步骤可以进一步包括：

(1)计算所述机械臂末端通过所述网格空间中每一条边长的最小时间。

在本发明实施例中分拣物品的过程中，为了防止物品飞出或掉落，机械臂末端姿态基本保持不变，在整个运动过程中仅绕Z轴旋转一定角度。因此机械臂末端姿态对路径规划影响较小。所以针对本本发明实施例中忽略机械臂末端姿态变化对路径规划的影响，假设仅机械臂末端位置发生变化。

在此基础上，由于对于每个机械臂产品来说，机械臂各关节最大速度以及最大旋转角度是确定的，可以预先在路径规划算法中初始化机械臂的这些速度参数信息，使得工控设备可以直接知晓这些速度参数信息。

此时，计算所述机械臂末端通过所述网格空间中每一条边长的最小时间可以包括：根据矢量积法计算机械臂末端的雅克比矩阵，根据机械臂末端的雅克比矩阵，计算机械臂通过所述网格空间中每一条边长的最小时间，下面以一个具体实施例进行介绍：

如图5所示，机械臂末端从点P(x0，y0，z0)位置在三维空间内沿x方向运动，根据矢量法，可以得出机械臂末端的雅克比矩阵J，根据运动学微分方程有：

其中，ν和ω分别表示机械臂末端线速度和角速度，

表示机械臂各关节速度。

当机械臂末端沿x方向移动时间

上述方程可以转换为：

因此，

与

线性相关，由于机械臂关节q₁,q₂,q₃位置和速度限制，所以机械臂关节速度存在最大值，如图4所示，当机械臂关节速度q₁,q₂,q₃中某个值达到最大时，可得机械臂末端从当前位置p(x0，y0，z0)在x方向

的最大速度

同理可得机械臂末端从当前位置p(x0，y0，z0)在y方向和z方向的最大速度分别为

和

当网格空间中网格划分足够小时，即边长d足够小时，从p(x0，y0，z0) 沿x方向匀速运动到点p(x1，y0，z0)的最 大速度为

从p(x0,y0,z0)沿x 方向匀速运动到点p(x1,y0,z0)的最小时间即为

同理可得沿着y 方向和沿z方向运动的最小时间为t_y-min和t_z-min。

按照同样的方法可以得到机械臂通过网格空间中每一条边长的最小时间为tmin，即可计算机械臂通过所述网格空间中每一条边长的最小时间，当已知机械臂当前的起始位置和终止位置时，采用如AStar、Dijkstra、Floyd-Warshall 等搜索算法，搜索从起始点到达目标点的最短时间路径，可以得到如下图6 所示的一段规划路径。

(2)根据所述网格空间中每一条边长的最小时间，确定所述所有可达规划路径中用时最短的路径。

由于上述步骤(1)计算除机械臂通过所述网格空间中每一条边长的最小时间，因此，对于所有可达规划路径，即可计算出每条可达规划路径的用时，具体的，及每条可达规划路径的最少用时，即为每条可达规划路径中对应的每条网格边长的时间之和。

在计算所有可达规划路径中每条可达规划路径的用时之后，根据时间长短比较，即可确定所述所有可达规划路径中用时最短的路径。

(3)对所述用时最短的路径进行样条拟合，得到所述最短时间路径。

如图6所示，其中起始点到目标的路径为搜索到的用时最短的路径，由于路径不连续，机械臂按照此路径分拣会产生抖动现象。本发明实施例中，可以采用不同的样条函数对所述用时最短的路径进行样条拟合，得到所述最短时间路径，例如，B-样条函数、三次样条函数或五次样条函数等，具体的如下图7 所示，在用时最短的路径上均匀选取若干控制点(即图7中的插入点)，利用三次样条函数分别对其x，y，z坐标进行三次样条拟合，得到拟合曲线，该拟合曲线即最短时间路径，保证最后得到最短时间路径的轨迹圆滑，避免对机械臂规划路径的抖动，提高了分拣效率。

样条拟合是将获取路径上的一些离散控制点，利用样条函数构造拟合曲线，而且拟合曲线在分界点还具有一定的光滑性，这就是所谓的样条拟合。在数学学科数值分析中，样条函数是一种特殊的函数，由多项式分段定义。在中国大陆，早期曾经被称做“齿函数”，后来因为工程学术语中“放样”一词而得名。在计算机科学的计算机辅助设计和计算机图形学中，样条通常是指分段定义的多项式参数曲线，由于样条构造简单，使用方便，拟合准确，并能近似曲线拟合和交互式曲线设计中复杂的形状，因此样条是这些领域中曲线的常用表示方法。

本发明实施例中，对于机械臂的末端姿态，采用姿态的四元数表示形式，根据工作场景中供件传送带和分拣柜的尺寸坐标可以得出机械臂末端的起始姿态qt1和终点姿态qt2，qt1、qt2分别为机械臂末端姿态的四元数表示形式，机械臂末端中间过程点采用四元数插值计算其机械臂末端末端姿态，具体如下公式：

qt_x＝slerp(q₁,q₂,x/(x_e-x_s))

其中，qt_x表示横坐标为x的点的机械臂末端姿态插值值，x_e和x_s分别表示机械臂末端终点和起点的x坐标，根据此公式可得到规划路径轨迹中所有点的机械臂末端姿态p(x，y，z，qt)，本发明实施例中可以给予机械臂末端姿态，实时监控机械臂在分拣过程的状态，避免机械臂掉件的情况，保证待分拣物品始终处于机械臂末端控制范围内，例如机械臂末端为铲子时，保证待分拣物品处于铲子内，不会掉出铲子。

本发明实施例中，当完成规划所述机械臂末端的运动路径之后，将所述运动路径发送给所述机械臂，以使得所述机械臂按照所述运动路径驱动所述机械臂末端运动。

由于随着物品的分拣，之前已分配的格口，后续可能重新分配存放新的待分拣物品，此时有可能，机械臂末端的起始位置，和该格口的位置信息一样的，此时，为了减少工控设备的运算量，提高分拣效率，可以直接获取之前的规划路径直接发送给机械臂控制装置使用，即无需重新规划，发送之前规划的路径给机械臂控制装置直接使用。

具体的，在本发明实施例中，所述机械臂路径规划方法还可以包括：

保存每个已分拣的物品对应的机械臂末端起始位置信息、对应的存放格口位置信息和对应的最短时间路径；

在所述机械臂末端承装待分拣的第二物品时，获取所述机械臂末端当前起始位置信息及存放所述第二物品的格口位置信息；

若所述当前起始位置信息与已分拣目标物品对应的机械臂末端起始位置信息匹配，且所述第二物品的格口位置信息与所述目标物品对应的存放格口位置信息匹配，则获取所述目标物品对应的最短时间路径；

将所述目标物品对应的最短时间路径，作为所述机械臂末端分拣所述第二物品的运动路径。

本发明实施例在机械臂的机械臂末端承装待分拣的第一物品时，获取机械臂末端的起始位置信息及存放第一物品的格口位置信息；确定机械臂末端的最大可达范围空间；将最大可达范围空间划分为网格空间；根据起始位置信息和格口位置信息，在网格空间规划机械臂末端的运动路径。本发明实施例中在现有随机为机械臂规划路径的基础上，先确定机械臂末端的最大可达范围空间，在此最大可达范围空间内划分网格空间，进而在网格空间规划机械臂末端的运动路径，由于网格空间的划分，使得机械臂末端路径的规划更加细化，路径规划更加细致，避免了现有路径规划的随意性，提高了路径规划的灵活性和效率，进而提高了物品分拣效率。

为了更好实施本发明实施例中机械臂路径规划方法，在机械臂路径规划方法基础之上，本发明实施例中还提供一种机械臂路径规划装置，应用于工控设备，如图8所示，所述机械臂路径规划装置800包括：

获取单元801，用于在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息；

确定单元802，用于确定所述机械臂末端的最大可达范围空间；

划分单元803，用于将所述最大可达范围空间划分为网格空间；

规划单元804，用于根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径。

在本申请一些实施例中，所述规划单元804具体用于：

在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的最短时间路径；

将所述最短时间路径，作为所述机械臂末端的运动路径。

在本申请一些实施例中，所述规划单元804具体用于：

在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的所有可达规划路径；

确定所述所有可达规划路径中的最短时间路径。

在本申请一些实施例中，所述规划单元804具体用于：

计算所述机械臂末端通过所述网格空间中每一条边长的最小时间；

根据所述网格空间中每一条边长的最小时间，确定所述所有可达规划路径中用时最短的路径；

对所述用时最短的路径进行样条拟合，得到所述最短时间路径。

在本申请一些实施例中，所述确定单元802具体用于：

采用D-H矩阵建立机器人运动学模型；

根据所述机器人运动学模型，确定所述机械臂末端的最大可达范围空间。

在本申请一些实施例中，所述机械臂路径规划装置还包括发送单元，所述发送单元用于：

将所述运动路径发送给所述机械臂，以使得所述机械臂按照所述运动路径驱动所述机械臂末端运动。

在本申请一些实施例中，所述规划单元804具体还用于：

保存每个已分拣的物品对应的机械臂末端起始位置信息、对应的存放格口位置信息和对应的最短时间路径；

在所述机械臂末端承装待分拣的第二物品时，获取所述机械臂末端当前起始位置信息及存放所述第二物品的格口位置信息；

若所述当前起始位置信息与已分拣目标物品对应的机械臂末端起始位置信息匹配，且所述第二物品的格口位置信息与所述目标物品对应的存放格口位置信息匹配，则获取所述目标物品对应的最短时间路径；

将所述目标物品对应的最短时间路径，作为所述机械臂末端分拣所述第二物品的运动路径。

本发明实施例获取单元801在机械臂的机械臂末端承装待分拣的第一物品时，获取机械臂末端的起始位置信息及存放第一物品的格口位置信息；确定单元802确定机械臂末端的最大可达范围空间；划分单元803将最大可达范围空间划分为网格空间；规划单元804根据起始位置信息和格口位置信息，在网格空间规划机械臂末端的运动路径。本发明实施例中在现有随机为机械臂规划路径的基础上，先确定机械臂末端的最大可达范围空间，在此最大可达范围空间内划分网格空间，进而在网格空间规划机械臂末端的运动路径，由于网格空间的划分，使得机械臂末端路径的规划更加细化，路径规划更加细致，避免了现有路径规划的随意性，提高了路径规划的灵活性和效率，进而提高了物品分拣效率。

本发明实施例还提供一种工控设备，其集成了本发明实施例所提供的任一种机械臂路径规划装置800，所述工控设备包括：

一个或多个处理器；

存储器；以及

一个或多个应用程序，其中所述一个或多个应用程序被存储于所述存储器中，并配置为由所述处理器执行上述机械臂路径规划方法实施例中任一实施例中所述的机械臂路径规划方法中的步骤。

本发明实施例还提供一种工控设备，其集成了本发明实施例所提供的任一种机械臂路径规划装置。如图9所示，其示出了本发明实施例所涉及的工控设备的结构示意图，具体来讲：

该工控设备可以包括一个或者一个以上处理核心的处理器901、一个或一个以上计算机可读存储介质的存储器902、电源903和输入单元904等部件。本领域技术人员可以理解，图9中示出的工控设备结构并不构成对工控设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。其中：

处理器901是该工控设备的控制中心，利用各种接口和线路连接整个工控设备的各个部分，通过运行或执行存储在存储器902内的软件程序和/或模块，以及调用存储在存储器902内的数据，执行工控设备的各种功能和处理数据，从而对工控设备进行整体监控。可选的，处理器901可包括一个或多个处理核心；优选的，处理器901可集成应用处理器和调制解调处理器，其中，应用处理器主要处理操作系统、用户界面和应用程序等，调制解调处理器主要处理无线通信。可以理解的是，上述调制解调处理器也可以不集成到处理器901中。

存储器902可用于存储软件程序以及模块，处理器901通过运行存储在存储器902的软件程序以及模块，从而执行各种功能应用以及数据处理。存储器902 可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据工控设备的使用所创建的数据等。此外，存储器902可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。相应地，存储器902还可以包括存储器控制器，以提供处理器901对存储器902的访问。

工控设备还包括给各个部件供电的电源903，优选的，电源903可以通过电源管理系统与处理器901逻辑相连，从而通过电源管理系统实现管理充电、放电、以及功耗管理等功能。电源903还可以包括一个或一个以上的直流或交流电源、再充电系统、电源故障检测电路、电源转换器或者逆变器、电源状态指示器等任意组件。

该工控设备还可包括输入单元904，该输入单元904可用于接收输入的数字或字符信息，以及产生与用户设置以及功能控制有关的键盘、鼠标、操作杆、光学或者轨迹球信号输入。

尽管未示出，工控设备还可以包括显示单元等，在此不再赘述。具体在本实施例中，工控设备中的处理器901会按照如下的指令，将一个或一个以上的应用程序的进程对应的可执行文件加载到存储器902中，并由处理器901来运行存储在存储器902中的应用程序，从而实现各种功能，如下：

在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息；

确定所述机械臂末端的最大可达范围空间；

将所述最大可达范围空间划分为网格空间；

根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径。

本领域普通技术人员可以理解，上述实施例的各种方法中的全部或部分步骤可以通过指令来完成，或通过指令控制相关的硬件来完成，该指令可以存储于一计算机可读存储介质中，并由处理器进行加载和执行。

为此，本发明实施例提供一种存储介质，该存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取记忆体(RAM，Random Access Memory)、磁盘或光盘等。该存储介质中存储有多条指令，该指令能够被处理器进行加载，以执行本发明实施例所提供的任一种机械臂路径规划方法中的步骤。例如，该指令可以执行如下步骤：

在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息；

确定所述机械臂末端的最大可达范围空间；

将所述最大可达范围空间划分为网格空间；

根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见上文针对其他实施例的详细描述，此处不再赘述。

具体实施时，以上各个单元或结构可以作为独立的实体来实现，也可以进行任意组合，作为同一或若干个实体来实现，以上各个单元或结构的具体实施可参见前面的方法实施例，在此不再赘述。

以上各个操作的具体实施可参见前面的实施例，在此不再赘述。

以上对本发明实施例所提供的一种机械臂路径规划方法、装置、工控设备及存储介质进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (7)

1.一种用于物流分拣的机械臂路径规划方法，其特征在于，应用于工控设备，所述工控设备位于物品分拣系统，所述机械臂路径规划方法，包括：

在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息；

确定所述机械臂末端的最大可达范围空间；

将所述最大可达范围空间划分为网格空间；

在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的所有可达规划路径；

根据矢量积法计算机械臂末端的雅克比矩阵，根据机械臂末端的雅克比矩阵，计算机械臂通过所述网格空间中每一条边长的最小时间；

根据所述网格空间中每一条边长的最小时间，确定所述所有可达规划路径中用时最短的路径；

对所述用时最短的路径进行样条拟合，得到所述机械臂末端从所述起始位置到所述格口位置之间的最短时间路径；

将所述最短时间路径，作为所述机械臂末端的运动路径；

其中，在整个规划过程中，机械臂末端姿态基本保持不变，在整个运动过程中仅绕Z轴旋转一定角度；

根据矢量积法计算机械臂末端的雅克比矩阵，根据机械臂末端的雅克比矩阵，计算机械臂通过所述网格空间中每一条边长的最小时间，具体包括：

机械臂末端从点P(x0，y0，z0)位置在三维空间内沿x方向运动，根据矢量法，得出机械臂末端的雅克比矩阵J，根据运动学微分方程有：

其中，ν和ω分别表示机械臂末端线速度和角速度，

表示机械臂各关节速度,

当机械臂末端沿x方向移动时间

上述方程转换为：

与

线性相关，当机械臂关节速度q₁,q₂,q₃中某个值达到最大时，可得机械臂末端从当前位置p(x0，y0，z0)在x方向

的最大速度

同理可得机械臂末端从当前位置p(x0，y0，z0)在y方向和z方向的最大速度分别为

和

,

从p(x0，y0，z0)沿x方向匀速运动到点p(x1，y0，z0)的最大速度为

从p(x0,y0,z0)沿x方向匀速运动到点p(x1,y0,z0)的最小时间即为

同理可得沿着y方向和沿z方向运动的最小时间为t_y-min和t_z-min,

按照同样的方法可以得到机械臂通过网格空间中每一条边长的最小时间为tmin，即得到机械臂通过所述网格空间中每一条边长的最小时间。

2.根据权利要求1所述的机械臂路径规划方法，其特征在于，所述确定机械臂的机械臂末端的最大可达范围空间，包括：

采用D-H矩阵建立机器人运动学模型；

根据所述机器人运动学模型，确定所述机械臂末端的最大可达范围空间。

3.根据权利要求1所述的机械臂路径规划方法，其特征在于，所述方法还包括：

将所述运动路径发送给所述机械臂，以使得所述机械臂按照所述运动路径驱动所述机械臂末端运动。

4.根据权利要求1所述的机械臂路径规划方法，其特征在于，所述方法还包括：

保存每个已分拣的物品对应的机械臂末端起始位置信息、对应的存放格口位置信息和对应的最短时间路径；

在所述机械臂末端承装待分拣的第二物品时，获取所述机械臂末端当前起始位置信息及存放所述第二物品的格口位置信息；

若所述当前起始位置信息与已分拣目标物品对应的机械臂末端起始位置信息匹配，且所述第二物品的格口位置信息与所述目标物品对应的存放格口位置信息匹配，则获取所述目标物品对应的最短时间路径；

将所述目标物品对应的最短时间路径，作为所述机械臂末端分拣所述第二物品的运动路径。

5.一种用于物流分拣的机械臂路径规划装置，其特征在于，应用于工控设备，所述工控设备位于物品分拣系统，所述机械臂路径规划装置，包括：

获取单元，用于在机械臂的机械臂末端承装待分拣的第一物品时，获取所述机械臂末端的起始位置信息及存放所述第一物品的格口位置信息；

确定单元，用于确定所述机械臂末端的最大可达范围空间；

划分单元，用于将所述最大可达范围空间划分为网格空间；

规划单元，用于根据所述起始位置信息和所述格口位置信息，在所述网格空间规划所述机械臂末端的运动路径；

所述规划单元具体用于：

在所述网格空间中，确定所述机械臂末端从所述起始位置到所述格口位置之间的所有可达规划路径；

根据矢量积法计算机械臂末端的雅克比矩阵，根据机械臂末端的雅克比矩阵，计算机械臂通过所述网格空间中每一条边长的最小时间；

根据所述网格空间中每一条边长的最小时间，确定所述所有可达规划路径中用时最短的路径；

对所述用时最短的路径进行样条拟合，得到所述机械臂末端从所述起始位置到所述格口位置之间的最短时间路径；

将所述最短时间路径，作为所述机械臂末端的运动路径；

其中，在整个规划过程中，机械臂末端姿态基本保持不变，在整个运动过程中仅绕Z轴旋转一定角度；

所述规划单元具体用于：

机械臂末端从点P(x0，y0，z0)位置在三维空间内沿x方向运动，根据矢量法，得出机械臂末端的雅克比矩阵J，根据运动学微分方程有：

其中，ν和ω分别表示机械臂末端线速度和角速度，

表示机械臂各关节速度,

当机械臂末端沿x方向移动时间

上述方程转换为：

与

线性相关，当机械臂关节速度q₁,q₂,q₃中某个值达到最大时，可得机械臂末端从当前位置p(x0，y0，z0)在x方向

的最大速度

同理可得机械臂末端从当前位置p(x0，y0，z0)在y方向和z方向的最大速度分别为

和

,

从p(x0，y0，z0)沿x方向匀速运动到点p(x1，y0，z0)的最大速度为

从p(x0,y0,z0)沿x方向匀速运动到点p(x1,y0,z0)的最小时间即为

同理可得沿着y方向和沿z方向运动的最小时间为t_y-min和t_z-min,

按照同样的方法可以得到机械臂通过网格空间中每一条边长的最小时间为tmin，即得到机械臂通过所述网格空间中每一条边长的最小时间。

6.一种工控设备，其特征在于，所述工控设备包括：

一个或多个处理器；

存储器；以及

一个或多个应用程序，其中所述一个或多个应用程序被存储于所述存储器中，并配置为由所述处理器执行以实现权利要求1至4中任一项所述的机械臂路径规划方法。

7.一种存储介质，其特征在于，其上存储有计算机程序，所述计算机程序被处理器进行加载，以执行权利要求1至4任一项所述的机械臂路径规划方法中的步骤。

Images