一种工业四轴机器人运动目标跟踪抓取方法

1.本发明属于机器人技术领域，涉及一种工业四轴机器人运动目标跟踪抓取方法。


背景技术：

2.针对工业机器人运动目标跟踪抓取，有的图像处理计算量大耗时长，在线更新慢，无法实时引导机器进行跟踪抓取，有的实时性相对较好，但是存在鲁棒性不足的缺点。另外一个问题是机器人抓取系统对于机器人收到抓取命令到执行抓取存在时间差，也就是存在抓取延迟，会能导致机器人抓取精度下降，这些问题都有待解决。


技术实现要素：

3.有鉴于此，本发明的目的在于提供一种工业四轴机器人运动目标跟踪抓取方法。，解决机器人驱动扭矩过大、跟踪算法实时性不够好、图像处理的计算量过大、机器人抓取延迟过长的问题。
4.为达到上述目的，本发明提供如下技术方案：
5.一种工业四轴机器人运动目标跟踪抓取方法，包括以下步骤：
6.s1：设计工业四轴机器人的结构；
7.s2：建立系统的运动目标跟踪与定位算法；
8.s3：建立系统的在线轨迹规划与追踪抓取策略；
9.s4：建立工业四轴机器人的软硬件系统架构。
10.可选的，所述s1中，工业四轴机器人的参数为：抓取对象为重量为1kg以内的物体，有效负载大于12.25n，额定负载大于12.32n，机器人的最大合成速度为1.5m/min，机器人的重复定位精确度为
±
2mm，机器人的工作半径为550mm；
11.工业四轴机器人为四轴串联机器人，四个轴分别为：jt1、jt2、jt3、jt4；从第jt1到jt4都是单杆连接形式，每两个轴都组成一个平面转动副，每个转动副实际驱动为一个步进电机和一个减速器；jt1与机器人底座连接，jt2、jt3安装在jt1的正上方，jt4为机器人末端。
12.可选的，所述s2中，运动目标跟踪与定位算法使用基于核相关滤波的目标追踪算法kcf，流程如下：
13.1)从当前帧中，获取初始化的目标区域r0；
14.2)以目标区域为中心，扩展一定区域，构建循环样本矩阵；
15.3)提取hog特征，训练得到分类器；
16.4)计算响应值，找出响应值最大的候选模板；
17.5)输出跟踪结果并更新分类器，更新下一帧目标区域；
18.6)转到第2)步；
19.在步骤2)中，使用基于循环的位移采样算法，该算法具体如下：
20.循环矩阵的一维形式是由一个n维向量每次向右循环移动一个元素，共移动m次，
直到生成一个m
×
n的矩阵；设向量x表示包含感兴趣目标的目标块，称为基础样本，针对一个一维基础样本，基于循环矩阵采样描述如下：
21.一维基础样本：
22.x＝[x1，x2，x3，...，xn]
t
ꢀꢀ
(6)
[0023]
转置矩阵：
[0024][0025]
对x进行向右循环移动：
[0026]
p
x
＝[xn，x1，x2，...，x
n-1
]
t
ꢀꢀ
(8)
[0027]
将基础样本x经过n-1次向右循环移动，生成n-1个新向量，然后将原基础样本和新向量组成一个n
×
n的循环矩阵c；
[0028]
在步骤3)中，方向梯度直方图hog特征是使用图像局部区域内的梯度方向直方图作为特征描述；基于核相关滤波的目标跟踪算法，在构建了循环样本矩阵之后，是通过提取候选样本的hog特征来训练算法中的分类器，再利用分类器对每一帧的候选模板计算响应值，响应值最大的候选模板输出为跟踪目标位置；
[0029]
在步骤3)中，使用和岭回归方法训练分类器。
[0030]
可选的，所述s3中，当接收到目标位置点后，机器人将从当前位置点出发，沿着一组由期望轨迹的内插函数所获得的时间序列值所构成的中间位置点运动，最终到达目标位置点；
[0031]
使用3-4-5次多项式方法进行轨迹规划，该方法具体描述如下：
[0032]
先进行归一化处理，令τ＝t/t，h＝s/s，v＝v/(s/t)，a＝a/(s/t2)；其中t是轨迹执行时间，t是运动的总时间，s为末端当前运行位移，s为末端轨迹总位移，v为末端速度，a为末端加速度；
[0033]
由当前位置点、目标位置点的加速度、速度为零有：
[0034]
h＝0，v＝0，va＝0 τ＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0035]
h＝1，v＝0，va＝0 τ＝1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0036]
位移h为5次多项式：
[0037]
h＝a0+a
1τ
+a2τ2+a3τ3+a4τ4+a5τ5ꢀꢀ
(17)
[0038]
速度v为4次多项式：
[0039][0040]
加速度a为3次多项式：
[0041][0042]
将(15)和(16)带入(17)、(18)、(19)，求解得到：a0＝0，a1＝0，a2＝0，a3＝10，a4＝-15，a5＝6；即3-4-5次多项式规划为：
[0043]
h＝10τ
3-15τ4+6τ5ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0044]
v＝30τ
2-60τ3+30τ4ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0045]
a＝60τ-180τ2+120τ3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0046]
进而求得最大速度v
max
和加速度a
max
为：
[0047]vmax
＝1.8750，a
max
＝5.7735
ꢀꢀ
(23)
[0048]
实际最大加速度a
max
＝(1.5m/min)/0.2s＝125mm/s2，则加速度的归一化比例系数为a
max
/a
max
＝66.66，则轨迹规划方程为：
[0049]
s(t)＝66.66(10τ
3-15τ4+6τ5)t2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(24)
[0050]
v(t)＝66.66(30τ
2-60τ3+30τ4)t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
[0051]
a(t)＝66.66(60τ-180τ2+120τ3)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0052]
对式(26)中加速度a(t)求一阶导，求得加速度的跃度，跃度是反应加速度变化速率的物理量，跃度的表达式如下：
[0053]
j(t)＝66.66(60-360τ+360τ2)
ꢀꢀ
(27)
[0054]
得出在3-4-5次多项式轨迹规划下，依据工业四轴机器人“门”字型运动轨迹；
[0055]
跟踪抓取策略的目的是，根据实际任务的要求，考虑机器人的抓取精度和抓取实时性来建立相关控制算法，最终实现准确的跟踪抓取；
[0056]
使用基于最小二乘法的运动目标跟踪抓取策略，具体如下：
[0057]
使用的输送线高度是水平不变的，即机器人在抓取时z方向的坐标值是常量，对x、y方向的坐标值进行预测；设已知前n个时刻运动目标形心坐标历史数据为(xi，yi)，i＝1，2，
…
，n，假设多项式拟合函数f(t)使得拟合的误差r最小，即
[0058][0059][0060]
其中：ai为待定系数，ti为时间，求r最小时待定系数的值，问题转变为求对原函数的极值问题，即函数在某点偏导数为零，以x方向的预测为例，(a0，a1，
…
，ar，)t为式(30)的解
[0061][0062]
可选的，所述s4中，系统整体软件设计为：包括工业相机、自主设计四轴机器人、调速输送线、编码器、电控安装板；
[0063]
以pc端的上位机作为客户使用终端，上位机利用与工业相机之间的通信获得运动目标的信息，然后再通过跟踪算法、抓取算法、机器人正逆运动学原理进行计算，把得到的信息转换成机器人所需要的位姿信号，并通知到下位机；下位机在获得上位机器的位姿信号后，同步操控机器人的三个步进电机和一个舵机，让机器人实现对运动目标的跟踪和抓取；整个系统还加入了编码器和光电传感器，编码器的作用是实时获取输送线的运动信息并提供给系统控制算法进行计算，光电传感器的作用是通知系统开始跟踪任务。
[0064]
本发明的有益效果在于：本发明设计了工业四轴机器人，选取kcf算法作为跟踪算法，提出了结合输送线编码器数据用于目标区域重构的方法，改善了目标区域丢失的问题。采用了由跟踪算法输出roi的方法，基于roi区域进行图像处理获取运动目标形心，减少了图像处理的数据量，改善算法实时性。通过3-4-5次多项式规划了典型的四轴机器人抓取运
动路径，通过最小二乘法预测形心在抓取时刻的位置，解决了抓取延迟的问题。
[0065]
本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
[0066]
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：
[0067]
图1为本发明中四轴串联机器人机构图；
[0068]
图2为系统整体布局图；
[0069]
图3为系统整体架构图；
[0070]
图4为运动目标跟踪抓取实现过程。
具体实施方式
[0071]
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
[0072]
其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
[0073]
本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
[0074]
请参阅图1～图4，本发明的实施过程如下所示：
[0075]
s1所述工业四轴机器人的结构设计；
[0076]
s2所述系统的运动目标跟踪与定位算法；
[0077]
s3所述系统的在线轨迹规划与追踪抓取策略；
[0078]
s4所述改进的工业四轴机器人运动目标跟踪抓取系统的软硬件系统架构；
[0079]
实施过程s1中，工业机器人为四轴串联机器人，四个轴分别为：jt1、jt2、jt3、jt4。该机构从第jt1到jt4都是单杆连接形式，每两个轴都组成一个平面转动副，每个转动副实际驱动为一个步进电机和一个减速器。jt1与机器人底座连接，jt2、jt3安装在jt1的正上
方，jt4为机器人末端。[0014]按照机器人每个轴所承载的扭矩计算所需的电机驱动参数，计算方法如下：
[0080]
取机器人jt1-jt4自身转动惯量分别为j
g1
，j
g2
，j
g3
，j
g4
。由理论力学相关原理可知，机器人其他部件对jt1的等效转动惯量为：
[0081]
j1＝(j
g1
+m1l
112
)+(j
g2
+m2l
212
)+(j
g3
+m3l
312
)+(j
g4
+m4l
x12
)
ꢀꢀ
(1)
[0082]
其中l
11
，l
21
，l
31
，l
41
分别为机器人第jt1～jt四轴各自几何重心到jt1电机轴线的最大距离，设计计算时，当几何重心计算误差较大，重心位置可取为每个部件离所计算轴的电机轴线的最远处，以计算保守值。
[0083]
又由于j
g1jg1
《《m1l
112
，j
g2
《《m2l
212
，j
g2
《《m2l
212
，j
g4
《《m4l
412
[0084]
因此，在计算转动惯量时，可以忽略，j
g2
，j
g3
，j
g4
，所以其他部件对第1轴的等效转动惯量可以使用下式：
[0085]
j1＝m1l
112
+m2l
212
+m3l
312
+m4l
412
ꢀꢀ
(2)
[0086]
同理可得其他部件对jt2～jt4的等效转动惯量计算公式：
[0087]
j2＝m2l
222
+m3l
322
+m4l
422
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0088]
j3＝m3l
332
+m4l
432
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0089]
j4＝m4l
442
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0090]
实施过程s2中，运动目标跟踪与定位算法使用kcf算法
[0091]
kcf算法流程如下：
[0092]
1)从当前帧中，获取初始化的目标区域r0；
[0093]
2)以目标区域为中心，扩展一定区域，构建循环样本矩阵；
[0094]
3)提取hog特征，训练得到分类器；
[0095]
4)计算响应值，找出响应值最大的候选模板；
[0096]
5)输出跟踪结果并更新分类器，更新下一帧目标区域。
[0097]
6)转到第2)步。
[0098]
在步骤2)中，使用基于循环的位移采样算法，该算法具体描述如下：
[0099]
循环矩阵是一种特殊的矩阵，它的一维形式是由一个n维向量每次向右循环移动一个元素，共移动m次，直到生成一个m
×
n的矩阵。设向量x表示包含感兴趣目标的目标块，也被称为基础样本，用一个一维基础样本举例，基于循环矩阵采样可描述如下：
[0100]
一维基础样本：
[0101]
x＝[x1，x2，x3，...，xn]
t
ꢀꢀ
(6)
[0102]
转置矩阵：
[0103][0104]
对x进行向右循环移动：
[0105]
p
x
＝[xn，x1，x2，...，x
n-1
]
t
ꢀꢀ
(8)
[0106]
将基础样本x经过n-1次向右循环移动，生成n-1个新向量，然后将原基础样本和新向量组成一个n
×
n的循环矩阵c
[0107]
在步骤3)中，方向梯度直方图(hog)特征是使用图像局部区域内的梯度方向直方图作为特征描述。基于核相关滤波的目标跟踪算法，在构建了循环样本矩阵之后，是通过提取候选样本的hog特征来训练算法中的分类器，再利用分类器对每一帧的候选模板计算响应值，响应值最大的候选模板输出为跟踪目标位置。
[0108]
在步骤3)中，使用和岭回归方法训练分类器，该方法具体描述如下：
[0109]
设通过核岭回归来寻找的分类器为
[0110][0111]
其中，y是满足高斯分布的样本输出，是将样本映射到核空间的函数，λ是惩罚系数，||
·
||为范数运算，分类器参数w是行向量张成的空间中的一个向量。
[0112]
令
[0113][0114]
则
[0115][0116]
该问题称为w的对偶问题，φ(x)φ(x)
t
类似于核空间变量的协方差矩阵，且φ(x)φ(x)
t
是可逆的，令k＝φ(x)φ(x)
t
表示核空间的核矩阵，那么
[0117]
α
*
＝(k+λi)-1yꢀꢀ
(12)
[0118]
由于训练样本都是由基础样本循环移位产生的，可以证明得到k是一个循环矩阵，则k能够在傅里叶空间中使用离散傅里叶矩阵进行对角化
[0119][0120][0121]
其中，f是离散傅里叶矩阵，y是满足高斯分布的样本输出，k
xx
＝φ(x)φ(x)
t
，是k的第一行。将α带入(10)可求得w，继而可求得分类器f(x)。
[0122]
实施过程s3中，在线轨迹规划与追踪抓取策略，使用3-4-5次多项式方法进行轨迹规划，使用基于最小二乘法的运动目标跟踪抓取策略。
[0123]
3-4-5次多项式所推导出的运动规律如下所示：
[0124]
令τ＝t/t，h＝s/s，v＝v/(s/t)，a＝a/(s/t2)。其中t是轨迹执行时间，t是运动的总时间，s为末端当前运行位移，s为末端轨迹总位移，v为末端速度，a为末端加速度。
[0125]
由当前位置点、目标位置点的加速度、速度为零有：
[0126]
h＝0，v＝0，va＝0 τ＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0127]
h＝1，v＝0，va＝0 τ＝1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0128]
位移h为5次多项式：
[0129]
h＝a0+a
1τ
+a2τ2+a3τ3+a4τ4+a5τ5ꢀꢀ
(17)
[0130]
速度v为4次多项式：
[0131]
[0132]
加速度a为3次多项式：
[0133][0134]
将(15)和(16)带入(17)、(18)、(19)，可求解得到：a0＝0，a1＝0，a2＝0，a3＝10，a4＝-15，a5＝6。即3-4-5次多项式规划为：
[0135]
h＝10τ
3-15τ4+6τ5ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0136]
v＝30τ
2-60τ3+30τ4ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0137]
a＝60τ-180τ2+120τ3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0138]
进而可求得最大速度v
max
和加速度a
max
为：
[0139]vmax
＝1.8750，a
max
＝5.7735
ꢀꢀ
(23)
[0140]
实际最大加速度a
max
＝(1.5m/min)/0.2s＝125mm/s2，则加速度的归一化比例系数为a
max
/a
max
＝66.66，则轨迹规划方程为：
[0141]
s(t)＝66.66(10τ
3-15τ4+6τ5)t2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(24)
[0142]
v(t)＝66.66(30τ
2-60τ3+30τ4)t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
[0143]
a(t)＝66.66(60τ-180τ2+120τ3)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0144]
对式(26)中加速度a(t)求一阶导，可求得加速度的跃度，跃度是反应加速度变化速率的物理量，跃度的表达式如下：
[0145]
j(t)＝66.66(60-360τ+360τ2)
ꢀꢀꢀ
(27)
[0146]
由以上的数学关系，可得出在3-4-5次多项式轨迹规划下，依据工业四轴机器人“门”字型运动轨迹。
[0147]
最小二乘法的运动目标跟踪抓取策略，该方法具体描述如下：
[0148]
本次使用的输送线高度是水平不变的，即机器人在抓取时z方向的坐标值是常量，因此对x、y方向的坐标值进行预测。设已知前n个时刻运动目标形心坐标历史数据为(xi，yi)，i＝1，2，
…
，n，假设多项式拟合函数f(t)使得拟合的误差r最小，即
[0149][0150][0151]
其中：ai为待定系数，ti为时间，求r最小时待定系数的值，问题转变为求对原函数的极值问题，即函数在某点偏导数为零，以x方向的预测为例，(a0，a1，
…
，ar，)t为式(30)的解
[0152][0153]
实施过程s4中，工业四轴机器人运动目标跟踪抓取系统的软硬件系统架构。
[0154]
系统整体软件设计：本发明设计的工业机器人运动目标跟踪抓取系统，包括工业相机、自主设计四轴机器人、调速输送线、编码器、电控安装板组成，系统整体布局如附图2所示。该系统的布局是按照工厂生产线的场景设计的，高度还原了工厂的实际生产情况。
[0155]
系统整体架构设计：本发明设计的整个系统架构，如附图3所示，以pc端的上位机作为客户使用终端，上位机利用与工业相机之间的通信获得运动目标的信息，然后再通过
跟踪算法、抓取算法、机器人正逆运动学原理进行计算，把得到的信息转换成机器人所需要的位姿信号，并通知到下位机。下位机在获得上位机器的位姿信号后，同步操控机器人的三个步进电机和一个舵机，让机器人实现对运动目标的跟踪和抓取。此外，整个系统还加入了编码器和光电传感器，编码器的作用是实时获取输送线的运动信息并提供给系统控制算法进行计算，光电传感器的作用是通知系统可以开始跟踪任务。
[0156]
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。