CN108549324A - 用于高速分拣系统的工件跟随抓取轨迹规划方法及系统 - Google Patents

Abstract

本发明公开了一种用于高速分拣系统的工件跟随抓取轨迹规划方法及系统，包括：建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；根据工件坐标确定工件轨迹所处情况，确定当前求解区间；根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；根据工件所处工作范围的上下半区确定抓取轨迹方程；将位移与时间公式代入轨迹方程；使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。本发明有益效果：充分利用机械手工作范围，充分利用S曲线加减速算法特性，提高抓取效率，改善抓取精度。

Description

用于高速分拣系统的工件跟随抓取轨迹规划方法及系统

技术领域

本发明涉及计算机数控系统运动控制领域，尤其涉及一种用于高速分拣系统的工件跟随抓取轨迹规划方法及系统。

背景技术

工件分拣是现代工厂流水线生产线上的一个重要环节。机器人分拣系统一般包含以下设备：机械手，传送带，工件托盘，控制器，工业相机。工业相机通过采集传送带上工件图像获取工件位置信息并将工件信息传递到机器人控制器，控制器进行抓取路径规划与速度规划，然后控制机械手周期插补规划好的抓取轨迹。传统的抓取路径一般采用“门”字型轨迹，在获取抓取指令时，机械手末端首先沿Z轴方向上升固定距离，然后水平运动到传送带上方的预测抓取位置，最后下降至工件上方，再控制吸盘执行抓取动作，吸盘吸气时抓取工件，吸盘吹气则放下工件。此方法的好处是，上升一定高度后再进行水平运动，可以有效避开运动过程中的障碍物，比如传送带边界。但是，吸盘吸气与吹气一般是由控制器控制相应IO完成，IO的控制延迟要远高于机械手插补的1ms，IO控制的延迟导致在机械手到达工件上方时不能及时执行抓取动作，工件继续沿着传送带方向运动一段距离，在IO执行抓取时，抓取位置已经偏离原来的计算位置，导致抓取位置不在工件中心，降低了抓取位置精度，同时放置精度也随之降低；另一方面，由于传送带表面凹凸不平，工件在机器人坐标系内的高度信息存在波动，且难以判断，波动的高度信息给抓取高度的设定带来了极大的困难，为了保证吸盘能够成功吸取工件，吸盘的高度必须尽量靠近工件上表面，从而保证抓取位置精度不受影响。

现有技术提出了根据工件在传送带上的分布密度来调整传送带速度的控制思想，以保证机器人总是处在最快抓取速度状态，并进行了简单的理论分析，但该方法实现较为困难，并且不符合对生产节拍有要求的生产过程。

现有技术提出使用修正梯形算法作为机器人加减速算法，虽然该算法计算简单，但由于修正梯形算法自身时间分配的不合理性，导致它在短时间内的运动具有跳跃性，长距离运动表现疲软的特点，动态特性不足，难以保证高效率的抓取。

综上所述，针对现有技术如何进一步有效动态跟踪运动目标以及提高抓取效率的问题，尚缺乏有效的解决方案。

发明内容

为克服现有技术的不足，本发明提出一种用于高速分拣系统的工件跟随抓取轨迹规划方法及系统，该方法一方面在抓取轨迹的规划过程中，将机器人工作范围和S曲线加减速算法的特点相结合，能够充分利用机器人工作范围；另一方面在保证运动速度的同时，采用圆弧轨迹改变运动方向，实现对于工件的匀速跟随。

为实现上述目的，本发明的具体方案如下：

本发明的第一目的是公开一种用于高速分拣系统的工件跟随抓取轨迹规划方法，包括：

建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；

根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

根据工件所处工作范围的上下半区确定抓取轨迹方程；

将位移与时间公式代入轨迹方程；

使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。

进一步地，所述建立跟随抓取轨迹模型，具体为：

将机器人的工件抓取轨迹分为四段：竖直上升段，水平直线运动段，水平圆弧过渡段和匀速跟随段；

其中，水平直线运动段采用S曲线加减速算法进行速度规划，通过水平圆弧过渡段实现在保持匀速的前提下将机械手末端运动方向过渡到与工件运动方向保持一致。

进一步地，所述机械手运动参数具体为：起点速度、指令速度、终点速度、最大速度以及待插补的位移。

进一步地，所述的根据工件坐标确定工件轨迹所处情况，确定当前求解区间，具体为：

基于S曲线加减速算法，将机械手的抓取工作范围划分为四个部分；

根据工件的坐标确定工件的运动轨迹；

根据工件的运动轨迹与机械手的抓取工作范围的相交点，将工作范围划分为若干个求解区间。

进一步地，所述基于S曲线加减速算法，将机械手的抓取工作范围划分为四个部分，具体为：

设定S型速度曲线的起点速度v_s＝0，终点速度v_e＞0，指令速度v_com，加速度限制a_max，加加速度限制J_max；并且直线插补终点速度v_e、圆弧插补速度和传送带速度保持一致；

假设S曲线的最大速度为加速度限制与加加速度限制的比值，判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第一部分的工作半径；

假设S曲线的最大速度为加速度限制与加加速度限制的比值与终点速度之和，判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第二部分的工作半径；

假设S曲线的最大速度为指令速度，分别计算S曲线加速段位移和减速段位移，得到第三部分的工作半径。

进一步地，所述根据当前求解区间，确定该区间位移与时间公式，具体为：

如果当前求解区间属于第一部分或者第二部分或者第三部分抓取工作范围，则该求解区间直线段运动时间为加速段时间和减速段时间之和；直线段位移为最大速度与终点速度的均值与减速段时间的乘积，加上最大速度与起点速度的均值与加速段时间的乘积；上述求解区间内单一变量为最大速度；

如果当前求解区间属于第四部分抓取工作范围，则该求解区间直线段运动时间为加速段时间、匀速段时间和减速段时间之和；此时，直线段位移的最大速度是指令速度，直线段位移为最大速度与终点速度的均值与减速段时间的乘积，加上最大速度与起点速度的均值与加速段时间的乘积，再加上最大速度与匀速段时间的乘积；上述求解区间内单一变量为匀速段时间。

进一步地，根据工件所处工作范围的上半区确定抓取轨迹方程，具体为：

如果跟随运动起点的X坐标小于机械手末端X坐标，则设为工作范围的上半区；

该种情况下的抓取轨迹方程为：

其中，E1(x_E1,y_E1)为上半区的机械手末端坐标，D1(x_D1,y_D1)为上半区的工件起始点，θ₃为上半区的圆弧插补的圆心角，T_E1A1为上半区的S曲线加减速算法规划的直线段时间，L_E1A1为上半区的为S曲线加减速算法规划的直线段位移；T_rise为抓取轨迹的上升段时间，R为圆弧插补半径，v_conveyor为传送带速度。

进一步地，根据工件所处工作范围的下半区确定抓取轨迹方程，具体为：

如果跟随运动起点的X坐标大于机械手末端X坐标，则设为工作范围的下半区；

该种情况下的抓取轨迹方程为：

其中，E2(x_E2,y_E2)为下半区的机械手末端坐标，D2(x_D2,y_D2)为下半区的工件起始点，θ₆为下半区的圆弧插补的圆心角，T_E2A2为下半区的S曲线加减速算法规划的直线段时间，L_E2A2为下半区的为S曲线加减速算法规划的直线段位移；T_rise为抓取轨迹的上升段时间，R为圆弧插补半径，v_conveyor为传送带速度。

本发明的第二目的是公开一种用于高速分拣系统的工件跟随抓取轨迹规划系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：

建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；

根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

根据工件所处工作范围的上下半区确定抓取轨迹方程；

将位移与时间公式代入轨迹方程；

使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。

本发明的第三目的是公开一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行以下步骤：

建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；

根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

根据工件所处工作范围的上下半区确定抓取轨迹方程；

将位移与时间公式代入轨迹方程；

使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。

本发明的有益效果：

本发明将基于S曲线加减速算法的位移分段方法应用于抓取轨迹的规划过程中，将机器人工作范围和S曲线加减速算法的特点相结合，能够充分利用机器人工作范围；

本发明提出基于圆弧过渡的跟随抓取轨迹规划方案，在保证运动速度的同时，采用圆弧轨迹改变运动方向，实现对于工件的匀速跟随。

本发明抓取轨迹方程的公式推导能够证明方程有且仅有一个未知变量，并且采用二分法的数值分析方法验证了该发明提出的抓取轨迹规划方法的可行性。

本发明相比于现有技术，能够充分利用机械手工作范围，充分利用S曲线加减速算法特性，提高抓取效率，改善抓取精度。

附图说明

图1为本发明抓取轨迹规划方法工作流程图；

图2为本发明上半区抓取运动示意图；

图3为本发明下半区抓取运动示意图；

图4为七段S曲线加减速示意图；

图5为位移分段流程图；

图6为S曲线加减速位移分段图；

图7为工件轨迹与机械手工作范围相交情况示意图；

图8为轨迹方程分段求解过程示意图；

图9为本发明机器人分拣系统结构示意图；

图10为本发明机器人分拣系统俯视图。

具体实施方式：

下面结合附图对本发明进行详细说明：

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

实施例一

为了解决背景技术提出的问题，本发明公开了一种用于高速分拣系统的工件跟随抓取轨迹规划方法，具体过程如图1所示，包括以下步骤：

Step1：工业相机拍照获得工件坐标，控制器获得机械手末端坐标，输入机械手运动参数；

Step2：根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

Step3：根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

Step4：根据工件所处工作范围的上下半区确定抓取轨迹方程；

Step5：将位移与时间公式代入轨迹方程；

Step6：使用二分法在取值范围内求解轨迹方程的解，若有解，则输出方程解转到Step7，若无解，转到下一区间执行Step3-Step6；

Step7：根据方程解规划抓取轨迹，执行抓取。

本发明方法所基于的机器人分拣系统如图9和图10所示，包括：工业相机，四轴高速机械手，吸盘，工件传送带，托盘，托盘传送带以及机器人控制器；工件放置在工件传送带上，工业相机通过采集传送带上工件图像获取工件位置信息并将工件信息传递到机器人控制器，控制器进行抓取路径规划与速度规划，然后控制四轴高速机械手周期插补规划好的抓取轨迹。

下面对上述过程的各个步骤进行详细的介绍。

本实施中，机器人的工件抓取轨迹分为四段：竖直上升段，水平直线运动段，水平圆弧过渡段，匀速跟随段。其中，水平直线运动采用S曲线加减速算法进行速度规划，水平圆弧过渡段是为了在保持匀速的前提下将机械手末端运动方向过渡到与工件运动方向保持一致。

图2和图3代表两种可能出现的抓取轨迹平面示意图。以图1为例，D1点是工件在传送带上运动的起点，E1点时机械手末端运动的起点。D1F1是工件运动轨迹，E1A1是机器人末端直线运动部分轨迹，A1F1是机器人末端圆弧运动部分轨迹，A1是圆弧起点，O1是圆弧圆心点，F1是圆弧终点，也是机器人末端和工件相遇点，F1F2是机器人末端匀速跟随工件运动部分轨迹，机器人末端在F1点时开始下降，直至F2下降至合适抓取高度。

抓取过程：在开始时刻，机械手末端位于点E1，工件位于点D1，并沿着D1F2方向匀速运动，机械手末端首先在E1点竖直上升固定高度，然后沿E1A1段进行直线运动，该段运动基于S曲线加减速算法规划速度，然后沿A1F1段进行圆弧运动，为了实现匀速跟随工件，圆弧插补速度、水平直线段终点速度和传送带速度三者相同。在机械手末端结束圆弧插补到达F1点时，工件也在该时刻运行至圆弧终点F1点，机械手末端位于工件正上方，并跟随工件朝向F2点运动，并在跟随过程中下降高度，运行至F2点时，机械手末端执行抓取动作，然后减速为0。至此便完成该工件抓取任务。

图4所示为S曲线加减速全过程中的速度、加速度以及加加速度曲线。加速过程包含加加速段、匀加速段及减加速段；减速过程包含加减速段、匀减速段及减减速段；加速段和减速段之间通过匀速运动衔接，整个过程分为7段。

本实施例中，定义S型速度曲线的起点速度v_s＝0，终点速度v_e＞0，指令速度v_com，且v_e＜v_com，加速度限制a_max，加加速度限制J_max，S型速度曲线七段运行时间分别为t₁,t₂,t₃,t₄,t₅,t₆,t₇。事实上，S曲线加减速可能不完全包含上述7段。根据给定的起点速度、指令速度、终点速度以及待插补的位移S，S曲线可能不包含匀速段、匀加速段或匀减速段。

本实施例中的S曲线加减速算法在使用上有所不同，传统的用法是根据已知的位移，起点速度，终点速度计算各段时间。而本发明假设最大速度，根据已知起点速度和终点速度，计算位移。假设的最大速度决定位移的大小，本发明提出的位移分段原则如图5所示，位移被分成四种情况，分段的关键在于根据假设的最大速度v_max计算相应位移R1、R2、R3。

在本实施例中，S曲线加减速算法应用于抓取轨迹的水平直线段，在执行水平直线插补之前，机械手竖直上升至固定高度，故水平直线段起点速度v_s＝0，直线段插补完成后，转入圆弧插补和匀速直线跟随，故本发明设定直线插补终点速度v_e、圆弧插补速度和传送带速度保持一致。

假设S曲线的最大速度为判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第一部分的工作半径；

假设S曲线的最大速度为判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第二部分的工作半径；

假设S曲线的最大速度为指令速度，分别计算S曲线加速段位移和减速段位移，得到第三部分的工作半径。

因此，如图5所示，R1、R2、R3均存在，S曲线位移被分成①、②、③、④四部分：

第①部分：0＜S≤R1、v_max∈(v_e,v_max1]；

第②部分：R1＜S≤R2、v_max∈(v_max1,v_max2]；

第③部分：R2＜S≤R3、v_max∈(v_max2,v_com]；

第④部分：S＞R3、v_max＝v_com。

根据位移分段的四种情况，绘制S曲线加减速位移分段图如图6所示，结合机械手末端的工作范围，以机械手末端点E为圆心，以R1、R2、R3为半径，设机器人工作范围为半径R₄，因为机械手工作范围不是典型的圆形，故使用虚线表示位移范围。

(1)第①部分：0＜S≤R1、v_max∈(v_e,v_max1]；

在该部分，运动过程不包含匀速段、匀加速段和匀减速段，加速段时间t_acc和减速段t_dec时间计算如下：

直线段运动时间T表示为：

T＝t_acc+t_dec (2)

直线段位移L便可以按照公式计算：

(2)第②部分：R1＜S≤R2、v_max∈(v_max1,v_max2]；

在该部分，运动过程不包含匀速段，匀减速段，但是包含匀加速段，加速段时间和减速段时间计算如下：

直线段时间可以按照公式(2)计算，位移L按照公式(3)计算。

(3)第③部分：R2＜S≤R3、v_max∈(v_max2,v_com]；

在该部分，运动过程不包含匀速段，但是包含匀加速段和匀减速段，加速段和减速段计算时间如下：

直线段时间可以按照公式(2)计算，位移L按照公式(3)计算。

(4)第④部分：S＞R3、v_max＝v_com。

在该部分，运动过程包含匀速段，匀加速段和匀减速段，加速段和减速段计算时间可以按照公式(6)计算，假设t_con为匀速段时间，

直线段时间可以计算为：

T＝t_acc+t_dec+t_con (7)

直线段位移L按照以下公式计算：

综上所述，本发明中由S曲线算法规划的直线段位移和时间均可以由单变量v_max或t_con表示：在第①、②、③部分，根据公式(1)—(6)可得，直线段位移L和时间T公式中有且仅有v_max一个未知变量；在第④部分，根据公式(7)和(8)可得，直线段位移L和时间T公式中有且仅有t_con一个未知变量。

本实施例中，根据跟随运动起点的X坐标大于机械手末端X坐标的大小，对工作范围进行分区，进而建立抓取轨迹计算模型。

如图2所示，本发明设定工件起始点坐标D1(x_D1,y_D1)，机械手末端坐标E1(x_E1,y_E1)，A1F1段起点坐标A1(x_A1,y_A1)，终点坐标F1(x_F1,y_F1)，也是机械手与工件在传送带上的相遇点，跟随段的起点。抓取点坐标为F2(x_F2,y_F2)，A1F1圆心点坐标为O1(x_O1,y_O1)，辅助坐标B1(x_B1,y_B1)。

同样，在图3中，设定工件起始点坐标D2(x_D2,y_D2)，机械手末端坐标E2(x_E2,y_E2)，A2F3段起点坐标A2(x_A2,y_A2)，终点坐标F3(x_F3,y_F3)，也是机械手与工件在传送带上的相遇点，跟随段的起点。抓取点坐标为F4(x_F4,y_F4)，A2F3圆心点坐标为O2(x_O2,y_O2)，辅助坐标B2(x_B2,y_B2)。设抓取轨迹的上升段时间为T_rise，T_rise设为定值。

一般来说，存在两种抓取轨迹，第一种，x_F1＜x_E1，即跟随运动起点的X坐标小于机械手末端X坐标，此时抓取轨迹如图2所示，此种情况该发明设为工作范围的上半区；

第二种，x_F3＞x_E2，即跟随运动起点的X坐标大于机械手末端X坐标，此时抓取轨迹如图3所示，此种情况设为工作范围的下半区。下面分两种情况分别讨论。

(1)工作范围的上半区

首先计算圆弧插补半径，根据向心加速度公式且a_n＝a_max，可得圆弧半径公式：

在图2中，设∠θ₁＝∠A1O1E1，∠θ₂＝∠B1O1E1，∠θ₃＝∠A1O1F1，E1A1段插补位移为L_E1A1，E1A1段S曲线加减速方法规划的运动时间为T_E1A1，A1F1段插补圆弧长度为L_A1F1，插补时间为T_A1F1.在本问题中，机械手末端点E1(x_E1,y_E1)，工件起始点D1(x_D1,y_D1)，传送带速度v_conveyor及运动限制参数v_com,a_max,J_max已知，且对于水平直线段E1A1，v_s＝0，v_e＝v_conveyor。所以该问题中关键在于求解点F1的坐标，因为y_F1＝y_D1，最终需要求解x_F1。

针对机械手末端，机械手末端需要依次插补直线段E1A1和圆弧段A1F1后运动到点F1，点F1的X坐标推导过程如下：

根据角度关系，表示出∠θ₁的正切值和∠θ₂的余弦值：

根据勾股定理，可得：

其中，

L_E1B1＝y_E1-y_D1-R (13)

根据公式(10)、(11)、(12)、(13)可用L_E1A1表示cos(∠θ₁)和cos(∠θ₂)如下

根据图1的X坐标关系，表示出x_F1：

然后将公式(10)带入公式(11)可得：

针对工件的运动轨迹，要满足的条件是在机械手末端到达点F1时，工件也必须到达点F1，所以点F1的X坐标推导过程如下：

x_F1＝x_D1+v_conveyor(T_E1A1+T_A1F1+T_rise) (18)

其中，T_E1A1通过S曲线加减速规划获得，且

其中，

L_A1F1＝R∠θ₃ (20)

并且

综合公式(17)和公式(18)可得方程：

将公式(14)、(15)、(19)带入公式(22)可得：

分析公式(23)不难看出，方程中未知数共有3个：∠θ₃、T_E1A1和L_E1A1，其中∠θ₃根据公式(14)、(15)和(21)可用L_E1A1表示，方程(23)中存在的未知数只剩下T_E1A1和L_E1A1，即S曲线算法规划的直线段位移与时间。

(2)工作范围的下半区

圆弧半径计算方法与上半区一致，如图3所示，设∠θ₄＝∠A2O2E2，∠θ₅＝∠B2O2E2，∠θ₆＝∠A2O2F3，E2A2段插补位移为L_E2A2，E2A2段S曲线加减速方法规划的运动时间为T_E2A2，A2F3段插补圆弧长度为L_A2F3，插补时间为T_A2F3.在本问题中，机械手末端点E2(x_E2,y_E2)，工件起始点D2(x_D2,y_D2)。

针对机械手末端，机械手末端需要依次插补直线段E2A2和圆弧段A2F3后运动到点F3，点F3F1的X坐标推导过程如下：

根据角度关系，表示出∠θ₄的正切值和∠θ₅的余弦值：

其中

L_O2B2＝y_E2-y_D2-R (25)

根据公式(24)、(25)、(26)可用L_E2A2表示cos(∠θ₄)和sin(∠θ₅)如下

根据图3的X坐标关系，表示出x_F3：

然后可得：

针对工件的运动轨迹，要满足的条件是在机械手末端到达点F3时，工件也必须到达点F3，所以点F3F1的X坐标推导过程如下：

x_F3＝x_D2+v_conveyor(T_E2A2+T_A2F3+T_rise)(31)

其中，T_E2A2通过S曲线加减速规划获得，且

其中，

L_A2F3＝R∠θ₆ (33)

并且

∠θ₆＝π-(∠θ₄+∠θ₅) (34)

综合公式(30)和公式(31)可得方程：

将公式(27)、(28)、(32)带入公式(35)可得：

分析公式(36)不难看出，方程中未知数共有3个：∠θ₆、T_E2A2和L_E2A2，其中∠θ₆根据公式(27)、(28)和(34)可用L_E2A2表示，方程(36)中存在的未知数只剩下T_E2A2和L_E2A2，即S曲线算法规划的直线段位移与时间。

综合上半区和下半区推导过程可得，抓取轨迹计算方程求解的关键在于求解S曲线加减速算法规划的直线段时间T和位移L。

方程(23)或(36)给出了计算抓取轨迹的计算模型，模型中未知变量为抓取轨迹水平运动直线段的运动时间T和位移L，同时，由于直线段的运动时间T和位移L可以由单一变量v_max或t_con表示，因此，抓取轨迹方程可以通过数值分析v_max或t_con的方法求解。本发明通过采用二分法分析变量v_max或t_con来求解轨迹方程，且本发明提出的S曲线加减速算法的位移分段方法提供了一种讨论依据，下面根据工件轨迹的分布详细讨论求解方法。

如图6所示，基于S曲线加减速算法，本发明将机械手的抓取工作范围划分为四个部分。另一方面，工件在传送带上沿X轴正方向以匀速度运动，工件的Y坐标决定工件在传送带上的轨迹，也能确定与图6所示工作范围的交点个数。其中，a_i(i＝1,2,3,4)是四种轨迹的相交情况的上限，也是抓取工作范围的上限；h_i(i＝1,2,3,4)是四种轨迹的相交情况的下限，也是抓取工作范围的下限。

所以，根据工件的Y坐标可以将工件轨迹分为图7所示的四种情况：

(1)工件轨迹属于情况α

在此情况下，工件轨迹与机械手工作空间的四个部分(①、②、③、④)都相交，a₁、b₁、c₁、d₁、f₁、g₁、h₁是两者的相交点。

(2)工件轨迹属于情况β

在此情况下，工件轨迹与机械手工作范围的三个部分(②、③、④)相交，a₂、b₂、c₂、f₂、g₂、h₂是两者的相交点。

(3)工件轨迹属于情况γ

在此情况下，工件轨迹与机械手工作范围的两个部分(③、④)相交，a₃、b₃、g₃、h₃是两者的相交点。

(4)工件轨迹属于情况δ

在此情况下，工件轨迹与机械手的工作范围只在第④部分相交，a₄、h₄是两者的相交点。

本实施例中，在实际抓取过程中，为了提高抓取效率，当工件进入机械手工作范围内，机械手便开始规划抓取轨迹。本发明根据工件轨迹与工作范围的相交情况，将工作范围划分为若干个求解区间，每个区间对应不同的求解公式。当工件进入工作范围后，根据工件X坐标由小到大的方向依次根据工件所在区间求解轨迹方程，若当前区间内无解，则转向下一区间，直至求出轨迹方程的解。求解过程如图8所示。

实施例二

本实施例中，公开了一种用于高速分拣系统的工件跟随抓取轨迹规划系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：

建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；

根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

根据工件所处工作范围的上下半区确定抓取轨迹方程；

将位移与时间公式代入轨迹方程；

使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。

实施例三

本实施例中，公开了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行以下步骤：

建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；

根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

根据工件所处工作范围的上下半区确定抓取轨迹方程；

将位移与时间公式代入轨迹方程；

使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，包括：

建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；

根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

根据工件所处工作范围的上下半区确定抓取轨迹方程；

将位移与时间公式代入轨迹方程；

使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。

2.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述建立跟随抓取轨迹模型，具体为：

将机器人的工件抓取轨迹分为四段：竖直上升段，水平直线运动段，水平圆弧过渡段和匀速跟随段；

其中，水平直线运动段采用S曲线加减速算法进行速度规划，通过水平圆弧过渡段实现在保持匀速的前提下将机械手末端运动方向过渡到与工件运动方向保持一致。

3.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述机械手运动参数具体为：起点速度、指令速度、终点速度、最大速度以及待插补的位移。

4.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述的根据工件坐标确定工件轨迹所处情况，确定当前求解区间，具体为：

基于S曲线加减速算法，将机械手的抓取工作范围划分为四个部分；

根据工件的坐标确定工件的运动轨迹；

根据工件的运动轨迹与机械手的抓取工作范围的相交点，将工作范围划分为若干个求解区间。

5.如权利要求4所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述基于S曲线加减速算法，将机械手的抓取工作范围划分为四个部分，具体为：

设定S型速度曲线的起点速度v_s＝0，终点速度v_e＞0，指令速度v_com，加速度限制a_max，加加速度限制J_max；并且直线插补终点速度v_e、圆弧插补速度和传送带速度保持一致；

假设S曲线的最大速度为加速度限制与加加速度限制的比值，判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第一部分的工作半径；

假设S曲线的最大速度为加速度限制与加加速度限制的比值与终点速度之和，判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第二部分的工作半径；

假设S曲线的最大速度为指令速度，分别计算S曲线加速段位移和减速段位移，得到第三部分的工作半径。

6.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述根据当前求解区间，确定该区间位移与时间公式，具体为：

如果当前求解区间属于第一部分或者第二部分或者第三部分抓取工作范围，则该求解区间直线段运动时间为加速段时间和减速段时间之和；直线段位移为最大速度与终点速度的均值与减速段时间的乘积，加上最大速度与起点速度的均值与加速段时间的乘积；上述求解区间内单一变量为最大速度；

如果当前求解区间属于第四部分抓取工作范围，则该求解区间直线段运动时间为加速段时间、匀速段时间和减速段时间之和；直线段位移为最大速度与终点速度的均值与减速段时间的乘积，加上最大速度与起点速度的均值与加速段时间的乘积，再加上最大速度与匀速段时间的乘积；上述求解区间内单一变量为匀速段时间。

7.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，根据工件所处工作范围的上半区确定抓取轨迹方程，具体为：

如果跟随运动起点的X坐标小于机械手末端X坐标，则设为工作范围的上半区；

该种情况下的抓取轨迹方程为：

其中，E1(x_E1,y_E1)为上半区的机械手末端坐标，D1(x_D1,y_D1)为上半区的工件起始点，θ₃为上半区的圆弧插补的圆心角，T_E1A1为上半区的S曲线加减速算法规划的直线段时间，L_E1A1为上半区的为S曲线加减速算法规划的直线段位移；T_rise为抓取轨迹的上升段时间，R为圆弧插补半径，v_conveyor为传送带速度。

8.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，根据工件所处工作范围的下半区确定抓取轨迹方程，具体为：

如果跟随运动起点的X坐标大于机械手末端X坐标，则设为工作范围的下半区；

该种情况下的抓取轨迹方程为：

其中，E2(x_E2,y_E2)为下半区的机械手末端坐标，D2(x_D2,y_D2)为下半区的工件起始点，θ₆为下半区的圆弧插补的圆心角，T_E2A2为下半区的S曲线加减速算法规划的直线段时间，L_E2A2为下半区的为S曲线加减速算法规划的直线段位移；T_rise为抓取轨迹的上升段时间，R为圆弧插补半径，v_conveyor为传送带速度。

9.用于高速分拣系统的工件跟随抓取轨迹规划系统，其特征在于，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：

建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；

根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

根据工件所处工作范围的上下半区确定抓取轨迹方程；

将位移与时间公式代入轨迹方程；

使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时执行以下步骤：

建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；

根据工件坐标确定工件轨迹所处情况，确定当前求解区间；

根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；

根据工件所处工作范围的上下半区确定抓取轨迹方程；

将位移与时间公式代入轨迹方程；

使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。