[发明专利]一种基于深度相机的机械手抓取方法

权利要求书

1.一种基于深度相机的机械手抓取方法，该方法包括以下步骤：

步骤1、离线阶段建立目标物体的模型数据库；利用已知目标物体3D模型获得点云数据和初始最佳抓取位姿T₀，将点云数据通过体素大小为V_size＝(w_v,h_v,d_v)的滤波器进行下采样，再提取点云数据的点对特征PPF，将相似的PPF放在同一组并存入哈希表中，每一对PPF可以表示为：

F_m(p_m,p_n)＝(d_f,θ₁,θ₂,θ₃) (1)

其中，p_m和p_n是点云数据中的任意两个点，d_f是点p_m与点p_n之间的欧式距离，θ₁和θ₂分别为点p_m与点p_n法线和这两个点构成向量之间的夹角，θ₃为点p_m与点p_n法线之间的夹角；

步骤2、通过深度相机获取当前环境下的RGB图像数据和深度图像数据；

步骤3、由于深度相机的彩色摄像头和红外传感器位置不同，导致原始的RGB图像和深度图像存在偏差，利用式(3)将深度图像和RGB图像进行对齐；

其中，p_rgb和p_ir为在RGB图像和深度图像中相对应的像素点，I_rgb和I_ir分别为RGB相机和红外相机内参数矩阵，E_rgb、E_ir、T_rgb和T_ir为RGB相机和红外相机的外参数；

步骤4、由于检测的边界框和实际物体边界可能存在偏差，需要在深度图像中对像素框范围进行修正；将步骤2中得到的像素框映射到对齐后的深度图像中；

步骤5、通过式(4)计算步骤4中纠正后像素框中所有像素点在相机坐标系下的坐标，获得场景点云数据；

其中，x_c、y_c、z_c是像素点(u,v)在相机坐标系下的坐标，d是像素点(u,v)处的深度距离，dx、dy、u₀、v₀和f是相机内参；

步骤6、将场景点云通过体素大小为V_size＝(w_v,h_v,d_v)的滤波器进行下采样，再估计点云表面法向量；

步骤7、随机生成场景点云的一系列参考点q_i，每个参考点q_i和场景点云构成点对特征F_s(q_i,q_j)；设置PPF法线角度和距离的筛选阈值θ_t和d_t，通过对步骤1中建立的哈希表进行搜索，在阈值内寻找与场景点对特征F_s(q_i,q_j)相似的模型点对特征F_m(p_m,p_n)，每一对相似的F_m(p_m,p_n)和F_s(q_i,q_j)中q_j和p_n的转换关系可以表示如下：

其中，T_qi和T_pm分别是将点q_i和p_m转换到相机坐标系原点的变换矩阵，α是将q_i和p_m变换到原点，并使其法向量对齐x轴后向量和间的夹角，R(α)是旋转角度为α的变换矩阵；

步骤8、使用投票策略方法对每一个参考点q_i所得到的多个位姿进行投票，得票最高的位姿作为该参考点的位姿结果；对所有参考点的位姿结果采用位姿聚类，对每个聚类进行评分，单个聚类分数等于其中所有位姿投票数的总和，将得分最高聚类中位姿的平均值作为最终结果；

步骤9、使用ICP算法对步骤8中的位姿结果进行修正，设定目标函数为：

其中，N为点对总数，p_i和q_i为场景点云和模型点云中的邻近点对；设置阈值f_t，当目标函数值小于阈值f_t时，则接受此时的修正结果R和T，否则继续迭代；

步骤10、将初始最佳抓取位姿T₀通过式(7)变换得到当前环境下的最佳抓取位姿，向机器人控制器发送控制命令，完成抓取；

其中，T₀为模型初始最佳抓取位姿，R和T为经过ICP修正后的旋转、平移矩阵。