CN110342153A - 一种基于三维点云的垃圾桶识别抓取方法 - Google Patents

Abstract

本发明公开了一种基于三维点云的垃圾桶识别抓取方法，属于计算机视觉与自动化领域。首先在传统挂桶垃圾车上增加三维点云激光雷达模块、将液压装置替换为机械臂执行单元。然后车载三维激光雷达扫描获得各扫描点的距离和角度信息，并将极坐标信息转换为激光雷达直角坐标系下的三维坐标点信息，并整合形成点云数据发送到数据处理单元。数据处理单元根据点云模型计算出垃圾桶和机械臂之间的相对位置关系，结合垃圾桶与机械臂之间的目标位置，将两个位置间的误差信息传递给控制解算器，产生控制信号传递给机械臂。最后机械臂准确到达给定的目标端点位置，完成垃圾桶抓取、垃圾的倾倒以及垃圾桶归位。本发明环境适应性强，提高了系统的可靠性及安全性。

Description

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

技术领域

本发明属于计算机视觉与自动化领域，具体是一种基于三维点云的垃圾桶识别抓取方法。

背景技术

生活垃圾从被投放开始，经收集被运至中转站或处理场的过程，是垃圾的收集过程。收集过程直接影响着居民环境的好坏，我国投入环保的力度不断加大，基础环卫设施越来越完善，生活垃圾转运系统也初具规模。

目前垃圾桶仍是最主要的接收和存储垃圾的容器，其形状和大小符合国际通用标准。而垃圾桶的收集目前大多有两种方式：1、由人力倾倒至垃圾车内。2、使用挂桶垃圾车(自装卸式垃圾车)完成收集。

挂桶垃圾车采用液压装置完成举升、开盖、合盖以及放置过程，一台车能配几十个垃圾桶，大大提高了垃圾的收集运输能力。但是，目前国内使用的挂桶垃圾车依旧需要靠工作人员将垃圾桶移至液压举升装置并进行固定后，操纵控制杆才能完成垃圾的倾倒收集工作。因此一台挂桶垃圾车通常配备一名司机和一名操作员。这种垃圾收集车虽然在一定程度上提高了垃圾的收集效率，但仍需要过多的人工参与。

因此，迫切需要一种能够自动识别、定位垃圾桶并完成抓取操作的垃圾收集车，实现垃圾收集的自动化操作。

发明内容

针对上述问题，本发明提出一种基于三维点云的垃圾桶识别抓取方法，以计算机视觉为基础，对传统挂桶垃圾车进行升级改造：增加三维点云激光雷达、将液压装置替换为可精准控制的机械臂；使传统挂桶垃圾车能够自动识别车辆附近的垃圾桶，驾驶员能够一键完成垃圾桶抓取和垃圾倾倒收集操作。

具体步骤如下：

步骤一、在传统挂桶垃圾车的基础上增加三维点云激光雷达模块、将液压装置替换为机械臂执行单元。

所述三维点云激光雷达模块包括三维激光雷达和数据处理单元；机械臂执行单元包括机械臂和控制解算器；

三维激光雷达安装在垃圾车侧面，与实现垃圾桶抓取功能的机械臂共轴，在所属的角度范围内进行实时扫描，产生的点云数据传输给数据处理单元，数据处理单元对垃圾桶进行建模和匹配识别，以获取精确的垃圾桶与机械臂之间的相对位置关系，作为控制信号传递给机械臂执行单元，机械臂执行单元控制机械臂对垃圾桶进行抓取、垃圾倾倒以及垃圾桶自动放回等操作。

机械臂采用连杆式折叠升降机构，在实现机构末端长距离水平伸出的同时，保证收缩状态下的较小的结构空间，以适配传统垃圾车液压升降装置的安装空间。

控制解算器采用双闭环形式，输入垃圾桶和机械臂之间的实际位置与目标位置的误差，经过结算得到机械臂的实际执行位置，转换成相应的控制信号传递给机械臂，实现精确控制。

步骤二、垃圾车行驶到垃圾桶附近，车载三维激光雷达扫描获得各扫描点的距离和角度信息，并将极坐标信息转换为激光雷达直角坐标系下的三维坐标点信息；

针对某个扫描点A，坐标转换公式如下：

式中，(x,y,z)为激光雷达直角坐标系下扫描点A的三维坐标点；ρ为扫描点A距离激光雷达中心的直线距离；α为该扫描点A所在扫描线的俯仰角，即垂直方向的角度；θ为该扫描点A所在扫描线的水平方向的航向角。

步骤三、将各个扫描点直角坐标系下的三维坐标整合到一起形成点云数据，发送到数据处理单元。

步骤四、数据处理单元提取点云数据，根据标准垃圾桶的点云模型识别出垃圾桶，并计算出垃圾桶和机械臂之间的相对位置关系；

具体过程为：

步骤401、采用下采样的方法对点云数据进行预处理去除离群点；

步骤402、根据欧氏距离将预处理后的点云数据进行分类；

分割方法如下：

首先，在点云数据中随机选取一点A₁，根据欧式距离找到距离它最近的点；

然后，判断该距离是否小于设定的阈值，如果是，将点A₁归为I类，否则归为其他类。

继续在点云数据中选择未被选取的点，重复上述过程进行与点A₁之间距离的判断，直到I类中没有新的点加入，则完成I类的搜索。

在其他类中，随机选择取一点B₁，根据欧式距离找到距离它最近的点；重复上述过程进行距离的判断，形成II类；

继续重复上述过程，形成III、IV…类等，直到每个点分类完成。

步骤403、针对各类点云，使用点特征直方图(PFH)完成每类点云中各点的特征提取，并将所有点的特征合并到集合P中。

具体为：

首先，针对某类点云，采用分析协方差矩阵的特征向量和特征值的方法，计算该点云中每个点的表面法线。

对于查询点p_i，对应的协方差矩阵为：

其中k是点p_i的近邻点个数，是近邻点的三维质心，则矩阵C的特征向量即是点p_i的表面法线。

然后，对于点p_i，利用邻域半径r内的近邻点p_j，i≠j，计算点p_i表面法线的坐标轴(u,v,w)；

邻域半径r的选取要综合特征精度和三位雷达的扫描精度，半径r越小计算的特征越多，计算量越大。

则坐标轴(u,v,w)的计算公式如下：

n_i为点p_i对应的表面法线向量，u轴方向与点p_i的表面法线方向一致，v轴方向为点p_i和p_j形成的向量与u轴向量叉乘的方向，w轴为u轴向量和v轴向量叉乘的方向。

然后，针对查询点p_i，利用表面法线的坐标轴(u,v,w)和近邻点p_j，计算点p_i的PFH特征三要素(α,φ,θ)，也就是点p_i表面法线的角度变换；并在直方图中表示出来。

计算公式如下：

n_j为点p_j对应的表面法线向量。

步骤404、将集合P中各类点云的特征与标准垃圾桶的源点云集合Q中各点的特征，采用迭代最近点算法(ICP)进行匹配，识别出垃圾桶。

标准垃圾桶的点云模型中，将源点云集合Q的质心设置在雷达直角坐标系的原点位置，通过ICP算法配准，得到两个三维模型之间的变换矩阵，即模型之间的旋转平移信息。

具体为：

首先、在待匹配的初始点集P中依次选择各点，同时在源点云集Q中选择与点集P中各点对应的最近点，组成最邻近点对。

然后、利用所有最邻近点对，计算两个点集的对齐配准转换参数R和T，使得目标误差函数最小。

误差函数为：

其中，n为最邻近点对的个数，R为旋转矩阵，T为平移向量；

最后、将初始的待匹配点集P按照旋转矩阵R和平移向量T进行变换，得到新的待匹配点集P'；判断新的待匹配点集P'与源点集Q之间的平均距离是否小于给定阈值，如果是，则停止迭代计算，完成垃圾桶的识别。否则，将新的待匹配点集P'替代初始点集P，重复上述步骤继续迭代，直到达到要求。

步骤405、根据停止迭代时对应的旋转矩阵R和平移向量T，得到垃圾桶质心坐标系与三维激光雷达坐标系各轴之间的旋转角度和平移关系，进而得到与三维激光雷达同轴的机械臂与垃圾桶之间的旋转角度和平移关系。

步骤五、根据垃圾桶和机械臂之间的实际相对位置信息，以及垃圾桶与机械臂之间的目标位置，两个位置之间的误差信息传递给控制解算器，产生控制信号传递给机械臂。

具体过程为：将机械臂与垃圾桶之间的旋转角度和平移距离作为实际相对位置信息，与目标位置之间的误差输入控制解算器中，计算出机械臂的实际执行位置后，将控制信号输出给机械臂。

步骤六、机械臂准确到达给定的目标端点位置，同时机械臂末端装配的接触传感器检测到与垃圾桶接触时自动完成抓取、锁紧操作。

步骤七、记录目标端点位置，完成垃圾桶倾倒之后将垃圾桶归位。

本发明的优点在于：

1、一种基于三维点云的垃圾桶识别抓取方法，采用三维激光雷达对周围环境扫描、建模完成对垃圾桶的识别和定位，能够快速确定垃圾桶和垃圾收集车的相对位置关系，环境适应性强。

2、一种基于三维点云的垃圾桶识别抓取方法，采用可精确控制的机械臂完成抓取，能够保证机械臂有较高的位置控制精度。

3、一种基于三维点云的垃圾桶识别抓取方法，用户显示界面保证了抓取操作的可视化，同时在特殊环境下，操作人员可以及时停止挂载操作，提高了系统的可靠性及安全性。

4、一种基于三维点云的垃圾桶识别抓取方法，利用三维激光雷达完成对垃圾桶的识别与定位，并配合高精度的机械臂完成抓取操作，能完成一键抓取操作，极大地提高了自动垃圾收集车的工作效率。

附图说明

图1是本发明一种基于三维点云的垃圾桶识别抓取方法的流程图；

图2是本发明一种基于三维点云的垃圾桶识别抓取装置的示意图；

图3是本发明一种基于三维点云的垃圾桶识别抓取方法机械臂抓取示意图；

图4是本发明数据处理单元提取点云数据识别出垃圾桶，并计算相对位置关系的流程图；

具体实施方式

下面结合实施例和附图，对本发明的实施方式做详细、清楚的描述。

本发明一种基于三维点云的垃圾桶识别抓取方法，如图1所示，具体步骤如下：

步骤一、在传统挂桶垃圾车的基础上增加三维激光雷达模块、将液压装置替换为机械臂抓取模块以及安装综合显示控制模块。

如图2所示，所述三维激光雷达模块包括三维激光雷达和数据处理单元；机械臂抓取模块包括机械臂和控制解算器；综合显示控制模块由高清摄像机、用户显示界面与人机接口构成。

三维激光雷达模块用来对特定角度范围内的机械臂周围环境进行扫描、建模，产生的点云数据由数据处理单元进行处理，在背景中识别出垃圾桶，并根据点云数据计算出垃圾桶和机械臂之间的相对位置关系，作为控制信号传递给机械臂抓取模块，经由综合显示控制模块完成一键抓取操作。

机械臂抓取模块用来抓取垃圾桶，位置控制精确度高，给定目标端点位置后能够准确到达，且最大提升重量满足所需最大重量。根据表1，额定载荷按照垃圾平均比重取0.4×103kg/m3计算可得到两种垃圾桶的额定载荷分别为48kg和96kg，因此抓取装置的额定载荷应该不低于96kg。

表1

如图3所示。同时在机械臂末端装配接触传感器，当机械臂接触到垃圾桶时自动完成抓取、锁紧操作。

综合显示控制系统实时显示抓取过程和设备状态信息，驾驶员能够一键完成机械臂的抓取操作，同时设置紧急停止按钮，在特殊情况下，能及时停止作业。

三维激光雷达安装在垃圾车侧面，与实现垃圾桶抓取功能的机械臂共轴；

机械臂采用连杆式折叠升降机构，在实现机构末端长距离水平伸出的同时，保证收缩状态下的较小的结构空间，以适配传统垃圾车液压升降装置的安装空间。

控制解算器采用双闭环形式，输入垃圾桶和机械臂之间的实际位置与目标位置的误差，经过结算得到机械臂的实际执行位置，转换成相应的控制信号传递给机械臂，实现精确控制。

步骤二、垃圾车行驶到垃圾桶附近，车载三维激光雷达扫描获得各扫描点的距离和角度信息，并将极坐标信息转换为激光雷达直角坐标系下的三维坐标点信息；

三维激光雷达相比深度相机环境适应性更强，在光线不充足、强光下均能正常工作。

针对某个扫描点A，坐标转换公式如下：

式中，(x,y,z)为激光雷达直角坐标系下扫描点A的三维坐标点；ρ为扫描点A距离激光雷达的距离；α为该扫描点A所在扫描线的俯仰角，即垂直方向的角度；θ为该扫描点A所在扫描线的水平方向的航向角。

步骤三、将各个扫描点直角坐标系下的三维坐标整合到一起形成点云数据，发送到数据处理单元。

步骤四、数据处理单元提取点云数据，根据标准垃圾桶的点云模型识别出垃圾桶，并计算出垃圾桶和机械臂之间的相对位置关系；

如图4所示，具体过程为：

步骤401、采用下采样的方法对点云数据进行预处理；

在点云数量较多的情况下采用下采样的方法降低点云数量，提高运算速度；

预处理包括：利用条件滤波或半径滤波去除离群点，同时删除点云数据中在滤波半径为0.2米内没有达到邻近的数据点。

步骤402、根据欧氏距离将预处理后的点云数据进行分类；

分割方法如下：

首先，在点云数据中随机选取一点A₁，根据欧式距离找到距离它最近的点；

然后，判断该距离是否小于设定的阈值，如果是，将点A₁归为I类，否则归为其他类。

继续在点云数据中选择未被选取的点，重复上述过程进行与点A₁之间距离的判断，直到I类中没有新的点加入，则完成I类的搜索。

在其他类中，随机选择取一点B₁，根据欧式距离找到距离它最近的点；重复上述过程进行距离的判断，形成II类；

继续重复上述过程，形成III、IV…类等，直到每个点分类完成。

步骤403、针对各类点云，使用点特征直方图(PFH)完成每类点云中各点的特征提取，并将所有点的特征合并到集合P中。

具体为：

首先，针对某类点云，采用分析协方差矩阵的特征向量和特征值的方法，计算该点云中每个点的表面法线。

对于查询点p_i，对应的协方差矩阵为：

其中k是点p_i的近邻点个数，是近邻点的三维质心，则矩阵C的特征向量即是点p_i的表面法线。

然后，对于点p_i，确定邻域半径r，以该点为原点，选择半径r内的近邻点p_j，i≠j，计算点p_i表面法线的坐标轴(u,v,w)；

邻域半径r的选取要综合特征精度和三位雷达的扫描精度，半径r越小计算的特征越多，计算量越大。

则坐标轴(u,v,w)的计算公式如下：

n_i为点p_i对应的表面法线向量，u轴方向与点p_i的表面法线方向一致，v轴方向为点p_i和p_j形成的向量与u轴向量叉乘的方向，w轴为u轴向量和v轴向量叉乘的方向。

然后，针对查询点p_i，利用表面法线的坐标轴(u,v,w)和近邻点p_j，计算点p_i的PFH特征三要素(α,φ,θ)，也就是点p_i表面法线的角度变换；并在直方图中表示出来生成最终的每个查询点的PFH特征。

计算公式如下：

n_j为点p_j对应的表面法线向量。

步骤404、将集合P中各类点云的特征与标准垃圾桶的源点云集合Q中各点的特征，采用迭代最近点算法(ICP)进行匹配，识别出垃圾桶。

标准垃圾桶的点云模型中，将源点云集合Q的质心设置在雷达直角坐标系的原点位置，通过ICP算法配准，得到两个三维模型之间的变换矩阵，即模型之间的旋转平移信息。

具体为：

首先、在待匹配的初始点集P中依次选择各点，同时在源点云集Q中选择与点集P中各点对应的最近点，组成最邻近点对。

针对点集P中的某点P_i，源点云集Q中选择点Q_i，且点Q_i是源点云集Q中与点P_i对应的最近点；

然后、利用所有最邻近点对，计算两个点集的对齐配准转换参数R和T，使得目标误差函数最小。

误差函数为：

其中，n为最邻近点对的个数，R为旋转矩阵，T为平移向量，s_xs_ys_z分别为各点云绕x,y,z对应的正弦值；c_xc_yc_z分别为各点云绕x,y,z对应的余弦值；X,Y,Z分别为在x,y,z轴方向上的平移距离。

最后、将初始的待匹配点集P按照旋转矩阵R和平移向量T进行变换，得到新的待匹配点集P'；判断新的待匹配点集P'与源点集Q之间的平均距离是否小于给定阈值，如果是，则停止迭代计算，完成垃圾桶的识别。否则，将新的待匹配点集P'替代初始点集P，重复上述步骤继续迭代，直到达到要求。

步骤405、根据停止迭代时对应的旋转矩阵R和平移向量T，得到垃圾桶质心坐标系与三维激光雷达坐标系各轴之间的旋转角度和平移关系，进而得到与三维激光雷达同轴的机械臂与垃圾桶之间的旋转角度和平移关系。

步骤五、根据垃圾桶和机械臂之间的实际相对位置信息，以及垃圾桶与机械臂之间的目标位置，两个位置之间的误差信息传递给控制解算器，产生控制信号传递给机械臂。

具体过程为：将机械臂与垃圾桶之间的旋转角度和平移距离作为实际相对位置信息，与目标位置之间的误差输入控制解算器中，计算出机械臂的实际执行位置后，将控制信号输出给机械臂。

步骤六、机械臂准确到达给定的目标端点位置，同时机械臂末端装配的接触传感器检测到与垃圾桶接触时自动完成抓取、锁紧操作。

步骤七、记录目标端点位置，完成垃圾桶倾倒之后将垃圾桶归位。

利用高清摄像机实时传回抓取过程，同时设置紧急停止按钮，避免特殊情况。用户显示界面所显示的信息包括抓取过程的实时画面以及机械臂的状态信息。

Claims (5)

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中所述的分类方法如下：

首先，在点云数据中随机选取一点A₁，根据欧式距离找到距离它最近的点；

然后，判断该距离是否小于设定的阈值，如果是，将点A₁归为I类，否则归为其他类；

继续在点云数据中选择未被选取的点，重复上述过程进行与点A₁之间距离的判断，直到I类中没有新的点加入，则完成I类的搜索；

在其他类中，随机选择取一点B₁，根据欧式距离找到距离它最近的点；重复上述过程进行距离的判断，形成II类；

继续重复上述过程，形成III、IV…类，直到每个点分类完成。

3.如权利要求1所述的一种基于三维点云的垃圾桶识别抓取方法，其特征在于，所述的步骤403具体为：

首先，针对某类点云，采用分析协方差矩阵的特征向量和特征值的方法，计算该点云中每个点的表面法线；

对于查询点p_i，对应的协方差矩阵为：

其中k是点p_i的近邻点个数，是近邻点的三维质心，则矩阵C的特征向量即是点p_i的表面法线；

然后，对于点p_i，利用邻域半径r内的近邻点p_j，i≠j，计算点p_i表面法线的坐标轴(u,v,w)；

邻域半径r的选取要综合特征精度和三位雷达的扫描精度，半径r越小计算的特征越多，计算量越大；

则坐标轴(u,v,w)的计算公式如下：

n_i为点p_i对应的表面法线向量，u轴方向与点p_i的表面法线方向一致，v轴方向为点p_i和p_j形成的向量与u轴向量叉乘的方向，w轴为u轴向量和v轴向量叉乘的方向；

然后，针对查询点p_i，利用表面法线的坐标轴(u,v,w)和近邻点p_j，计算点p_i的PFH特征三要素(α,φ,θ)，也就是点p_i表面法线的角度变换；并在直方图中表示出来；

计算公式如下：

n_j为点p_j对应的表面法线向量。

4.如权利要求1所述的一种基于三维点云的垃圾桶识别抓取方法，其特征在于，所述的步骤404具体为：

首先、在待匹配的初始点集P中依次选择各点，同时在源点云集Q中选择与点集P中各点对应的最近点，组成最邻近点对；

然后、利用所有最邻近点对，计算两个点集的对齐配准转换参数R和T，使得目标误差函数最小；

误差函数为：

其中，n为最邻近点对的个数，R为旋转矩阵，T为平移向量；

最后、将初始的待匹配点集P按照旋转矩阵R和平移向量T进行变换，得到新的待匹配点集P'；判断新的待匹配点集P'与源点集Q之间的平均距离是否小于给定阈值，如果是，则停止迭代计算，完成垃圾桶的识别；否则，将新的待匹配点集P'替代初始点集P，重复上述步骤继续迭代，直到达到要求。

5.如权利要求1所述的一种基于三维点云的垃圾桶识别抓取方法，其特征在于，所述的步骤五具体过程为：将机械臂与垃圾桶之间的旋转角度和平移距离作为实际相对位置信息，与目标位置之间的误差输入控制解算器中，计算出机械臂的实际执行位置后，将控制信号输出给机械臂。