基于椭圆锥人工势场的服务机器人抓取目标物体的方法与流程

本发明涉及服务机器人技术领域，具体涉及一种基于椭圆锥人工势场的服务机器人抓取目标物体的方法、系统、装置。

背景技术：

随着机器人技术的快速发展，服务机器人逐渐进入人们的日常生活。为更好的满足人类的需求，提供更为优质的服务，服务机器人自身携带机械臂。国内外研究人员在安装有机械臂的服务机器人的目标物体抓取方面开展了深入研究，其中普遍采用视觉传感器对抓取场景进行感知。在服务机器人到达指定操作区域后，服务机器人将结合视觉传感器的感知结果，控制机械臂实施抓取。服务机器人的目标物体抓取涉及基于视觉的目标物体检测以及机械臂规划两方面。基于视觉的目标物体检测是服务机器人抓取的前提，传统的物体检测方法通常需要手工设计特征，环境适应性较差。随着深度学习的发展，基于深度学习的物体检测方法成为当前研究的重点，代表性方法包括fasterr-cnn(fasterregion-basedconvolutionalneuralnetwork)、yolo(youonlylookonce)、ssd(singleshotmultiboxdetector)等。在完成目标物体的检测后，服务机器人将基于目标物体的检测结果进行自身机械臂的运动规划，进而实施对目标物体的抓取。一种常见的进行机械臂运动规划的方式是调用moveit！功能包，目前moveit！功能包已经广泛应用于主流的机械臂运动规划中。另外，对于获取的点云数据，一种常见的方式是直接利用在服务机器人和无人驾驶等领域得到广泛应用的pcl点云库进行点云数据的处理(包括直通滤波、欧式聚类、拟合等)，还有在pcl点云库基础上对点云数据进行更深入的处理，例如基于主成分分析(principalcomponentanalysis，pca)的最小包围盒获取算法等，可参考文献：孙永伟.基于最小包围盒及自适应聚类的三维r*-树索引结构.山东理工大学硕士学位论文,2011。

现有的服务机器人抓取技术通常考虑目标物体能够被直接抓取的情形，从而在完成对指定目标物体的检测后，直接控制机械臂抓取目标物体。但是在现实环境中，一些情况下目标物体周围会存在障碍物体，特别是部分障碍物体的存在将使得目标物体不能直接被机械臂抓取，这就需要先行将阻碍物体(即妨碍目标物体被直接抓取的障碍物体)搬移，最后抓取目标物体。针对阻碍物体的搬移，通常有两种解决方案。第一种是将阻碍物体搬移到人工指定位置，该解决方案缺乏智能性。第二种解决方案限定阻碍物体的朝向进而简化该阻碍物体的被搬移位置的计算过程；在实际环境中阻碍物体是随意放置的，从而它的朝向可以任意方向，这使得第二种解决方案缺乏通用性。另外，如何结合阻碍物体的形状和朝向，更好的计算阻碍物体的待放置位置也需要进一步研究。为此，需要对现有的机器人抓取技术进行更深入的研究，以解决现有技术在目标物体周围存在阻碍物体的情形下，难以有效的应对搬移任意朝向的阻碍物体时的服务机器人抓取问题。

技术实现要素：

为了解决现有技术中的上述问题，即为了解决现有的抓取方法在目标物体周围存在阻碍物体的情形下，难以有效的搬移任意朝向的阻碍物体，导致服务机器人抓取质量较差的问题，本发明提出一种基于椭圆锥人工势场的服务机器人抓取目标物体的方法，包括以下步骤：

步骤s100，服务机器人通过传感器获取周围环境的彩色图像以及其在相机坐标系下的原始点云数据；

步骤s200，通过基于深度学习的物体检测方法获取所述彩色图像中目标物体的包围框；并将所述目标物体的包围框在原始点云数据中对应的点云数据作为目标物体的第一点云数据，剩余的点云数据作为第一环境点云数据；

步骤s300，根据相机坐标系与服务机器人机械臂坐标系orxryrzr的坐标转换关系，对所述目标物体的第一点云数据、所述第一环境点云数据进行转换，得到目标物体的第二点云数据、第二环境点云数据；

步骤s400，去除服务机器人的机械臂在xryr平面的工作空间外的第二环境点云数据，将剩余的第二环境点云数据作为第三环境点云数据，并通过pcl点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程；

步骤s500，基于所述平面方程，去除第三环境点云数据中位于目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数据，进行第四环境点云数据的点云簇聚类；通过第一算法获得聚类后各点云簇所对应障碍物体的最小包围盒，进而得到障碍物体的尺寸、位置及朝向信息；所述第一算法为基于主成分分析的最小包围盒获取算法；

步骤s600，对目标物体，结合所述平面方程，在所述目标物体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；

步骤s700，基于目标物体和各障碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络；

步骤s800，基于目标物体和各障碍物体的尺寸、位置及朝向信息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否则，结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取。

在一些优选的实施方式中，所述相机坐标系与服务机器人机械臂坐标系orxryrzr的坐标转换关系为：

其中，(xcp,ycp,zcp)^t和(xrp,yrp,zrp)^t分别为原始点云数据ds中的点在相机坐标系ocxcyczc和服务机器人机械臂坐标系orxryrzr中的坐标，tm为预设的矩阵。

在一些优选的实施方式中，步骤s400中“通过pcl点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程”，其方法为：

通过pcl点云库的pass.filter滤波算法对第三环境点云数据进行滤波处理；

处理后，通过pcl点云库中的ransac拟合算法进行平面拟合，获得拟合平面的法向量(aop,bop,cop)和截距dop，进而得到目标物体所在平面的平面方程为aop·x+bop·y+cop·z+dop＝0。

在一些优选的实施方式中，步骤s500中“进行第四环境点云数据的点云簇聚类”，其方法为：通过欧式聚类算法将第四环境点云数据进行点云簇聚类。

在一些优选的实施方式中，步骤s500中“进而得到障碍物体的尺寸、位置及朝向信息”，其方法为：

基于各障碍物体相应的最小包围盒bj，j＝1,2,…,nc，得到bj在机械臂坐标系orxryrzr下的8个顶点的坐标nc为障碍物体的个数，根据计算出对应障碍物体的高度；

将各障碍物体相应的最小包围盒bj垂直投影到目标物体所在平面上，得到相应投影矩形的4个顶点的坐标根据计算相应投影矩形的中心点坐标、长、宽以及该投影矩形的长边相对于xr轴方向的角度；

投影矩形的中心点坐标反映了对应障碍物体的位置信息，将投影矩形的长边相对于xr轴方向的角度作为对应障碍物体的朝向信息，投影矩形的长、宽以及障碍物体的高度描述了对应障碍物体的尺寸信息。

在一些优选的实施方式中，步骤s700“基于目标物体和各障碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络”，其方法为：

根据物体oj的投影矩形的顶点信息中心点坐标长宽以及该投影矩形的长边相对于xr轴方向的角度计算最小椭圆包络的离心率e、半长轴a和半短轴b；

将得到的半长轴a和半短轴b代入标准椭圆方程并展开，得到最小椭圆包络的一般方程：

其中，为最小椭圆包络上的任意一点的坐标。

在一些优选的实施方式中，步骤s800中“通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取”，其方法为：

步骤s810，计算各障碍物体相应的投影矩形的中心点坐标与目标物体相应的投影矩形的中心点坐标之间的距离，并与各障碍物体的id号结合，构建障碍物体影响序列；

步骤s811，选取障碍物体影响序列中最小距离所对应的障碍物体作为本次待搬移阻碍物体；

步骤s812，在机械臂作用的xryr平面的工作空间srm内，生成本次待搬移阻碍物体对应的椭圆锥引力势场；

步骤s813，生成除本次待搬移阻碍物体之外的其他障碍物体以及目标物体在srm内相应的椭圆锥斥力势场；

步骤s814，将椭圆锥斥力势场与椭圆锥引力势场进行叠加，得到本次待搬移阻碍物体对应的椭圆锥合势场；

步骤s815，选取椭圆锥合势场的最小值对应的位置，作为本次待搬移阻碍物体的待放置位置，如果椭圆锥合势场的最小值对应的位置数量不止一个，选取距离本次待搬移阻碍物体的投影矩形的中心点最近的位置作为待放置位置；

步骤s816，基于本次待搬移阻碍物体的当前位置、尺寸、朝向信息以及待放置位置、其他障碍物体和目标物体的位置、尺寸和朝向信息，采用moveit！功能包进行机械臂的运动规划，完成对本次待搬移阻碍物体的搬移；将本次待搬移阻碍物体移出障碍物体影响序列，同时，本次待搬移阻碍物体相应投影矩形的中心点坐标用它搬移后的位置加以更新；

步骤s817，基于目标物体的位置、尺寸、朝向信息，以及本次待搬移阻碍物体的尺寸、朝向、搬移后的位置信息、其他障碍物体的当前位置、尺寸、朝向信息，通过moveit！功能包判断目标物体是否可以被直接抓取，如果目标物体不能被直接抓取，则返回步骤s811；否则，通过moveit！功能包进行机械臂的运动规划，控制机械臂完成对目标物体的抓取。

本发明的第二方面，提出了一种基于椭圆锥人工势场的服务机器人抓取目标物体的系统，该系统包括：获取模块、检测模块、转换模块、拟合模块、聚类模块、目标物体信息提取模块、最小椭圆包络构建模块、抓取模块；

所述获取模块，配置为服务机器人通过传感器获取周围环境的彩色图像以及其在相机坐标系下的原始点云数据；

所述检测模块，配置为通过基于深度学习的物体检测方法获取所述彩色图像中目标物体的包围框；并将所述目标物体的包围框在原始点云数据中对应的点云数据作为目标物体的第一点云数据，剩余的点云数据作为第一环境点云数据；

所述转换模块，配置为根据相机坐标系与服务机器人机械臂坐标系orxryrzr的坐标转换关系，对所述目标物体的第一点云数据、所述第一环境点云数据进行转换，得到目标物体的第二点云数据、第二环境点云数据；

所述拟合模块，配置为去除服务机器人的机械臂在xryr平面的工作空间外的第二环境点云数据，将剩余的第二环境点云数据作为第三环境点云数据，并通过pcl点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程；

所述聚类模块，配置为基于所述平面方程，去除第三环境点云数据中位于目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数据，进行第四环境点云数据的点云簇聚类；通过第一算法获得聚类后各点云簇所对应障碍物体的最小包围盒，进而得到障碍物体的尺寸、位置及朝向信息；所述第一算法为基于主成分分析的最小包围盒获取算法；

所述目标物体信息提取模块，配置为对目标物体，结合所述平面方程，在所述目标物体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；

所述最小椭圆包络构建模块，配置为基于目标物体和各障碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络；

所述抓取模块，配置为基于目标物体和各障碍物体的尺寸、位置及朝向信息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否则，结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取。

本发明的第三方面，提出了一种存储装置，其中存储有多条程序，所述程序应用由处理器加载并执行以实现上述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法。

本发明的第四方面，提出了一种处理装置，包括处理器、存储装置；处理器，适用于执行各条程序；存储装置，适用于存储多条程序；所述程序适用于由处理器加载并执行以实现上述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法

本发明的有益效果：

本发明提高了服务机器人的抓取质量。当存在妨碍目标物体被直接抓取的阻碍物体时，本发明通过结合物体的形状和朝向，设计椭圆锥人工势场，通过先搬移阻碍物体而后抓取目标物体的方式，实现服务机器人对目标物体的抓取，有效提高服务机器人的抓取质量，并为服务机器人在家庭、办公、医护等环境下的抓取作业提供技术支持。

附图说明

图1是本发明一种实施例的基于椭圆锥人工势场的服务机器人抓取目标物体的方法的流程示意图；

图2是本发明一种实施例的基于椭圆锥人工势场的服务机器人抓取目标物体的系统的框架示意图。

具体实施方式

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

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明的一种基于椭圆锥人工势场的服务机器人抓取目标物体的方法，如图1所示，包括以下步骤：

步骤s100，服务机器人通过传感器获取周围环境的彩色图像以及其在相机坐标系下的原始点云数据；

步骤s200，通过基于深度学习的物体检测方法获取所述彩色图像中目标物体的包围框；并将所述目标物体的包围框在原始点云数据中对应的点云数据作为目标物体的第一点云数据，剩余的点云数据作为第一环境点云数据；

步骤s300，根据相机坐标系与服务机器人机械臂坐标系orxryrzr的坐标转换关系，对所述目标物体的第一点云数据、所述第一环境点云数据进行转换，得到目标物体的第二点云数据、第二环境点云数据；

步骤s400，去除服务机器人的机械臂在xryr平面的工作空间外的第二环境点云数据，将剩余的第二环境点云数据作为第三环境点云数据，并通过pcl点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程；

步骤s500，基于所述平面方程，去除第三环境点云数据中位于目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数据，进行第四环境点云数据的点云簇聚类；通过第一算法获得聚类后各点云簇所对应障碍物体的最小包围盒，进而得到障碍物体的尺寸、位置及朝向信息；所述第一算法为基于主成分分析的最小包围盒获取算法；

步骤s600，对目标物体，结合所述平面方程，在所述目标物体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；

步骤s700，基于目标物体和各障碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络；

步骤s800，基于目标物体和各障碍物体的尺寸、位置及朝向信息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否则，结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取。

为了更清晰地对本发明基于椭圆锥人工势场的服务机器人抓取目标物体的方法进行说明，下面结合附图对本发明方法一种实施例中各步骤进行展开详述。

步骤s100，服务机器人通过传感器获取周围环境的彩色图像以及其在相机坐标系下的原始点云数据；

在本实施例中，服务机器人通过安装于自身的kinect传感器获取彩色图像is以及is在相机坐标系ocxcyczc下的原始点云数据ds，其中相机坐标系ocxcyczc为右手系，ocxcyczc的原点oc位于kinect传感器的中心，yc轴垂直于kinect传感器的底面且方向向上，zc轴垂直于yc轴且与kinect传感器的正前方保持一致。

步骤s200，通过基于深度学习的物体检测方法获取所述彩色图像中目标物体的包围框；将所述目标物体的包围框在原始点云数据中对应的点云数据作为目标物体的第一点云数据，剩余的点云数据作为第一环境点云数据；

在本实施例中，基于彩色图像is，利用基于深度学习的物体检测方法ssd进行目标物体的检测，得到目标物体的包围框，进而从原始点云数据ds中获得目标物体的包围框所对应的点云数据，这些点云数据构成目标物体的第一点云数据dto；在ds中去除dto后剩余的点云数据称为第一环境点云数据drs。

步骤s300，根据相机坐标系与服务机器人机械臂坐标系orxryrzr的坐标转换关系，对所述目标物体的第一点云数据、所述第一环境点云数据进行转换，得到目标物体的第二点云数据、第二环境点云数据；

在本实施例中，根据相机坐标系ocxcyczc到服务机器人的机械臂坐标系orxryrzr的坐标变换关系，将目标物体的第一点云数据dto转换到机械臂坐标系orxryrzr下，得到目标物体的第二点云数据dt；将第一环境点云数据drs转换到机械臂坐标系orxryrzr下，得到第二环境点云数据dr；机械臂坐标系orxryrzr为右手系，orxryrzr的原点or位于服务机器人的机械臂的基座中心，zr轴垂直于地面且方向向上，yr轴垂直于zr轴且与服务机器人正前方的方向相反；服务机器人的机械臂的基座底面平行于地面；

该步骤中，相机坐标系ocxcyczc到机械臂坐标系orxryrzr的坐标变换关系如下：其中，(xcp,ycp,zcp)^t和(xrp,yrp,zrp)^t分别为原始点云数据ds中的点在相机坐标系ocxcyczc和机械臂坐标系orxryrzr中的坐标，tm为预设的矩阵。

步骤s400，去除服务机器人的机械臂在xryr平面的工作空间外的第二环境点云数据，将剩余的第二环境点云数据作为第三环境点云数据，并通过pcl点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程；

在本实施例中，结合机械臂在xryr平面的工作空间srm，其中srm为矩形区域，其左下角和右上角坐标分别为(xmin,ymin)和(xmax,ymax)，xmin、ymin、xmax和ymax均为预设的阈值，对第二环境点云数据dr进行缩小范围处理，得到第三环境点云数据df；进而基于第三环境点云数据df，进行目标物体所在平面的平面拟合，得到目标物体所在平面的平面方程；

该步骤中，对第二环境点云数据dr进行缩小范围处理的具体过程如下：去除yr轴方向上数值位于[ymin,ymax]区间范围外的数据，以及去除xr轴方向上数值位于[xmin,xmax]区间范围外的数据，得到第三环境点云数据df；

该步骤中，基于第三环境点云数据df，进行目标物体所在平面的平面拟合，得到目标物体所在平面的平面方程的具体过程如下：将df经过pcl点云库中的pass.filter滤波算法的处理，而后调用pcl点云库中的ransac拟合算法进行平面拟合，获得拟合平面的法向量(aop,bop,cop)和截距dop，从而得到目标物体所在平面的平面方程为aop·x+bop·y+cop·z+dop＝0。

步骤s500，基于所述平面方程，去除第三环境点云数据中位于目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数据，进行第四环境点云数据的点云簇聚类；通过第一算法获得聚类后各点云簇所对应障碍物体的最小包围盒，进而得到障碍物体的尺寸、位置及朝向信息；所述第一算法为基于主成分分析的最小包围盒获取算法；

在本实施例中，结合目标物体所在平面的平面方程，对第三环境点云数据df进行处理，去除df中位于目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数据do，进而采用欧式聚类算法对第四环境点云数据do进行点云簇聚类，聚类后的每个点云簇对应一个障碍物体，获得各障碍物体对应的最小包围盒，得到各障碍物体的尺寸、位置以及朝向信息。

其中，采用欧式聚类算法对第四环境点云数据do进行点云簇聚类，获得各障碍物体对应的最小包围盒，进而得到障碍物体的尺寸、位置以及朝向信息的具体过程如下：

步骤s510，使用pcl点云库中的euclideanclusterextraction欧式聚类算法对第四环境点云数据do进行点云簇聚类，将聚类结果放到聚类集c中；

步骤s511，得到聚类集c中的点云簇个数，记为nc，如果nc＝0，意味着没有产生点云簇，也就是不存在障碍物体，跳转到步骤s600；如果nc>0，意味着有nc个点云簇，每个点云簇对应一个障碍物体，从而共有nc个障碍物体，用oj(j＝1,2,…,nc)进行表示，对每个点云簇采用基于主成分分析的最小包围盒获取算法进行处理，得到相应的最小包围盒bj(j＝1,2,…,nc)；

步骤s512，基于各障碍物体相应的最小包围盒bj，j＝1,2,…,nc，得到bj在机械臂坐标系orxryrzr下的8个顶点的坐标根据中zr轴坐标的最大值与最小值求出障碍物体oj的高度

步骤s513，将各障碍物体相应的最小包围盒bj垂直投影到目标物体所在平面上，j＝1,2,…,nc，得到相应投影矩形的4个顶点的坐标进而根据计算出投影矩形的中心点坐标长宽以及该投影矩形的长边相对于xr轴方向的角度和分别反映了障碍物体oj的位置和朝向信息；障碍物体oj的尺寸信息用和进行描述。

步骤s600，对目标物体，结合所述平面方程，在所述目标物体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；

在本实施例中，结合目标物体所在平面的平面方程，在目标物体的第二点云数据dt中去除该平面以及该平面下方的点所对应的数据，得到目标物体的第三点云数据dtr，获得目标物体对应的最小包围盒，进而得到目标物体的尺寸、位置以及朝向信息。

该步骤中，为了将障碍物体和目标物体的最小包围盒统一表征，目标物体用oj(j＝nc+1)即进行表示。对目标物体的第三点云数据dtr采用基于主成分分析的最小包围盒获取算法进行处理，得到目标物体对应的最小包围盒，用进行描述，进而得到在机械臂坐标系orxryrzr下的8个顶点的坐标根据中zr轴坐标的最大值与最小值得到目标物体的高度将垂直投影到目标物体所在平面上，得到相应投影矩形的4个顶点的坐标进而根据计算出投影矩形的中心点坐标长宽以及该投影矩形的长边相对于xr轴方向的角度和分别反映了目标物体的位置和朝向信息；目标物体的尺寸信息用和进行描述。

步骤s700，基于目标物体和各障碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络；

在本实施例中，计算物体oj(j＝1,2,…,nc+1)的投影矩形的最小椭圆包络的一般方程，表征了物体oj对应的最小椭圆包络。

该步骤中，计算物体oj的投影矩形的最小椭圆包络的一般方程的具体过程如下：

步骤s710，结合投影矩形的顶点信息中心点坐标长宽以及投影矩形的长边相对于xr轴方向的角度计算的离心率e、半长轴a和半短轴b如下：

其中，

步骤s711，将得到的半长轴a和半短轴b代入标准椭圆方程其中，是上的任意一点的坐标；将该标准椭圆方程展开得到的一般方程如下：

其中，

步骤s800，基于目标物体和各障碍物体的尺寸、位置及朝向信息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否则，结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取。

在本实施例中，若不存在障碍物体即nc＝0时，意味着可以直接抓取目标物体，此时，基于目标物体的尺寸、位置、朝向信息，通过moveit！功能包进行机械臂的运动规划，控制机械臂完成对目标物体的抓取。若存在障碍物体，基于目标物体和各障碍物体的尺寸、位置、朝向信息，通过moveit！功能包判断服务机器人的机械臂是否可以直接抓取目标物体，如果能够直接抓取目标物体，则机械臂将根据moveit！功能包的规划结果直接对目标物体进行抓取，否则，先对阻碍物体进行搬移再完成对目标物体的抓取，其中，机械臂末端的抓取方向均为沿zr轴的负方向竖直向下抓取。

对阻碍物体进行搬移再完成对目标物体的抓取，具体处理过程如下：

步骤s810，计算各障碍物体相应的投影矩形的中心点坐标与目标物体相应的投影矩形的中心点坐标之间的距离dk，k＝1,2,…,nc，将各障碍物体的id号及其对应的距离dk组合起来，形成障碍物体影响序列；

步骤s811，根据障碍物体影响序列中各障碍物体对应的距离，选取最小距离所对应的障碍物体作为本次待搬移阻碍物体oig；

步骤s812，在机械臂作用的xryr平面的工作空间srm内，生成本次待搬移阻碍物体oig对应的椭圆锥引力势场fatt：

其中，描述工作空间srm内的任意一点，当该点位于本次待搬移阻碍物体oig的投影矩形的最小椭圆包络边界上，当该点位于本次待搬移阻碍物体oig的投影矩形的最小椭圆包络内部时，当时，dcg是本次待搬移阻碍物体oig与其它物体(除oig之外的其他障碍物体以及目标物体)之间距离的最小值，μ是预设的引力势场系数，和描述本次待搬移阻碍物体oig的尺寸信息，分别对应oig的长、宽和高，cat为预设的常数；

步骤s813，对于每一个其他物体or(除本次待搬移阻碍物体oig之外的其他障碍物体以及目标物体)，在机械臂作用的xryr平面的工作空间srm内，产生相应的椭圆锥斥力势场计算如下：

其中，描述工作空间srm内的任意一点，当该点位于or的投影矩形的最小椭圆包络边界上，当该点位于or的投影矩形的最小椭圆包络内部时，当时，是物体or与oig之间的距离，σ是预设的斥力势场系数；和描述or的尺寸信息，分别对应or的长、宽和高；crep为预设的常数；

步骤s814，上述得到的椭圆锥引力势场fatt和nc个椭圆锥斥力势场都是椭圆锥人工势场，将所有的椭圆锥人工势场按照下式叠加起来，形成本次待搬移阻碍物体oig对应的椭圆锥合势场fres：

其中，τ是预设权重；

步骤s815，选取椭圆锥合势场fres的最小值对应的位置，作为oig的待放置位置，如果fres的最小值对应的位置数量不止一个，选取距离oig的投影矩形的中心点最近的位置作为待放置位置；

步骤s816，基于本次待搬移阻碍物体oig的当前位置、尺寸、朝向信息以及它的待放置位置、其他障碍物体和目标物体的位置、尺寸和朝向信息，采用moveit！功能包进行机械臂的运动规划，完成对oig的搬移(至此，前面提到的oig的“待放置位置”改称为oig的“搬移后的位置”，oig也由“本次待搬移阻碍物体”改称为“被搬移物体”)；将oig移出障碍物体影响序列，同时，oig在xryr平面上投影矩形的中心点坐标用它搬移后的位置加以更新；

步骤s817，基于目标物体的位置、尺寸、朝向信息，以及被搬移物体oig的尺寸、朝向、搬移后的位置信息、其他障碍物体的当前位置、尺寸、朝向信息，通过moveit！功能包判断目标物体是否可以被直接抓取，如果目标物体不能被直接抓取，则返回步骤s811；否则，通过moveit！功能包进行机械臂的运动规划，控制机械臂完成对目标物体的抓取。

在一个具体实施例中，kinect传感器倾斜向下安装，其倾斜角度为45°，xmin＝-0.35米，xmax＝0.35米，ymin＝-0.6米，ymax＝-0.3米，μ＝0.4，cat＝0.1，σ＝0.06，crep＝1.0，τ＝0.9。

采用本发明的抓取方法，当存在妨碍目标物体被直接抓取的阻碍物体时，能够结合物体的形状和朝向，并且基于椭圆锥人工势场，先进行阻碍物体的搬移操作而后完成对目标物体的抓取，为服务机器人在家庭、办公、医护等环境下的抓取作业提供技术支持，能够实现较好的技术效果。

本发明第二实施例的一种基于椭圆锥人工势场的服务机器人抓取目标物体的系统，如图2所示，包括：获取模块100、检测模块200、转换模块300、拟合模块400、聚类模块500、目标物体信息提取模块600、最小椭圆包络构建模块700、抓取模块800；

所述获取模块100，配置为服务机器人通过传感器获取周围环境的彩色图像以及其在相机坐标系下的原始点云数据；

所述检测模块200，配置为通过基于深度学习的物体检测方法获取所述彩色图像中目标物体的包围框；并将所述目标物体的包围框在原始点云数据中对应的点云数据作为目标物体的第一点云数据，剩余的点云数据作为第一环境点云数据；

所述转换模块300，配置为根据相机坐标系与服务机器人机械臂坐标系orxryrzr的坐标转换关系，对所述目标物体的第一点云数据、所述第一环境点云数据进行转换，得到目标物体的第二点云数据、第二环境点云数据；

所述拟合模块400，配置为去除服务机器人的机械臂在xryr平面的工作空间外的第二环境点云数据，将剩余的第二环境点云数据作为第三环境点云数据，并通过pcl点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程；

所述聚类模块500，配置为基于所述平面方程，去除第三环境点云数据中位于目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数据，进行第四环境点云数据的点云簇聚类；通过第一算法获得聚类后各点云簇所对应障碍物体的最小包围盒，进而得到障碍物体的尺寸、位置及朝向信息；所述第一算法为基于主成分分析的最小包围盒获取算法；

所述目标物体信息提取模块600，配置为对目标物体，结合所述平面方程，在所述目标物体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；

所述最小椭圆包络构建模块700，配置为基于目标物体和各障碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络；

所述抓取模块800，配置为基于目标物体和各障碍物体的尺寸、位置及朝向信息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否则，结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取。

所述技术领域的技术人员可以清楚的了解到，为描述的方便和简洁，上述描述的系统的具体的工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。

需要说明的是，上述实施例提供的基于椭圆锥人工势场的服务机器人抓取目标物体的系统，仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块来完成，即将本发明实施例中的模块或者步骤再分解或者组合，例如，上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块，以完成以上描述的全部或者部分功能。对于本发明实施例中涉及的模块、步骤的名称，仅仅是为了区分各个模块或者步骤，不视为对本发明的不当限定。

本发明第三实施例的一种存储装置，其中存储有多条程序，所述程序适用于由处理器加载并实现上述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法。

本发明第四实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法。

所述技术领域的技术人员可以清楚的了解到，为描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考前述方法实例中的对应过程，在此不再赘述。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

术语“第一”、“第二”、“第三”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。