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

Abstract

本发明公开了一种抓取传送带上运动物体的机械臂轨迹规划方法及装置，通过定义传送带上动态物体抓取的优化问题和机械臂轨迹表示方法，进行预抓取轨迹末端约束的转化、机械臂关节运动学约束的转化、机械臂碰撞约束的转化，再对转化后的总目标函数求梯度，采用优化求解器进行求解，并将轨迹输入控制器控制机械臂抓取物体。本发明能够针对实时输入的传送带上的物体位置和速度，在有限时间内规划出一条抓取传送带上匀速运动物体的轨迹，该轨迹具有轨迹时长和平滑性均衡的局部最优性质，能够使机械臂快速接近动态物体并准确抓取，并且符合关节运动学限制的约束以及避障约束，本发明具有领先的成功率，能够在较高的传送带运动速度下进行快速准确的抓取。

Description

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

技术领域

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

背景技术

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

传统的机械臂抓取方法仅针对固定目标进行运动规划，无法满足动态物体抓取时间与空间约束的问题，即在一定时间到达动态物体当前所在的正确位置进行抓取。针对这一问题，文献“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：进行机械臂关节运动学约束和碰撞约束的转化；基于各关节的预抓取轨迹和机械臂对应的关节最大最小位姿，转化预抓取轨迹的机械臂关节位姿约束；基于各关节的轨迹速度和机械臂对应的关节最大速度，转化预抓取轨迹的机械臂关节速度约束；基于各关节的轨迹加速度和机械臂对应的关节最大加速度，转化预抓取轨迹的机械臂关节加速度约束；基于预抓取轨迹和抓取目标物体上表面高度，转化避障约束；

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

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

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

其中β(t)＝(1,t,...,t^2s-1)^T是该段轨迹曲线的基，是第i段轨迹的系数矩阵，s为轨迹的可指定导数值阶数，t_i-1和t_i为该段轨迹的最小和最大时间限制，该第i段轨迹具有时长T_i＝t_i-t_i-1；对于M段的预抓取轨迹，所有轨迹时长构成了轨迹时长向量T＝(T₁,...,T_M)^T，所有段轨迹的系数矩阵构成了轨迹系数矩阵

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

s.t.T＞0

ξ(0)＝q₀

x⁽¹⁾(ξ(T))＝v_c+v_⊥

ξ⁽²⁾(T)＝0

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

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

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

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

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

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

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

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

其中，q_M由以下方式求解：

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

ξ⁽¹⁾(T)＝J⁺(q_M)(v_c+v_⊥)。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，过渡矩阵E_i,定义为：

E_i＝(β(T_i),β(T_i),...,β^(2s-1)(T_i))^T

F_i＝(0,-β(0),...,-β^(2s-1)(0))^T

边界过渡矩阵E_M,定义为：

F₀＝(β(0),...,β^(s-1)(0))^T

E_M＝(β(T_M),...,β^(s-1)(T_M))^T

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

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

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

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

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

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

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

附图说明

图1是本发明中机械臂轨迹规划方法的流程图。

图2是本发明中机械臂抓取传送带上运动物体的示意图。

图3a是本发明实施例中在0.5m/s传送带速度下规划出的机械臂抓取轨迹的速度曲线。

图3b是本发明实施例中在0.5m/s传送带速度下规划出的机械臂抓取轨迹的加速度曲线。

图4是本发明中机械臂轨迹规划装置的结构示意图。

具体实施方式

以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

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

步骤1：定义传送带上动态物体的抓取问题和机械臂轨迹表示方法。将采用机械臂进行传送带上匀速运动物体的抓取定义为两个轨迹的规划：预抓取轨迹和抓取轨迹。如图2所示，机械臂末端执行器夹爪逐步向右下方移动，在抓取轨迹起始端形成预抓取姿态，在抓取轨迹末端形成抓取姿态。

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

其中β(t)＝(1,t,...,t^2s-1)^T是该段轨迹曲线的基，是第i段轨迹的系数矩阵，s为轨迹的可指定导数值阶数，t_i-1和t_i为该段轨迹的最小和最大时间限制，该第i段轨迹具有时长T_i＝t_i-t_i-1。对于M段的预抓取轨迹，所有轨迹时长构成了轨迹时长向量T＝(T₁,...,T_M)^T，所有段轨迹的系数矩阵构成了轨迹系数矩阵

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

s.t.T＞0

ξ(0)＝q₀

x⁽¹⁾(ξ(T))＝v_c+v_⊥

ξ⁽²⁾(T)＝0

其中，q＝(q₁,...,q_M)为轨迹关键点，其内包含轨迹时长向量T中一一对应的机械臂位姿，为初始目标函数，ρ为时间优化权重，为机械臂初始位姿，为正运动学映射，由关节空间映射到笛卡尔空间是传送带运动速度，是初始抓取姿态，即在初始时间t＝0时的抓取姿态，为抓取姿态接近向量，为单位化的抓取姿态接近向量，是无碰撞的关节空间，q_max,q_min分别是机械臂的最大与最小关节位置，v_max,a_max分别为关节速度限制与关节加速度限制，ξ⁽¹⁾(t),ξ⁽²⁾(t),ξ⁽³⁾(t)分别为轨迹速度、轨迹加速度和轨迹加加速度。

接下来，定义抓取轨迹及其生成方法；抓取轨迹为从预抓取位姿到抓取位姿的轨迹，生成方法为：

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

其中，J⁺为机械臂雅可比矩阵的伪逆，在这个过程中，机械臂夹爪逐渐闭合，直到机械臂到达抓取位姿，抓取动作阶段结束。

步骤2：进行预抓取轨迹末端约束的转化；轨迹末端位姿约束转化为，添加末端位姿目标函数定义如下：

其中，∈是一个小值，μ为一个可定义的参数，函数φ_μ[·]定义为：

q_M为当前迭代下的预抓取位姿，由以下方式求解：

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

ξ⁽¹⁾(T)＝J⁺(q_M)(v_c+v_⊥)。

步骤3：进行机械臂关节运动学约束和碰撞约束的转化；预抓取轨迹的机械臂关节位姿约束转化为，添加关节位姿目标函数定义如下：

其中，下标j代表第j个关节的参数，即各符号所表示向量的第j项元素。

预抓取轨迹的机械臂关节速度约束转化为，添加关节速度目标函数定义如下：

预抓取轨迹的机械臂关节加速度约束转化为，添加关节加速度目标函数定义如下：

避障约束转化为定义简化的避障目标函数如下：

其中，x_z为正运动学求解后位置向量的沿z轴移动的分量，z轴定义为负的加速度方向，z_obj为待抓取目标的上表面高度，函数ψ_ε[·]定义如下：

其中，ε为另一个可定义的参数。

步骤4：对转化后的总目标函数求梯度，采用优化求解器进行求解，并将轨迹输入控制器控制机械臂抓取物体；上述问题经过前述步骤的约束转化后，可转化为以下无约束的优化求解问题：

其中*表示符号合集*＝{g,q,v,a,c}，与前述所定义的目标函数的下标一一对应，ρ_*表示各目标函数的权重，为总目标的数。

接下来求解总目标函数关于{q,T}的梯度，总目标函数关于q_i的梯度计算方式为

其中e_(2i-1)s+1为单位矩阵I_2Ms的第((2i-1)s+1)列。中间矩阵定义为：

其中，过渡矩阵E_i,定义为：

E_i＝(β(T_i),β(T_i),...,β^(2s-1)(T_i))^T

F_i＝(0,-β(0),...,-β^(2s-1)(0))^T

特别的，边界过渡矩阵E_M,定义为：

F₀＝(β(0),...,β^(s-1)(0))^T

E_M＝(β(T_M),...,β^(s-1)(T_M))^T

将求出的每项作为矩阵的第i列内容，即可以得到总目标函数关于轨迹关键点向量的梯度

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

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

计算与后，该无约束优化问题可以采用L-BFGS求解器进行求解。将求解出的轨迹输入控制器，控制机械臂抓取物体。

本发明的一个实施例：

本发明的实施技术方案为：

1)定义传送带上动态物体的抓取问题和机械臂轨迹表示方法：

将采用机械臂进行传送带上匀速运动物体的抓取划分两个轨迹的规划进行：预抓取轨迹和抓取轨迹。

预抓取轨迹定义为从机械臂初始位姿到预抓取位姿的轨迹，预抓取位姿定义为机械臂的末端执行器抓取姿态沿接近向量负方向运动一段后退距离h的预抓取姿态的逆运动学。对于一个m自由度的串联机械臂，在实施中所使用的机械臂m＝7，预抓取轨迹表示为ξ(t)，是时间t到机械臂关节空间的映射，具有轨迹总时长T，是一个具有轨迹分段数M的分段函数，其第i段轨迹ξ_i(t)的定义方法如下：

其中β(t)＝(1,t,...,t^2s-1)^T是该段轨迹曲线的基，是第i段轨迹的系数矩阵，s为轨迹的可指定导数值阶数，t_i-1和t_i为该段轨迹的最小和最大时间限制，该第i段轨迹具有时长T_i＝t_i-t_i-1。对于M段的预抓取轨迹，所有轨迹时长构成了轨迹时长向量T＝(T₁,...,T_M)^T，所有段轨迹的系数矩阵构成了轨迹系数矩阵在实施中取s＝3，M＝3。

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

s.t.T＞0

ξ(0)＝q₀

x⁽¹⁾(ξ(T))＝v_c+v_⊥

ξ⁽²⁾(T)＝0

其中，q＝(q₁,...,q_M)为轨迹关键点，其内包含轨迹时长向量T中一一对应的机械臂位姿，为初始目标函数，ρ为时间优化权重，为机械臂初始位姿，为正运动学映射，由关节空间映射到笛卡尔空间是传送带运动速度，是初始抓取姿态，即在初始时间t＝0时的抓取姿态，为抓取姿态接近向量，为单位化的抓取姿态接近向量，是无碰撞的关节空间。q_max,q_min分别是机械臂的最大与最小关节位置，v_max,a_max分别为关节速度限制与关节加速度限制，ξ⁽¹⁾(t),ξ⁽²⁾(t)分别为轨迹速度与轨迹加速度。

在实施中，设定ρ＝1500，q_max＝[360°,128.9°,360°,147.8°,360°,120.3°,360°]^T，q_min＝-q_max，q₀＝[0°,-1.6°,-175.1°,-92.1°,0°,-86.8°,94.5°]^T，v_c＝0.5m/s。

接下来，定义抓取轨迹和其生成方法。抓取轨迹为从预抓取位姿到抓取位姿的轨迹，生成方法为：

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

其中，J⁺为机械臂雅可比矩阵的伪逆。在这个过程中，机械臂夹爪逐渐闭合，直到机械臂到达抓取位姿，抓取动作阶段结束。

2)进行预抓取轨迹末端约束的转化。轨迹末端位姿约束转化为，添加末端位姿目标函数定义如下：

其中，∈是一个小值，μ为一个可定义的参数，函数φ_μ[·]定义为：

在实施中，设定∈＝0.5×10^-3，μ＝1。

q_M为当前迭代下的预抓取位姿，由以下方式求解：

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

ξ⁽¹⁾(T)＝J⁺(q_M)(v_c(T)+v_⊥)

3)进行机械臂关节运动学约束和碰撞约束的转化。预抓取轨迹的机械臂关节位姿约束转化为，添加关节位姿目标函数定义如下：

其中，下标j代表第j个关节的参数，即各符号所表示向量的第j项元素。

预抓取轨迹的机械臂关节速度约束转化为，添加关节速度目标函数定义如下：

预抓取轨迹的机械臂关节加速度约束转化为，添加关节加速度目标函数定义如下：

避障约束转化为定义简化的避障目标函数如下：

其中，x_z为正运动学求解后位置向量的沿z轴移动的分量，z轴定义为负的加速度方向，z_obj为待抓取目标的上表面高度，函数ψ_ε[·]定义如下：

其中，ε为另一个可定义的参数，在实施中，设定ε＝0.02。

上述四项目标函数在实施中需要进行离散化：

其中，σ＝{q,v,a,c}，代表上述目标函数中积分符号里的内容，权重系数向量取值为κ_i为离散化的分辨率，取所有κ_i＝20。

4)对转化后的总目标函数求梯度，采用优化求解器进行求解，并将轨迹输入控制器控制机械臂抓取物体。上述问题经过前述步骤的约束转化后，可转化为以下无约束的优化求解问题：

其中*表示符号合集*＝{g,q,v,a,c}，与前述所定义的目标函数的下标一一对应，ρ_*表示各目标函数的权重，为总目标函数。实施中，设定各目标函数权重为：

ρ_g＝1×10⁴,ρ_q＝1×10⁴,ρ_v＝0.5×10³,ρ_a＝1×10⁴,ρ_c＝1×10³

接下来求解总目标函数关于{q,T}的梯度，总目标函数关于q_i的梯度计算方式为：

其中e_(2i-1)s+1为单位矩阵I_2Ms的第((2i-1)s+1)列。中间矩阵定义为：

其中，过渡矩阵E_i,定义为：

E_i＝(β(T_i),β(T_i),...,β^(2s-1)(T_i))^T

F_i＝(0,-β(0),...,-β^(2s-1)(0))^T

特别的，边界过渡矩阵E_M,定义为：

F₀＝(β(0),...,β^(s-1)(0))^T

E_M＝(β(T_M),...,β^(s-1)(T_M))^T

将求出的每项作为矩阵的第i列内容，即可以得到总目标函数关于轨迹关键点向量的梯度

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

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

计算与后，该无约束优化问题可以采用L-BFGS求解器进行求解。将求解出的轨迹输入控制器，控制机械臂抓取物体。

如图3a、图3b所示是在该设定传送带速度下规划出的机械臂抓取轨迹的速度和加速度曲线。

与前述一种抓取传送带上运动物体的机械臂轨迹规划方法的实施例相对应，本发明还提供了一种抓取传送带上运动物体的机械臂轨迹规划装置的实施例。

参见图4，本发明实施例提供的一种抓取传送带上运动物体的机械臂轨迹规划装置，包括存储器和一个或多个处理器，存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现上述实施例中的一种抓取传送带上运动物体的机械臂轨迹规划方法。

本发明一种抓取传送带上运动物体的机械臂轨迹规划装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图4所示，为本发明一种抓取传送带上运动物体的机械臂轨迹规划装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图4所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。

上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。

对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的一种抓取传送带上运动物体的机械臂轨迹规划方法。

所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是任意具备数据处理能力的设备的外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡(Smart Media Card，SMC)、SD卡、闪存卡(Flash Card)等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的范围。

Claims (10)

1.一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，包括以下步骤：

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

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

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

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

2.根据权利要求1所述的一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，

所述步骤1包括如下具体步骤：

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

其中β(t)＝(1,t,...,t^2s-1)^T是该段轨迹曲线的基，是第i段轨迹的系数矩阵，s为轨迹的可指定导数值阶数，t_i-1和t_i为该段轨迹的最小和最大时间限制，该第i段轨迹具有时长T_i＝t_i-t_i-1；对于M段的预抓取轨迹，所有轨迹时长构成了轨迹时长向量T＝(T₁,...,T_M)^T，所有段轨迹的系数矩阵构成了轨迹系数矩阵

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

s.t.T＞0

ξ(0)＝q₀

x⁽¹⁾(ξ(T))＝v_c+v_⊥

ξ⁽²⁾(T)＝0

其中，q＝(q₁,...,q_M)为轨迹关键点，其内包含轨迹时长向量T中一一对应的机械臂位姿，为初始目标函数，ρ为时间优化权重，为机械臂初始位姿，x:为正运动学映射，由关节空间映射到笛卡尔空间是传送带运动速度，是初始抓取姿态，即在初始时间t＝0时的抓取姿态，为抓取姿态接近向量，为单位化的抓取姿态接近向量，是无碰撞的关节空间；q_max,q_min分别是机械臂的最大与最小关节位置，v_max,a_max分别为关节速度限制与关节加速度限制，ξ⁽¹⁾(t),ξ⁽²⁾(t),ξ⁽³⁾(t)分别为轨迹速度、轨迹加速度和轨迹加加速度；

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

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

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

3.根据权利要求1所述的一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，

所述步骤2具体分为两个阶段：

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

其中，ξ(T)表示时间t为轨迹总时长T时的预抓取轨迹函数值，q_M为当前迭代下的预抓取位姿，是一个小值，μ为一个可定义的参数，函数φ_μ[·]定义为：

其中，q_M由以下方式求解：

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

ξ⁽¹⁾(T)＝J⁺(q_M)(v_c+v_⊥)。

4.根据权利要求1所述的一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，所述步骤3中，预抓取轨迹的机械臂关节位姿约束转化为，添加关节位姿目标函数定义如下：

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

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

5.根据权利要求1所述的一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，所述步骤3中，预抓取轨迹的机械臂关节速度约束转化为，添加关节速度目标函数定义如下：

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

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

6.根据权利要求1所述的一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，所述步骤3中，预抓取轨迹的机械臂关节加速度约束转化为，添加关节加速度目标函数定义如下：

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

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

7.根据权利要求1所述的一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，所述步骤3中，避障约束转化为定义简化的避障目标函数如下：

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

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

8.根据权利要求1所述的一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，所述步骤4中，经过约束转化后，转化为以下无约束的优化求解问题：

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

9.根据权利要求1所述的一种抓取传送带上运动物体的机械臂轨迹规划方法，其特征在于，所述步骤4中，求解总目标函数关于{q,T}的梯度，总目标函数关于q_i的梯度计算方式为：

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

其中，过渡矩阵定义为：

边界过渡矩阵定义为：

F₀＝(β(0),...,β^(s-1)(0))^T

E_M＝(β(T_M),...,β^(s-1)(T_M))^T

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

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

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

10.一种抓取传送带上运动物体的机械臂轨迹规划方法装置，其特征在于，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现权利要求1-9中任一项所述的一种抓取传送带上运动物体的机械臂轨迹规划方法。