CN106842216B - 一种基于Kinect与三维激光协同的工件位姿在线检测方法 - Google Patents

Abstract

本发明公开了一种基于Kinect传感器与三维激光测距系统协同的工件位姿在线检测方法，应用在人机协作交互系统以及工业机器人对工作物件位姿感知的技术领域。其使用Kinect传感器与三维激光测距系统之间协同工作进行数据采集，该方法通过对获取点云的预处理、Kinect传感器与三维激光测距系统之间的联合标定和Kinect传感器与三维激光测距系统的在线检测三个部分，解决了Kinect传感器点云稀疏、数据精度不够以及三维激光测距系统精度与扫描速率存在矛盾关系的问题，有助于提升工业机器人高效准确获取工作物件的精确尺寸信息和位姿信息，并提高精度及工作效率。本发明可用在工业机器人对工作环境理解等人工智能领域。

Description

一种基于Kinect与三维激光协同的工件位姿在线检测方法

技术领域

本发明属于人机协作交互系统以及工业机器人对工作物件位姿感知的技术领域，涉及到Kinect传感器与三维激光测距系统之间联合标定及协同检测，主要运用到Kinect传感器与三维激光测距系统协同对工作物件的位置和姿态的在线检测。

背景技术

工业机器人是集机械、电子、控制等多学科先进技术于一体的现代制造业重要的自动化装备，技术附加值很高并且应用范围广泛。要实现具有与人协同工作能力的新一代工业机器人系统，如何解决工业机器人对人机共享环境的建模、感知与场景理解是最根本的问题。

在复杂的工业机器人的应用场景中，单一传感器无法满足工作环境感知和场景理解等任务的需求，因而多个传感器之间的协同工作是提高工业机器人对环境感知与场景理解性能的必要手段。国内外学者已经意识到该研究领域的重要性，并开展了多传感器协同工作方向探索性的研究工作。Kinect传感器可以使用深度相机与视觉相机生成深度图像信息。(CAO Hongjun,WU Panlong,YAO Xiang，On Depth Smoothing and Fake LaserRangefinder Based on Kinect，Proceedings of the 33rd Chinese ControlConference,2014，8491-8494)Kinect传感器主要由视觉、红外发射器和红外接收器组成。Kinect传感器可以每秒30帧的速度生成深度图像信息，但其获取的点云数据相对稀疏，误差在厘米级，可以用来获取工作场景中工件粗略的位置信息和尺寸信息，却不能够准确获取工作场景中工件准确的尺寸信息和位姿信息。三维激光测距系统扫描是通过记录激光脉冲的方向及目标表面反射激光的时间来获取目标点三维坐标，(张启福，孙现申，三维激光扫描仪测量方法与前景展望，北京测绘2011年第1期)三维激光测距系统可以获取扫描物体表面的高精度点云数据集，其具有数据精度精确、测量非接触等优点。但是使用三维激光测距系统采集点云数据来构建场景的三维模型时，为了使得数据更加准确，保证对场景中的某些细节进行精确描述，所需采集的点云数据需要尽可能的多，尽可能的密集，因此精度与扫描速率存在矛盾关系。

发明内容

为了克服现有技术中存在的不足，本发明提供了一种基于Kinect传感器与三维激光测距系统协同的工件位姿在线检测方法，获取工件尺寸、位置、姿态的精确信息。Kinect传感器具有很好的实时性，可以快速的完成对场景数据的采集工作，三维激光测距系统采集的点云数据相对精细，具有很高的精度。因此，在本发明专利中将两种传感器协同使用，有效的提高了获取工作物件尺寸和位姿信息的精度和效率，并实现在线检测。如附图1所示，光学实验平台安装了Kinect传感器与三维激光测距传感器，图中1区域为Kinect传感器所能扫描的区域，2区域为三维激光测距系统所能扫描的区域，两者传感器的扫描区域均能覆盖实验平台的工作区域。

本发明主要使用Kinect传感器与三维激光测距系统，利用计算机对数据进行实时的采集及处理。使用Kinect传感器对空间中的工件进行实时的点云获取，可得到工件粗略的尺寸信息和位置信息；再将工件的初步位置检测结果转化成三维激光测距系统所要扫描的角度信息，通过CAN总线传输，使用驱动器控制电机的旋转方向和速度，将三维激光测距系统快速旋转到工件所在区域，并对其进行精确的数据获取，从而得到工件更加准确的激光点云数据。通过算法处理，可计算出工件精确的尺寸和位姿信息。

本发明的技术方案采用如下步骤：

一、数据的采集及点云的预处理

Kinect传感器和三维激光测距系统在工作时都会受到噪声的影响，这种影响无法避免，因此在使用采集到的点云数据之前，需要对其进行预处理，即去除离群点。在点云数据中指定每个点的邻域范围内，如果近邻点的数目少于一定阈值时，则称之为离群点。如附图2所示，如果取阈值数目为1时(即一定范围内的个数为1)，那么其中三角形点为离群点，将会被剔除；而取阈值数目为2时，那么图中三角形点和正方形点将被当为离群点被剔除。显然，离群点跟所设定的阈值数目大小有关。通过移除点云中的离群点，可以有效的去除部分不合理的点或者错误的孤点，对于降低点云数目和提高点云精度是很有帮助的。离群点与研究对象毫不相关，却以无用的信息形式出现，会对实验数据带来一定的影响，也会对后续的重建质量产生严重的影响，因此，对于噪声进行处理就成为一项十分重要的工作。

在计算三维点云数据之前，首先需要消除深度图像中的噪声。我们主要是基于高斯分布的方法移除离群点，对每个点的邻域进行数学统计，剔除离群点的滤波方法需要经过两步的迭代：

(1)首先，对于某个点通过k-d树查找算法得到它的k邻域点，并计算出邻域点到该点欧式距离的平均值m，计算公式如下：

其中，x、y、z均为点的坐标值。

(2)然后，计算上面得到的所有m的平均值m₁和标准差std，根据这两个参数拟合一个距离的正态分布曲线。根据所有点的分布情况确定离群点，置信区间R＝[S₁,S₂]，计算公式如下所示：

S₁＝m₁-mult×std (2)

S₂＝m₁+mult×std (3)

公式中,mult是一个常数因素。当点在区间R外时，该点被定义为离群点。本文中常数因子mult取1，即认为超过均值一倍方差范围内的点为离群点，则剔除该离群点即可。

二、Kinect传感器与三维激光测距系统的联合标定

点云数据的联合标定是将Kinect传感器和三维激光测距系统采集的点云数据，以一个初始位置作为初值，迭代求取更加精确的刚体变换。设三维激光测距系统和Kinect传感器采集的点云数据分别为P和P₁，本专利中将Kinect传感器的坐标系标定到三维激光测距系统的坐标系中，则有

P＝RP₁+t (4)

其中R为旋转矩阵，t为平移矩阵。Kinect传感器和三维激光测距系统之间坐标系的映射关系如附图3所示。

标定过程，首先对点云中的点进行配对，由P₁中的点在P中搜索出其最近的点，组成一个点对，将两个点云数据中所有的点对分别找出，点对的集合相当于进行有效计算得到新点集对，根据新得到的点集对，计算两个点集的重心，由新点集对，计算出下一步计算的旋转矩阵R和平移矩阵t，根据得到的旋转矩阵R和平移矩阵t，就可以计算点集P进行刚体变换之后的新点集P′，并计算P到P′的距离平方和，再连续计算两次距离平方和之差的绝对值作为是否达到收敛的依据，若小于阈值就停止收敛，得到最优的旋转矩阵R和平移向量t。但是此算法具有比较明显的缺陷，要求数据点云上的每一点在模型点云上都要找到对应点，为了寻找对应点，算法需要遍历模型点云上的每一点，配准速度很慢，所以一般通过采用匹配特征点之间的旋转平移矩阵，降低此步骤的时间消耗，从而提高效率。

三、Kinect传感器与三维激光测距系统协同的在线检测

针对上述三维激光测距系统和Kinect传感器的特性，设计出了三维激光测距系统与Kinect传感器协同的在线检测方法。使用Kinect传感器获取工件的点云信息，先要采集背景信息的点云数据，当有工件在工作区域中时，Kinect传感器获取实时的点云数据P，其数据点在坐标轴方向上的极值分别为x_max,y_max,z_max。按照给定边长L将空间点云划分为n个栅格，根据公式(5)可以得到n：

立体栅格化分是一种常用且有效的方法。对于空间散乱点云数据，首先求出所有数据点坐标的最大值与最小值，再根据最大值和最小值的坐标建立一个与坐标轴平行的最小长方体栅格。根据给定的划分边长，将长方体空间划分成一系列栅格，把所有点云数据归入到其对应的栅格中，并做差提取点云空间中工件的点云信息。

使用立体栅格化得到被测工件的点云后，要使用滤波算法去除杂点，得到准确的工件点云信息。之后读取工件在Kinect传感器坐标系中各个点的三维点云信息，即每个点的x、y、z值。根据点云坐标值进行分析比对，获得在水平方向上x的最大值和最小值，即x_max和x_min，以及在垂直方向上z的最大值和最小值,即z_max和z_min，并将这四个点投影到XZ坐标系中。

实验原理示意图如图4所示，A点为Kinect传感器的坐标点，B点为三维激光测距系统坐标点，C点为实验平台的边界点,D点为z_max在XZ坐标系的投影点，F点为z_min在XZ坐标系的投影点，G点为x_max在XZ坐标系的投影点，E点为x_min在XZ坐标系的投影点。

由标定实验，可以得到A、B、C三个点在三维激光测距系统坐标系中的坐标值，根据Kinect传感器与三维激光测距系统的联合标定求出的旋转平移矩阵R和t，可以得到D，E，F，G点在三维激光坐标系下的坐标值。以D点为例，

[x_D,y_D,z_D]^T＝R[x₁,y₁,z₁]^T+t (6)

根据三角形余弦定理，分别求出∠DBE,∠DBG,∠DBF,∠EBG,∠EBF,∠GBF的度数，比较得出最大的角度，即为三维激光测距系统所要扫描的工件所在的区域，在此范围内三维激光测距系统精细扫描，获取工件准确的尺寸信息和位姿信息。在△BCF中，同样根据三角形余弦定理，计算出∠CBF，则为三维激光测距系统快速旋转区域。

计算机得到三维激光测距系统快速扫描范围和精细扫描范围后，转换成控制器的控制代码(控制代码包括旋转角度和旋转速度)。三维激光测距系统采集过程包括快速扫描模式和精细扫描模式，具体过程如下：从三维激光测距系统的初始位置开始进行快速扫描模式，使激光传感器快速旋转到精细扫描区域的起始角度，则开始精细扫描模式，在此模式下激光传感器精细扫描，旋转到终止角度时，则此工作周期完成，并进入到下一个工作周期。

采集后的点云数据提取出精细扫描模式中的数据，采集的点云数据密集，能够得到工件精确的的尺寸、位置和姿态信息。

附图说明

图1是Kinect传感器与三维激光测距系统的扫描区域示意图。

图2是去除离散点原理图。

图3是Kinect传感器和三维激光测距系统坐标系联合标定示意图。

图4是Kinect传感器和三维激光测距系统角度转换示意图。

具体实施方法

为了验证本方法的有效性，本发明的具体实施方式包括三个方面，一是Kinect传感器的数据采集过程，二是将Kinect传感器的点云数据栅格化得到工件的粗略位置信息，三是根据位置信息换算到三维激光测距系统的扫描角度，进行数据的精细采集。

本方法使用的传感器是Kinect传感器和三维激光测距系统。Kinect传感器是微软公司开发的一款体感外设，可获取深度图像信息和视觉图像信息。但是Kinect传感器感知环境的最佳距离是1.2米至3.5米，误差在厘米级，但是随着感知距离的增大，误差也会增大。本发明使用的三维激光测距系统是由二维激光传感器和带有绝对编码器的电机的旋转云台构成的，旋转云台在水平面上旋转，二维激光传感器扇形扫描获取三维点云数据。三维激光测距系统的最佳感知距离是0.5米至10米，误差在毫米级，精度较高、稳定性强，但是获取信息的时间较长，因此，三维激光测距系统与Kinect传感器组合成为三维场景重构和环境感知的优秀组合。我们使用的硬件平台为配置Linux操作系统的计算机，配置环境为ROS(Robot Operating System)系统。

首先使用Kinect传感器和三维激光测距系统对环境进行数据采集，对采集到的点云数据进行预处理，并剔除离散点。之后对Kinect传感器和三维激光测距系统的点云信息进行标定，获得Kinect传感器和三维激光测距系统之间的旋转矩阵R和平移矩阵t。

实验过程中，使用Kinect传感器对工件进行信息采集，对采集到的点云数据进行立体栅格化处理，进而获取工件所在的区域信息，并且通过角度转换得到三维激光测距系统所需要旋转的角度，生成指令，通过CAN总线进行传输，以控制电机进行旋转。三维激光测距系统在工作时，快速扫描模式时三维激光测距系统电机的转速为每分钟30转，精细扫描模式时三维激光测距系统电机的转速为每分钟3转，使用此方法可以快速寻找到工件，获取工件精确的尺寸信息和位姿信息，同时实现在线的检测功能。

Claims (4)

1.一种基于Kinect传感器与三维激光测距系统协同的工件位姿在线检测方法，其特征在于，包括如下步骤：

1)数据的采集及点云的预处理

使用Kinect传感器和三维激光测距系统采集点云数据，利用滤波算法移除离群点；

2)Kinect传感器与三维激光测距系统的联合标定

通过将Kinect传感器和三维激光测距系统采集的点云数据迭代，采用点云匹配算法，得到Kinect传感器与三维激光测距系统之间的旋转矩阵R和平移向量t；

3)Kinect传感器与三维激光测距系统协同的在线检测

使用Kinect传感器获取背景的点云信息和实时的点云信息，采用立体栅格化的方法作差提取点云空间中粗略的工件点云信息；再使用滤波算法去除杂点，得到准确的工件点云信息；再读取工件在Kinect传感器坐标系中各个点的三维点云信息，获得在水平方向上x的最大值和最小值，即x_max和x_min，以及在垂直方向上z的最大值和最小值，即z_max和z_min，并将这四个点投影到三维激光测距系统坐标系的XZ平面坐标系内，得到四个影投点；

根据三维激光测距系统所在位置点，即三维激光测距系统坐标系的原点，以及三维激光测距系统坐标系的XZ平面坐标系内x值为正的实验平台边角点和上述四个投影点的坐标值，分别求出三维激光测距系统精细扫描区域以及三维激光测距系统快速旋转区域，并将其转换成控制器的控制代码；进而实现三维激光测距系统采集过程，该采集过程包括快速扫描模式和精细扫描模式，具体为从三维激光测距系统的初始位置开始进行快速扫描模式，使激光传感器快速旋转到精细扫描区域的起始角度，则开始精细扫描模式，在此模式下激光传感器精细扫描，旋转到终止角度时，则此工作周期完成，并进入到下一个工作周期；从采集后的点云数据提取出精细扫描模式中的点云数据，能够快速得到工件精确的尺寸、位置和姿态信息。

2.根据权利要求1所述的工件位姿在线检测方法，其特征在于，步骤1)所述的滤波算法为基于高斯分布的方法。

3.根据权利要求1或2所述的工件位姿在线检测方法，其特征在于，步骤2)所述的点云匹配算法为ICP匹配算法，即将Kinect传感器的坐标系标定到三维激光测距系统的坐标系：

P＝RP₁+t (1)

其中，P和P₁分别为三维激光测距系统和Kinect传感器采集的点云数据，R为旋转矩阵，t为平移矩阵。

4.根据权利要求3所述的工件位姿在线检测方法，其特征在于，步骤3)所述的立体栅格化方法为：

其中，n为栅格个数，x_max,y_max,z_max分别为数据点在Kinect传感器坐标系中坐标轴方向上的极值，L为给定边长。