CN110906932B - 一种动态物料追随拾捡轨迹的生成方法 - Google Patents

Abstract

本发明涉及一种动态物料追随拾捡轨迹的生成方法，该方法包括：1、确定下一个待抓取物料，并获取其位姿数据作为主运动的目标点，机器人当前位姿为主运动的起点；2、根据起点终点，轨迹路径参数和运动参数，规划主运动运动轨迹；3、根据轨迹规划结果，确定追随运动的发生点和结束点；4、启动主运动并开始实时计算动态物料的运动距离；5、主运动行进到追随运动启动时间点时，考虑物料运动方向动态规划和生成追随运动轨迹；6、将主运动和追随运动生成的结果叠加，获取动态物料追随运动轨迹。与现有技术相比，本发明方法可实现自动对动态物料进行有效的追随和拾捡作业，且能够自动适应该物料的速度变化确保追随的准确性。

Description

一种动态物料追随拾捡轨迹的生成方法

技术领域

本发明涉及动态物料可靠抓取技术领域，尤其是涉及一种动态物料追随拾捡轨迹的生成方法。

背景技术

拾捡作业是机器人应用的一个常见任务类型，即将物料从一个位置抓取，移动放置到另一个位置。静态物料的拾捡是一个简单任务，已经得到了很好的应用。然而动态物料拾捡仍是一个具有挑战的任务，它的作业过程可简述为：物料在输送线上高速通过机器人下方，具备视觉能力的机器人在一定的抓取策略下筛选和定位抓取目标，然后在运动和追随算法的支撑下高速运动并追随物料完成抓取动作。该应用场景的一个典型配置如图1所示。

此动态拾捡作业的轨迹路径一般包含三段：上升段，平移段和下落段；机器人在上升段离开输送线表面上升至高于物料的位置开始进入平移段；在平移段起点处，机器人开始追赶物料，并在平移段结束处追随上物料并位于其正上方，此时机器人运动速度达到了物料的运动速度，运动轨迹进入下落段；在下落段，机器人正向接近，接触和抓取物料，整个下落过程中两者始终保持同样速度直至抓取动作完成，此时可以触发机器人的返回动作使物料脱离输送线。

该作业过程的技术难点在于：1)物料动态移动并且移动速度可能一直随机变化，这样导致无法预测抓取作业的终点位置，也就无法在抓取开始前预先规划作业轨迹。这就要求机器人在抓取作业过程中必须能够适应物料的速度变化，必须具备动态调整和执行追随运动的轨迹的能力，保证在规定的时间内抵达物料的正上方；2)在抓取动作过程中(接近，接触和抓取物料)机器人必须能够始终维持与物料相同的速度，抓手与物料始终处于相对静止的状态，否则会导致两者之间发生碰擦，造成抓取动作不稳定，增加抓取失败的风险。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种自动适应物料速度、追随稳定准确的动态物料追随拾捡轨迹的生成方法。

本发明的目的可以通过以下技术方案来实现：

一种动态物料追随拾捡轨迹的生成方法，该方法包括以下步骤：

S1：确定机器人的下一个待抓取物料，将物料的当前位姿作为主运动的目标点，将机器人的当前位姿作为主运动的起点。主运动包括上升段、平移段和下落段。

S2：获取包括最大速度限值、最大加速度限值和最大加加速度限值的主运动的运动参数，获取主运动的轨迹路径参数，结合主运动的起点和目标点对主运动轨迹进行规划。主运动的轨迹路径参数包括上升段的竖直高度和弯角高度，以及下落段的竖直高度和弯角高度。

优选地，采用S型速度轨迹规划方法对主运动的三段轨迹分别进行规划，将主运动的三段轨迹设计为由七段曲线组合而成的轨迹曲线，七段曲线包括八个关键点，通过步骤1)给定的主运动起点和目标点以及轨迹路径参数获取三段轨迹各段的起点和目标点；然后结合主运动的运动参数计算各段轨迹的八个关键点运动参量，最后依据各段关键点的规划结果和轨迹路径参数融合生成主运动轨迹。

优选地，关键点的运动参量包括每处时间点所对应的位置、速度、加速度和加加速度。

S3：指定追随运动的发生点和结束点，根据主运动的规划结果确定追随运动的启动时刻和总用时。

指定追随运动的启动时刻和总用时，需保证追随运动发生和完成时间在主运动时间历程之内。即在指定追随运动的发生点和结束点时，可以指定追随运动与主运动同时发生，或者追随运动开始于主运动平移段行程的一半位置处，也可以指定在其他位置；且追随运动一定要在主运动结束之前达到物料运动速度并转入紧密跟随运动阶段。

优选地，在确定追随运动的时间点时，采用数值二分算法计算轨迹上某点位置对应的时间。

S4：触发主运动并同时开始实时计算动态物料的运动距离。具体内容为：

触发主运动时刻，记录物料的当前位姿，随后将其清零，并开始实时计算物料从此刻起在输送线上的运动距离，并获取输送线在机器人基坐标系中的方向，将实时获取的运动方向和运动距离作为规划追随运动的基础。

S5：当主运动行进到追随运动的启动时间点时，结合物料运动方向开始动态规划并生成追随运动轨迹。

S6：将主运动轨迹与追随运动轨迹叠加，生成动态物料追随拾捡轨迹。

与现有技术相比，本发明基于拾捡作业的特点和其对轨迹规划的要求，将拾捡作业的运动过程分解为主运动及追随运动两部分运动，通过运动同步方法和叠加方法，实现了自动对动态物料进行有效的追随和拾捡作业，能够自动适应物料的速度变化以确保追随和保持跟随的准确性，进而能够提高物料抓取动作的稳定性和成功率。

附图说明

图1为动态物料追随拾捡作业原理示意图；

图2为本发明中动态物料追随拾捡运动轨迹生成流程图；

图3为典型的动态物料追随拾捡运动轨迹示意图；

图4为典型的动态物料追随拾捡运动主运动的运动轨迹；

图5为典型的动态物料追随拾捡运动追随运动的运动轨迹；

图6为典型的单段轨迹规划结果图；

图7为采用二分法对给定位置求解时间点的流程图；

图8为本发明实施例中生成的动态物料追随拾捡运动的主运动轨迹；

图9为本发明实施例中生成的动态物料追随拾捡运动的追随运动轨迹；

图10为本发明实施例中生成的动态物料追随拾捡运动的追随拾捡运动轨迹。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

本发明涉及一种动态物料追随拾捡轨迹的生成方法，包括下列步骤：

步骤一、在物料进入机器人可抓取区域后，触发动态追随拾捡运动。将机器人当前的位姿指定为主运动的起点P₀＝(-0.3，0.3，-0.8，0，0，0)；将动态物料当前的位姿指定为主运动的终点P₃(0.1，-0.2，-0.8，0，0，0)。主运动跳跃轨迹两侧的竖直端高度h₁＝0.01、h₂＝0.01，以及弯角高度c₁＝0.02和c₂＝0.02，可得辅助点P₁(-0.3，0.3，-0.77，0，0，0)和P₂(0.1，-0.2，-0.77，0,0，0)。(P₀,P₁,P₂和P₃确定了主运动轨迹规划所需的上升段，平移段和下落段的起点和目标点)。主运动的运动参数约束指定为最大速度V_max＝5m/s，最大加速度Acc_max＝80m/s²，最大加加速度J_c＝2400m/s³。

步骤二、一个典型的主运动的轨迹规划结果如图4所示，主运动一般由三段组成，即起跳向上运动(上升段)、水平跨越运动(平移段)和降落向下运动(下落段)。每一段轨迹可以采用S型速度曲线规划方法分别规划，然后进行融合。指定追随运动的启动时刻和总用时，在理论上只要保证追随运动发生和完成时间在主运动时间历程之内即可。例如，可以指定追随运动与主运动同时发生，或者追随运动开始于主运动平移段行程的一半的位置如图3所示的点1处，也可以指定在别的位置；追随运动一定要在主运动结束之前达到物料运动速度并转入紧密跟随运动阶段。在本发明中，规定追随运动发生于图4中的1处，即平移段轨迹开始处，于4处完成追随运动阶段而进入跟随运动阶段，即平移段轨迹结束处，此时刚好处在物料的正上方并与其保持同速。

一个典型S型速度曲线的规划结果由7段曲线组合而成，如图6所示。轨迹由7段曲线拼接而成。6个拼接点加上起点和终点，一共有8个关键点。在单段轨迹的规划过程中可以计算出这7段的时长以及8个关键点处的运动参数，包括位置y，速度v，加速度a，加加速度j。

其中，j₀,a₀,v₀,y₀为各段的初始值，表示如下：

第1段：

y₀＝0,v₀＝0,a₀＝0,j₀＝j_c

第2段：

y₀＝y_t1，v₀＝v_t1，a₀＝a_max，j₀＝0

第3段：

y₀＝y_t2，v₀＝v_t2，a₀＝a_max，j₀＝-j_c

第4段

y₀＝y_t3，v₀＝v_t3，a₀＝0，j₀＝0

第5段：

y₀＝y_t4，v₀＝v_t4,a₀＝0，j₀＝j_c

第6段：

y₀＝y_t5，v₀＝v_t5，a₀＝-a_max，j₀＝0

第7段：

y₀＝y_t6，v₀＝v_t6，a₀＝-a_max，j₀＝j_c

依据步骤一给定的运动参数和轨迹参数规划主运动，主运动的三段轨迹规划各自的8个关键点运动参数结果如表1所示：

表1上升段运动轨迹(路径1)的关键点数据

	1	2	3	4	5	6	7	8
									t	0.0	0.0155	0.0155	0.031	0.031	0.0465	0.0465	0.062
j	4000	0.0	-4000	0.0	-4000	0.0	4000	0.0
									a	0.0	62.1447	62.1447	0.0	0.0	-62.1447	-62.1447	0.0
v	0.0	0.4827	0.4827	0.9655	0.9655	0.4827	0.4827	0.0
									y	0.0	0.0025	0.0025	0.015	0.015	0.0275	0.0275	0.03

表2平移段运动轨迹(路径2)的关键点数据

	1	2	3	4	5	6	7	8
									t	0.0	0.02	0.0625	0.0825	0.1281	0.1481	0.1906	0.2106
j	4000	0.0	-4000	0.0	-4000	0.0	4000	0.0
									a	0	80	80	0	0	-80	-80	0
v	0.0	0.8	4.2	5	5	4.2	0.8	0.0
									y	0.0	0.0053	0.1116	0.2062	0.4341	0.5287	0.6350	0.6403

表3下落段运动轨迹(路径3)的关键点数据

根据路径1给定的竖直段高度h₁＝0.01m和弯角高度c₁＝0.02m，调用函数[t,y]＝f(h,traj,eps)计算路径1在高度h₁处所对应的时间，该时间点既是主运动启动平移段轨迹生成的开始点也是启动追随运动轨迹生成的起始点。同理可计算获得主运动下落段轨迹生成的启动时间点。通过提前启动下一段轨迹的方式，三段轨迹可以融合形成如图4所示的主运动轨迹。

该函数采用数值二分算法计算轨迹上某点位置对应的时间。函数的输入traj表示上面的轨迹规划结果，即8个关键点处的运动参数，输入h表示所指定的轨迹上的位置，函数的输出y为输入h的计算逼近值，输入eps为逼近误差，输出t为轨迹上位置y所对应的时间。图7描述了计算过程，其中dtraj表示轨迹行程总时间；t_ceil表示二分法中时间的上限值，上限最大值等于dtraj的值，最小值大于t_floor的取值；t_floor表示二分法中时间的下限值，下限最大值小于t_ceil的取值，最小值等于0；用t表示取两者的中间值，n表示二分法计算的次数。通过尝试计算不同的t值所对应的位置y，比较其与给定的h的大小关系，来缩小二分法的上下限值，使得y的计算值逼近h。二分法结束的条件是，t值所对应的位置值y与h的差值的绝对值小于阀值eps，或者尝试计算的次数超过了10次。对于前者，阀值eps的设定根据应用精度要求来设定，一般可设置为0.1毫米；对于后者，超过允许的计算次数被认定为计算失败，可以增加计算次数或者宽松阀值eps。

本实施例中路径1为从低处P₀点向高处P₁点运动。本实施例中t₁＝f(h₁，traj，eps)＝0.0362s，其中h₁＝0.01m,traj为表1的参数，eps＝0.0001m，即当主运动的第一段轨迹运动了0.0362s时启动第二段平移段的运动，此时追随也运动开始。

典型的追随运动曲线如图5所示。追随运动的总用时等于平移段运动轨迹的运动用时，参照表2，即追随运动的总用时during_track＝4d_j+2d_a+d_v＝4*0.02+2*0.0425+0.0456＝0.2106s，即历经时间0.2106s追随上物料，并与其保持同速度，完成追随，此后转入紧密跟随物料运动阶段。

步骤三、触发主运动的同时，将物料的位置清零，将物料从此刻起在输送线上的运动距离记为distance_prod＝[0,0,0,0,0,0]，并重新开始实时计算distance_prod：

从触发主运动时刻起，t开始计时。

输送线在机器人基坐标系中的方向Direction_belt通过预先示教，其结果如下：

动态物料的移动位移Delta_prod可由下式表达：

其中：

式中，prod_x、prod_y、prod_z分别为物料在机器人笛卡尔坐标系中的x轴、y轴和z轴方向的位置。

步骤四、当主运动行进至图4中的1处时，即触发追随运动，追随运动的总时长在上述步骤二中被设计为during_track。一个有效的追随运动轨迹可以设计如下：

式中，Xd_prod为机器人在物料运动方向上的追随距离，coef为追随距离系数变量，k为追随运动当前时间与追随运动总时间之比。

选取系数a、b，使得当t＝during_track时，

Xd_prod＝Delta_prod

利用步骤三、步骤四生成的动态物料追随拾捡运动的主运动轨迹如图8所示，生成的动态物料追随拾捡运动的追随运动轨迹如图9所示。

步骤五、在步骤四中，追随运动已经被触发，此时机器人同时执行主运动的跟随运动，这两个运动的叠加结果可记为：

Xd＝Xd_pm+Xd_prod

式中，Xd_pm为主运动轨迹距离。叠加后的追随运动捡拾轨迹效果如图10所示。至此完成了动态物料追随拾捡轨迹生成。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种动态物料追随拾捡轨迹的生成方法，其特征在于，该方法包括以下步骤：

1)确定机器人的下一个待抓取物料，将物料的当前位姿作为主运动的目标点，将机器人的当前位姿作为主运动的起点；

2)获取主运动的运动参数及主运动的轨迹路径参数，结合主运动的起点和目标点对主运动轨迹进行规划；

3)指定追随运动的发生点和结束点，根据主运动的规划结果确定追随运动的启动时刻和总用时；

4)触发主运动并同时开始实时计算动态物料的运动距离；

5)当主运动行进到追随运动的启动时间点时，结合物料运动方向开始动态规划并生成追随运动轨迹；

6)将主运动轨迹与追随运动轨迹叠加，生成动态物料追随拾捡轨迹。

2.根据权利要求1所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，所述的主运动的运动参数包括最大速度限值、最大加速度限值和最大加加速度限值。

3.根据权利要求2所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，所述的主运动由上升段、平移段和下落段三段轨迹组成，主运动的轨迹路径参数包括上升段的竖直高度和弯角高度，以及下落段的竖直高度和弯角高度。

4.根据权利要求3所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，步骤2)具体包括以下步骤：

201)采用S型速度轨迹规划方法对主运动的三段轨迹分别进行规划，将各段轨迹设计为由七段曲线组合而成的轨迹曲线，七段曲线包括八个关键点；

202)通过步骤1)给定的主运动的起点和目标点，结合主运动的轨迹路径参数，获取三段轨迹各自的起点和目标点；

203)将三段轨迹各自的起点和目标点结合主运动的运动参数，获取各段轨迹的八个关键点的运动参量；

204)依据各关键点的运动参量及主运动的轨迹路径参数，生成主运动轨迹。

5.根据权利要求4所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，关键点的运动参量包括每处时间点所对应的位置、速度、加速度和加加速度。

6.根据权利要求3所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，步骤3)中，所述的追随运动的发生点和结束点以在主运动时间历程之内为限制。

7.根据权利要求6所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，所述的追随运动与主运动同时发生，并结束于主运动结束之前。

8.根据权利要求6所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，所述的追随运动起始于主运动平移段行程的一半位置处，并结束于主运动结束之前。

9.根据权利要求6所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，所述的追随运动起始于上升段的弯角处，并结束于主运动结束之前。

10.根据权利要求1所述的一种动态物料追随拾捡轨迹的生成方法，其特征在于，步骤4)的具体内容为：

触发主运动时刻，记录物料的当前位姿，随后将其清零，并开始实时计算物料从此刻起在输送线上的运动距离，并获取输送线在机器人基坐标系中的方向，将实时获取的运动方向和运动距离作为规划追随运动的基础。

Images