基于视觉的服务机器人抓取目标物体的方法技术

本发明专利技术涉及服务机器人技术领域，具体涉及一种基于视觉的服务机器人抓取目标物体的方法，旨在解决存在阻碍目标物体被直接抓取的障碍物情形下的机器人抓取问题。本发明专利技术中抓取目标物体的方法包括：获取彩色图像以及相机坐标系下的原始三维点云数据；进行目标物体的检测，获取目标物体的点云数据，并得到目标物体周围的环境点云数据，将上述点云数据变换到机械臂坐标系下；在机械臂坐标系下，拟合目标物体所在平面的平面方程；在此基础上，获取各障碍物的位置和尺寸信息，以及目标物体的位置及尺寸信息；基于上述位置和尺寸信息，先对阻碍目标物体被直接抓取的障碍物进行搬移，而后完成对目标物体的抓取。本发明专利技术有效提高了服务机器人的抓取能力。

Method of Grabbing Target Objects by Visual-based Service Robot

The invention relates to the technical field of service robots, in particular to a visual-based method for service robots to grasp target objects, aiming at solving the problem of robot grasping in the presence of obstacles that prevent target objects from being grasped directly. The method of capturing the target object in the present invention includes: acquiring the color image and the original three-dimensional point cloud data in the camera coordinate system; detecting the target object, acquiring the point cloud data of the target object, and obtaining the environmental point cloud data around the target object, and transforming the point cloud data into the coordinate system of the manipulator; Fitting the plane equation of the target object in the arm coordinate system; on this basis, obtaining the position and size information of each obstacle, as well as the position and size information of the target object; based on the above position and size information, the obstacle that prevents the target object from being grasped directly is moved first, and then the target is completed. Grabbing of marker objects. The invention effectively improves the grasping ability of the service robot.

【技术实现步骤摘要】
基于视觉的服务机器人抓取目标物体的方法
本专利技术涉及服务机器人
，具体涉及一种基于视觉的服务机器人抓取目标物体的方法。
技术介绍
随着服务机器人和人工智能技术的不断发展，其应用领域也在不断拓展。智能服务机器人的核心技术除了环境感知、定位、导航外，物体抓取技术也非常重要，是服务机器人提供优质服务的前提。例如，在家庭、办公、医护等环境中，物品的取拿等服务均需要服务机器人具有抓取物体的能力。为了使得服务机器人实施对物体的抓取，需要将机械臂安装在服务机器人的移动平台上。在移动平台到达指定操作区域后，服务机器人将控制机械臂实施抓取。服务机器人抓取涉及到对目标的检测和机械臂的运动规划，其中基于视觉的机器人抓取目标物体的方法研究最为普遍。国内外研究人员在基于视觉的机器人抓取方面开展了深入的研究。目标检测是服务机器人抓取的前提，传统的目标检测方法通常需要手工设计特征，泛化能力不强，鲁棒性较差。近年来深度学习受到关注，其优势在于特征提取环节不需要手工设计特征，而是通过利用大规模数据集对模型进行训练，学得目标物体具备的特征，具有较强的鲁棒性。目前常见的基于深度学习的目标检测方法有RCNN、Fast-RCNN、Faster-RCNN、YOLO、SSD等，其中，SSD结合了YOLO快速和Faster-RCNN检测精度高的优点而受到普遍关注。在完成目标物体检测后，服务机器人即可进行自身机械臂的运动规划，进而完成对目标物体的抓取，一种常见的进行机械臂运动规划的方式是调用MoveIt！功能包。MoveIt！功能包集成了机械臂的正逆运动学求解、运动规划、碰撞检测等功能，其中运动规划使用开源运动规划库(OpenMotionPlanningLibrary，OMPL)，目前MoveIt！功能包已经广泛应用于主流的机械臂运动规划中。现有的机器人抓取技术通常在完成对指定目标物体的检测后，直接实施对该目标物体的抓取。实际上，在现实环境中，目标物体周围有时会存在一些阻碍服务机器人的机械臂直接抓取的障碍物。为此，需要对现有的机器人抓取技术进行更深入的研究，以解决现有技术难以较好地满足存在阻碍目标物体被直接抓取的障碍物情形下的机器人抓取问题。
技术实现思路
为了解决现有技术中的上述问题，本专利技术提出了一种基于视觉的服务机器人抓取目标物体的方法，能够在存在阻碍目标物体被直接抓取的障碍物的情形下，实现服务机器人对目标物体的抓取，提高了服务机器人的抓取能力。本专利技术提出一种基于视觉的服务机器人抓取目标物体的方法，包括以下步骤：步骤S1，通过安装于服务机器人上面的Kinect传感器获取彩色图像Is以及相机坐标系OcXcYcZc下的原始三维点云数据Ds；步骤S2，基于所述彩色图像Is，利用深度学习SSD(SingleShotmultiBoxDetector)目标检测方法进行目标物体的检测，进而从所述原始三维点云数据Ds中获得所述目标物体的第一点云数据Dto；在所述原始三维点云数据Ds中去除所述目标物体的第一点云数据Dto，得到第一环境点云数据Drs；步骤S3，根据相机坐标系OcXcYcZc到所述服务机器人的机械臂坐标系OrXrYrZr的坐标变换关系，将所述第一环境点云数据Drs转换到所述机械臂坐标系OrXrYrZr下，得到第二环境点云数据Dr；将所述目标物体的第一点云数据Dto转换到所述机械臂坐标系OrXrYrZr下，得到所述目标物体的第二点云数据Dt；步骤S4，结合机械臂在XrYr平面的工作空间Srm，对所述第二环境点云数据Dr，去除Yr轴方向上数值位于[ymin,ymax]区间范围外的数据，以及去除Xr轴方向上数值位于[xmin,xmax]区间范围外的数据，得到第三环境点云数据Df；其中，所述工作空间Srm的左下角和右上角坐标分别为(xmin,ymin)和(xmax,ymax)，xmin、ymin、xmax和ymax均为预设的阈值；步骤S5，基于所述第三环境点云数据Df，采用随机抽样一致性(RandomSampleConsensus，RANSAC)算法对所述目标物体所在平面进行拟合，得到所述目标物体所在平面的平面方程；步骤S6，结合所述目标物体所在平面的平面方程，在所述第三环境点云数据Df中去除该平面以及该平面下方的点所对应的数据，得到第四环境点云数据Do，进而进行点云簇聚类，并获取各点云簇所对应障碍物的位置及尺寸信息；步骤S7，结合所述平面方程，在所述目标物体的第二点云数据Dt中去除该平面以及该平面下方的点所对应的数据，得到所述目标物体的第三点云数据Dtr，并求出所述目标物体的位置及尺寸信息；步骤S8，基于所述目标物体的位置和尺寸信息，以及各障碍物的位置和尺寸信息，先对阻碍所述目标物体被直接抓取的障碍物进行搬移，而后完成对所述目标物体的抓取。优选地，步骤S3中所述相机坐标系OcXcYcZc到服务机器人的机械臂坐标系OrXrYrZr的坐标变换关系为：其中，(xcp,ycp,zcp)T和(xrp,yrp,zrp)T分别为所述原始三维点云数据Ds中的点在所述相机坐标系OcXcYcZc和所述机械臂坐标系OrXrYrZr中的坐标，Tm为手眼关系矩阵；所述手眼关系矩阵的计算方法包括：控制机械臂末端移动到不同的位置，分别获得所述机械臂末端在所述相机坐标系OcXcYcZc和所述机械臂坐标系OrXrYrZr中的ng个三维坐标和对应的齐次坐标分别为和其中，ng为预设的常数，t＝1,2,…,ng；根据下式计算所述手眼关系矩阵：其中，表示求取矩阵的伪逆。优选地，获得所述机械臂末端在所述相机坐标系OcXcYcZc中坐标的方法具体为：在机械臂末端固定一个红色小球，基于所述Kinect传感器提供的彩色图像，采用基于颜色特征的目标识别算法识别所述红色小球，从而获得所述红色小球的中心点在所述相机坐标系OcXcYcZc中的坐标(xc,yc,zc)T，作为所述机械臂末端在所述相机坐标系OcXcYcZc中的坐标。优选地，获得所述机械臂末端在所述机械臂坐标系OrXrYrZr中坐标的方法具体为：利用MoveIt！功能包通过正运动学求解得到所述机械臂末端在所述机械臂坐标系OrXrYrZr中的三维坐标(xr,yr,zr)T。优选地，步骤S5中“基于所述第三环境点云数据Df，采用随机抽样一致性算法对所述目标物体所在平面进行拟合，得到所述目标物体所在平面的平面方程”，具体包括：步骤S51，从所述第三环境点云数据Df中任选3个数据，根据所述3个数据所对应的点计算出一个平面；步骤S52，求取所述第三环境点云数据Df中所述3个数据以外的数据所对应的点到该平面的距离，将距离在dh范围内的点作为内点构成该平面对应的内点集，其中dh为预设的距离阈值；步骤S53，重复执行步骤S51-S52，直至所述内点集之中内点的数量与所述第三环境点云数据Df中所有数据的数量的比值超过λ或达到预设迭代次数MK；其中λ为预设的数量比的阈值；步骤S54，以最大的内点数量所对应的平面方程作为所述目标物体所在平面的平面方程，该平面方程在所述机械臂坐标系OrXrYrZr中表示为：Aop·x+Bop·y+Cop·z+Dop＝0其中，Aop、Bop、Cop、Dop为该平面方程的四个系数。优选地，步骤S6中“结合所述目标物体所在平面的平面方本文档来自技高网...

【技术保护点】
1.一种基于视觉的服务机器人抓取目标物体的方法，其特征在于，包括以下步骤：步骤S1，通过安装于服务机器人上面的Kinect传感器获取彩色图像Is以及相机坐标系OcXcYcZc下的原始三维点云数据Ds；步骤S2，基于所述彩色图像Is，利用深度学习SSD目标检测方法进行目标物体的检测，进而从所述原始三维点云数据Ds中获得所述目标物体的第一点云数据Dto；在所述原始三维点云数据Ds中去除所述目标物体的第一点云数据Dto，得到第一环境点云数据Drs；步骤S3，根据相机坐标系OcXcYcZc到所述服务机器人的机械臂坐标系OrXrYrZr的坐标变换关系，将所述第一环境点云数据Drs转换到所述机械臂坐标系OrXrYrZr下，得到第二环境点云数据Dr；将所述目标物体的第一点云数据Dto转换到所述机械臂坐标系OrXrYrZr下，得到所述目标物体的第二点云数据Dt；步骤S4，结合机械臂在XrYr平面的工作空间Srm，对所述第二环境点云数据Dr，去除Yr轴方向上数值位于[ymin,ymax]区间范围外的数据，以及去除Xr轴方向上数值位于[xmin,xmax]区间范围外的数据，得到第三环境点云数据Df；其中，所述工作空间Srm的左下角和右上角坐标分别为(xmin,ymin)和(xmax,ymax)，xmin、ymin、xmax和ymax均为预设的阈值；步骤S5，基于所述第三环境点云数据Df，采用随机抽样一致性算法对所述目标物体所在平面进行拟合，得到所述目标物体所在平面的平面方程；步骤S6，结合所述目标物体所在平面的平面方程，在所述第三环境点云数据Df中去除该平面以及该平面下方的点所对应的数据，得到第四环境点云数据Do，进而进行点云簇聚类，并获取各点云簇所对应障碍物的位置及尺寸信息；步骤S7，结合所述平面方程，在所述目标物体的第二点云数据Dt中去除该平面以及该平面下方的点所对应的数据，得到所述目标物体的第三点云数据Dtr，并求出所述目标物体的位置及尺寸信息；步骤S8，基于所述目标物体的位置和尺寸信息，以及各障碍物的位置和尺寸信息，先对阻碍所述目标物体被直接抓取的障碍物进行搬移，而后完成对所述目标物体的抓取。...

【技术特征摘要】
1.一种基于视觉的服务机器人抓取目标物体的方法，其特征在于，包括以下步骤：步骤S1，通过安装于服务机器人上面的Kinect传感器获取彩色图像Is以及相机坐标系OcXcYcZc下的原始三维点云数据Ds；步骤S2，基于所述彩色图像Is，利用深度学习SSD目标检测方法进行目标物体的检测，进而从所述原始三维点云数据Ds中获得所述目标物体的第一点云数据Dto；在所述原始三维点云数据Ds中去除所述目标物体的第一点云数据Dto，得到第一环境点云数据Drs；步骤S3，根据相机坐标系OcXcYcZc到所述服务机器人的机械臂坐标系OrXrYrZr的坐标变换关系，将所述第一环境点云数据Drs转换到所述机械臂坐标系OrXrYrZr下，得到第二环境点云数据Dr；将所述目标物体的第一点云数据Dto转换到所述机械臂坐标系OrXrYrZr下，得到所述目标物体的第二点云数据Dt；步骤S4，结合机械臂在XrYr平面的工作空间Srm，对所述第二环境点云数据Dr，去除Yr轴方向上数值位于[ymin,ymax]区间范围外的数据，以及去除Xr轴方向上数值位于[xmin,xmax]区间范围外的数据，得到第三环境点云数据Df；其中，所述工作空间Srm的左下角和右上角坐标分别为(xmin,ymin)和(xmax,ymax)，xmin、ymin、xmax和ymax均为预设的阈值；步骤S5，基于所述第三环境点云数据Df，采用随机抽样一致性算法对所述目标物体所在平面进行拟合，得到所述目标物体所在平面的平面方程；步骤S6，结合所述目标物体所在平面的平面方程，在所述第三环境点云数据Df中去除该平面以及该平面下方的点所对应的数据，得到第四环境点云数据Do，进而进行点云簇聚类，并获取各点云簇所对应障碍物的位置及尺寸信息；步骤S7，结合所述平面方程，在所述目标物体的第二点云数据Dt中去除该平面以及该平面下方的点所对应的数据，得到所述目标物体的第三点云数据Dtr，并求出所述目标物体的位置及尺寸信息；步骤S8，基于所述目标物体的位置和尺寸信息，以及各障碍物的位置和尺寸信息，先对阻碍所述目标物体被直接抓取的障碍物进行搬移，而后完成对所述目标物体的抓取。2.根据权利要求1所述的方法，其特征在于，步骤S3中所述相机坐标系OcXcYcZc到服务机器人的机械臂坐标系OrXrYrZr的坐标变换关系为：其中，(xcp,ycp,zcp)T和(xrp,yrp,zrp)T分别为所述原始三维点云数据Ds中的点在所述相机坐标系OcXcYcZc和所述机械臂坐标系OrXrYrZr中的坐标，Tm为手眼关系矩阵；所述手眼关系矩阵的计算方法包括：控制机械臂末端移动到不同的位置，分别获得所述机械臂末端在所述相机坐标系OcXcYcZc和所述机械臂坐标系OrXrYrZr中的ng个三维坐标和对应的齐次坐标分别为和其中，ng为预设的常数，t＝1,2,…,ng；根据下式计算所述手眼关系矩阵：其中，表示求取矩阵的伪逆。3.根据权利要求2所述的方法，其特征在于，获得所述机械臂末端在所述相机坐标系OcXcYcZc中坐标的方法具体为：在机械臂末端固定一个红色小球，基于所述Kinect传感器提供的彩色图像，采用基于颜色特征的目标识别算法识别所述红色小球，从而获得所述红色小球的中心点在所述相机坐标系OcXcYcZc中的坐标(xc,yc,zc)T，作为所述机械臂末端在所述相机坐标系OcXcYcZc中的坐标。4.根据权利要求2所述的方法，其特征在于，获得所述机械臂末端在所述机械臂坐标系OrXrYrZr中坐标的方法具体为：利用MoveIt！功能包通过正运动学求解得到所述机械臂末端在所述机械臂坐标系OrXrYrZr中的三维坐标(xr,yr,zr)T。5.根据权利要求1所述的方法，其特征在于，步骤S5中“基于所述第三环境点云数据Df，采用随机抽样一致性算法对所述目标物体所在平面进行拟合，得到所述目标物体所在平面的平面方程”，具体包括：步骤S51，从所述第三环境点云数据Df中任选3个数据，根据所述3个数据所对应的点计算出一个平面；步骤S52，求取所述第三环境点云数据Df中所述3个数据以外的数据所对应的点到该平面的距离，将距离在dh范围内的点作为内点构成该平面对应的内点集，其中dh为预设的距离阈值；步骤S53，重复执行步骤S51-S52，直至所述内点集之中内点的数量与所述第三环境点云数据Df中所有数据的数量的比值超过λ或达到预设迭代次数MK；其中λ为预设的数量比的阈值；步骤S54，以最大的内点数量所对应的平面方程作为所述目标物体所在平面的平面方程，该平面方程在所述机械臂坐标系OrXrYrZr中表示为：Aop·x+Bop·y+Cop·z+Dop＝0其中，Aop、Bop、Cop、Dop为该平面方程的四个系数。6.根据权利要求1所述的方法，其特征在于，步骤S6中“结合所述目标物体所在平面的平面方程，在所述第三环境点云数据Df中去除该平面以及该平面下方的点所对应的数据，得到第四环境点云数据Do，进而进行点云簇聚类，并获取各点云簇所对应障碍物的位置及尺寸信息”，包括：步骤S61，结合所述目标物体所在平面的平面方程，在所述第三环境点云数据Df中去除该平面以及该平面下方的点所对应的数据，得到第四环境点云数据Do；步骤S62，基于所述第四环境点云数据Do，利用欧式聚类算法进行点云簇聚类，获取各点云簇所对应障碍物的位置及尺寸信息；其中，步骤S62具体包括：步骤S621，对所述第四环境点云数据Do，设置聚类集C和队列Q，初始时所述聚类集C和所述队列Q均为空集，并标记Do中的各数据为未处理状态；步骤S622，判断所述第四环境点云数据Do中的数据所对应的点pi，如果点pi在所述第四环境点云数据Do中所对应的数据处于已处理状态，则继续判断点pi的下一个点；当所述第四环境点云数据Do中的数据全部处于已处理状态，则转至步骤S626；若点pi在所述第四环境点云数据Do中所对应的数据处于未处理状态，则转至步骤S623；其中，i＝0,1…,Nd-1，Nd为Do中数据的总数；步骤S623，将所述队列Q置为空集，将点pi放入所述队列Q中，并标记该点在所述第四环境点云数据Do中所对应的数据为已处理状态；步骤S624，从所述队列Q中读取一个点，记为q，求取所述第四环境点云数据Do内所有处于未处理状态的数据对应的点与点q之间的距离，当距离不超过dth时，将相应的点加入所述队列Q中，同...

【专利技术属性】
技术研发人员：曹志强，赵雄雷，耿文杰，吴叶兰，蔡俊，喻俊志，谭民，
申请(专利权)人：中国科学院自动化研究所，北京工商大学，
类型：发明
国别省市：北京,11