一种抓取传送带上运动物体的机械臂轨迹规划方法及装置

本发明属于机器人运动规划领域，具体涉及一种抓取传送带上运动物体的机械臂轨迹规划方法及装置。

背景技术：

1、传送带能够短途输送大量物品，有效节省搬运的人力。采用机械臂进行传送带上物体的自动化抓取放置，在工业、仓储与物流分拣等领域都有很高的需求。然而，大部分应用于传送带场景的机械臂抓取技术只能在传送带停止或以极低速运行时进行物体抓取，且未能考虑机械臂轨迹的时间最优，这极大阻碍了生产和运输的效率。

2、传统的机械臂抓取方法仅针对固定目标进行运动规划，无法满足动态物体抓取时间与空间约束的问题，即在一定时间到达动态物体当前所在的正确位置进行抓取。针对这一问题，文献“provably constant-time planning and replanning for real-timegrasping objects off a conveyor belt”(f.islam et al.,the internationaljournal of robotics research,2021，40(12-14),1370-1384)提出了生成固定时间的机械臂轨迹的方法，且轨迹符合机械臂运动学限制，从而能够完成传送带上运动物体的抓取。然而，目前的方法没有考虑轨迹的时间最优性，无法在更高的传送带运动速度下实现快速准确的抓取。动态物体的快速抓取对机械臂轨迹规划提出了很高的技术要求，既要考虑轨迹时间最优，又需要满足机械臂关节约束以及避开障碍，同时还必须满足在特定时间到达特定位置从而抓取运动物体的轨迹末端条件。

技术实现思路

1、为解决现有技术的不足，实现机械臂安全快速地抓取传送带上运动物体的目的，本发明采用如下的技术方案：

2、一种抓取传送带上运动物体的机械臂轨迹规划方法，包括以下步骤：

3、步骤1：定义传送带上动态物体的抓取和机械臂轨迹表示；将采用机械臂进行传送带上匀速运动物体的抓取定义为预抓取轨迹和抓取轨迹，预抓取轨迹为从机械臂初始位姿到预抓取位姿的轨迹，抓取轨迹为从预抓取位姿到抓取位姿的轨迹；

4、步骤2：进行预抓取轨迹末端约束的转化；将预抓取轨迹末端位姿约束转化为添加末端位姿目标函数，基于笛卡尔空间下末端轨迹关键点的约束，逆运动学运算后获取预抓取轨迹末端速度约束；

5、步骤3：进行机械臂关节运动学约束和碰撞约束的转化；基于各关节的预抓取轨迹和机械臂对应的关节最大最小位姿，转化预抓取轨迹的机械臂关节位姿约束；基于各关节的轨迹速度和机械臂对应的关节最大速度，转化预抓取轨迹的机械臂关节速度约束；基于各关节的轨迹加速度和机械臂对应的关节最大加速度，转化预抓取轨迹的机械臂关节加速度约束；基于预抓取轨迹和抓取目标物体上表面高度，转化避障约束；

6、步骤4：基于转化后的约束，构建轨迹的总目标函数并求梯度，得到总目标函数关于轨迹关键点和轨迹时长向量的梯度，采用优化求解器对梯度进行求解，并将得到的轨迹输入控制器，控制机械臂抓取目标物体。

7、进一步地，所述步骤1包括如下具体步骤：

8、首先，定义预抓取轨迹的表示方法和规划求解问题，预抓取轨迹定义为从机械臂初始位姿到预抓取位姿的轨迹，预抓取位姿定义为机械臂的末端执行器抓取姿态沿接近向量负方向运动一段后退距离h的预抓取姿态的逆运动学；对于一个m自由度的串联机械臂，预抓取轨迹表示为ξ(t)，是时间t到机械臂关节空间的映射，具有轨迹总时长t，是一个具有轨迹分段数m的分段函数，其第i段轨迹ξi(t)的定义方法如下：

9、

10、其中β(t)＝(1,t,...,t2s-1)t是该段轨迹曲线的基，是第i段轨迹的系数矩阵，s为轨迹的可指定导数值阶数，ti-1和ti为该段轨迹的最小和最大时间限制，该第i段轨迹具有时长ti＝ti-ti-1；对于m段的预抓取轨迹，所有轨迹时长构成了轨迹时长向量t＝(t1,...,tm)t，所有段轨迹的系数矩阵构成了轨迹系数矩阵

11、预抓取轨迹规划的问题定义为解决如下优化问题：

12、

13、s.t.t＞0

14、ξ(0)＝q0

15、

16、x(1)(ξ(t))＝vc+v⊥

17、ξ(2)(t)＝0

18、

19、

20、

21、

22、其中，q＝(q1,...,qm)为轨迹关键点，其内包含轨迹时长向量t中一一对应的机械臂位姿，为初始目标函数，ρ为时间优化权重，为机械臂初始位姿，为正运动学映射，由关节空间映射到笛卡尔空间是传送带运动速度是初始抓取姿态，即在初始时间t＝0时的抓取姿态，为抓取姿态接近向量，为单位化的抓取姿态接近向量，是无碰撞的关节空间；qmax,qmin分别是机械臂的最大与最小关节位置，vmax,amax分别为关节速度限制与关节加速度限制，ξ(1)(t),ξ(2)(t),ξ(3)(t)分别为轨迹速度、轨迹加速度和轨迹加加速度；

23、然后，定义抓取轨迹和其生成方法；抓取轨迹为从预抓取位姿到抓取位姿的轨迹，生成方法为：

24、给定机械臂当前的位姿qj，下一时刻的机械臂速度为：

25、

26、其中，j+为机械臂雅可比矩阵的伪逆；在此过程中，机械臂夹爪逐渐闭合，直到机械臂到达抓取位姿，抓取动作阶段结束。

27、进一步地，所述步骤2具体分为两个阶段：

28、轨迹末端位姿约束转化为添加末端位姿目标函数定义如下：

29、

30、其中，ξ(t)表示时间t为轨迹总时长t时的预抓取轨迹函数值，qm为当前迭代下的预抓

31、取位姿，∈是一个小值，μ为一个可定义的参数，函数φμ[·]定义为：

32、

33、其中，qm由以下方式求解：

34、

35、预抓取轨迹的末端速度约束转化为：

36、ξ(1)(t)＝j+(qm)(vc+v⊥)。

37、进一步地，所述步骤3中，预抓取轨迹的机械臂关节位姿约束转化为，添加关节位姿目标函数定义如下：

38、

39、其中，ξ(t)表示预抓取轨迹，qmax,qmin分别是机械臂的最大与最小关节位置，下标j代表第j个关节的参数，即各符号所表示向量的第j项元素，m为机械臂自由度的个数，t为轨迹总时长，函数φμ[·]定义为：

40、

41、其中，x表示[]中的值，μ为一个可定义的参数。

42、进一步地，所述步骤3中，预抓取轨迹的机械臂关节速度约束转化为，添加关节速度目标函数定义如下：

43、

44、其中，ξ(1)(t)表示轨迹速度，vmax表示关节速度限制，下标j代表第j个关节的参数，即各符号所表示向量的第j项元素，m为机械臂自由度的个数，t为轨迹总时长，函数φμ[·]定义为：

45、

46、其中，x表示[]中的值，μ为一个可定义的参数。

47、进一步地，所述步骤3中，预抓取轨迹的机械臂关节加速度约束转化为，添加关节加速度目标函数定义如下：

48、

49、其中，ξ(2)(t)表示轨迹加速度，amax表示关节加速度限制，下标j代表第j个关节的参数，即各符号所表示向量的第j项元素，m为机械臂自由度的个数，t为轨迹总时长，函数φμ[·]定义为：

50、

51、其中，x表示[]中的值，μ为一个可定义的参数。

52、进一步地，所述步骤3中，避障约束转化为定义简化的避障目标函数如下：

53、

54、其中，ξ(t)表示预抓取轨迹，xz为正运动学求解后位置向量的沿z轴移动的分量，z轴定义为负的加速度方向，zobj为待抓取目标的上表面高度，t为轨迹总时长，函数ψε[·]定义如下：

55、

56、其中，x表示[]中的值，ε为另一个可定义的参数。

57、进一步地，所述步骤4中，经过约束转化后，转化为以下无约束的优化求解问题：

58、

59、其中，c表示轨迹系数矩阵，q表示轨迹关键点，t表示轨迹时长向量，表示初始目标函数，*表示符号合集*＝{g,q,v,a,c}，分别对应基于末端约束转化得到的末端位姿目标函数、基于关节位姿约束转化得到的关节位姿目标函数、基于关节速度约束转化得到的关节速度目标函数、基于关节加速度约束转化得到的关节加速度目标函数、基于避障约束约束转化得到的避障目标函数，ρ*表示各目标函数的权重，为总目标函数。

60、进一步地，所述步骤4中，求解总目标函数关于{q,t}的梯度，总目标函数关于qi的梯度计算方式为：

61、

62、其中e(2i-1)s+1为单位矩阵i2ms的第((2i-1)s+1)列，为总目标函数，c表示轨迹系数矩阵，中间矩阵定义为：

63、

64、其中，过渡矩阵ei,定义为：

65、ei＝(β(ti),β(ti),...,β(2s-1)(ti))t

66、fi＝(0,-β(0),...,-β(2s-1)(0))t

67、边界过渡矩阵em,定义为：

68、f0＝(β(0),...,β(s-1)(0))t

69、em＝(β(tm),...,β(s-1)(tm))t

70、其中，β是轨迹曲线的基，s为轨迹的可指定导数值阶数，tm表示第m段预抓取轨迹的轨迹时长向量，将求出的每项作为矩阵的第i列内容，即可以得到总目标函数关于轨迹关键点向量的梯度

71、总目标函数关于ti的梯度计算方式为：

72、

73、其中tr{·}为求取矩阵的迹，将求出的每项作为向量第i列内容，即可以得到总目标函数关于轨迹时长向量的梯度

74、计算与后，该无约束优化问题可以采用l-bfgs求解器进行求解。

75、一种抓取传送带上运动物体的机械臂轨迹规划方法装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现所述的一种抓取传送带上运动物体的机械臂轨迹规划方法。

76、本发明的优势和有益效果在于：

77、本发明提出了一种抓取传送带上运动物体的机械臂轨迹规划方法及装置，能够针对实时输入的传送带上的物体位置和速度，在有限时间内规划出一条机械臂抓取传送带上匀速运动物体的轨迹，该轨迹具有轨迹时长和平滑性均衡的局部最优性质，能够使机械臂快速接近动态物体并准确抓取，并且符合关节运动学限制的约束以及避障约束。本发明具有更高的成功率，能够在更高的传送带运动速度下进行快速准确的抓取。