一种基于高阶平滑规划和速度叠加的动态抓取算法制造技术

本发明专利技术涉及机器人抓取技术领域，公开了一种基于高阶平滑规划和速度叠加的动态抓取算法，首先获取当前机器人的位置P

【技术实现步骤摘要】
一种基于高阶平滑规划和速度叠加的动态抓取算法


[0001]本专利技术涉及机器人抓取
，具体为一种基于高阶平滑规划和速度叠加的动态抓取算法。

技术介绍

[0002]机械臂对于随传送带移动的物体进行动态抓取，设计到机械手的抓取动作要和物体的移动速度相协调，即机械手要对随传送带移动的物料位置进行跟踪和预测，等位置和速度一致时再进行动态抓取动作。目前，常用的传送带跟踪功能通常采用基于PID的动态跟踪方法和基于位置预测的拦截方法。
[0003]传统的基于PID的跟踪方法在传送带速度相对高时，容易造成跟踪轨迹曲率大，恶化跟踪效果。传统PID形式跟踪算法本质是通过高频的目标位置刷新，实时调整机器人末端的位置和方向，使机器人末端的运动方向始终指向目标物体的当前位置，直至跟踪完成两者的运动方向和速度一致，之后随着目标物体移动。
[0004]拦截式抓取是根据当前物体的位置，速度和加速度来计算出器将来任意时刻的位置，然后规划机器人末端的动作，使其能在最短的时间内与目标物体相遇。
[0005]通过计算提前得出机器人末端与目标物体相遇的位置，从而控制机器人直接运动至此位置，这样就可以缩短机器人末端的运动距离，从而缩短抓取时间，提高抓取效率。
[0006]目前拦截式抓取对于速度一定的匀速运动或者加速度一定的匀加速运动，在变加速运动情况下容易造成定位精度和跟踪速度误差较大，不满足精度要求高的场所。所以就需要一种基于高阶平滑规划和速度叠加的动态抓取算法。

技术实现思路

[0007]本专利技术的目的在于提供一种基于高阶平滑规划和速度叠加的动态抓取算法，本专利技术继承了拦截式抓取耗时短、抓取效率高的特点。同时通过分段规划方式，避免了由于传送带速度波动或者在变加速情况下造成的位置预测不准，进而导致的定位精度和跟踪速度误差较大的缺点。因而在无需附加传感器和提高硬件成本的前提下，减小传送带跟踪误差、保证了跟踪和抓取精度。能够对于追击过程中的误差进行补偿，提高了跟踪和动态抓取精度；
[0008]本专利技术是这样实现的：
[0009]本专利技术提供一种基于高阶平滑规划和速度叠加的动态抓取算法，具体按以下步骤执行：
[0010]S1：获取当前机器人的位置P
r
(x
r
,y
r
,z
r
)和速度V
r
以及传送带上物体的抓取点位置P0(x0,y0,z0)和运行速度V
C0
及加速度A
C0
；
[0011]S2：传送带上物体的抓取点位置P0(x0,y0,z0)和抓取策略确定偏移高度h2，从而确定接近点P0’
的位置，再根据高阶平滑速度规划算法规划一条由P
r
(x
r
,y
r
,z
r
)点到接近点P0’
的轨迹，经计算后可得到h1段和S段位移所需的运行总时间T
01
；所述高阶平滑速度规划算法包括但不限于梯形速度规划算法、五段S型速度规划算法、七段S型速度规划算法或者其他
高阶多项式速度规划算法，传送带上物体的位置P0点上方h2处的接近点P0’
点为标准门型轨迹的交点，所述接近点P0’
点也可以是基于圆角过渡的门型轨迹的过渡圆弧的起点。
[0012]S3：采用五段S型曲线进行速度平滑规划，同时以位置值的方式进行叠加，五段S型曲线速度规划条件下的位置、速度和加速度表达式如式(1)
‑
式(3)；
[0013][0014][0015][0016]其中，s(t)表示S型曲线，所述v(t)表示速度，a(t)表示加速度。
[0017]S4：根据运动总时间T
01
和测量得到的传送带运行速度V
C
和加速度A
C
，计算得到T
01
时间内传送带移动的距离L
C
以及机器人的速度V
r1
；具体如式(4)
‑
式(5)；
[0018]L
C
＝V
C0
*T
01
+0.5*A
C0
*T
012
ꢀꢀꢀꢀꢀꢀꢀꢀ
式(4)
[0019]Vc1＝V
r
*T
01
+0.5*A
C0
*T
01
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
式(5)
[0020]以传送带布置方向与机器人的坐标轴Y轴方向平行、传送带速度方向沿机器人的Y轴正向进而得到T
01
时间后传送带上物体的位置P1(x1,y1,z1)；其中，如式(6)；
[0021]P1.x1＝P0.x0[0022]P1.y1＝P0.y0+L
C
ꢀꢀꢀꢀꢀꢀ
式(6)
[0023]P1.z1＝P0.z0[0024]S5：在所述误差更新及补偿同步阶段时，由于h1段和S段所需时间较长且运动过程中传送带的速度和加速度不可避免的存在波动，导致T
01
时刻机器人的跟踪位置P1(x1,y1,z1)和传送带上物体的实际位置P1’
(x
10
,y
10
,z
10
)之间存在位置偏差ΔL和速度偏差ΔV；所述位置偏差ΔL为(x
10
‑
x1,y
10
‑
y1,z
10
‑
z1)，表示跟踪位置P1与实际位置P1’
之间分别在X、Y和Z轴方向的偏差；其中，位置偏差ΔL表示如式(7)；
[0025]ΔL.x＝x
10
‑
x1＝0
[0026]ΔL.y＝y
10
‑
y1[0027]ΔL.z＝z
10
‑
z1＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
式(7)
[0028]其中，ΔL表示位置偏差；
[0029]S6：计算、更新步骤S5中的误差，并根据高阶平滑速度规划算法规划h2段的时间T
02
，根据当前传送带上物体的实际位置P1’
、运行速度V
C1
’
、加速度A
C1
’
以及运动时间T
02
；
[0030]S7：计算得到T
02
时刻后，传送带上物体沿传送带方向的理论位置P2以及沿传送带方向的补偿进给速度V
C2
；具体如式(8)；
[0031]P2.x＝P0.x0[0032]P2.y＝P0.y0+L
C
+ΔL.y+V
C1
’
*T
02
+0.5*A
C1
’
*T
022
ꢀꢀꢀꢀ
式(8)...

【技术保护点】

【技术特征摘要】
ΔL.z＝z
10
‑
z1＝0
ꢀꢀꢀꢀ
式(7)其中，ΔL表示位置偏差；S6：计算、更新步骤S5中的偏差误差，并根据高阶平滑速度规划算法规划h2段的时间T
02
，根据当前传送带上物体的实际位置P1’
、运行速度V
C1
’
、加速度A
C1
’
以及运动时间T
02
；S7：计算得到T
02
时刻后，传送带上物体沿传送带方向的理论位置P2以及沿传送带方向的补偿进给速度V
C2
；具体如式(8)；V
C2
＝V
C1
’
+A
C1
’
*T
02
S8：得到机器人沿Z向下降时h2段的运动速度和轨迹曲线；S9：在抓取速度叠加阶段，机器人将计算得到的沿传送带方向的补偿进给速度和沿Z向下降的运动速度进行叠加，采用五次多项式进行速度平滑规划，同时以位置值的方式进行叠加，五次多项式速度规划条件下的位置、速度和加速度表达式如式(9)；S
10
：通过对比位移和速度曲线，保证机器人在下降抓取过程中补偿原位移误差，使机器人下降到最低点时与传送带上物体保持同步...

【专利技术属性】
技术研发人员：请求不公布姓名，
申请(专利权)人：北京立迈胜控制技术有限责任公司，
类型：发明
国别省市：