一种用于机械臂基于动态规划的传送带跟踪方法与流程

本发明涉及机器人抓取，具体为一种用于机械臂基于动态规划的传送带跟踪方法。

背景技术：

1、机械臂对于随传送带移动的物体进行动态抓取，涉及到机械手的抓取动作要和物体的移动速度相协调，即机械手要对随传送带移动的物料位置进行跟踪和预测，等位置和速度一致时再进行动态抓取动作。目前，常用的传送带跟踪功能通常采用基于pid的动态跟踪方法和基于位置预测的拦截方法。

2、传统的基于pid的跟踪方法在传送带速度相对高时，容易造成跟踪轨迹曲率大，恶化跟踪效果。传统pid形式跟踪算法本质是通过高频的目标位置刷新，实时调整机器人末端的位置和方向，使机器人末端的运动方向始终指向目标物体的当前位置，直至跟踪完成两者的运动方向和速度一致，之后随着目标物体移动。

3、拦截式抓取是根据当前物体的位置，速度和加速度来计算出器将来任意时刻的位置，然后规划机器人末端的动作，从而控制机器人直接运动至此位置，这样就可以缩短机器人末端的运动距离，从而缩短抓取时间，提高抓取效率。

4、目前拦截式抓取对于速度一定的匀速运动或者加速度一定的匀加速运动，在变加速运动情况下容易造成定位精度和跟踪速度误差较大，不满足精度要求高的场所，同时密化拦截式计算频率容易导致运动过程中机器人的速度和加速度变化频繁甚至存在换向，因此为保证机器人运动平滑无换向冲击，就需要一种基于动态规划的传送带跟踪方法。

技术实现思路

1、本发明的目的在于提供一种用于机械臂基于动态规划的传送带跟踪方法，本发明继承了拦截式抓取耗时短、抓取效率高的特点。同时通过步进式动态规划方式，避免了由于传送带速度波动或者在变加速情况下造成的位置预测不准，进而导致的定位精度和跟踪速度误差较大的缺点。也能避免拦截式方法频繁规划带来的速度和加速度频繁变化的问题，因而可以在无需附加传感器和提高硬件成本的前提下，减小传送带跟踪误差、保证了跟踪和抓取精度。

2、本发明是这样实现的：

3、本发明提供一种用于机械臂基于动态规划的传送带跟踪方法，s1：在机械臂的每个控制周期开始，首先获取机械臂机器人的当前状态，具体包括机器人机械臂的当前位置p_r(xr,yr,zr)、当前速度v_r和当前加速度acc_r，同时需要获得当前传送带上物体的位置p_c(xc,yc,zc)、运行速度v_c及加速度acc_c，

4、s2：根据s1获得的数据，计算得到机器人与传送带上物体的相对位置p、相对速度值v和相对加速度a，作为后续速度规划的输入参数，如式(1)-式(3)；

5、p＝p_c-p_r 式1

6、v＝v_c-v_r 式2

7、a＝acc_c-acc_r 式3

8、s3：再根据物体速度v_c大小和方向，对于给定的轮廓速度vp进行补偿，得到新的正向轮廓速度vpp和反向轮廓速度vpn；对于机器人机械臂给定的加速度acc和减速度dec进行补偿，得到新的正向轮廓加速度accp和反向轮廓减速度decn，如式(4)-式(5)；

9、accp＝acc-acc_r 式4

10、decn＝dec+acc_r 式5

11、s4：最后根据对轨迹的平滑需求选择不同方式的规划方法并以计算得到的参数作为输入进行轨迹规划，得到轨迹各段的加速度值和持续时间；

12、以t型曲线速度规划算法为例进行说明，

13、通用t型曲线规划算法的输入接口包括：起始位置ps、终止位置pe、起始速度vs、终止速度ve、轮廓加速度accle和轮廓减速度decle；结合s1-s3可知，如式(6)-式(11)；

14、ps＝p_r 式6

15、pe＝p_c 式7

16、vs＝v 式8

17、ve＝0 式9

18、accle＝accp 式10

19、decle＝decn 式11

20、t型曲线规划算法规划得到的参数为轨迹各段的加速度值和持续时间。根据初始相对位置和相对速度的方向，所规划轨迹最多可分为：反向减速段、正向加速段、匀速段、正向减速段、反向加速段。

21、各段的加速度值和持续时间分别为：

22、反向减速段：加速度值ad1＝(accp或decn)；持续时间tad1。

23、正向加速段：加速度值ad2＝(accp或decn)；持续时间tad2。

24、匀速段：加速度值0(accp或decn)；持续时间tad3。

25、正向减速段：加速度值ad4＝(accp或decn)；持续时间tad4。

26、反向加速段：加速度值ad5＝(accp或decn)；持续时间tad5。

27、s5：将s1获得的机器人和传送带上物体的各项参数，以及规划得到的各段加速度值和持续时间带入到t型曲线规划算法的位置、速度求解中，得到下一个周期输出的机器人的加速度值、速度值和位置值；

28、s6：由于传送带在运动过程中可能为匀速、匀变速和变加速运动，仅一次规划得到的抓取位置和抓取速度与实际抓取位置和抓取位置之间可能存在位置偏差，因此需要不断规划修正，即每一个周期开始前都需要根据当前状态进行动态规划。因此在下一周期开始时，周期内部重复s1-s5步骤，进行动态规划。

29、进一步，在步骤s1中提到的位置和速度均在相同参考坐标系下表示，该参考坐标系可以选择但不限于世界坐标系或者机器人坐标系。根据自动化行业惯例或相关标准，传送带布置方向与机器人的坐标轴x轴方向平行，传送带速度方向沿机器人的x轴正向。

30、进一步，在步骤s6中，速度规划方法包括但不限于采用t型速度规划、五段式s型速度规划或者七段式s型速度规划。

31、进一步，本发明提供一种计算机存储介质，在所述存储介质上存储有计算机程序，当计算机程序被执行时，实现如上述任一所述的方法。

32、与现有技术相比，本发明的有益效果是：

33、1.拦截式抓取的关键式抓取点的求取以及机器人整个运动过程中的速度规划。相较于以往将抓取点求取转化为追击问题，对追击时间和位置进行优化求解，本方法基于选定的速度规划方法进行动态规划，从而求得轨迹中各段时间，进而计算得到理论的追击位置。

34、2.本发明提供的思路适用于各类高阶平滑速度规划算法，可拓展应用到不同的应用场所。

35、3.本发明规划得到的加速度和速度曲线平滑，无频繁换向情况发生，从而提高了跟踪和动态抓取精度。

技术特征：

1.一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于：具体按以下步骤执行：

2.根据权利要求1所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，在步骤s1中提到的位置和速度均在相同参考坐标系下表示，该参考坐标系可以选择但不限于世界坐标系或者机器人坐标系，传送带布置方向与机器人的坐标轴x轴方向平行，传送带速度方向沿机器人的x轴正向。

3.根据权利要求1所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，在步骤s4中，通用t型曲线规划算法的输入接口包括起始位置ps、终止位置pe、起始速度vs、终止速度ve、轮廓加速度accle和轮廓减速度decle；结合s1-s3，壳得到式(6)-式(11)；

4.根据权利要求3所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，根据t型曲线规划算法规划得到的参数为轨迹各段的加速度值和持续时间，根据初始相对位置和相对速度的方向，所规划轨迹最多可分为反向减速段、正向加速段、匀速段、正向减速段、反向加速段。

5.根据权利要求4所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，各段的加速度值和持续时间分别为：

6.根据权利要求1所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，在步骤s6中，速度规划方法包括但不限于采用t型速度规划、五段式s型速度规划或者七段式s型速度规划。

7.一种计算机存储介质，其特征在于，在所述存储介质上存储有计算机程序，当计算机程序被执行时，实现如上述权利要求1-6任一所述的方法。

技术总结
本发明涉及机器人抓取技术领域，公开了一种用于机械臂基于动态规划的传送带跟踪方法，首先在每个控制周期内获取机器人和传送带上物体的当前状态，包括机器人的位置、速度和加速度以及传送带上物体的位置、运行速度及加速度，然后计算得到机器人与传送带上物体的相对位置、相对速度和相对加速度，得到新的正向轮廓速度和反向轮廓速度；对机器人给定的加速度和减速度进行补偿，得到新的正向轮廓加速度和反向轮廓减速度，最后对轨迹的平滑需求选择不同方式的规划方法通过得到的参数作为输入进行轨迹规划。本发明避免了由于传送带速度波动或者在变加速情况下造成的位置预测不准，导致的定位精度和跟踪速度误差较大的缺点，保证了跟踪和抓取精度。

技术研发人员：请求不公布姓名
受保护的技术使用者：北京立迈胜控制技术有限责任公司
技术研发日：
技术公布日：2024/1/13