CN111882610B - 基于椭圆锥人工势场的服务机器人抓取目标物体的方法 - Google Patents

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

Info

Publication number
CN111882610B
CN111882610B CN202010682937.7A CN202010682937A CN111882610B CN 111882610 B CN111882610 B CN 111882610B CN 202010682937 A CN202010682937 A CN 202010682937A CN 111882610 B CN111882610 B CN 111882610B
Authority
CN
China
Prior art keywords
target object
point cloud
cloud data
obstacle
plane
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010682937.7A
Other languages
English (en)
Other versions
CN111882610A (zh
Inventor
耿文杰
曹志强
李忠辉
亢晋立
喻俊志
景奉水
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Nengchuang Technology Co ltd
Institute of Automation of Chinese Academy of Science
Original Assignee
Beijing Nengchuang Technology Co ltd
Institute of Automation of Chinese Academy of Science
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Nengchuang Technology Co ltd, Institute of Automation of Chinese Academy of Science filed Critical Beijing Nengchuang Technology Co ltd
Priority to CN202010682937.7A priority Critical patent/CN111882610B/zh
Publication of CN111882610A publication Critical patent/CN111882610A/zh
Application granted granted Critical
Publication of CN111882610B publication Critical patent/CN111882610B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/62—Analysis of geometric attributes of area, perimeter, diameter or volume
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/90—Determination of colour characteristics

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Geometry (AREA)
  • Manipulator (AREA)

Abstract

本发明属于服务机器人技术领域，具体涉及一种基于椭圆锥人工势场的服务机器人抓取目标物体的方法、系统、装置，旨在解决抓取方法难以有效的搬移任意朝向的阻碍物体，导致抓取质量较差的问题。本发明方法包括：获取周围环境的彩色图像及原始点云数据；获取目标物体的包围框，并将其对应的点云数据作为第一点云数据，剩余的作为第一环境点云数据；对第一点云数据、第一环境点云数据进行转换；拟合目标物体所在平面的平面方程；获取障碍物体的尺寸、位置及朝向信息；获取目标物体的尺寸、位置及朝向信息；构建最小椭圆包络；若可以直接抓取目标物体，则抓取，否则先对阻碍物体进行搬移而后完成对目标物体的抓取。本发明提高了服务机器人的抓取质量。

Description

基于椭圆锥人工势场的服务机器人抓取目标物体的方法
技术领域
本发明涉及服务机器人技术领域，具体涉及一种基于椭圆锥 人工势场的服务机器人抓取目标物体的方法、系统、装置。
背景技术
随着机器人技术的快速发展，服务机器人逐渐进入人们的日常 生活。为更好的满足人类的需求，提供更为优质的服务，服务机器人自身携 带机械臂。国内外研究人员在安装有机械臂的服务机器人的目标物体抓取方 面开展了深入研究，其中普遍采用视觉传感器对抓取场景进行感知。在服务 机器人到达指定操作区域后，服务机器人将结合视觉传感器的感知结果，控 制机械臂实施抓取。服务机器人的目标物体抓取涉及基于视觉的目标物体检 测以及机械臂规划两方面。基于视觉的目标物体检测是服务机器人抓取的前 提，传统的物体检测方法通常需要手工设计特征，环境适应性较差。随着深 度学习的发展，基于深度学习的物体检测方法成为当前研究的重点，代表性 方法包括Faster R-CNN(fasterregion-based convolutional neural network)、 YOLO(you only look once)、SSD(single shot multibox detector)等。在 完成目标物体的检测后，服务机器人将基于目标物体的检测结果进行自身机 械臂的运动规划，进而实施对目标物体的抓取。一种常见的进行机械臂运动 规划的方式是调用MoveIt！功能包，目前MoveIt！功能包已经广泛应用于主流 的机械臂运动规划中。另外，对于获取的点云数据，一种常见的方式是直接 利用在服务机器人和无人驾驶等领域得到广泛应用的PCL点云库进行点云 数据的处理(包括直通滤波、欧式聚类、拟合等)，还有在PCL点云库基 础上对点云数据进行更深入的处理，例如基于主成分分析(Principal Component Analysis，PCA)的最小包围盒获取算法等，可参考文献：孙永 伟.基于最小包围盒及自适应聚类的三维R*-树索引结构.山东理工大学硕 士学位论文,2011。
现有的服务机器人抓取技术通常考虑目标物体能够被直接抓 取的情形，从而在完成对指定目标物体的检测后，直接控制机械臂抓取目标 物体。但是在现实环境中，一些情况下目标物体周围会存在障碍物体，特别 是部分障碍物体的存在将使得目标物体不能直接被机械臂抓取，这就需要先 行将阻碍物体(即妨碍目标物体被直接抓取的障碍物体)搬移，最后抓取目 标物体。针对阻碍物体的搬移，通常有两种解决方案。第一种是将阻碍物体 搬移到人工指定位置，该解决方案缺乏智能性。第二种解决方案限定阻碍物 体的朝向进而简化该阻碍物体的被搬移位置的计算过程；在实际环境中阻碍 物体是随意放置的，从而它的朝向可以任意方向，这使得第二种解决方案缺 乏通用性。另外，如何结合阻碍物体的形状和朝向，更好的计算阻碍物体的 待放置位置也需要进一步研究。为此，需要对现有的机器人抓取技术进行更 深入的研究，以解决现有技术在目标物体周围存在阻碍物体的情形下，难以 有效的应对搬移任意朝向的阻碍物体时的服务机器人抓取问题。
发明内容
为了解决现有技术中的上述问题，即为了解决现有的抓取方法 在目标物体周围存在阻碍物体的情形下，难以有效的搬移任意朝向的阻碍物 体，导致服务机器人抓取质量较差的问题，本发明提出一种基于椭圆锥人工 势场的服务机器人抓取目标物体的方法，包括以下步骤：
步骤S100，服务机器人通过传感器获取周围环境的彩色图像以 及其在相机坐标系下的原始点云数据；
步骤S200，通过基于深度学习的物体检测方法获取所述彩色图 像中目标物体的包围框；并将所述目标物体的包围框在原始点云数据中对应 的点云数据作为目标物体的第一点云数据，剩余的点云数据作为第一环境点 云数据；
步骤S300，根据相机坐标系与服务机器人机械臂坐标系 OrXrYrZr的坐标转换关系，对所述目标物体的第一点云数据、所述第一环境 点云数据进行转换，得到目标物体的第二点云数据、第二环境点云数据；
步骤S400，去除服务机器人的机械臂在XrYr平面的工作空间外 的第二环境点云数据，将剩余的第二环境点云数据作为第三环境点云数据， 并通过PCL点云库对目标物体所在平面进行拟合，得到目标物体所在平面的 平面方程；
步骤S500，基于所述平面方程，去除第三环境点云数据中位于 目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数 据，进行第四环境点云数据的点云簇聚类；通过第一算法获得聚类后各点云 簇所对应障碍物体的最小包围盒，进而得到障碍物体的尺寸、位置及朝向信 息；所述第一算法为基于主成分分析的最小包围盒获取算法；
步骤S600，对目标物体，结合所述平面方程，在所述目标物 体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的 数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的 最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；
步骤S700，基于目标物体和各障碍物体的尺寸、位置以及朝 向信息，构建目标物体、各障碍物体对应的最小椭圆包络；
步骤S800，基于目标物体和各障碍物体的尺寸、位置及朝向信 息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否则， 结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻碍物 体进行搬移而后完成对目标物体的抓取。
在一些优选的实施方式中，所述相机坐标系与服务机器人机 械臂坐标系OrXrYrZr的坐标转换关系为：
Figure BDA0002586507820000041
其中，(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个顶点的坐标
Figure BDA0002586507820000042
Nc为障碍物体的个数， 根据
Figure BDA0002586507820000043
计算出对应障碍物体的高度；
将各障碍物体相应的最小包围盒Bj垂直投影到目标物体所在 平面上，得到相应投影矩形
Figure BDA0002586507820000044
的4个顶点的坐标
Figure BDA0002586507820000045
根据
Figure BDA0002586507820000051
计算相应投影矩形
Figure BDA0002586507820000052
的中心点坐标、长、宽以及该投影矩形 的长边相对于Xr轴方向的角度；
投影矩形的中心点坐标反映了对应障碍物体的位置信息，将投 影矩形的长边相对于Xr轴方向的角度作为对应障碍物体的朝向信息，投影矩 形的长、宽以及障碍物体的高度描述了对应障碍物体的尺寸信息。
在一些优选的实施方式中，步骤S700“基于目标物体和各障 碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的 最小椭圆包络”，其方法为：
根据物体Oj的投影矩形
Figure BDA0002586507820000053
的顶点信息
Figure BDA0002586507820000054
中心点坐 标
Figure BDA0002586507820000055
长
Figure BDA0002586507820000056
宽
Figure BDA0002586507820000057
以及该投影矩形的长边相对于Xr轴方向的角度
Figure BDA0002586507820000058
计算最小椭圆包络
Figure BDA0002586507820000059
的离心率e、半长轴a和半短轴b；
将得到的半长轴a和半短轴b代入标准椭圆方程
Figure BDA00025865078200000510
并展开，得到最小椭圆包络
Figure BDA00025865078200000511
的一般方程：
Figure BDA00025865078200000512
其中，
Figure BDA00025865078200000513
Figure BDA00025865078200000514
Figure BDA00025865078200000515
为最小椭圆包络上的任意一点的坐标。
在一些优选的实施方式中，步骤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的坐 标变换关系如下：
Figure BDA0002586507820000111
其中，(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个顶点的坐标
Figure BDA0002586507820000131
根据
Figure BDA0002586507820000132
中Zr轴坐标的最大值
Figure BDA0002586507820000133
与最小值
Figure BDA0002586507820000134
求出障碍物体Oj的高度
Figure BDA0002586507820000135
步骤S513，将各障碍物体相应的最小包围盒Bj垂直投影到目 标物体所在平面上，j＝1,2,…,Nc，得到相应投影矩形
Figure RE-GDA0002586896970000136
的4个顶点的坐标
Figure RE-GDA0002586896970000137
进而根据
Figure RE-GDA0002586896970000138
计算出投影矩形
Figure RE-GDA0002586896970000139
的中心点坐标
Figure RE-GDA00025868969700001310
长
Figure RE-GDA00025868969700001311
宽
Figure RE-GDA00025868969700001312
以及该投影矩形的长边相对于Xr轴方向的角度
Figure RE-GDA00025868969700001313
和
Figure RE-GDA00025868969700001314
分别反映了障碍物体Oj的位置和朝向信息；障碍物体Oj的尺寸信息用
Figure RE-GDA00025868969700001315
和
Figure RE-GDA00025868969700001316
进行描述。
步骤S600，对目标物体，结合所述平面方程，在所述目标物 体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的 数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的 最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；
在本实施例中，结合目标物体所在平面的平面方程，在目标物 体的第二点云数据Dt中去除该平面以及该平面下方的点所对应的数据，得 到目标物体的第三点云数据Dtr，获得目标物体对应的最小包围盒，进而得 到目标物体的尺寸、位置以及朝向信息。
该步骤中，为了将障碍物体和目标物体的最小包围盒统一表征， 目标物体用Oj(j＝Nc+1)即
Figure BDA00025865078200001423
进行表示。对目标物体的第三点云数据Dtr采 用基于主成分分析的最小包围盒获取算法进行处理，得到目标物体对应的最 小包围盒，用
Figure BDA0002586507820000141
进行描述，进而得到
Figure BDA0002586507820000142
在机械臂坐标系OrXrYrZr下的8 个顶点的坐标
Figure BDA0002586507820000143
根据
Figure BDA0002586507820000144
中Zr轴坐标的最大值
Figure BDA0002586507820000145
与最小值
Figure BDA0002586507820000146
得到目标物体的高度
Figure BDA0002586507820000147
将
Figure BDA0002586507820000148
垂直 投影到目标物体所在平面上，得到相应投影矩形
Figure BDA0002586507820000149
的4个顶点的坐标
Figure BDA00025865078200001410
进而根据
Figure BDA00025865078200001411
计算出投影矩形
Figure BDA00025865078200001412
的中心点坐标
Figure BDA00025865078200001413
长
Figure BDA00025865078200001414
宽
Figure BDA00025865078200001415
以及该投影矩形的长边相对于Xr轴方 向的角度
Figure BDA00025865078200001416
和
Figure BDA00025865078200001417
分别反映了目标物体的位置和朝向 信息；目标物体的尺寸信息用
Figure BDA00025865078200001418
和
Figure BDA00025865078200001419
进行描述。
步骤S700，基于目标物体和各障碍物体的尺寸、位置以及 朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络；
在本实施例中，计算物体Oj(j＝1,2,…,Nc+1)的投影矩形
Figure BDA00025865078200001420
的最 小椭圆包络
Figure BDA00025865078200001421
的一般方程，
Figure BDA00025865078200001422
表征了物体Oj对应的最小椭圆包络。
该步骤中，计算物体Oj的投影矩形
Figure BDA0002586507820000151
的最小椭圆包络
Figure BDA0002586507820000152
的 一般方程的具体过程如下：
步骤S710，结合投影矩形
Figure BDA0002586507820000153
的顶点信息
Figure BDA0002586507820000154
中心 点坐标
Figure BDA0002586507820000155
长
Figure BDA0002586507820000156
宽
Figure BDA0002586507820000157
以及投影矩形的长边相对于Xr轴方向的 角度
Figure BDA0002586507820000158
计算
Figure BDA0002586507820000159
的离心率e、半长轴a和半短轴b如下：
Figure BDA00025865078200001510
Figure BDA00025865078200001511
Figure BDA00025865078200001512
其中，
Figure RE-GDA00025868969700001513
Figure RE-GDA00025868969700001514
步骤S711，将得到的半长轴a和半短轴b代入标准椭圆方程
Figure BDA00025865078200001515
其中，
Figure BDA00025865078200001516
Figure BDA00025865078200001517
是
Figure BDA00025865078200001518
上的任意一点 的坐标；将该标准椭圆方程展开得到
Figure BDA00025865078200001519
的一般方程如下：
Figure BDA00025865078200001520
其中，
Figure BDA00025865078200001521
Figure BDA00025865078200001522
Figure BDA00025865078200001523
步骤S800，基于目标物体和各障碍物体的尺寸、位置及朝向 信息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否 则，结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻 碍物体进行搬移而后完成对目标物体的抓取。
在本实施例中，若不存在障碍物体即Nc＝0时，意味着可以直 接抓取目标物体，此时，基于目标物体的尺寸、位置、朝向信息，通过MoveIt！ 功能包进行机械臂的运动规划，控制机械臂完成对目标物体的抓取。若存在 障碍物体，基于目标物体和各障碍物体的尺寸、位置、朝向信息，通过MoveIt！ 功能包判断服务机器人的机械臂是否可以直接抓取目标物体，如果能够直接 抓取目标物体，则机械臂将根据MoveIt！功能包的规划结果直接对目标物体 进行抓取，否则，先对阻碍物体进行搬移再完成对目标物体的抓取，其中， 机械臂末端的抓取方向均为沿Zr轴的负方向竖直向下抓取。
对阻碍物体进行搬移再完成对目标物体的抓取，具体处理过程 如下：
步骤S810，计算各障碍物体相应的投影矩形的中心点坐标
Figure BDA0002586507820000161
与目标物体相应的投影矩形的中心点坐标
Figure BDA0002586507820000162
之 间的距离dk，k＝1,2,…,Nc，将各障碍物体的ID号及其对应的距离dk组合起 来，形成障碍物体影响序列；
步骤S811，根据障碍物体影响序列中各障碍物体对应的距离， 选取最小距离所对应的障碍物体作为本次待搬移阻碍物体Oig；
步骤S812，在机械臂作用的XrYr平面的工作空间Srm内，生成 本次待搬移阻碍物体Oig对应的椭圆锥引力势场Fatt：
Figure BDA0002586507820000163
其中，
Figure BDA0002586507820000164
Figure BDA0002586507820000165
描述工作空间Srm内的任意一点，当该点位于本次待搬移阻碍物体 Oig的投影矩形的最小椭圆包络边界上，
Figure BDA0002586507820000166
当该点位于本次待搬移阻 碍物体Oig的投影矩形的最小椭圆包络内部时，
Figure BDA0002586507820000171
当
Figure BDA0002586507820000172
时，
Figure BDA0002586507820000173
dcg是本次待搬移阻碍物体Oig与其它物体(除Oig之外的其 他障碍物体以及目标物体)之间距离的最小值，μ是预设的引力势场系数，
Figure BDA0002586507820000174
Figure BDA0002586507820000175
和
Figure BDA0002586507820000176
描述本次待搬移阻碍物体Oig的尺寸 信息，分别对应Oig的长、宽和高，Cat为预设的常数；
步骤S813，对于每一个其他物体Or(除本次待搬移阻碍物体 Oig之外的其他障碍物体以及目标物体)，在机械臂作用的XrYr平面的工作 空间Srm内，产生相应的椭圆锥斥力势场
Figure BDA0002586507820000177
计算如下：
Figure BDA0002586507820000178
其中，
Figure BDA0002586507820000179
Figure BDA00025865078200001710
描述工作空间Srm内的任意一点，当该点位于Or的投影矩形的最 小椭圆包络边界上，
Figure BDA00025865078200001711
当该点位于Or的投影矩形的最小椭圆包络内 部时，
Figure BDA00025865078200001712
当
Figure BDA00025865078200001713
时，
Figure BDA00025865078200001714
Figure BDA00025865078200001715
是物体Or与Oig之间的距 离，σ是预设的斥力势场系数；
Figure BDA00025865078200001716
Figure BDA00025865078200001717
和
Figure BDA00025865078200001718
描 述Or的尺寸信息，分别对应Or的长、宽和高；Crep为预设的常数；
步骤S814，上述得到的椭圆锥引力势场Fatt和Nc个椭圆锥斥 力势场
Figure BDA00025865078200001719
都是椭圆锥人工势场，将所有的椭圆锥人工势场按照下式叠加起 来，形成本次待搬移阻碍物体Oig对应的椭圆锥合势场Fres：
Figure BDA00025865078200001720
其中，
Figure BDA0002586507820000181
τ是预设权重；
步骤S815，选取椭圆锥合势场Fres的最小值对应的位置，作为 Oig的待放置位置，如果Fres的最小值对应的位置数量不止一个，选取距离 Oig的投影矩形的中心点最近的位置作为待放置位置；
步骤S816，基于本次待搬移阻碍物体Oig的当前位置、尺寸、 朝向信息以及它的待放置位置、其他障碍物体和目标物体的位置、尺寸和朝 向信息，采用MoveIt！功能包进行机械臂的运动规划，完成对Oig的搬移(至 此，前面提到的Oig的“待放置位置”改称为Oig的“搬移后的位置”，Oig也由“本次待搬移阻碍物体”改称为“被搬移物体”)；将Oig移出障碍物 体影响序列，同时，Oig在XrYr平面上投影矩形的中心点坐标用它搬移后的 位置加以更新；
步骤S817，基于目标物体的位置、尺寸、朝向信息，以及被 搬移物体Oig的尺寸、朝向、搬移后的位置信息、其他障碍物体的当前位置、 尺寸、朝向信息，通过MoveIt！功能包判断目标物体是否可以被直接抓取， 如果目标物体不能被直接抓取，则返回步骤S811；否则，通过MoveIt！功能 包进行机械臂的运动规划，控制机械臂完成对目标物体的抓取。
在一个具体实施例中，Kinect传感器倾斜向下安装，其倾斜角 度为45°，
Figure BDA0002586507820000182
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、或技术领域内所 公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的 可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及 步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案 的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来 使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明 的范围。
术语“第一”、“第二”、“第三”等是用于区别类似的对 象，而不是用于描述或表示特定的顺序或先后次序。
至此，已经结合附图所示的优选实施方式描述了本发明的技术 方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限 于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可 以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案 都将落入本发明的保护范围之内。

Claims (8)

1.一种基于椭圆锥人工势场的服务机器人抓取目标物体的方法，其特征在于，该方法包括：
步骤S100，服务机器人通过传感器获取周围环境的彩色图像以及其在相机坐标系下的原始点云数据；
步骤S200，通过基于深度学习的物体检测方法获取所述彩色图像中目标物体的包围框；并将所述目标物体的包围框在原始点云数据中对应的点云数据作为目标物体的第一点云数据，剩余的点云数据作为第一环境点云数据；
步骤S300，根据相机坐标系与服务机器人机械臂坐标系OrXrYrZr的坐标转换关系，对所述目标物体的第一点云数据、所述第一环境点云数据进行转换，得到目标物体的第二点云数据、第二环境点云数据；
步骤S400，去除服务机器人的机械臂在XrYr平面的工作空间外的第二环境点云数据，将剩余的第二环境点云数据作为第三环境点云数据，并通过PCL点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程；
步骤S500，基于所述平面方程，去除第三环境点云数据中位于目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数据，进行第四环境点云数据的点云簇聚类；通过第一算法获得聚类后各点云簇所对应障碍物体的最小包围盒，进而得到障碍物体的尺寸、位置及朝向信息；所述第一算法为基于主成分分析的最小包围盒获取算法；
其中，进而得到障碍物体的尺寸、位置及朝向信息，其方法为：
基于各障碍物体相应的最小包围盒Bj，j＝1,2,…,Nc，得到Bj在机械臂坐标系OrXrYrZr下的8个顶点的坐标
Figure FDA0003781678610000011
Nc为障碍物体的个数，根据
Figure FDA0003781678610000012
计算出对应障碍物体的高度；
将各障碍物体相应的最小包围盒Bj垂直投影到目标物体所在平面上，得到相应投影矩形
Figure FDA0003781678610000021
的4个顶点的坐标
Figure FDA0003781678610000022
根据
Figure FDA0003781678610000023
计算相应投影矩形
Figure FDA0003781678610000024
的中心点坐标、长、宽以及该投影矩形的长边相对于Xr轴方向的角度；
投影矩形的中心点坐标反映了对应障碍物体的位置信息，将投影矩形的长边相对于Xr轴方向的角度作为对应障碍物体的朝向信息，投影矩形的长、宽以及障碍物体的高度描述了对应障碍物体的尺寸信息；
步骤S600，对目标物体，结合所述平面方程，在所述目标物体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；
步骤S700，基于目标物体和各障碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络：
根据物体Oj的投影矩形
Figure FDA0003781678610000025
的顶点信息
Figure FDA0003781678610000026
中心点坐标
Figure FDA0003781678610000027
长
Figure FDA0003781678610000028
宽
Figure FDA0003781678610000029
以及该投影矩形的长边相对于Xr轴方向的角度
Figure FDA00037816786100000210
计算最小椭圆包络
Figure FDA00037816786100000211
的离心率e、半长轴a和半短轴b；
将得到的半长轴a和半短轴b代入标准椭圆方程
Figure FDA00037816786100000212
并展开，得到最小椭圆包络
Figure FDA00037816786100000213
的一般方程：
Figure FDA00037816786100000214
其中，
Figure FDA00037816786100000215
Figure FDA00037816786100000216
Figure FDA00037816786100000217
Figure FDA00037816786100000218
为最小椭圆包络上的任意一点的坐标，
Figure FDA00037816786100000219
步骤S800，基于目标物体和各障碍物体的尺寸、位置及朝向信息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否则，结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取。
2.根据权利要求1所述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法，其特征在于，所述相机坐标系与服务机器人机械臂坐标系OrXrYrZr的坐标转换关系为：
Figure FDA0003781678610000031
其中，(xcp,ycp,zcp)T和(xrp,yrp,zrp)T分别为原始点云数据Ds中的点在相机坐标系OcXcYcZc和服务机器人机械臂坐标系OrXrYrZr中的坐标，Tm为预设的矩阵。
3.根据权利要求2所述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法，其特征在于，步骤S400中“通过PCL点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程”，其方法为：
通过PCL点云库的pass.filter滤波算法对第三环境点云数据进行滤波处理；
处理后，通过PCL点云库中的ransac拟合算法进行平面拟合，获得拟合平面的法向量(Aop,Bop,Cop)和截距Dop，进而得到目标物体所在平面的平面方程为Aop·x+Bop·y+Cop·z+Dop＝0。
4.根据权利要求3所述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法，其特征在于，步骤S500中“进行第四环境点云数据的点云簇聚类”，其方法为：通过欧式聚类算法将第四环境点云数据进行点云簇聚类。
5.根据权利要求4所述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法，其特征在于，步骤S800中“通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取”，其方法为：
步骤S810，计算各障碍物体相应的投影矩形的中心点坐标与目标物体相应的投影矩形的中心点坐标之间的距离，并与各障碍物体的ID号结合，构建障碍物体影响序列；
步骤S811，选取障碍物体影响序列中最小距离所对应的障碍物体作为本次待搬移阻碍物体；
步骤S812，在机械臂作用的XrYr平面的工作空间Srm内，生成本次待搬移阻碍物体对应的椭圆锥引力势场；
步骤S813，生成除本次待搬移阻碍物体之外的其他障碍物体以及目标物体在Srm内相应的椭圆锥斥力势场；
步骤S814，将椭圆锥斥力势场与椭圆锥引力势场进行叠加，得到本次待搬移阻碍物体对应的椭圆锥合势场；
步骤S815，选取椭圆锥合势场的最小值对应的位置，作为本次待搬移阻碍物体的待放置位置，如果椭圆锥合势场的最小值对应的位置数量不止一个，选取距离本次待搬移阻碍物体的投影矩形的中心点最近的位置作为待放置位置；
步骤S816，基于本次待搬移阻碍物体的当前位置、尺寸、朝向信息以及待放置位置、其他障碍物体和目标物体的位置、尺寸和朝向信息，采用MoveIt！功能包进行机械臂的运动规划，完成对本次待搬移阻碍物体的搬移；将本次待搬移阻碍物体移出障碍物体影响序列，同时，本次待搬移阻碍物体相应投影矩形的中心点坐标用它搬移后的位置加以更新；
步骤S817，基于目标物体的位置、尺寸、朝向信息，以及本次待搬移阻碍物体的尺寸、朝向、搬移后的位置信息、其他障碍物体的当前位置、尺寸、朝向信息，通过MoveIt！功能包判断目标物体是否可以被直接抓取，如果目标物体不能被直接抓取，则返回步骤S811；否则，通过MoveIt！功能包进行机械臂的运动规划，控制机械臂完成对目标物体的抓取。
6.一种基于椭圆锥人工势场的服务机器人抓取目标物体的系统，其特征在于，该系统包括：获取模块、检测模块、转换模块、拟合模块、聚类模块、目标物体信息提取模块、最小椭圆包络构建模块、抓取模块；
所述获取模块，配置为服务机器人通过传感器获取周围环境的彩色图像以及其在相机坐标系下的原始点云数据；
所述检测模块，配置为通过基于深度学习的物体检测方法获取所述彩色图像中目标物体的包围框；并将所述目标物体的包围框在原始点云数据中对应的点云数据作为目标物体的第一点云数据，剩余的点云数据作为第一环境点云数据；
所述转换模块，配置为根据相机坐标系与服务机器人机械臂坐标系OrXrYrZr的坐标转换关系，对所述目标物体的第一点云数据、所述第一环境点云数据进行转换，得到目标物体的第二点云数据、第二环境点云数据；
所述拟合模块，配置为去除服务机器人的机械臂在XrYr平面的工作空间外的第二环境点云数据，将剩余的第二环境点云数据作为第三环境点云数据，并通过PCL点云库对目标物体所在平面进行拟合，得到目标物体所在平面的平面方程；
所述聚类模块，配置为基于所述平面方程，去除第三环境点云数据中位于目标物体所在平面以及该平面下方的点所对应的数据，得到第四环境点云数据，进行第四环境点云数据的点云簇聚类；通过第一算法获得聚类后各点云簇所对应障碍物体的最小包围盒，进而得到障碍物体的尺寸、位置及朝向信息；所述第一算法为基于主成分分析的最小包围盒获取算法；
其中，进而得到障碍物体的尺寸、位置及朝向信息，其方法为：
基于各障碍物体相应的最小包围盒Bj，j＝1,2,…,Nc，得到Bj在机械臂坐标系OrXrYrZr下的8个顶点的坐标
Figure FDA0003781678610000061
Nc为障碍物体的个数，根据
Figure FDA0003781678610000062
计算出对应障碍物体的高度；
将各障碍物体相应的最小包围盒Bj垂直投影到目标物体所在平面上，得到相应投影矩形
Figure FDA0003781678610000063
的4个顶点的坐标
Figure FDA0003781678610000064
根据
Figure FDA0003781678610000065
计算相应投影矩形
Figure FDA0003781678610000066
的中心点坐标、长、宽以及该投影矩形的长边相对于Xr轴方向的角度；
投影矩形的中心点坐标反映了对应障碍物体的位置信息，将投影矩形的长边相对于Xr轴方向的角度作为对应障碍物体的朝向信息，投影矩形的长、宽以及障碍物体的高度描述了对应障碍物体的尺寸信息；
所述目标物体信息提取模块，配置为对目标物体，结合所述平面方程，在所述目标物体的第二点云数据中去除目标物体所在平面以及该平面下方的点所对应的数据，得到目标物体的第三点云数据，并通过第一算法获得目标物体对应的最小包围盒，进而得到目标物体的尺寸、位置及朝向信息；
所述最小椭圆包络构建模块，配置为基于目标物体和各障碍物体的尺寸、位置以及朝向信息，构建目标物体、各障碍物体对应的最小椭圆包络：
根据物体Oj的投影矩形
Figure FDA0003781678610000067
的顶点信息
Figure FDA0003781678610000068
中心点坐标
Figure FDA0003781678610000069
长
Figure FDA00037816786100000610
宽
Figure FDA00037816786100000611
以及该投影矩形的长边相对于Xr轴方向的角度
Figure FDA00037816786100000612
计算最小椭圆包络
Figure FDA00037816786100000613
的离心率e、半长轴a和半短轴b；
将得到的半长轴a和半短轴b代入标准椭圆方程
Figure FDA00037816786100000614
并展开，得到最小椭圆包络
Figure FDA00037816786100000615
的一般方程：
Figure FDA00037816786100000616
其中，
Figure FDA00037816786100000617
Figure FDA0003781678610000071
Figure FDA0003781678610000072
Figure FDA0003781678610000073
为最小椭圆包络上的任意一点的坐标，
Figure FDA0003781678610000074
所述抓取模块，配置为基于目标物体和各障碍物体的尺寸、位置及朝向信息，判断是否可以直接抓取目标物体，若可以，则直接抓取目标物体；否则，结合目标物体和各障碍物体的最小椭圆包络，通过人工势场法，先对阻碍物体进行搬移而后完成对目标物体的抓取。
7.一种存储装置，其中存储有多条程序，其特征在于，所述程序应用由处理器加载并执行以实现权利要求1-5任一项所述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法。
8.一种处理装置，包括处理器、存储装置；处理器，适用于执行各条程序；存储装置，适用于存储多条程序；其特征在于，所述程序适用于由处理器加载并执行以实现权利要求1-5任一项所述的基于椭圆锥人工势场的服务机器人抓取目标物体的方法。
CN202010682937.7A 2020-07-15 2020-07-15 基于椭圆锥人工势场的服务机器人抓取目标物体的方法 Active CN111882610B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010682937.7A CN111882610B (zh) 2020-07-15 2020-07-15 基于椭圆锥人工势场的服务机器人抓取目标物体的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010682937.7A CN111882610B (zh) 2020-07-15 2020-07-15 基于椭圆锥人工势场的服务机器人抓取目标物体的方法

Publications (2)

Publication Number Publication Date
CN111882610A CN111882610A (zh) 2020-11-03
CN111882610B true CN111882610B (zh) 2022-09-20

Family

ID=73154588

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010682937.7A Active CN111882610B (zh) 2020-07-15 2020-07-15 基于椭圆锥人工势场的服务机器人抓取目标物体的方法

Country Status (1)

Country Link
CN (1) CN111882610B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112802093B (zh) * 2021-02-05 2023-09-12 梅卡曼德（北京）机器人科技有限公司 对象抓取方法及装置
CN113240678B (zh) * 2021-05-10 2023-05-30 青岛小鸟看看科技有限公司 平面信息检测方法及系统
US11741621B2 (en) 2021-05-10 2023-08-29 Qingdao Pico Technology Co., Ltd. Method and system for detecting plane information
CN113284129A (zh) * 2021-06-11 2021-08-20 梅卡曼德（北京）机器人科技有限公司 基于3d包围盒的压箱检测方法及装置
CN113492405B (zh) * 2021-06-17 2022-08-02 达闼机器人股份有限公司 机器人开门方法、装置、存储介质及电子设备
CN116175541B (zh) * 2021-11-28 2023-11-10 梅卡曼德（北京）机器人科技有限公司 抓取控制方法、装置、电子设备和存储介质
CN114973006B (zh) * 2022-08-02 2022-10-18 四川省机械研究设计院(集团)有限公司 花椒采摘方法、装置、系统及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107608346A (zh) * 2017-08-30 2018-01-19 武汉理工大学 基于人工势场的船舶智能避障方法及系统
CN108858199A (zh) * 2018-07-27 2018-11-23 中国科学院自动化研究所 基于视觉的服务机器人抓取目标物体的方法
CN110244713A (zh) * 2019-05-22 2019-09-17 江苏大学 一种基于人工势场法的智能车辆换道轨迹规划系统及方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6606442B2 (ja) * 2016-02-24 2019-11-13 本田技研工業株式会社 移動体の経路計画生成装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107608346A (zh) * 2017-08-30 2018-01-19 武汉理工大学 基于人工势场的船舶智能避障方法及系统
CN108858199A (zh) * 2018-07-27 2018-11-23 中国科学院自动化研究所 基于视觉的服务机器人抓取目标物体的方法
CN110244713A (zh) * 2019-05-22 2019-09-17 江苏大学 一种基于人工势场法的智能车辆换道轨迹规划系统及方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Grasping of unknown objects on a planar surface using a single depth image;Toshitaka Suzuki 等;《2016 IEEE International Conference on Advanced Intelligent Mechatronics (AIM)》;20160715;第572-577页 *
基于椭圆碰撞锥的无人艇动态避障方法;蒲华燕 等;《仪器仪表学报》;20170731;第38卷(第7期);第1762-1756页 *

Also Published As

Publication number Publication date
CN111882610A (zh) 2020-11-03

Similar Documents

Publication Publication Date Title
CN111882610B (zh) 基于椭圆锥人工势场的服务机器人抓取目标物体的方法
CN108858199B (zh) 基于视觉的服务机器人抓取目标物体的方法
US11325252B2 (en) Action prediction networks for robotic grasping
CN108247637B (zh) 一种工业机器人手臂视觉防撞操控方法
JP6091560B2 (ja) 画像解析方法
JP2006520055A (ja) ２次元画像からの３次元オブジェクトの不変視点検出および識別
CN110653820B (zh) 一种结合几何约束的机器人抓取位姿估计方法
CN109923583A (zh) 一种姿态的识别方法、设备及可移动平台
Dang et al. Semantic grasping: planning task-specific stable robotic grasps
CN109858437B (zh) 基于生成查询网络的行李体积自动分类方法
JPH08212329A (ja) 適応的認識システム
JPWO2019030794A1 (ja) 情報処理装置、モデルデータ作成プログラム、モデルデータ作成方法
CN111079565A (zh) 视图二维姿态模板的构建方法及识别方法、定位抓取系统
Krzeszowski et al. Articulated body motion tracking by combined particle swarm optimization and particle filtering
WO2022031232A1 (en) Method and device for point cloud based object recognition
JP2018169660A (ja) オブジェクト姿勢検出装置、制御装置、ロボットおよびロボットシステム
CN114494594B (zh) 基于深度学习的航天员操作设备状态识别方法
CN113420648A (zh) 一种具有旋转适应性的目标检测方法及系统
JP7051751B2 (ja) 学習装置、学習方法、学習モデル、検出装置及び把持システム
KR101107735B1 (ko) 카메라 포즈 결정 방법
CN111709095A (zh) 一种面向复杂曲面6d虚拟夹具构造方法
CN115862074A (zh) 人体指向确定、屏幕控制方法、装置及相关设备
Hirt et al. Geometry extraction for ad hoc redirected walking using a slam device
JP3610087B2 (ja) 移動物体追跡装置
Schnaubelt et al. Autonomous assistance for versatile grasping with rescue robots

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant