基于深度视觉与增量闭环的机械臂精准定位及抓取方法与流程

1.本发明属于机械臂领域，涉及机器人控制及机器视觉技术，具体涉及一种基于深度视觉与增量闭环的机械臂精准定位及抓取方法。


背景技术：

2.随着机器视觉和机器人技术的发展与革新，机械臂作为工业机器人领域中的一员，也越来越受欢迎，在智能制造背景的驱动之下，机械臂在人们的日常生活中也越来越普及。对于移动机器人来说，许多移动机器人需要配备机械臂从而更好的完成一些抓取任务。目前，市面上存在的高精度机械臂误差小，但是造价昂贵，对于应用场景环境要求也比较高。而价格稍低的机械臂往往因为使用多个串行的舵机而导致其机械臂抓取定位误差大，机械臂的夹爪抓取的位置和实际位置存在较大的误差，如果对物体抓取的空间位置要求较高，则很容易出现机械臂的夹爪抓取失败的情况。


技术实现要素：

3.发明目的：为了克服现有技术中存在的不足，提供一种实时性强、识别率高、抗背景环境干扰能力强的基于深度视觉与增量闭环的机械臂精准定位及抓取方法。
4.技术方案：为实现上述目的，本发明提供一种基于深度视觉与增量闭环的机械臂精准定位及抓取方法，包括如下步骤：
5.s1：利用安装于机械臂的夹爪后部的深度摄像头识别待抓取的物体位置，标定机械臂的空间三维坐标系o
机械臂
，再通过上位机进行编程向机械臂发送待抓取物体的机械臂空间坐标；
6.s2：通过逆运动学解析算法，将待抓取物体的机械臂空间坐标信息转换为机械臂舵机的角度信息；
7.常见的六自由度机械臂含有六个舵机，分别解算这六个舵机的运动角度信息，可以生成多种机械臂的空间姿态，逆运动学解析算法会根据路径最优的原则进行机械臂最优运动轨迹的设计，从而将机械臂的夹爪运动到待抓取物体的机械臂空间坐标位置；
8.s3：将深度摄像头拍摄得到的场景彩色图像进行裁剪及二值化，获得含有待抓取物体的目标图像；
9.图像的二值化，就是将图像上的像素点的灰度值设置为0或255，也就是将整个图像呈现出明显的只有黑和白的视觉效果；
10.s4：建立目标图像像素坐标系及深度摄像头空间坐标系，对目标图像中的待抓取物体进行形心位置计算；
11.s5：判断待抓取物体形心是否在深度摄像头中心线上，若在，则根据机械臂舵机的角度信息驱动机械臂抓取物体；若不在，则通过待抓取物体的形心位置来计算机械臂的偏移量，并通过修正得到反馈修正偏移量，即增量；
12.s6：将增量作为反馈信号输送到步骤s2的逆运动学解析算法，得到更新后的待抓
取物体的机械臂空间坐标，重复步骤s2-s5，直至待抓取物体形心处于深度摄像头中心线上，驱动机械臂抓取物体。
13.进一步地，所述步骤s4中深度摄像头空间坐标系的建立方法为：初始化深度摄像头空间坐标位置，建立深度摄像头空间坐标系o
摄
，其坐标轴方向分别为：z
摄
轴为深度摄像头镜头中心延长线，x
摄
轴为垂直于机械臂的夹爪方向的中心延长线，y
摄
轴垂直于z
摄
轴与x
摄
轴。
14.进一步地，所述步骤s4中目标图像像素坐标系的建立方式为：以图像左下角为坐标原点，建立图像像素坐标系o
像素
。
15.进一步地，所述步骤s4中待抓取物体的形心位置计算公式为：
[0016][0017][0018]
其中，x
pixel
为目标图像在图像像素坐标系o
像素
的横轴的像素坐标，y
pixel
为目标图像在图像像素坐标系o
像素
的纵轴的像素坐标，为待抓物体的图像像素坐标系o
像素
下的待抓取物体的形心坐标，a
pixel
为深度摄相机拍摄待抓取物体的像素面积。
[0019]
进一步地，所述步骤s4中a
pixel
的计算公式如下：
[0020]apixel
＝a
area-a
smooth
[0021]
其中，a
area
为待抓取物体模糊像素面积，a
smooth
为待抓取物体边界光滑像素面积；
[0022]aarea
和a
smooth
的计算方式为：在待抓取物体的外轮廓边缘随机采样k个像素点，将相邻的两点依次连成直线，所围成的区域即是待抓取物体模糊像素面积a
area
，其计算公式为：
[0023]aarea
＝∑δpoint
pixel
[0024]asmooth
＝∑δmargin
pixel
[0025]
其中，δpoint
pixel
为待抓取物体的单位像素点面积，δmargin
pixel
为待抓取物体边缘光滑单位像素点面积。
[0026]
进一步地，所述步骤s5中待抓取物体形心是否在深度摄像头中心线上的判断方法为：
[0027]
a1：建立深度摄像头空间坐标系的初始原点坐标o
摄
(0，0，0)，其中，o
摄
(0，0，0)坐标点的前两位坐标值与图像像素坐标系o
像素
的图像中心重合，其定义为：
[0028][0029]
其中，x
up-left
、x
down-right
分别为图像像素坐标系o
像素
下的图像左上角横坐标像素点与图像右下角横坐标像素点；y
up-left
、y
down-right
分别为图像像素坐标系o
像素
下的图像左上角纵坐标像素点与图像右下角纵坐标像素点；
[0030]
a2：定义深度摄像头中心线为一条过图像像素坐标系o
像素
平面平面坐标点，且垂直于o
像素
平面的一条虚拟直线，也即为摄像头空间坐标系o
摄
中的z
摄
轴；
[0031]
a3：将待抓取物体图像像素坐标系下的形心坐标转换为深度摄像头空间坐标系o
摄
下的待抓取物体形心坐标，其公式如下：
[0032][0033][0034]
其中，f为深度摄像头的焦距，l为深度d摄像头所测量的深度。xs是待抓取物体的深度摄像头空间坐标系o
摄
下的形心横坐标，ys待抓取物体的深度摄像头空间坐标系o
摄
下的形心纵坐标，则待抓取物体形心坐标为(xs，ys，l)；
[0035]
a4：设计一种阈值系数α，其计算公式如下：
[0036][0037]
其中，x
摄right
、x
摄left
为待抓取物体在o
摄
坐标系下最右端与最左端边缘点的横坐标，y
摄up
、y
摄down
为待抓取物体在o
摄
坐标系下最上端与最下端边缘点的纵坐标，n为机械臂舵机个数；
[0038]
a5：将阈值系数α用于判断物体形心是否处于深度摄像头中心的延伸线上，当时，认定为待抓取物体形心处于深度摄像头中心线上，否则认定为待抓取物体在深度摄像头空间坐标下的形心不在深度摄像头中心线上。
[0039]
进一步地，所述步骤s5中增量的计算方法为：
[0040]
b1：通过在o
摄
坐标系下待抓取物体形心位置来计算机械臂空间坐标的偏移量，通过刚体转换矩阵r将位于o
摄
坐标系下的坐标(xs，ys，l)转换为o
机械臂
坐标系下的坐标(xj，yj，zj)，其公式如下：
[0041][0042]
将上述坐标点(xs，ys，l)通过刚体转换矩阵r转换得到的o
机械臂
坐标系下的坐标(xj，yj，zj)与待抓取物体的机械臂空间坐标(x，y，z)进行坐标做差运算，从而获得机械臂空间坐标偏移量δx，δy，δz，
[0043]
δx＝x
j-x,δy＝y
j-y，δz＝z
j-z；
[0044]
其中，δx为o
机械臂
坐标系下横轴坐标的偏移量，δy为o
机械臂
坐标系下纵轴坐标的偏移量，δz为o
机械臂
坐标系下竖轴坐标的偏移量；
[0045]
b2：设计修正反馈系数β，通过修正反馈系数得到增量，其公式如下：
[0046][0047]
其中，n为机械臂舵机个数，μ为单个舵机的误差百分数，ε为机械臂闭环修正次数，
k1、k2、k3为机械臂空间坐标系o
机械臂
下的横轴、纵轴、竖轴的权重调整参数，可根据实际抓取环境及抓取物体大小等选取经验值；其中进行反馈的增量δx
增
＝βδx，δy
增
＝βδy，δz
增
＝βδz；
[0048]
其中，δx
增
为o
机械臂
坐标系下横轴坐标进行反馈的增量，δy
增
为o
机械臂
坐标系下纵轴坐标进行反馈的增量，δz
增
为o
机械臂
坐标系下竖轴坐标进行反馈的增量。
[0049]
有益效果：本发明与现有技术相比，采用成本相对低廉，且环境要求较低的视觉传感器，通过深度视觉系统对待抓取物体的空间位置进行计算，再通过增量闭环系统对机械臂的夹爪空间位置进行一个修正反馈与闭环，从而提高了机械臂夹爪定位精度，将机械臂的抓取成功率有效提高，同时兼顾了传感器的设备价格与机械臂执行效率，通过深度视觉与增量闭环的机械臂精准定位及抓取方法后，机械臂的定位准确度和抓取物体的成功率都有显著的提高，具有实时性强、识别率高、抗背景环境干扰能力强等诸多优点。
附图说明
[0050]
图1是本发明方法的流程框图；
[0051]
图2是本实施例提供的六自由度机械臂示意图；
[0052]
图3是本实施例提供的搭载深度摄像头的机械臂示意图；
[0053]
图4是本实施例提供的深度摄像头拍摄抓取物体效果图；
[0054]
图5是本实施例中深度摄像头处理待抓取方块物体的示意图；
[0055]
图6是本实施例中带有深度摄像头的机械臂抓取象棋的示意图；
[0056]
图7是本实施例中机械臂进行抓取测试500次的抓取成功率展示图。
具体实施方式
[0057]
下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本技术所附权利要求所限定的范围。
[0058]
本发明提供一种基于深度视觉与增量闭环的机械臂精准定位及抓取方法，如图1所示，其包括如下步骤：
[0059]
s1：在图2所示的机械臂的夹爪后部安装深度摄像头，形成如图3所示的搭载深度摄像头的机械臂，利用深度摄像头识别待抓取的物体位置，标定机械臂的空间三维坐标系o
机械臂
，再通过上位机进行编程向机械臂发送一条含有待抓取物体的机械臂空间坐标位置(x，y，z)的指令；
[0060]
s2：根据机械臂空间坐标位置(x，y，z)，机械臂通过逆运动学解析算法，将待抓取物体的机械臂空间坐标信息转换为机械臂舵机的角度信息；
[0061]
本实施例中机械臂为六自由度机械臂，含有六个舵机，分别解算这六个舵机的运动角度信息，可以生成多种机械臂的空间姿态，逆运动学解析算法会根据路径最优的原则进行机械臂最优运动轨迹的设计，从而将机械臂的夹爪运动到待抓取物体的机械臂空间坐标位置；
[0062]
s3：将深度摄像头拍摄得到的场景彩色图像进行裁剪及二值化，获得含有待抓取物体的目标图像；
[0063]
图像的二值化，就是将图像上的像素点的灰度值设置为0或255，也就是将整个图像呈现出明显的只有黑和白的视觉效果；
[0064]
如图4所示，(a)为深度相机所拍摄采集到的原始的待抓取物体的rgb图像，(b)为原始的待抓取物体的rgb图像经过二值化处理并裁剪的图像，(c)为(b)经过形心位置计算后得到的待抓取物体形心位置图，(d)为深度摄像头拍摄到的深度图。
[0065]
s4：建立目标图像像素坐标系及深度摄像头空间坐标系，对目标图像中的待抓取物体进行形心位置计算：
[0066]
目标图像像素坐标系的建立方式为：以图像左下角为坐标原点，建立图像像素坐标系o
像素
。
[0067]
深度摄像头空间坐标系的建立方法为：初始化深度摄像头空间坐标位置，建立深度摄像头空间坐标系o
摄
，其坐标轴方向分别为：z
摄
轴为深度摄像头镜头中心延长线，x
摄
轴为垂直于机械臂的夹爪方向的中心延长线，y
摄
轴垂直于z
摄
轴与x
摄
轴。
[0068]
待抓取物体的形心位置计算公式为：
[0069][0070][0071]
其中，x
pixel
为目标图像在图像像素坐标系o
像素
的横轴的像素坐标，y
pixel
为目标图像在图像像素坐标系o
像素
的纵轴的像素坐标，为待抓物体的图像像素坐标系o
像素
下的待抓取物体的形心坐标，a
pixel
为深度摄相机拍摄待抓取物体的像素面积。
[0072]apixel
的计算公式如下：
[0073]apixel
＝a
area-a
smooth
[0074]
其中，a
area
为待抓取物体模糊像素面积，a
smooth
为待抓取物体边界光滑像素面积；
[0075]aarea
和a
smooth
的计算方式为：在待抓取物体的外轮廓边缘随机采样k个像素点，将相邻的两点依次连成直线，所围成的区域即是待抓取物体模糊像素面积a
area
，其计算公式为：
[0076]aarea
＝∑δpoint
pixel
[0077]asmooth
＝∑δmargin
pixel
[0078]
其中，δpoint
pixel
为待抓取物体的单位像素点面积，δmargin
pixel
为待抓取物体边缘光滑单位像素点面积。
[0079]
s5：判断待抓取物体形心是否在深度摄像头中心线上，具体判断方法为：
[0080]
a1：建立深度摄像头空间坐标系的初始原点坐标o
摄
(0，0，0)，其中，o
摄
(0，0，0)坐标点的前两位坐标值与图像像素坐标系o
像素
的图像中心重合，其定义为：
[0081]o摄
(0，0，0)定义为
[0082]
其中，x
up-left
、x
down-right
分别为图像像素坐标系o
像素
下的图像左上角横坐标像素点与图像右下角横坐标像素点；y
up-left
、y
down-right
分别为图像像素坐标系o
像素
下的图像左上角纵坐标像素点与图像右下角纵坐标像素点；
[0083]
a2：定义深度摄像头中心线为一条过图像像素坐标系o
像素
平面平面坐标点，且垂直于o
像素
平面的一条虚拟直线，也即为摄像头空间坐标系o
摄
中的z
摄
轴；
[0084]
a3：将待抓取物体图像像素坐标系下的形心坐标转换为深度摄像头空间坐标系o
摄
下的待抓取物体形心坐标，其公式如下：
[0085][0086][0087]
其中，f为深度摄像头的焦距，l为深度d摄像头所测量的深度。xs是待抓取物体的深度摄像头空间坐标系o
摄
下的形心横坐标，ys待抓取物体的深度摄像头空间坐标系o
摄
下的形心纵坐标，则待抓取物体形心坐标为(xs，ys，l)；
[0088]
a4：设计一种阈值系数α，其计算公式如下：
[0089][0090]
其中，x
摄right
、x
摄left
为待抓取物体在o
摄
坐标系下最右端与最左端边缘点的横坐标，y
摄up
、y
摄down
为待抓取物体在o
摄
坐标系下最上端与最下端边缘点的纵坐标，n为机械臂舵机个数；
[0091]
a5：将阈值系数α用于判断物体形心是否处于深度摄像头中心的延伸线上，当时，认定为待抓取物体形心处于深度摄像头中心线上，否则认定为待抓取物体在深度摄像头空间坐标下的形心不在深度摄像头中心线上。
[0092]
若在，则根据机械臂舵机的角度信息驱动机械臂抓取物体；若不在，则通过待抓取物体的形心位置来计算机械臂的偏移量，并通过修正得到反馈修正偏移量，即增量；
[0093]
增量的计算方法为：
[0094]
b1：通过在o
摄
坐标系下待抓取物体形心位置来计算机械臂空间坐标的偏移量，通过刚体转换矩阵r将位于o
摄
坐标系下的坐标(xs，ys，l)转换为o
机械臂
坐标系下的坐标(xj，yj，zj)，其公式如下：
[0095][0096]
将上述坐标点(xs，ys，l)通过刚体转换矩阵r转换得到的o
机械臂
坐标系下的坐标(xj，yj，zj)与待抓取物体的机械臂空间坐标(x，y，z)进行坐标做差运算，从而获得机械臂空间坐标偏移量δx，δy，δz，
[0097]
δx＝x
j-x,δy＝y
j-y，δz＝z
j-z；
[0098]
其中，δx为o
机械臂
坐标系下横轴坐标的偏移量，δy为o
机械臂
坐标系下纵轴坐标的偏
移量，δz为o
机械臂
坐标系下竖轴坐标的偏移量；
[0099]
b2：设计修正反馈系数β，通过修正反馈系数得到增量，其公式如下：
[0100][0101]
其中，n为机械臂舵机个数，μ为单个舵机的误差百分数，ε为机械臂闭环修正次数，k1、k2、k3为机械臂空间坐标系o
机械臂
下的横轴、纵轴、竖轴的权重调整参数，可根据实际抓取环境及抓取物体大小等选取经验值；其中进行反馈的增量δx
增
＝βδx，δy
增
＝βδy，δz
增
＝βδz；
[0102]
其中，δx
增
为o
机械臂
坐标系下横轴坐标进行反馈的增量，δy
增
为o
机械臂
坐标系下纵轴坐标进行反馈的增量，δz
增
为o
机械臂
坐标系下竖轴坐标进行反馈的增量。
[0103]
s6：将增量作为反馈信号输送到步骤s2的逆运动学解析算法，得到更新后的待抓取物体的机械臂空间坐标o
机械臂
，重复步骤s2-s5，直至待抓取物体形心处于深度摄像头中心线上，得到机械臂空间坐标o
机械臂
下抓取物体的坐标(x
new
，y
new
，z
new
)，驱动机械臂抓取物体。
[0104]
本实施例中六自由度机械臂的自由度与舵机个数相关，六自由度机械臂拥有六个舵机，从下至上分别为舵机1，舵机2，舵机3，舵机4，舵机5，舵机6。当其底部舵机1旋转固定后，其机械臂可视为处于一个二维平面，此时分别解算舵机2，舵机3，舵机4，舵机5，舵机6各自的舵机角度即可反解出机械臂的位置信息。从而得到更新后的机械臂位置为机械臂空间坐标(x
new
，t
new
，z
new
)。
[0105]
本实施例中将如图2所示的现有六自由度机械臂进行物体抓取，图2中红色区域是机械臂的夹爪能够抓取到物体的区域，绿色区域是机械臂的夹爪能够抓取物体的区域，由于舵机等误差导致绿色区域与红色区域之间重合度低，这表明机械臂在抓取物体的时候大概率会失败。而图5为本发明提供的装备深度摄像头的改良六自由度机械臂，其中，图5中红色区域是机械臂的夹爪能够抓取到物体的区域，绿色区域为机械臂的夹爪抓取物体的区域，改良的机械臂采用本发明所使用的基于深度视觉与增量闭环的机械臂精准定位及抓取方法后，红色区域和绿色区域显著性的高度重合，说明改良后的机械臂对于待抓取物体的精准定位和抓取成功率有很大的提升。
[0106]
本实施例中将本发明提供的机械臂和抓取方法用来抓取棋子，如图6所示，可见能够成功抓起。
[0107]
为了进一步验证本发明方案的效果，本实施例中采用本发明方法的机械臂与不使用本发明方法的机械臂各自测试500轮抓取同一物体，其中，待抓取的物品为象棋棋子，其棋子特点为棋子小，且为圆状，侧壁光滑，对机械臂抓取有一定的难度与挑战。测试结果如图7所示，采用本发明的改良机械臂抓取可以达到93％的成功率，而不采用本发明的机械臂只能达到75％的成功率，说明改良后的机械臂对于抓取成功率有很大的提升。