一种面向人机协作的手持物体位姿实时检测方法与流程

本发明属于人机协作交互系统以及工业机器人对工作物件位姿感知的技术领域，涉及到一种面向人机协作的手持物体位姿实时检测方法。

背景技术：

机器人技术是跨越传统工程界限发展而来的现代技术中一个相对年轻的领域，它覆盖了电气工程、机械工程、系统和工业工程、计算机科学、经济学和数学等领域。20世纪60年代，随着第一台可编程机器人的诞生，机器人领域得到了快速的发展。但从其发展历史上看，大体经过如下三个阶段。早期阶段，第一代机器人称为示教机器人，它主要通过操作者进行示教，内部程序存储信息，然后在工作时读取信息，重复示教动作；第二代机器人称为可感知外界信息的机器人，它主要通过配置各种传感器，具有一定程度的视觉、触觉、力觉等的感知能力；第三代机器人称为智能机器人，它只需要操作者告知做什么，而不用告诉它怎么做，就能完成感知、运动、推理和人机交互等动作和任务。

近年来，随着对智能机器人的深入研究，要实现具有与人协同工作能力的新一代工业机器人系统，如何解决工业机器人对人机共享环境的建模、感知与场景理解是最根本的问题，而对场景中手持物体进行实时位姿检测则是其中最为重要的部分之一。

传统的物体位姿检测方法主要是基于单目视觉，以目标物体的几何特征和摄像机成像原理为基础，实现对目标物体的姿态估计。其中，应用最为广泛的算法则为pnp算法。该算法通过3d与2d点之间的对应关系求解相机姿态，即n点投影问题。该算法在已知摄像机内参的前提条件下，通过摄像机拍摄一幅包含n个特征点的图像，根据这n个特征点在世界坐标系的位置和对应在图像坐标系之间的投影关系，计算摄像机的位置和姿态，该姿态的逆矩阵即为物体相对摄像机坐标系的姿态(贾未.基于视觉的机械臂空间目标位姿识别与抓取[d].大连理工大学,2018.)。这种位姿检测方法的优点是易操作，效率高，缺点是，在物体信息不完全(手持物体存在局部遮挡)的情况下会造成关键点缺失，无法准确得出场景中物体的姿态信息。

另外一种常用的检测方法是通过神经网络训练模型，将场景中背景和物体提前学习好，分割出场景中的目标物体，再通过点云匹配得到当前物体的位姿。这种方法的优点是准确性高，但其应用场景单一，当换到另外一个场景时，则需要大量的时间和数据重新训练模型(zenga,yukt,songs,etal.multi-viewself-superviseddeeplearningfor6dposeestimationintheamazonpickingchallenge[j].2016:1386-1383.)。因此，上述两种方法都在对场景中手持物体进行实时位姿检测时存在不足。

技术实现要素：

本发明提出了基于物体三维点云信息的物体位姿实时检测方法。通过3d体感摄影机对待检测物体各部分进行深度图像拍摄，得到物体局部的三维点云信息，使用点云处理软件将各局部点云对齐合并为物体完整的三维点云模型。再使用3d体感摄影机对场景进行实时数据采集，得到当前场景的rgb彩色图像，及包含场景三维点云信息的深度图像。将得到的rgb图像进行自动阈值分割，获得图像中表示物体的像素点，接着将采集到的深度图像中对应点云与之融合，得到场景中物体的带颜色信息的三维点云(rgb-d)图像。最后利用icp算法将物体的rgb-d图像与物体完整的三维点云图像进行点云匹配，获得场景中手持物体的位置及姿态。

本发明的技术方案：

一种面向人机协作的手持物体位姿实时检测方法，包括如下步骤：

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

将待测物体放置于实验平台上，使用3d体感摄影机对其进行拍摄，通过多角度拍摄待测物体各个局部得到不同角度的点云图像，将得到的点云图像进行滤波，滤掉图像中的离群点。

2)构建待测物体完整的点云模型图像

通过关键点匹配的方法，以滤波后的一幅点云图像作为基准，其他点云图像参照此基准实现点云对齐。再将对齐后的点云图像进行合并，得到待测物体完整的点云模型图像。

3)图像的实时采集

在3d体感摄影机摄像头的可视范围内，将戴好黑色手套的“人手”及所持待测物体置于设置好的实验平台上方。通过3d体感摄影机对当前场景进行深度图像和彩色图像的实时采集。

4)图像分割

对步骤3)采集的彩色图像转化为灰度图像，利用自适应阈值分割算法，对灰度图像进行图像分割，将灰度图像分成背景和目标两部分。找到分割后图像中的最大连通域即为待检测物体，删除其余杂点部分，得到只包含待测物体的灰度图像，将只包含待测物体的灰度图像对应转化为待测物体的彩色图像。

5)得到待测物体彩色三维点云图像(rgb-d图像)

根据3d体感摄影机自身的相机内参，得到深度图像和待测物体的彩色图像的像素坐标之间的转换关系，根据转换关系完成深度图像和待测物体的彩色图像的融合，得到待测物体彩色三维点云图像。

所述深度图像和待测物体的彩色图像的像素坐标之间的转换过程如下：

其中，3d体感摄影机深度图像素坐标为(u,v)，深度摄像头的相机坐标为(x,y,z)，dx和dy表示每一列和每一行分别代表多少mm，即每一列中1像素＝dxmm，每一行中1像素＝dymm，fx和fy分别表示相机在水平方向和竖直方向上的焦距，u0、v0分别为光心在像素坐标系的坐标与像素坐标系原点的水平偏移量和竖直偏移量。

当3d体感摄影机的彩色图像像素坐标为(u',v')，彩色摄像头的相机坐标为(x',y',z')时，则两个坐标之间同样存在的转换关系为：

深度摄像头和彩色摄像头之间的旋转平移矩阵通过3d体感摄影机自身参数得到，两个摄像头相机坐标转换关系如下式：

其中，a表示深度摄像头和彩色摄像头之间在水平方向上的位移。

联立式(1)、(2)、(3)得到：

在深度摄像头的相机坐标系和彩色摄像头的相机坐标系中，认为二者对应像素点的深度相等，即z＝z'，则得到：

公式(5)即为深度图像和彩色图像的像素坐标之间的转换关系。

6)实时检测待测物体位姿

将步骤2)得到的点云模型图像作为基准位置，再把待测物体的rgb-d图像与点云模型图像进行比对，通过icp点云匹配算法，求得场景中物体的三维点云相对点云模型图像中的三维点云的旋转平移矩阵，即得到场景中物体点云相对基准位置三维点云的旋转分量和平移分量，通过将其旋转矩阵转化为欧拉角形式，进而得到当前场景下手持物体的位姿。

所述的icp点云匹配算法为：

在每次迭代的过程中，对待测物体彩色三维点云图像中的每一点，在模型点云图像中寻找欧式距离最近点作为对应点，通过这组对应点使目标函数最小化：

其中，f(r,t)为目标函数；qi为模型点云图像中的第i个点；n为需校准点云的所有点的数量；r为两幅待测物体彩色三维点云图像之间的旋转分量；pi为需校准点云中的第i个点；t为两幅待测物体彩色三维点云图像相差的平移分量。

通过上式得到最优的平移向量t和旋转矩阵r，将平移向量t和旋转矩阵r作用到待测物体彩色三维点云图像上，得到新的点云数据带入下次迭代过程，直到满足迭代停止条件，输出求得的旋转平移矩阵rt：

平移向量t表示为：[t0t1t2]^t；

其中，t0、t1、t2分别为两幅待测物体彩色三维点云图像中对应点之间在三个坐标方向上的距离。

旋转向量r表示为：

其中，r为3*3的矩阵，r00、r01、r02、r10、r11、r12、r20、r21、r22分别为矩阵中对应位置的值；

为使机械臂能够准确的抓取当前场景中的待测物体，需将待测物体相对基准位置的旋转平移矩阵rt转换为机械臂能识别的形式表示，即用欧拉角表示(x,y,z,rx,ry,rz)，再将转换后的信息发送给机械臂。

通过icp算法求得的旋转平移矩阵为：

实际的待测物体点云相对基准位置三维点云在三个坐标方向上的平移分量x、y、z分别为:

x＝t0，y＝t1，z＝t2(9)；

实际的待测物体点云相对基准位置三维点云在三个坐标方向上的旋转分量rx、ry、rz分别为：

rx＝atan2(r22,r21)(10)；

rz＝atan2(r10,r00)(12)；

待测物体的位姿为(x,y,z,rx,ry,rz)。

本发明的有益效果：本发明中对手持物体位姿的实时检测方法，可以实现传统单目视觉在物体信息不完全(手持物体存在局部遮挡)时无法使用的情况下得到准确的检测结果。

附图说明

图1为kinect拍摄的待测物体上面和左侧面的深度图。

图2为kinect拍摄的待测物体上面和右侧面以及前侧面的深度图。

图3为kinect以拍摄的待测物体底面和后侧面的深度图。

图4为物体完整的三维点云模型示意图。

图5为当前场景灰度图。

图6为只包含待测物体的灰度图。

图7为只包含待测物体的rgb-d图。

图8为待测物体的当前位姿与物体完整点云模型的初始相对位置示意图。

图9为icp点云迭代次数为5次时结果图。

图10为icp点云迭代次数为10次时结果图。

图11为icp点云迭代次数为20次时结果图。

图12为icp点云最终匹配结果图。

具体实施方式

下面将结合具体实施例和附图对本发明的技术方案进行进一步的说明。

本发明具体实施过程中使用的是kinect传感器对待检测物体进行各个方位的深度图像的拍摄，最终得到物体完整的三维点云模型。kinect传感器的有效视角为水平方向57°，垂直方向43°，其可视范围为0.4米～3.5米。kinect传感器可以每秒30帧的速度生成深度图像和彩色图像，两种图像的像素均为640*480。

一种面向人机协作的手持物体位姿实时检测方法，包括如下步骤：

第一步，采集数据并进行点云的预处理，建立物体完整三维点云模型

仅通过一幅深度图像，总会由于存在不同程度的遮挡，无法得到物体各个面的三维点云信息。因此，我们采取多角度拍摄物体各个局部的深度图像，再按照各部分所表示的物体实际位置进行对齐合并，最终得到物体完整的三维模型图像。

具体的操作为：将待检测物体放置于实验平台上，使用kinect传感器对实验平台上待检测物体进行深度图像拍摄，将物体水平旋转90度，再用kinect传感器对其进行拍摄，多次旋转及拍摄，确保得到物体各部分的局部点云图像。如图1、图2、图3所示。再将得到的点云图像进行滤波，滤掉图像中的离群点。将其中一幅图像作为基准，其他点云图像则参照此基准通过关键点匹配的方法实现对齐操作。再把对齐之后的几幅点云图像进行合并，使之成为物体完整的点云模型图像，如图4所示。

第二步，获得场景中物体rgb-d图像

首先通过kinect获得了场景中手持物体的rgb彩色图像，再将此彩色图像转化为灰度图像并进行自动阈值分割，将待测物体和人手以及实验平台等背景区分开来。然后把只含有待测物体的彩色图像与其对应的深度图像进行融合，最终得到物体的rgb-d图像。具体操作如下：

1)图像的实时采集

将戴好黑色手套的“人手”及所持物体至于设置好的实验平台上方，以保证在kinect摄像头可视范围之内。通过kinect对当前场景进行深度图像和彩色图像的实时采集。

2)图像分割

首先，将kinect采集的rgb图像转化为灰度图像，如图5所示，再使用自适应阈值分割算法对图像进行分割。该方法利用图像的灰度特性，将图像分成背景和目标两部分。即待测物体的灰度值为255(白色像素点)，而其他包括人手、实验平台、机械臂等都视为背景，其灰度值为0(黑色像素点)。背景和目标之间的类间方差越大，说明构成图像的两部分的差别越大，当部分目标错分为背景或部分背景错分为目标都会导致两部分差别变小。因此，使类间方差最大的分割意味着错分概率最小。为了使分割结果更为准确，持物体的“人手”需要戴上黑色的手套，否则可能会因为人的肤色与待测物体颜色相近，而造成分割结果不理想。

由于噪声、光线等影响，自适应阈值分割后的图像中，会包含一些杂点。杂点的面积都比较小，所以在分割后的图像中，寻找最大连通域即为待检测物体，然后删除掉其余部分，得到只包含待测物体的灰度图像，如图6所示。

物体的灰度图像中所有灰度值为255的像素点即为表示物体的像素点，进而在原rgb图像中，找到所有像素坐标与之对应的像素点，即为带颜色信息的表示物体的像素点集合，除此之外，其他像素点的值设置为(0,0,0)，即可将原rgb图像转化为只包含待测物体的rgb图像。

3)得到待测物体彩色三维点云图像(rgb-d图像)

统计上一步求得的图像中，像素值为255的像素点个数，若像素点少于1500，则说明手部遮挡信息过多，即此帧图像可信度不高，不再继续下一步，而是返回步骤2)，处理下一帧图像。

将上述步骤中获得的只包含待测物体的rgb图像，与由kinect采集原场景中的深度图像进行融合。由于kinect的深度摄像头和彩色摄像头存在一定的物理位移，所以将两幅图像进行融合前，需要求得深度图像和彩色图像的像素坐标之间的转换关系。

在任何一个光学相机中，都包含相机坐标系和像素坐标系，而二者之间，存在一定的转换关系。假设kinect深度图像素坐标为(u,v),深度摄像头的相机坐标为(x,y,z)，则它们之间的转换关系如公式(1)所示。

其中，kinect传感器深度图像素坐标为(u,v)，深度摄像头的相机坐标为(x,y,z)，dx和dy表示每一列和每一行分别代表多少mm，即每一列中1像素＝dxmm，每一行中1像素＝dymm，fx和fy分别表示相机在水平方向和竖直方向上的焦距，u0、v0分别为光心在像素坐标系的坐标与像素坐标系原点的水平偏移量和竖直偏移量。

当kinect传感器的彩色图像像素坐标为(u',v')，彩色摄像头的相机坐标为(x',y',z')时，则两个坐标之间同样存在的转换关系为：

深度摄像头和彩色摄像头之间的旋转平移矩阵通过kinect传感器自身参数得到，两个摄像头相机坐标转换关系如下式。

其中，a表示深度摄像头和彩色摄像头之间在水平方向上的位移。

联立式(1)、(2)、(3)得到：

在深度摄像头的相机坐标系和彩色摄像头的相机坐标系中，认为二者对应像素点的深度相等，即z＝z'，则得到：

公式(5)即为深度图像和彩色图像的像素坐标之间的转换关系。

根据本实施例中kinect自身的相机内参可知，深度摄像头和相机的彩色摄像头之间的位移a为2cm,即a＝0.02，fy/dy即为相机焦距，其值为550，因此，可以得到深度图像和彩色图像的像素坐标之间的转换关系为：

这样，就得到了深度图像和彩色图像的像素坐标之间的转换关系，通过对应的像素点坐标可以将场景的深度图像与彩色图像进行融合，最终获得场景中待测物体的rgb-d图像，如图7所示。

第三步，场景中物体位姿实时检测

将建立好的物体完整的三维点云模型作为基准位置，再把物体的rgb-d图像与物体完整的三维点云模型图像进行比对，通过icp点云匹配算法，即可得到当前场景下手持物体的位置及姿态。

icp算法是最常用的点云精确配准方法，算法在每次迭代的过程中，对点云数据的每一点，在模型点云中寻找欧式距离最近点作为对应点，通过这组对应点使目标函数最小化：

其中，f(r,t)为目标函数；qi为模型点云图像中的第i个点；n为需校准点云的所有点的数量；r为两幅待测物体彩色三维点云图像之间的旋转分量；pi为需校准点云中的第i个点；t为两幅待测物体彩色三维点云图像相差的平移分量。

通过上式得到最优的平移向量t和旋转矩阵r，将平移向量t和旋转矩阵r作用到点云数据上，得到新的点云数据带入下次迭代过程，直到满足迭代停止条件，即与对应点的距离小于设定值或达到设置的最大迭代上限，方停止迭代，输出求得的rt矩阵。

平移向量t可表示为：[t0t1t2]^t；

其中，t0、t1、t2分别为两幅待测物体彩色三维点云图像中对应点之间在三个坐标方向上的距离。

旋转向量可表示为：

其中，r为3*3的矩阵，r00、r01、r02、r10、r11、r12、r20、r21、r22分别为矩阵中对应位置的值；

由于icp点云匹配算法耗时较长，为了解决这一缺陷，先将两幅点云图像进行适当降采样，减少点的数量，达到提高匹配效率的目的。通过对两幅点云图像进行icp点云匹配，得到当前场景下物体相对基准位置的旋转平移矩阵。

为使机械臂能够准确的抓取当前场景中的物体，需将物体相对基准位置的旋转平移矩阵rt转换为机械臂可识别的形式表示，即欧拉角(x,y,z,rx,ry,rz)表示，再将转换后的信息发送给机械臂。

实际的待测物体点云相对基准位置三维点云在三个坐标方向上的平移分量x、y、z分别为：

x＝t0，y＝t1，z＝t2(9)；

实际的待测物体点云相对基准位置三维点云在三个坐标方向上的旋转分量rx、ry、rz分别为：

rx＝atan2(r22,r21)(10)；

rz＝atan2(r10,r00)(12)；

因此，可以得到待测物体的位姿(x,y,z,rx,ry,rz)。

本实施例通过icp点云匹配算法，求得场景中物体的三维点云相对物体完整模型中的三维点云的旋转平移矩阵，即为当前场景下手持物体的位姿。以图6为例，待测物体的当前位姿与物体完整点云模型的初始相对位置如图8所示，通过icp匹配算法，则求得旋转平移矩阵的结果为：

将求得的旋转平移矩阵转换为欧拉角的形式表示：

x＝-0.0817,y＝0.4572,z＝0.0089(15)；

rx＝-3.1211,ry＝-0.0013,rz＝-0.5566(16)；

进而求得物体的位姿为(-0.0817，0.4572，0.0089，-3.1211，-0.0013，-0.5566)。

icp匹配过程如图9、10、11所示，三幅图分别为点云迭代次数为5次、10次、和20次时匹配效果。而如图12所示，为达到最终迭代停止条件，即匹配完成时的匹配效果。