详情

原文传递一种基于三维点云的垃圾桶识别抓取方法

专利名称：	一种基于三维点云的垃圾桶识别抓取方法
摘要：	本发明公开了一种基于三维点云的垃圾桶识别抓取方法，属于计算机视觉与自动化领域。首先在传统挂桶垃圾车上增加三维点云激光雷达模块、将液压装置替换为机械臂执行单元。然后车载三维激光雷达扫描获得各扫描点的距离和角度信息，并将极坐标信息转换为激光雷达直角坐标系下的三维坐标点信息，并整合形成点云数据发送到数据处理单元。数据处理单元根据点云模型计算出垃圾桶和机械臂之间的相对位置关系，结合垃圾桶与机械臂之间的目标位置，将两个位置间的误差信息传递给控制解算器，产生控制信号传递给机械臂。最后机械臂准确到达给定的目标端点位置，完成垃圾桶抓取、垃圾的倾倒以及垃圾桶归位。本发明环境适应性强，提高了系统的可靠性及安全性。
专利类型：	发明专利
国家地区组织代码：	北京;11
申请人：	齐霄强
发明人：	齐霄强;赵凯;宋佳
专利状态：	有效
申请日期：	2019-07-29T00:00:00+0800
发布日期：	2019-10-18T00:00:00+0800
申请号：	CN201910690105.7
公开号：	CN110342153A
代理机构：	北京永创新实专利事务所
代理人：	冀学军
分类号：	B65F3/02(2006.01);B;B65;B65F;B65F3
申请人地址：	100000 北京市房山区长阳镇中粮万科长阳半岛小区1号院2号楼1504室
主权项：	1.一种基于三维点云的垃圾桶识别抓取方法，其特征在于，具体步骤如下：步骤一、在传统挂桶垃圾车的基础上增加三维点云激光雷达模块、将液压装置替换为机械臂执行单元；所述三维点云激光雷达模块包括三维激光雷达和数据处理单元；机械臂执行单元包括机械臂和控制解算器；三维激光雷达安装在垃圾车侧面，与实现垃圾桶抓取功能的机械臂共轴，在所属的角度范围内进行实时扫描，产生的点云数据传输给数据处理单元，数据处理单元对垃圾桶进行建模和匹配识别，以获取精确的垃圾桶与机械臂之间的相对位置关系，作为控制信号传递给机械臂执行单元，机械臂执行单元控制机械臂对垃圾桶进行抓取、垃圾倾倒以及垃圾桶自动放回；机械臂采用连杆式折叠升降机构，在实现机构末端长距离水平伸出的同时，保证收缩状态下的较小的结构空间，以适配传统垃圾车液压升降装置的安装空间；控制解算器采用双闭环形式，输入垃圾桶和机械臂之间的实际位置与目标位置的误差，经过解算得到机械臂的实际执行位置，转换成相应的控制信号传递给机械臂，实现精确控制；步骤二、垃圾车行驶到垃圾桶附近，车载三维激光雷达扫描获得各扫描点的距离和角度信息，并将极坐标信息转换为激光雷达直角坐标系下的三维坐标点信息；针对某个扫描点A，坐标转换公式如下：式中，(x,y,z)为激光雷达直角坐标系下扫描点A的三维坐标点；ρ为扫描点A距离激光雷达中心的直线距离；α为该扫描点A所在扫描线的俯仰角，即垂直方向的角度；θ为该扫描点A所在扫描线的水平方向的航向角；步骤三、将各个扫描点直角坐标系下的三维坐标整合到一起形成点云数据，发送到数据处理单元；步骤四、数据处理单元提取点云数据，根据标准垃圾桶的点云模型识别出垃圾桶，并计算出垃圾桶和机械臂之间的相对位置关系；具体过程为：步骤401、采用下采样的方法对点云数据进行预处理去除离群点；步骤402、根据欧氏距离将预处理后的点云数据进行分类；步骤403、针对各类点云，使用点特征直方图(PFH)完成每类点云中各点的特征提取，并将所有点的特征合并到集合P中；步骤404、将集合P中各类点云的特征与标准垃圾桶的源点云集合Q中各点的特征，采用迭代最近点算法(ICP)进行匹配，识别出垃圾桶；标准垃圾桶的点云模型中，将源点云集合Q的质心设置在雷达直角坐标系的原点位置，通过ICP算法配准，得到两个三维模型之间的变换矩阵，即模型之间的旋转平移信息；步骤405、根据停止迭代时对应的旋转矩阵R和平移向量T，得到垃圾桶质心坐标系与三维激光雷达坐标系各轴之间的旋转角度和平移关系，进而得到与三维激光雷达同轴的机械臂与垃圾桶之间的旋转角度和平移关系；步骤五、根据垃圾桶和机械臂之间的实际相对位置信息，以及垃圾桶与机械臂之间的目标位置，两个位置之间的误差信息传递给控制解算器，产生控制信号传递给机械臂；步骤六、机械臂准确到达给定的目标端点位置，同时机械臂末端装配的接触传感器检测到与垃圾桶接触时自动完成抓取、锁紧操作；步骤七、记录目标端点位置，完成垃圾桶倾倒之后将垃圾桶归位。 2.如权利要求1所述的一种基于三维点云的垃圾桶识别抓取方法，其特征在于，步骤402中所述的分类方法如下：首先，在点云数据中随机选取一点A1，根据欧式距离找到距离它最近的点；然后，判断该距离是否小于设定的阈值，如果是，将点A1归为I类，否则归为其他类；继续在点云数据中选择未被选取的点，重复上述过程进行与点A1之间距离的判断，直到I类中没有新的点加入，则完成I类的搜索；在其他类中，随机选择取一点B1，根据欧式距离找到距离它最近的点；重复上述过程进行距离的判断，形成II类；继续重复上述过程，形成III、IV…类，直到每个点分类完成。 3.如权利要求1所述的一种基于三维点云的垃圾桶识别抓取方法，其特征在于，所述的步骤403具体为：首先，针对某类点云，采用分析协方差矩阵的特征向量和特征值的方法，计算该点云中每个点的表面法线；对于查询点pi，对应的协方差矩阵为：其中k是点pi的近邻点个数，是近邻点的三维质心，则矩阵C的特征向量即是点pi的表面法线；然后，对于点pi，利用邻域半径r内的近邻点pj，i≠j，计算点pi表面法线的坐标轴(u,v,w)；邻域半径r的选取要综合特征精度和三位雷达的扫描精度，半径r越小计算的特征越多，计算量越大；则坐标轴(u,v,w)的计算公式如下： ni为点pi对应的表面法线向量，u轴方向与点pi的表面法线方向一致，v轴方向为点pi和pj形成的向量与u轴向量叉乘的方向，w轴为u轴向量和v轴向量叉乘的方向；然后，针对查询点pi，利用表面法线的坐标轴(u,v,w)和近邻点pj，计算点pi的PFH特征三要素(α,φ,θ)，也就是点pi表面法线的角度变换；并在直方图中表示出来；计算公式如下： nj为点pj对应的表面法线向量。 4.如权利要求1所述的一种基于三维点云的垃圾桶识别抓取方法，其特征在于，所述的步骤404具体为：首先、在待匹配的初始点集P中依次选择各点，同时在源点云集Q中选择与点集P中各点对应的最近点，组成最邻近点对；然后、利用所有最邻近点对，计算两个点集的对齐配准转换参数R和T，使得目标误差函数最小；误差函数为：其中，n为最邻近点对的个数，R为旋转矩阵，T为平移向量；最后、将初始的待匹配点集P按照旋转矩阵R和平移向量T进行变换，得到新的待匹配点集P'；判断新的待匹配点集P'与源点集Q之间的平均距离是否小于给定阈值，如果是，则停止迭代计算，完成垃圾桶的识别；否则，将新的待匹配点集P'替代初始点集P，重复上述步骤继续迭代，直到达到要求。 5.如权利要求1所述的一种基于三维点云的垃圾桶识别抓取方法，其特征在于，所述的步骤五具体过程为：将机械臂与垃圾桶之间的旋转角度和平移距离作为实际相对位置信息，与目标位置之间的误差输入控制解算器中，计算出机械臂的实际执行位置后，将控制信号输出给机械臂。
所属类别：	发明专利