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

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

Workpiece following grasping trajectory planning method and system for high-speed sorting system

The invention discloses a method and system for workpiece tracking and grasping trajectory planning for high-speed sorting system, including: establishing a tracking and grasping trajectory model, obtaining workpiece coordinates, manipulator end coordinates and manipulator motion parameters respectively; determining the workpiece trajectory location according to workpiece coordinates, and determining the current solution interval; According to the current solution interval, the time displacement and time formula are determined, and the value range of a single variable in the interval is determined; the grabbing trajectory equation is determined according to the upper and lower half of the working range of the workpiece; the displacement and time formula is substituted into the trajectory equation; the solution of the trajectory equation is solved by the dichotomy method within the value range, if any. Solution, then according to the equation solution planning grabbing trajectory, execution of grabbing; if no solution, to the next solution interval to repeat the above solution process. The invention has the advantages of fully utilizing the working range of the manipulator, fully utilizing the characteristics of the S curve acceleration and deceleration algorithm, improving the grasping efficiency and improving the grasping accuracy.

【技术实现步骤摘要】
用于高速分拣系统的工件跟随抓取轨迹规划方法及系统
本专利技术涉及计算机数控系统运动控制领域，尤其涉及一种用于高速分拣系统的工件跟随抓取轨迹规划方法及系统。
技术介绍
工件分拣是现代工厂流水线生产线上的一个重要环节。机器人分拣系统一般包含以下设备：机械手，传送带，工件托盘，控制器，工业相机。工业相机通过采集传送带上工件图像获取工件位置信息并将工件信息传递到机器人控制器，控制器进行抓取路径规划与速度规划，然后控制机械手周期插补规划好的抓取轨迹。传统的抓取路径一般采用“门”字型轨迹，在获取抓取指令时，机械手末端首先沿Z轴方向上升固定距离，然后水平运动到传送带上方的预测抓取位置，最后下降至工件上方，再控制吸盘执行抓取动作，吸盘吸气时抓取工件，吸盘吹气则放下工件。此方法的好处是，上升一定高度后再进行水平运动，可以有效避开运动过程中的障碍物，比如传送带边界。但是，吸盘吸气与吹气一般是由控制器控制相应IO完成，IO的控制延迟要远高于机械手插补的1ms，IO控制的延迟导致在机械手到达工件上方时不能及时执行抓取动作，工件继续沿着传送带方向运动一段距离，在IO执行抓取时，抓取位置已经偏离原来的计算位置，导致抓取位置不在工件中心，降低了抓取位置精度，同时放置精度也随之降低；另一方面，由于传送带表面凹凸不平，工件在机器人坐标系内的高度信息存在波动，且难以判断，波动的高度信息给抓取高度的设定带来了极大的困难，为了保证吸盘能够成功吸取工件，吸盘的高度必须尽量靠近工件上表面，从而保证抓取位置精度不受影响。现有技术提出了根据工件在传送带上的分布密度来调整传送带速度的控制思想，以保证机器人总是处在最快抓取速度状态，并进行了简单的理论分析，但该方法实现较为困难，并且不符合对生产节拍有要求的生产过程。现有技术提出使用修正梯形算法作为机器人加减速算法，虽然该算法计算简单，但由于修正梯形算法自身时间分配的不合理性，导致它在短时间内的运动具有跳跃性，长距离运动表现疲软的特点，动态特性不足，难以保证高效率的抓取。综上所述，针对现有技术如何进一步有效动态跟踪运动目标以及提高抓取效率的问题，尚缺乏有效的解决方案。
技术实现思路
为克服现有技术的不足，本专利技术提出一种用于高速分拣系统的工件跟随抓取轨迹规划方法及系统，该方法一方面在抓取轨迹的规划过程中，将机器人工作范围和S曲线加减速算法的特点相结合，能够充分利用机器人工作范围；另一方面在保证运动速度的同时，采用圆弧轨迹改变运动方向，实现对于工件的匀速跟随。为实现上述目的，本专利技术的具体方案如下：本专利技术的第一目的是公开一种用于高速分拣系统的工件跟随抓取轨迹规划方法，包括：建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；根据工件坐标确定工件轨迹所处情况，确定当前求解区间；根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；根据工件所处工作范围的上下半区确定抓取轨迹方程；将位移与时间公式代入轨迹方程；使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。进一步地，所述建立跟随抓取轨迹模型，具体为：将机器人的工件抓取轨迹分为四段：竖直上升段，水平直线运动段，水平圆弧过渡段和匀速跟随段；其中，水平直线运动段采用S曲线加减速算法进行速度规划，通过水平圆弧过渡段实现在保持匀速的前提下将机械手末端运动方向过渡到与工件运动方向保持一致。进一步地，所述机械手运动参数具体为：起点速度、指令速度、终点速度、最大速度以及待插补的位移。进一步地，所述的根据工件坐标确定工件轨迹所处情况，确定当前求解区间，具体为：基于S曲线加减速算法，将机械手的抓取工作范围划分为四个部分；根据工件的坐标确定工件的运动轨迹；根据工件的运动轨迹与机械手的抓取工作范围的相交点，将工作范围划分为若干个求解区间。进一步地，所述基于S曲线加减速算法，将机械手的抓取工作范围划分为四个部分，具体为：设定S型速度曲线的起点速度vs＝0，终点速度ve＞0，指令速度vcom，加速度限制amax，加加速度限制Jmax；并且直线插补终点速度ve、圆弧插补速度和传送带速度保持一致；假设S曲线的最大速度为加速度限制与加加速度限制的比值，判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第一部分的工作半径；假设S曲线的最大速度为加速度限制与加加速度限制的比值与终点速度之和，判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第二部分的工作半径；假设S曲线的最大速度为指令速度，分别计算S曲线加速段位移和减速段位移，得到第三部分的工作半径。进一步地，所述根据当前求解区间，确定该区间位移与时间公式，具体为：如果当前求解区间属于第一部分或者第二部分或者第三部分抓取工作范围，则该求解区间直线段运动时间为加速段时间和减速段时间之和；直线段位移为最大速度与终点速度的均值与减速段时间的乘积，加上最大速度与起点速度的均值与加速段时间的乘积；上述求解区间内单一变量为最大速度；如果当前求解区间属于第四部分抓取工作范围，则该求解区间直线段运动时间为加速段时间、匀速段时间和减速段时间之和；此时，直线段位移的最大速度是指令速度，直线段位移为最大速度与终点速度的均值与减速段时间的乘积，加上最大速度与起点速度的均值与加速段时间的乘积，再加上最大速度与匀速段时间的乘积；上述求解区间内单一变量为匀速段时间。进一步地，根据工件所处工作范围的上半区确定抓取轨迹方程，具体为：如果跟随运动起点的X坐标小于机械手末端X坐标，则设为工作范围的上半区；该种情况下的抓取轨迹方程为：其中，E1(xE1,yE1)为上半区的机械手末端坐标，D1(xD1,yD1)为上半区的工件起始点，θ3为上半区的圆弧插补的圆心角，TE1A1为上半区的S曲线加减速算法规划的直线段时间，LE1A1为上半区的为S曲线加减速算法规划的直线段位移；Trise为抓取轨迹的上升段时间，R为圆弧插补半径，vconveyor为传送带速度。进一步地，根据工件所处工作范围的下半区确定抓取轨迹方程，具体为：如果跟随运动起点的X坐标大于机械手末端X坐标，则设为工作范围的下半区；该种情况下的抓取轨迹方程为：其中，E2(xE2,yE2)为下半区的机械手末端坐标，D2(xD2,yD2)为下半区的工件起始点，θ6为下半区的圆弧插补的圆心角，TE2A2为下半区的S曲线加减速算法规划的直线段时间，LE2A2为下半区的为S曲线加减速算法规划的直线段位移；Trise为抓取轨迹的上升段时间，R为圆弧插补半径，vconveyor为传送带速度。本专利技术的第二目的是公开一种用于高速分拣系统的工件跟随抓取轨迹规划系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；根据工件坐标确定工件轨迹所处情况，确定当前求解区间；根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；根据工件所处工作范围的上下半区确定抓取轨迹方程；将位移与时间公式代入轨迹方程；使用二分法在取值范围内求解轨迹方程本文档来自技高网...

【技术保护点】
1.用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，包括：建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；根据工件坐标确定工件轨迹所处情况，确定当前求解区间；根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；根据工件所处工作范围的上下半区确定抓取轨迹方程；将位移与时间公式代入轨迹方程；使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。

【技术特征摘要】
1.用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，包括：建立跟随抓取轨迹模型，分别获得工件坐标，机械手末端坐标以及机械手运动参数；根据工件坐标确定工件轨迹所处情况，确定当前求解区间；根据当前求解区间，确定该区间位移与时间公式，确定该区间内单一变量的取值范围；根据工件所处工作范围的上下半区确定抓取轨迹方程；将位移与时间公式代入轨迹方程；使用二分法在取值范围内求解轨迹方程的解，若有解，则根据方程解规划抓取轨迹，执行抓取；若无解，转到下一求解区间重复上述求解过程。2.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述建立跟随抓取轨迹模型，具体为：将机器人的工件抓取轨迹分为四段：竖直上升段，水平直线运动段，水平圆弧过渡段和匀速跟随段；其中，水平直线运动段采用S曲线加减速算法进行速度规划，通过水平圆弧过渡段实现在保持匀速的前提下将机械手末端运动方向过渡到与工件运动方向保持一致。3.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述机械手运动参数具体为：起点速度、指令速度、终点速度、最大速度以及待插补的位移。4.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述的根据工件坐标确定工件轨迹所处情况，确定当前求解区间，具体为：基于S曲线加减速算法，将机械手的抓取工作范围划分为四个部分；根据工件的坐标确定工件的运动轨迹；根据工件的运动轨迹与机械手的抓取工作范围的相交点，将工作范围划分为若干个求解区间。5.如权利要求4所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述基于S曲线加减速算法，将机械手的抓取工作范围划分为四个部分，具体为：设定S型速度曲线的起点速度vs＝0，终点速度ve＞0，指令速度vcom，加速度限制amax，加加速度限制Jmax；并且直线插补终点速度ve、圆弧插补速度和传送带速度保持一致；假设S曲线的最大速度为加速度限制与加加速度限制的比值，判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第一部分的工作半径；假设S曲线的最大速度为加速度限制与加加速度限制的比值与终点速度之和，判断最大速度是否小于指令速度，如果是，分别计算S曲线加速段位移和减速段位移，得到第二部分的工作半径；假设S曲线的最大速度为指令速度，分别计算S曲线加速段位移和减速段位移，得到第三部分的工作半径。6.如权利要求1所述的用于高速分拣系统的工件跟随抓取轨迹规划方法，其特征在于，所述根据当前求解区间，确定该区间位移与时间公式，具体为：如果当前求解区间属于第一部分或者第二部分或者第三部分抓取工作范围，则该求解区间直线段运动时间为加速段时间和减速段时间之和；直线段位移为最大速度与终点速度的均值与减速段时间的乘积，加上最大速度与起点速度的均值与加速段时间的乘积；上述求解区间内单一变量为最大速度；如果当前求解区间属...

【专利技术属性】
技术研发人员：张承瑞，陈齐志，倪鹤鹏，梁雪，胡天亮，刘亚男，
申请(专利权)人：山东大学，
类型：发明
国别省市：山东,37