一种基于点云和图像的识别定位方法及系统与流程

本发明涉及视觉伺服中目标识别与定位技术领域，特别是涉及一种基于点云和图像的识别定位方法及系统。

背景技术：

使用相机获取图像，并对图像进行处理，分析和理解，从而给机器人提供反馈信息，对机器人进行控制的技术称为视觉伺服技术。目前，随着无人工厂概念的提出与发展，视觉伺服技术在工业中使用越来越广泛。在传统的视觉伺服技术中，往往使用的是二维平面信息，对于在三维环境中工作的机器人来说会遗漏掉很多空间的立体信息。在二维图像中只有点的x轴和y轴坐标，而通过三维重建得到的点云中还包含点的z轴坐标，即深度信息，可以对目标的识别与定位起到关键的作用。并且随着深度相机不断发展，深度相机极大地帮助了其在工业机器人视觉伺服中更广泛的应用，使工业机器人有能力处理更高维的信息。但是由于点云处理往往存在速度较慢，精度较低的特点，二维图像处理虽然精度能做到很高但是包含的信息不足，因此如何进行快速地、多尺度地的目标识别与定位是亟待解决的问题。

技术实现要素：

本发明的目的是提供一种基于点云和图像的识别定位方法及系统，将三维的点云和二维的图像信息结合，能够快速多尺度的对目标进行识别与定位。

为实现上述目的，本发明提供了如下方案：

一种基于点云和图像的识别定位方法，包括：

获取相机内参数、待识别目标的第一图像、与所述第一图像对应的深度图像以及待识别目标的特征信息；

根据所述第一图像、所述深度图像和所述相机内参数对待识别目标进行三维重建，得到点云数据；

根据所述点云数据和所述待识别目标的特征信息，采用点云坐标求取平均值方法确定待识别目标初始位置；

根据所述待识别目标初始位置采用二维图像目标识别定位方法确定待识别目标最终位置。

可选的，所述根据所述第一图像、所述深度图像和所述相机内参数对待识别目标进行三维重建，得到点云数据，具体包括：

获取第一像素坐标系下第一图像像素点坐标；所述第一像素坐标系为以第一图像左上角点为原点、摄像装置像平面为xoy平面建立第一像素坐标系；

根据所述深度图像确定每一第一图像像素点对应的深度值；

根据所述第一图像像素点坐标、所述深度值和所述相机内参数确定第一图像每一像素的点云坐标，得到点云数据。

可选的，所述根据所述点云数据和所述待识别目标的特征信息，采用点云坐标求取平均值方法确定待识别目标初始位置，具体包括：

对所述点云数据进行去除离群点操作，得到去除离群点后的点云；

根据所述特征信息判断所述去除离群点后的点云是否满足待识别目标特征范围，并将不满足待识别目标特征的点云去除，得到特征处理后的点云；所述特征信息包括颜色信息和位置信息；所述颜色信息为rgb信息，所述位置信息为空间坐标信息；

对特征处理后的点云坐标进行平均值计算得到平均坐标点，将所述平均坐标点作为第一像素坐标系下的待识别目标初始位置。

可选的，所述根据所述待识别目标初始位置采用二维图像目标识别定位方法确定待识别目标最终位置，具体包括：

获取坐标系转换参数；所述坐标系转换参数包括旋转参数、位移参数和坐标尺度比例参数；

根据所述坐标系转换参数和所述相机内参数，将所述第一像素坐标系下的待识别目标初始位置转化为工具坐标系下的待识别目标初始位置；

获取待识别目标的第二图像；所述第二图像为将机械臂末端移动至工具坐标系下的待识别目标初始位置后拍摄的图像；

对所述第二图像采用二维图像目标识别定位方法确定在第二像素坐标系下待识别目标最终位置；所述第二像素坐标系为以第二图像左上角点为原点、摄像装置像平面为xoy平面建立第二像素坐标系；所述摄像装置安装于所述机械臂末端。

可选的，在对所述第二图像采用二维图像目标识别定位方法确定在第二像素坐标系下待识别目标最终位置，之后还包括：

根据所述坐标系转换参数和所述相机内参数，将第二像素坐标系下待识别目标最终位置转化为工具坐标系下的待识别目标最终位置；

根据所述工具坐标系下的待识别目标最终位置控制所述机械臂对目标进行操作。

本发明还提供一种基于点云和图像的识别定位系统，包括：

获取模块，用于获取相机内参数、待识别目标的第一图像、与所述第一图像对应的深度图像以及待识别目标的特征信息；

三维重建模块，用于根据所述第一图像、所述深度图像和所述相机内参数对待识别目标进行三维重建，得到点云数据；

初始位置确定模块，用于根据所述点云数据和所述待识别目标的特征信息，采用点云坐标求取平均值方法确定待识别目标初始位置；

最终位置确定模块，用于根据所述待识别目标初始位置采用二维图像目标识别定位方法确定待识别目标最终位置。

可选的，所述三维重建模块，具体包括：

第一图像像素点坐标获取单元，用于获取第一像素坐标系下第一图像像素点坐标；所述第一像素坐标系为以第一图像左上角点为原点、摄像装置像平面为xoy平面建立第一像素坐标系；

深度值确定单元，用于根据所述深度图像确定每一第一图像像素点对应的深度值；

点云数据确定单元，用于根据所述第一图像像素点坐标、所述深度值和所述相机内参数确定第一图像每一像素的点云坐标，得到点云数据。

可选的，所述初始位置确定模块，具体包括：

去除离群点操作单元，用于对所述点云数据进行去除离群点操作，得到去除离群点后的点云；

特征处理单元，用于根据所述特征信息判断所述去除离群点后的点云是否满足待识别目标特征范围，并将不满足待识别目标特征的点云去除，得到特征处理后的点云；所述特征信息包括颜色信息和位置信息；所述颜色信息为rgb信息，所述位置信息为空间坐标信息；

初始位置确定单元，用于对特征处理后的点云坐标进行平均值计算得到平均坐标点，将所述平均坐标点作为第一像素坐标系下的待识别目标初始位置。

可选的，所述最终位置确定模块，具体包括：

转换参数获取单元，用于获取坐标系转换参数；所述坐标系转换参数包括旋转参数、位移参数和坐标尺度比例参数；

第一位置转换单元，用于根据所述坐标系转换参数和所述相机内参数，将所述第一像素坐标系下的待识别目标初始位置转化为工具坐标系下的待识别目标初始位置；

第二图像获取单元，用于获取待识别目标的第二图像；所述第二图像为将机械臂末端移动至工具坐标系下的待识别目标初始位置后拍摄的图像；

最终位置确定单元，用于对所述第二图像采用二维图像目标识别定位方法确定在第二像素坐标系下待识别目标最终位置；所述第二像素坐标系为以第二图像左上角点为原点、摄像装置像平面为xoy平面建立第二像素坐标系；所述摄像装置安装于所述机械臂末端。

可选的，所述最终位置确定模块，还包括：

第二位置转换单元，用于根据所述坐标系转换参数和所述相机内参数，将第二像素坐标系下待识别目标最终位置转化为工具坐标系下的待识别目标最终位置；

控制单元，用于根据所述工具坐标系下的待识别目标最终位置控制所述机械臂对目标进行操作。

与现有技术相比，本发明的有益效果是：

本发明提出了一种基于点云和图像的识别定位方法及系统，通过获取相机内参数、待识别目标的第一图像及其深度图像和待识别目标的特征信息；对待识别目标进行三维重建，得到点云数据；根据点云数据和待识别目标的特征信息，采用点云坐标求取平均值方法确定待识别目标初始位置；根据待识别目标初始位置采用二维图像目标识别定位方法确定待识别目标最终位置。本发明将三维的点云和二维的图像信息结合，既利用了点云中具有深度的物体的三维形状信息，又利用了二维图像处理快速性的特点，能够快速多尺度的对目标进行识别与定位。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中基于点云和图像的识别定位方法流程图；

图2为本发明实施例中基于点云和图像的识别定位装置结构图；

图3为本发明实施例中基于点云和图像的识别定位系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于点云和图像的识别定位方法及系统，将三维的点云和二维的图像信息结合，能够快速多尺度的对目标进行识别与定位。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例

图1为本发明实施例中基于点云和图像的识别定位方法流程图，图2为本发明实施例中基于点云和图像的识别定位装置结构图，本发明将深度相机安装在机械臂末端实现目标识别与定位。

如图1-2所示，一种基于点云和图像的识别定位方法，包括：

步骤101：获取相机内参数、待识别目标的第一图像、与第一图像对应的深度图像以及待识别目标的特征信息。

步骤102：根据第一图像、深度图像和相机内参数对待识别目标进行三维重建，得到点云数据。

步骤102，具体包括：

获取第一像素坐标系下第一图像像素点坐标。第一像素坐标系为以第一图像左上角点为原点、摄像装置像平面为xoy平面建立第一像素坐标系。

根据深度图像确定每一第一图像像素点对应的深度值。

根据第一图像像素点坐标、深度值和相机内参数确定第一图像每一像素的点云坐标，得到点云数据。

具体的，要获取点云首先要对目标所在环境进行三维重建，将机械臂末端移动到一个离待识别目标较高处的位置，且相机近似平行于目标所在平面，此时深度相机能获得一个较大的视野范围。且将机械臂末端为原点的工具坐标系的z轴设为与目标平面垂直，则机械臂末端沿z轴平移，即是沿着目标平面法向量平移，对后续的处理带来很大的方便。对采集到的rgb图像和其对齐的深度图像进行三维重建，即可得到该视野下的点云，其中对齐是利用rgb相机到深度相机的外参矩阵使深度图像的坐标系转换到rgb图像的坐标系下。通常三维重建只需要一对rgb图像及深度图即可，若还需要待识别目标的侧面等无法从正面采集到的信息，可以移动相机采集更多的图像，采集速率可以设置为30帧每秒。其中对于一对rgb图像及深度图像获取点云的计算步骤如下：

若相机内参矩阵(即相机内参数)为：

其中，fx是相机焦距的像素x轴长度，fy是相机焦距的像素y轴长度，cx是第一图像中心与第一图像和光轴交点的x轴偏置，cy是第一图像中心与第一图像和光轴交点的y轴偏置。

深度相机的深度比例为depth_scale，d为图像坐标(u,v)处的深度，则点云中各点的坐标为:

x＝(u-cx)×z/fx(1)

y＝(v-cy)×z/fy(2)

z＝d/depth_scale(3)

这些点云构成的集合即为所获得的点云数据。对于多对rgb及深度图而言，需要将多个点云统一到相同的坐标系下形成一个较为完整的点云，这叫做点云配准。点云配准的经典方法是迭代最近点算法(iterativeclosestpoint,icp)，通过求取源点云和目标点云之间的对应点对，基于对应点对构造旋转平移矩阵，并利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，若误差函数值大于阈值，则迭代进行上述运算直到满足给定的误差要求。

步骤103：根据点云数据和待识别目标的特征信息，采用点云坐标求取平均值方法确定待识别目标初始位置。

步骤103，具体包括：

对点云数据进行去除离群点操作，得到去除离群点后的点云。

根据特征信息判断去除离群点后的点云是否满足待识别目标特征范围，并将不满足待识别目标特征的点云去除，得到特征处理后的点云。其中，特征信息包括颜色信息和位置信息；颜色信息为rgb信息，位置信息为空间坐标信息。

对特征处理后的点云坐标进行平均值计算得到平均坐标点，将平均坐标点作为第一像素坐标系下的待识别目标初始位置。

具体的，由于初始得到的点云数据庞大，处理比较耗时，需要对点云先进行降采样，去除离群点等操作来减少点云中点的数量，其中离群点可以定义为点云中以一点为中心，半径为r的球体邻域内的其他点的总个数若少于m个，则称该点为离群点。再利用待识别目标的已知信息进行筛选得到目标的初定位坐标，这些已知信息包括目标的颜色(即rgb或hsv值的范围)，目标的空间范围(即xyz值的范围)等。如以集合pi＝{xi,yi,zi,ri,gi,bi,…}表示空间中的一个点，则pointcloud＝{p1,p2,p3,…pn}表示一组点云数据，遍历这些点，对每个点判断其内部的元素是否均满足目标的颜色、空间等范围。将筛选后剩余得到的点的坐标求取平均值得到坐标(x0,y0,z0)，代表在图像中的坐标系即像素坐标系下(若点云由多组rgb及深度图重构产生，则坐标系取为第一帧图像的像素坐标系)(x0,y0)处的点距离相机的深度值为z0。

步骤104：根据待识别目标初始位置采用二维图像目标识别定位方法确定待识别目标最终位置。

步骤104，具体包括：

获取坐标系转换参数；坐标系转换参数包括旋转参数、位移参数和坐标尺度比例参数。

根据坐标系转换参数和相机内参数，将第一像素坐标系下的待识别目标初始位置转化为工具坐标系下的待识别目标初始位置。

获取待识别目标的第二图像。其中，第二图像为将机械臂末端移动至工具坐标系下的待识别目标初始位置后拍摄的图像。

对第二图像采用二维图像目标识别定位方法确定在第二像素坐标系下待识别目标最终位置。其中，第二像素坐标系为以第二图像左上角点为原点、摄像装置像平面为xoy平面建立第二像素坐标系；摄像装置安装于机械臂末端。

根据坐标系转换参数和相机内参数，将第二像素坐标系下的待识别目标最终位置转化为工具坐标系下的待识别目标最终位置。

根据工具坐标系下的待识别目标最终位置控制机械臂对目标进行操作。

具体的，由于深度相机采集到的深度会出现误差，并且点云使用了降采样处理，所以初始坐标并不一定准确。为了进一步得到更精确的目标位置，将机械臂末端移动到上一步得到的待识别目标位置的上方，高度低于三维重建时采集图像的高度，使其能保证末端上的深度相机的视野能包含待识别目标，即末端移动到像素坐标系(x0,y0)处，相机离目标的深度为z1，且z1<z0。

由于机器人移动需要借助工具坐标系下的坐标，所以还需将坐标转化到工具坐标系得到坐标(xt0,yt0,zt0)，机器人得以移动。将像素坐标系转化到工具坐标系的公式如下：

其中，图像像素坐标系到相机坐标系的内参矩阵的参数值与步骤101三维重建时用到的内参矩阵的值相同，一般可通过直接读取相机参数或手动标定得到。相机坐标系到工具坐标系的外参矩阵[rt]中，r为代表两个坐标系旋转关系的3x3矩阵，将相机坐标系和工具坐标系三个坐标轴方向取为一样，则r为单位阵，而t＝[txtytz]^t为两个坐标系原点的位移矩阵，可通过测量得到。s为两个坐标系尺度的比例因子。且由于相机坐标系的z轴与待识别目标所在平面垂直，工具坐标系下z轴坐标zt即为机械臂末端到目标的距离，也就是机械臂末端到相机的距离加上相机到目标的距离，前者可测量得到而后者已知，因此由上式计算可以得到工具坐标系下(xt,yt,zt)的值。

机械臂末端移动到(xt0,yt0,zt0)后，相机采集图像，此时利用深度学习中目标检测方法(如faster-rcnn、ssd)或者传统的目标识别与定位方法(如各种特征提取+分类器的方法)可以得到待识别目标的在第二图像上的精确坐标(xr,yr)，再将其转化到工具坐标系下，得到坐标(xt1,yt1,zt1)，得到此坐标后，机器人执行抓取、按压等操作。

以传统的二维图像目标识别与定位方法为例：

1)预先采集一张只包含待识别目标的灰度模板图像。

2)对实时获取到的rgb图像进行滤波和灰度化处理。

3)将实时图像和模板图像同时提取sift特征点，过滤掉匹配度较低的特征点对，再利用这些剩下的匹配的特征点进行通过随机一致性采样算法得到模板图像到实时图像的透视变换矩阵h。

4)对灰度模板图像上的点进行透视变换即可得到此点在实时图像上的坐标，由于待识别目标在模板图像上的坐标已知，所以实时图像上目标的任意坐标均可求得。

图3为本发明实施例中基于点云和图像的识别定位系统结构图。如图3所示，一种基于点云和图像的识别定位系统，包括：

获取模块301，用于获取相机内参数、待识别目标的第一图像、与所述第一图像对应的深度图像以及待识别目标的特征信息。

三维重建模块302，用于根据第一图像、深度图像和相机内参数对待识别目标进行三维重建，得到点云数据。

三维重建模块302，具体包括：

第一图像像素点坐标获取单元，用于获取第一像素坐标系下第一图像像素点坐标；第一像素坐标系为以第一图像左上角点为原点、摄像装置像平面为xoy平面建立第一像素坐标系。

深度值确定单元，用于根据深度图像确定每一第一图像像素点对应的深度值。

点云数据确定单元，用于根据第一图像像素点坐标、深度值和相机内参数确定第一图像每一像素的点云坐标，得到点云数据。

初始位置确定模块303，用于根据点云数据和待识别目标的特征信息，采用点云坐标求取平均值方法确定待识别目标初始位置。

初始位置确定模块303，具体包括：

去除离群点操作单元，用于对点云数据进行去除离群点操作，得到去除离群点后的点云。

特征处理单元，用于根据特征信息判断去除离群点后的点云是否满足待识别目标特征范围，并将不满足待识别目标特征的点云去除，得到特征处理后的点云；特征信息包括颜色信息和位置信息；颜色信息为rgb信息，位置信息为空间坐标信息。

初始位置确定单元，用于对特征处理后的点云坐标进行平均值计算得到平均坐标点，将平均坐标点作为第一像素坐标系下的待识别目标初始位置。

最终位置确定模块304，用于根据待识别目标初始位置采用二维图像目标识别定位方法确定待识别目标最终位置。

最终位置确定模块304，具体包括：

转换参数获取单元，用于获取坐标系转换参数；坐标系转换参数包括旋转参数、位移参数和坐标尺度比例参数。

第一位置转换单元，用于根据坐标系转换参数和相机内参数，将第一像素坐标系下的待识别目标初始位置转化为工具坐标系下的待识别目标初始位置。

第二图像获取单元，用于获取待识别目标的第二图像；第二图像为将机械臂末端移动至工具坐标系下的待识别目标初始位置后拍摄的图像。

最终位置确定单元，用于对第二图像采用二维图像目标识别定位方法确定在第二像素坐标系下待识别目标最终位置；第二像素坐标系为以第二图像左上角点为原点、摄像装置像平面为xoy平面建立第二像素坐标系；摄像装置安装于机械臂末端。

第二位置转换单元，用于根据坐标系转换参数和相机内参数，将第二像素坐标系下的待识别目标最终位置转化为工具坐标系下的待识别目标最终位置。

控制单元，用于根据工具坐标系下的待识别目标最终位置控制机械臂对目标进行操作。

对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本发明通过先使用三维重建得到的点云进行目标的初定位，再利用图像进行精确识别定位，既利用了点云中具有深度的物体的三维形状信息，又利用了二维图像处理快速性的特点。通过多尺度融合的检测方法，对待识别目标及其周围的环境可以准确感知，同时深度信息的加入使机械臂末端对目标的操作能更加精确，对于之后机械臂的避障路径规划也大有帮助。本发明可以既准确又快速得到待识别目标的位置，完成了基于图像的视觉伺服中的关键一步，对未来复杂环境下的机器人视觉伺服场景中能发挥重要的应用。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上，本说明书内容不应理解为对本发明的限制。