一种基于深度相机的机械手抓取方法

1.本发明属于自动化技术领域，涉及一种基于深度相机的机械手抓取方法。


背景技术：

2.机器人自主抓取技术使得机器人能够获取、移动和运输目标物体，在工业化环境中抓取的往往是固定位姿的目标物体，实现在非结构化环境中准确抓取是机器人智能化的关键技术之一。在非结构化环境中，机器人抓取系统主要分为三个部分：感知、位姿估计和控制，首先通过传感器获取到目标物体信息，再计算得到抓取位姿，最后控制机器人实现抓取。
3.相比较传统的相机而言，深度相机能够感知外部环境的深度信息，从而获取更多的外部物体信息，因此被广泛的应用于无人驾驶、三维重建、视觉slam等领域。利用深度相机获取到物体的图像信息和深度信息，通过计算得到目标物体位姿，实现在非结构化环境下的抓取，是抓取方法研究的一个重要方向。


技术实现要素：

4.本发明针对使用点云数据估计目标物体位姿进行抓取时，存在的计算量过大及精度较低问题，提出了一种基于深度相机的机械手抓取方法。首先，通过yolo-v5识别出被抓取物体，获得其在rgb图像中的像素范围，将该范围映射到深度图中，得到与其对应深度图中的像素范围。然后，通过结合相机内参计算出深度图中像素范围内的点云数据，将该点云数据和离线阶段建立的模型数据库进行匹配得到初始位姿，通过迭代最近点(iterative closest point，icp)算法对初始位姿进行修正。最后，将修正后的最终位姿结果转化后发给机器人控制系统，精确控制机器人实现抓取。
5.一种基于深度相机的机械手抓取方法，具体包括以下步骤：
6.步骤1、离线阶段建立目标物体的模型数据库。利用已知目标物体的3d模型获得点云数据和初始最佳抓取位姿t0，将点云数据通过体素大小为v
size
＝(wv,hv,dv)的滤波器进行下采样，再提取点云数据的点对特征(point pair feature，ppf)，将相似的ppf放在同一组并存入哈希表中，每一对ppf可以表示为：
7.fm(pm,pn)＝(df,θ1,θ2,θ3)
ꢀꢀꢀ
(1)
8.其中，pm和pn是点云数据中的任意两个点，df是点pm与点pn之间的欧式距离，θ1和θ2分别为点pm与点pn法线和这两个点构成向量之间的夹角，θ3为点pm与点pn法线之间的夹角。
9.步骤2、通过深度相机获取当前环境下的rgb图像数据和深度图像数据，采用yolo-v5目标检测算法，检测当前环境中的目标物体，获得其在rgb图像中的像素范围：
10.pixel
obj
＝{(x
obj
,y
obj
)|(x
left
,y
left
)＜(x
obj
,y
obj
)＜(x
right
,y
right
)}
ꢀꢀꢀ
(2)
11.其中，(x
obj
,y
obj
)为目标物体的像素点，(x
left
,y
left
)和(x
right
,y
right
)分别像素框的左上像素点和右下像素点。
12.步骤3、由于深度相机的彩色摄像头和红外传感器位置不同，导致原始的rgb图像
和深度图像存在偏差，利用式(3)将深度图像和rgb图像进行对齐。
[0013][0014]
其中，p
rgb
和p
ir
为在rgb图像和深度图像中相对应的像素点，i
rgb
和i
ir
分别为rgb相机和红外相机内参数矩阵，e
rgb
、e
ir
、t
rgb
和t
ir
为rgb相机和红外相机的外参数。
[0015]
步骤4、将步骤2中得到的像素框映射到对齐后的深度图像中，由于检测的边界框和实际物体边界可能存在偏差，需要在深度图像中对像素框范围进行修正。
[0016]
步骤5、通过式(4)计算步骤4中纠正后像素框中所有像素点在相机坐标系下的坐标，获得场景点云数据。
[0017][0018]
其中，xc、yc、zc是像素点(u,v)在相机坐标系下的坐标，d是像素点(u,v)处的深度距离，dx、dy、u0、v0和f是相机内参。
[0019]
步骤6、将场景点云通过体素大小为v
size
＝(wv,hv,dv)的滤波器进行下采样，再估计点云表面法向量。
[0020]
步骤7、随机生成场景点云的一系列参考点qi，每个参考点qi和场景点云构成点对特征fs(qi,qj)。设置ppf法线角度和距离的筛选阈值θ
t
和d
t
，通过对步骤1中建立的哈希表进行搜索，在阈值内寻找与场景点对特征fs(qi,qj)相似的模型点对特征fm(pm,pn)，每一对相似的fm(pm,pn)和fs(qi,qj)中qj和pn的转换关系可以表示如下：
[0021][0022]
其中，t
qi
和t
pm
分别是将点qi和pm转换到相机坐标系原点的变换矩阵，α是将qi和pm变换到原点，并使其法向量对齐x轴后向量和间的夹角，r(α)是旋转角度为α的变换矩阵。
[0023]
步骤8、使用投票策略方法对每一个参考点qi所得到的多个位姿进行投票，得票最高的位姿作为该参考点的位姿结果。对所有参考点的位姿结果采用位姿聚类，对每个聚类进行评分，单个聚类分数等于其中所有位姿投票数的总和，将得分最高聚类中位姿的平均值作为最终结果。
[0024]
步骤9、使用icp算法对步骤8中的位姿结果进行修正，设定目标函数为：
[0025][0026]
其中，n为点对总数，pi和qi为场景点云和模型点云中的邻近点对。设置阈值f
t
，当目标函数值小于阈值f
t
时，则接受此时的修正结果r和t，否则继续迭代。
[0027]
步骤10、将初始最佳抓取位姿t0通过式(7)变换得到当前环境下的最佳抓取位姿，向机器人控制器发送控制命令，完成抓取。
[0028][0029]
其中，t0为模型初始最佳抓取位姿，r和t为经过icp修正后的旋转、平移矩阵。作为优选，步骤4计算出像素框中心点的深度值dc，设置阈值d
t
＝d
c-d0，d0为常数；计算四条边界框上的像素点的深度值，如果某一条边框上存在深度值小于设定的阈值d
t
的像素点，则将该条边框向外移动n个像素点，直到四条边框上的像素点的深度值均大于设定的阈值。
[0030]
本发明的实质性效果在于：本发明利用yolo-v5算法快速寻找到被抓取物体范围，用该范围限制场景点云数据，剔除环境中大部分无关物体的点云数据，极大地提高了位姿估计的精度。同时，相比较使用全部场景点云进行位姿估计而言，本发明所使用的方法大大地减少了位姿估计计算时间。
附图说明
[0031]
图1为本发明的离线阶段模型数据库建立流程图；
[0032]
图2为本发明的在线阶段抓取流程图。
具体实施方式
[0033]
下面结合具体实施例对本发明做进一步的分析。
[0034]
以realsense d435i深度相机结合图2为例，基于深度相机的抓取方法实施步骤如下：
[0035]
步骤(1)、如图1所示，离线阶段建立目标物体的模型数据库。利用已知目标物体的3d模型获得点云数据和初始最佳抓取位姿t0，将点云数据通过体素大小为v
size
＝(wv,hv,dv)的滤波器进行下采样，再提取点云数据的点对特征(point pair feature，ppf)，将相似的ppf放在同一组并存入哈希表中，每一对ppf可以表示为：
[0036]fm
(pm,pn)＝(df,θ1,θ2,θ3)
ꢀꢀꢀ
(1)
[0037]
其中，pm和pn是点云数据中的任意两个点，df是点pm与点pn之间的欧式距离，θ1和θ2分别为点pm与点pn法线和这两个点构成向量之间的夹角，θ3为点pm与点pn法线之间的夹角。
[0038]
步骤(2)、通过深度相机获取当前环境下的rgb图像数据和深度图像数据，采用yolo-v5目标检测算法，检测当前环境中的目标物体，获得其在rgb图像中的像素范围：
[0039]
pixel
obj
＝{(x
obj
,y
obj
)|(x
left
,y
left
)＜(x
obj
,y
obj
)＜(x
right
,y
right
)}
ꢀꢀꢀ
(2)
[0040]
其中，(x
obj
,y
obj
)为目标物体的像素点，(x
left
,y
left
)和(x
right
,y
right
)分别像素框的左上像素点和右下像素点。
[0041]
步骤(3)、由于深度相机的彩色摄像头和红外传感器位置不同，导致原始的rgb图像和深度图像存在偏差，利用式(3)将深度图像和rgb图像进行对齐。
[0042][0043]
其中，p
rgb
和p
ir
为在rgb图像和深度图像中相对应的像素点，i
rgb
和i
ir
分别为rgb相机和红外相机内参数矩阵，e
rgb
、e
ir
、t
rgb
和t
ir
为rgb相机和红外相机的外参数。
[0044]
步骤(4)、将步骤(2)中得到的像素框映射到对齐后的深度图像中，由于检测的边界框和实际物体边界可能存在偏差，需要在深度图像中对像素框范围进行修正。计算出像
素框中心点的深度值dc，设置阈值d
t
＝d
c-d0，d0为常数。计算四条边界框上的像素点的深度值，如果某一条边框上存在深度值小于设定的阈值d
t
的像素点，则将该条边框向外移动n个像素点，直到四条边框上的像素点的深度值均大于设定的阈值。
[0045]
步骤(5)、通过式(4)计算步骤(4)中纠正后像素框中所有像素点在相机坐标系下的坐标，获得场景点云数据。
[0046][0047]
其中，xc、yc、zc是像素点(u,v)在相机坐标系下的坐标，d是像素点(u,v)处的深度距离，dx、dy、u0、v0和f是相机内参。
[0048]
步骤(6)、将场景点云通过体素大小为v
size
＝(wv,hv,dv)的滤波器进行下采样，再估计点云表面法向量。
[0049]
步骤(7)、随机生成场景点云的一系列参考点qi，每个参考点qi和场景点云构成点对特征fs(qi,qj)。设置ppf法线角度和距离的筛选阈值θ
t
和d
t
，通过对步骤(1)中建立的哈希表进行搜索，在阈值内寻找与场景点对特征fs(qi,qj)相似的模型点对特征fm(pm,pn)，每一对相似的fm(pm,pn)和fs(qi,qj)中qj和pn的转换关系可以表示如下：
[0050][0051]
其中，t
qi
和t
pm
分别是将点qi和pm转换到相机坐标系原点的变换矩阵，α是将qi和pm变换到原点，并使其法向量对齐x轴后向量和间的夹角，r(α)是旋转角度为α的变换矩阵。
[0052]
步骤(8)、使用投票策略方法对每一个参考点qi所得到的多个位姿进行投票，得票最高的位姿作为该参考点的位姿结果。对所有参考点的位姿结果采用位姿聚类，对每个聚类进行评分，单个聚类分数等于其中所有位姿投票数的总和，将得分最高聚类中位姿的平均值作为最终结果。
[0053]
步骤(9)、使用icp算法对步骤(8)中的位姿结果进行修正，设定目标函数为：
[0054][0055]
其中，m为点对总数，pi和qi为场景点云和模型点云中的邻近点对。设置阈值f
t
，当目标函数值小于阈值f
t
时，则接受此时的修正结果r和t，否则继续迭代。
[0056]
步骤(10)、将初始最佳抓取位姿t0通过式(7)变换得到当前环境下的最佳抓取位姿，向机器人控制器发送控制命令，完成抓取。
[0057][0058]
其中，t0为模型初始最佳抓取位姿，r和t为经过icp修正后的旋转、平移矩阵。