CN108858199A - 基于视觉的服务机器人抓取目标物体的方法 - Google Patents
基于视觉的服务机器人抓取目标物体的方法 Download PDFInfo
- Publication number
- CN108858199A CN108858199A CN201810841533.0A CN201810841533A CN108858199A CN 108858199 A CN108858199 A CN 108858199A CN 201810841533 A CN201810841533 A CN 201810841533A CN 108858199 A CN108858199 A CN 108858199A
- Authority
- CN
- China
- Prior art keywords
- target object
- barrier
- point cloud
- plane
- cloud data
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 230000004888 barrier function Effects 0.000 claims abstract description 151
- 238000001514 detection method Methods 0.000 claims abstract description 14
- 230000006870 function Effects 0.000 claims description 25
- 239000011159 matrix material Substances 0.000 claims description 12
- 238000013135 deep learning Methods 0.000 claims description 5
- 235000013399 edible fruits Nutrition 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 239000004576 sand Substances 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 241001269238 Data Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
- B25J9/1697—Vision controlled systems
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J11/00—Manipulators not otherwise provided for
- B25J11/008—Manipulators for service tasks
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明涉及服务机器人技术领域,具体涉及一种基于视觉的服务机器人抓取目标物体的方法,旨在解决存在阻碍目标物体被直接抓取的障碍物情形下的机器人抓取问题。本发明中抓取目标物体的方法包括:获取彩色图像以及相机坐标系下的原始三维点云数据;进行目标物体的检测,获取目标物体的点云数据,并得到目标物体周围的环境点云数据,将上述点云数据变换到机械臂坐标系下;在机械臂坐标系下,拟合目标物体所在平面的平面方程;在此基础上,获取各障碍物的位置和尺寸信息,以及目标物体的位置及尺寸信息;基于上述位置和尺寸信息,先对阻碍目标物体被直接抓取的障碍物进行搬移,而后完成对目标物体的抓取。本发明有效提高了服务机器人的抓取能力。
Description
技术领域
本发明涉及服务机器人技术领域,具体涉及一种基于视觉的服务机器人抓取目标物体的方法。
背景技术
随着服务机器人和人工智能技术的不断发展,其应用领域也在不断拓展。智能服务机器人的核心技术除了环境感知、定位、导航外,物体抓取技术也非常重要,是服务机器人提供优质服务的前提。例如,在家庭、办公、医护等环境中,物品的取拿等服务均需要服务机器人具有抓取物体的能力。为了使得服务机器人实施对物体的抓取,需要将机械臂安装在服务机器人的移动平台上。在移动平台到达指定操作区域后,服务机器人将控制机械臂实施抓取。服务机器人抓取涉及到对目标的检测和机械臂的运动规划,其中基于视觉的机器人抓取目标物体的方法研究最为普遍。
国内外研究人员在基于视觉的机器人抓取方面开展了深入的研究。目标检测是服务机器人抓取的前提,传统的目标检测方法通常需要手工设计特征,泛化能力不强,鲁棒性较差。近年来深度学习受到关注,其优势在于特征提取环节不需要手工设计特征,而是通过利用大规模数据集对模型进行训练,学得目标物体具备的特征,具有较强的鲁棒性。目前常见的基于深度学习的目标检测方法有RCNN、Fast-RCNN、Faster-RCNN、YOLO、SSD等,其中,SSD结合了YOLO快速和Faster-RCNN检测精度高的优点而受到普遍关注。在完成目标物体检测后,服务机器人即可进行自身机械臂的运动规划,进而完成对目标物体的抓取,一种常见的进行机械臂运动规划的方式是调用MoveIt!功能包。MoveIt!功能包集成了机械臂的正逆运动学求解、运动规划、碰撞检测等功能,其中运动规划使用开源运动规划库(OpenMotionPlanning Library,OMPL),目前MoveIt!功能包已经广泛应用于主流的机械臂运动规划中。
现有的机器人抓取技术通常在完成对指定目标物体的检测后,直接实施对该目标物体的抓取。实际上,在现实环境中,目标物体周围有时会存在一些阻碍服务机器人的机械臂直接抓取的障碍物。为此,需要对现有的机器人抓取技术进行更深入的研究,以解决现有技术难以较好地满足存在阻碍目标物体被直接抓取的障碍物情形下的机器人抓取问题。
发明内容
为了解决现有技术中的上述问题,本发明提出了一种基于视觉的服务机器人抓取目标物体的方法,能够在存在阻碍目标物体被直接抓取的障碍物的情形下,实现服务机器人对目标物体的抓取,提高了服务机器人的抓取能力。
本发明提出一种基于视觉的服务机器人抓取目标物体的方法,包括以下步骤:
步骤S1,通过安装于服务机器人上面的Kinect传感器获取彩色图像Is以及相机坐标系OcXcYcZc下的原始三维点云数据Ds;
步骤S2,基于所述彩色图像Is,利用深度学习SSD(Single ShotmultiBoxDetector)目标检测方法进行目标物体的检测,进而从所述原始三维点云数据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,采用随机抽样一致性(Random SampleConsensus,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中“结合所述目标物体所在平面的平面方程,在所述第三环境点云数据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中,同时将该点在Do中所对应的数据标记为已处理状态;重复执行,直到所述队列Q中所有的点都遍历完成而且没有新的点加入到所述队列Q为止;其中,dth为预设的阈值;
步骤S625,若所述队列Q中点的数目大于Nmin,则将所述队列Q中所有的点记为一个点云簇并放入所述聚类集C中;转至步骤S622;其中,Nmin为预设的阈值;
步骤S626,判断所述聚类集C是否为空集,若不是空集,则计算每个点云簇所对应障碍物的位置和尺寸信息。
优选地,步骤S8中“基于所述目标物体的位置和尺寸信息,以及各障碍物的位置和尺寸信息,先对阻碍所述目标物体被直接抓取的障碍物进行搬移,而后完成对所述目标物体的抓取”,具体包括:
步骤S81,基于所述目标物体的位置和尺寸信息,以及各障碍物的位置和尺寸信息,通过MoveIt!功能包判断所述目标物体是否能够被直接抓取,若是,则根据MoveIt!功能包的规划结果实施对所述目标物体的抓取;否则,执行步骤S82;其中,机械臂末端的抓取方向为沿Zr轴竖直向下抓取;
步骤S82,对阻碍所述目标物体被直接抓取的障碍物进行搬移,进而完成对所述目标物体的抓取。
优选地,步骤S82中“对阻碍所述目标物体被直接抓取的障碍物进行搬移,进而完成对所述目标物体的抓取”,具体包括:
步骤S821,在所述目标物体的第三点云数据Dtr中,选取在Zr轴方向上数值位于[zmax-zh,zmax]内的所有数据,作为所述目标物体抓取区的点云数据Dgr;其中,zmax为Dtr中各数据在Zr轴方向上的最大值,zh为预设的阈值;
步骤S822,对于每个障碍物,求取所述目标物体抓取区的点云数据Dgr所对应的点集合中的各点与该障碍物相应点云簇中各点之间距离的最小值,该最小值记为该障碍物到所述目标物体的距离,将该障碍物到所述目标物体的距离以及该障碍物的ID号组合起来形成障碍物影响序列;
步骤S823,对机械臂在XrYr平面的工作空间Srm进行栅格划分,得到所述工作空间Srm的栅格图;分别将所述目标物体的第三点云数据Dtr对应的所有点以及各障碍物所对应的点云簇,投影到所述机械臂坐标系OrXrYrZr的XrYr平面上,从而获得相应的外接矩形;
步骤S824,根据所述障碍物影响序列中各障碍物所对应的到所述目标物体的距离,选定到所述目标物体距离最小的障碍物作为待搬移障碍物Obs_mov;
步骤S825,对所述工作空间Srm中的每一个栅格进行赋值,生成所述待搬移障碍物Obs_mov对应的引力势场栅格图;
步骤S826,对于每一个其他物体,将所述工作空间Srm中的每一个栅格重新赋值,生成该物体对应的斥力势场栅格图;其中,所述其他物体包括:除所述待搬移障碍物Obs_mov之外的其他障碍物以及所述目标物体;
步骤S827,将所述待搬移障碍物Obs_mov对应的引力势场栅格图和所述其他物体的斥力势场栅格图叠加起来,生成最终的势场栅格图;选取该势场栅格图中数值最小的栅格所对应的位置作为所述待搬移障碍物Obs_mov的待放置位置;若数值最小的栅格数量大于1,则选取距离所述待搬移障碍物Obs_mov在XrYr平面上投影得到的外接矩形的中心点最近的栅格所对应的位置作为所述待搬移障碍物Obs_mov的待放置位置;
步骤S828,基于所述待搬移障碍物Obs_mov的当前位置和尺寸信息,以及所述待搬移障碍物Obs_mov的待放置位置、其他障碍物和所述目标物体的位置和尺寸信息,采用MoveIt!功能包进行机械臂的运动规划,完成对所述待搬移障碍物Obs_mov的搬移;将所述待搬移障碍物Obs_mov改称为被搬移障碍物Obs_mov,将该障碍物的所述待放置位置改称为新位置;将所述被搬移障碍物Obs_mov移出所述障碍物影响序列,该障碍物在XrYr平面上投影得到的外接矩形的中心点的位置用该障碍物被搬移后的新位置加以更新;
步骤S829,基于所述目标物体的位置和尺寸信息,以及所述被搬移障碍物Obs_mov的新位置和尺寸信息、其他障碍物的当前位置和尺寸信息,通过MoveIt!功能包判断所述目标物体是否能够被直接抓取,若是,则通过MoveIt!功能包进行机械臂的运动规划,进而对所述目标物体进行抓取;否则,转至步骤S824。
优选地,步骤S825中“对所述工作空间Srm中的每一个栅格进行赋值,生成所述待搬移障碍物Obs_mov对应的引力势场栅格图”具体包括:
将所述待搬移障碍物Obs_mov在XrYr平面上投影得到的外接矩形内所有栅格的数值,赋值为该障碍物沿Zr轴方向的高度;
基于所述待搬移障碍物Obs_mov在XrYr平面上投影得到的外接矩形,以栅格为单位向外扩展,每次在Xr轴正方向、Yr轴正方向、Xr轴负方向、Yr轴负方向上同时扩展一个栅格,从而产生一系列扩展矩形,直到最***的扩展矩形将所述工作空间Srm内的所有栅格全部包含为止,其中,每个扩展矩形用产生该扩展矩形时的扩展次数进行表征,位于所述待搬移障碍物Obs_mov在XrYr平面上投影得到的外接矩形之外且在所述工作空间Srm之内的栅格Gat的数值根据下式进行赋值:
Fat=nat·ka
其中,Fat为栅格Gat的数值,ka为预设的引力场系数,nat为包含栅格Gat的最小面积的扩展矩形所对应的扩展次数,nat=1,2,3,…。
优选地,步骤S826中“对于每一个其他物体,将所述工作空间Srm中的每一个栅格重新赋值,生成该物体对应的斥力势场栅格图”具体包括:
将该物体在XrYr平面上投影得到的外接矩形内所有栅格的数值,赋值为该物体沿Zr轴方向的高度;
基于该物体在XrYr平面上投影得到的外接矩形,以栅格为单位向外扩展,每次在Xr轴正方向、Yr轴正方向、Xr轴负方向、Yr轴负方向上同时扩展一个栅格,从而产生一系列扩展矩形,直到最***的扩展矩形将所述工作空间Srm内的所有栅格全部包含为止,其中,每个扩展矩形用产生该扩展矩形时的扩展次数进行表征,位于该物体在XrYr平面上投影得到的外接矩形之外且在所述工作空间Srm之内的栅格Gre的数值根据下式进行赋值:
其中,Fre为栅格Gre的数值,kr为预设的斥力场系数,Oh为物体的高度,nre为包含栅格Gre的最小面积的扩展矩形所对应的扩展次数,nre=1,2,3,…,μ是给定的高斯分布的均值,σ是给定的高斯分布的标准差。
采用本发明的抓取方法,当存在阻碍目标物体被直接抓取的障碍物时,能够先进行障碍物搬移操作,而后完成对目标物体的抓取,为服务机器人在家庭、办公、医护等环境下的抓取作业提供技术支持。
附图说明
图1是本发明一种基于视觉的服务机器人抓取目标物体的方法实施例的流程示意图;
图2a-图2e是本发明实施例中生成待搬移障碍物对应的引力势场栅格图的方法示意图。
具体实施方式
下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是,这些实施方式仅用于解释本发明的技术原理,并非旨在限制本发明的保护范围。
参阅附图1,图1给出了本发明一种基于视觉的服务机器人抓取目标物体的方法实施例的流程示意图。如图1所示,本实施例的抓取方法包括下述步骤:
步骤S1,服务机器人通过安装于自身的Kinect传感器获取彩色图像Is以及相机坐标系OcXcYcZc下的原始三维点云数据Ds,其中Kinect传感器倾斜向下安装;相机坐标系OcXcYcZc为右手系,相机坐标系OcXcYcZc的原点Oc位于Kinect传感器的中心,Yc轴垂直于Kinect传感器的底面且方向向上,Zc轴垂直于Yc轴且与Kinect传感器的正前方保持一致。
步骤S2,基于彩色图像Is,利用深度学习SSD(Single Shot multiBox Detector)目标检测方法进行目标物体的检测,得到目标物体的包围框,进而从原始三维点云数据Ds中获得目标物体的包围框所对应的三维数据,这些三维数据构成目标物体的第一点云数据Dto;在原始三维点云数据Ds中去除目标物体的第一点云数据Dto,将得到的三维点云数据记为第一环境点云数据Drs。
步骤S3,根据相机坐标系OcXcYcZc到服务机器人的机械臂坐标系OrXrYrZr的坐标变换关系,将相机坐标系OcXcYcZc下的第一环境点云数据Drs转换到机械臂坐标系OrXrYrZr下,得到第二环境点云数据Dr;同样的,将相机坐标系OcXcYcZc下的目标物体的第一点云数据Dto转换到机械臂坐标系OrXrYrZr下,得到目标物体的第二点云数据Dt;机械臂坐标系OrXrYrZr为右手系,机械臂坐标系OrXrYrZr的原点Or位于服务机器人的机械臂的基座中心,Zr轴垂直于地面且方向向上,Xr轴垂直于Zr轴且与服务机器人正前方保持一致,其中,服务机器人的机械臂的基座底面平行于地面。
该步骤中,相机坐标系OcXcYcZc到机械臂坐标系OrXrYrZr的坐标变换关系如公式(1)所示:
其中,(xcp,ycp,zcp)T和(xrp,yrp,zrp)T分别为原始三维点云数据Ds中的点在相机坐标系OcXcYcZc和机械臂坐标系OrXrYrZr中的坐标,Tm为手眼关系矩阵,手眼关系矩阵Tm的具体获取方法如下:
首先在机械臂末端固定一个红色小球,基于Kinect传感器提供的彩色图像,采用基于HSV(Hue,Saturation,Value)颜色特征的目标识别算法来识别红色小球,从而获得该红色小球的中心点在相机坐标系OcXcYcZc中的坐标(xc,yc,zc)T,用该中心点对机械臂末端进行描述,得到机械臂末端在相机坐标系OcXcYcZc中的三维坐标为(xc,yc,zc)T,其齐次坐标为pc=(xc,yc,zc,1)T;并利用MoveIt!功能包通过正运动学求解得到机械臂末端在机械臂坐标系OrXrYrZr中的三维坐标(xr,yr,zr)T,其齐次坐标为pr=(xr,yr,zr,1)T;然后控制机械臂末端移动到不同的位置,分别获得机械臂末端在相机坐标系OcXcYcZc和机械臂坐标系OrXrYrZr中的ng个三维坐标和t=1,2,…,ng,其中ng为预设的常数,从而得到对应的齐次坐标分别为和t=1,2,…,ng;基于和t=1,2,…,ng,以及公式(1),得到公式(2):
计算手眼关系矩阵Tm如下:
其中,表示求取矩阵的伪逆。
步骤S4,结合机械臂在XrYr平面的工作空间Srm,对第二环境点云数据Dr进行缩小范围的处理,其中,机械臂在XrYr平面的工作空间Srm为矩形区域,其左下角和右上角坐标分别为(xmin,ymin)和(xmax,ymax),xmin、ymin、xmax和ymax均为预设的阈值;对第二环境点云数据Dr进行处理的过程为:去除Yr轴方向上数值位于[ymin,ymax]区间范围外的数据,以及去除Xr轴方向上数值位于[xmin,xmax]区间范围外的数据,得到第三环境点云数据Df。
步骤S5,基于第三环境点云数据Df,采用RANSAC(Random Sample Consensus,随机抽样一致性)算法进行目标物体所在平面的平面拟合,得到目标物体所在平面的平面方程。具体包括S51-S54:
步骤S51,从第三环境点云数据Df中任选3个数据,根据这3个数据所对应的点计算出一个平面;
步骤S52,求取第三环境点云数据Df中的其它数据(步骤S51中所选取的3个数据以外的数据)所对应的点到该平面的距离,将距离在dh范围内的点作为内点构成该平面对应的内点集,其中dh为预设的距离阈值;
步骤S53,重复执行步骤S51-S52,直至内点集之中内点的数量与第三环境点云数据Df中所有数据的数量的比值超过λ或达到预设迭代次数MK;其中λ为预设的数量比的阈值;
步骤S54,以最大的内点数量所对应的平面方程作为目标物体所在平面的平面方程,该平面方程在机械臂坐标系OrXrYrZr中表示为Aop·x+Bop·y+Cop·z+Dop=0,其中Aop、Bop、Cop、Dop为该平面方程的四个系数。
步骤S6,结合目标物体所在平面的平面方程,在第三环境点云数据Df中去除该平面以及该平面下方的点所对应的数据,得到第四环境点云数据Do,进而采用欧式聚类算法进行点云簇聚类,获取各点云簇所对应障碍物的位置及尺寸信息。具体包括步骤S61-S62:
步骤S61,结合目标物体所在平面的平面方程,在第三环境点云数据Df中去除该平面以及该平面下方的点所对应的数据,得到第四环境点云数据Do。
步骤S62,基于第四环境点云数据Do,利用欧式聚类算法进行点云簇聚类,获取各点云簇所对应障碍物的位置及尺寸信息。具体包括步骤S621-S626:
步骤S621,对第四环境点云数据Do,设置聚类集C和队列Q,初始时聚类集C和队列Q均为空集,并标记Do中的各数据为未处理状态;
步骤S622,对第四环境点云数据Do中的数据所对应的点pi进行判断(i=0,1…,Nd-1,Nd为Do中数据的总数,其中点pi的坐标即为该点在Do中所对应的数据),如果点pi在第四环境点云数据Do中所对应的数据处于已处理状态,则继续判断点pi的下一个点,当第四环境点云数据Do中的数据全部处于已处理状态,跳转到步骤S626;如果点pi在第四环境点云数据Do中所对应的数据处于未处理状态,则执行步骤S623;
步骤S623,队列Q置为空集,将点pi放入队列Q中,并标记该点在第四环境点云数据Do中所对应的数据为已处理状态;
步骤S624,从队列Q中读取一个点,记为q,求取第四环境点云数据Do内所有处于未处理状态的数据对应的点与点q之间的距离,当距离不超过dth时,将相应的点加入队列Q中,同时将该点在Do中所对应的数据标记为已处理状态,其中dth为预设的阈值。重复执行该步骤,直到队列Q中所有的点都遍历完成而且没有新的点加入队列Q为止;
步骤S625,如果队列Q中的点的数目大于Nmin,则将队列Q的所有点记为一个点云簇,并放入聚类集C中,其中,Nmin为预设的阈值,返回步骤S622;
步骤S626,得到聚类集C中的点云簇的个数,记为Nc,每个点云簇对应一个障碍物;聚类集C为空集即Nc=0,意味着没有产生点云簇;当聚类集C不是空集时,获得每个点云簇所对应障碍物的位置和尺寸信息,其中将障碍物相应点云簇的所有点的坐标的平均值记为该障碍物的位置信息;障碍物的尺寸信息根据障碍物相应点云簇中的所有点在Xr轴、Yr轴、Zr轴方向的值,将Xr轴方向上的最大值与最小值的差记为该障碍物沿Xr轴方向的长度,将Yr轴方向上的最大值与最小值的差记为该障碍物沿Yr轴方向的长度,将Zr轴方向上的最大值与最小值的差记为该障碍物沿Zr轴方向的高度。
步骤S7,结合目标物体所在平面的平面方程,在目标物体的第二点云数据Dt中去除该平面以及该平面下方的点所对应的数据,得到目标物体的第三点云数据Dtr,将Dtr中所有数据的平均值记为目标物体的位置信息;目标物体的尺寸信息根据Dtr中所有数据在Xr轴、Yr轴、Zr轴方向的值,将Xr轴方向上的最大值与最小值的差记为目标物体沿Xr轴方向的长度,将Yr轴方向上的最大值与最小值的差记为目标物体沿Yr轴方向的长度,将Zr轴方向上的最大值与最小值的差记为目标物体沿Zr轴方向的高度;
步骤S8,基于目标物体的位置和尺寸信息,以及各障碍物的位置和尺寸信息,先对阻碍目标物体被直接抓取的障碍物进行搬移,而后完成对目标物体的抓取。具体包括步骤S81-S82:
步骤S81,基于目标物体的位置和尺寸信息,以及各障碍物的位置和尺寸信息,通过MoveIt!功能包判断服务机器人的机械臂是否可以直接抓取目标物体,如果能够直接抓取目标物体,则机械臂将直接根据MoveIt!功能包的规划结果实施对目标物体的抓取;否则,执行步骤S82;其中,机械臂末端的抓取方向均为沿Zr轴竖直向下抓取;
步骤S82,对阻碍目标物体被直接抓取的障碍物进行搬移,而后完成对目标物体的抓取。具体包括步骤S821-S829:
步骤S821,在目标物体的第三点云数据Dtr中,选取在Zr轴方向上数值位于[zmax-zh,zmax]内的所有数据,作为目标物体抓取区的点云数据Dgr,其中,zmax为Dtr中各数据在Zr轴方向上的最大值,zh为预设的阈值;
步骤S822,对于每个障碍物,求取目标物体抓取区的点云数据Dgr所对应的点集合中的各点与该障碍物相应点云簇中各点之间距离的最小值,该最小值记为该障碍物到目标物体的距离,将该障碍物到目标物体的距离以及该障碍物的ID号组合起来形成障碍物影响序列;
步骤S823,对于机械臂在XrYr平面的工作空间Srm,以cx×cx为单位进行栅格划分得到机械臂在XrYr平面的工作空间Srm的栅格图,其中cx为预设的栅格划分单位,cx能整除xmax-xmin,且cx能整除ymax-ymin(前面步骤S4中提到(xmin,ymin)和(xmax,ymax)分别为工作空间Srm的左下角和右上角坐标);分别将目标物体的第三点云数据Dtr对应的所有点以及各障碍物所对应的点云簇,投影到机械臂坐标系OrXrYrZr的XrYr平面上,并获得各自对应的外接矩形;
步骤S824,根据障碍物影响序列中各障碍物所对应的到目标物体的距离,选定到目标物体距离最小的障碍物作为本次待搬移的障碍物Obs_mov;
步骤S825,对于机械臂在XrYr平面的工作空间Srm中的每一个栅格进行赋值,生成待搬移障碍物Obs_mov对应的引力势场栅格图。具体赋值方法为:将障碍物Obs_mov在XrYr平面上投影得到的外接矩形内所有栅格的数值,赋值为该障碍物沿Zr轴方向的高度;基于障碍物Obs_mov在XrYr平面上投影得到的外接矩形,以栅格为单位向外扩展,每次在Xr轴正方向、Yr轴正方向、Xr轴负方向、Yr轴负方向上同时扩展一个栅格,从而产生一系列扩展矩形,直到最***的扩展矩形将机械臂在XrYr平面的工作空间Srm内的所有栅格全部包含为止,其中,每个扩展矩形用产生该扩展矩形时的扩展次数进行表征,位于投影得到的外接矩形之外且在Srm之内的栅格Gat的数值Fat按照公式(4)进行赋值:
Fat=nat·ka (4)
其中,ka为预设的引力场系数,nat为包含栅格Gat的最小面积的扩展矩形所对应的扩展次数,nat=1,2,3,…。
为了更清楚地描述生成引力势场栅格图的方法,我们在这里举例说明,假设机械臂在XrYr平面的工作空间Srm由8×6个栅格组成,待搬移障碍物在XrYr平面上投影得到的外接矩形包括3×2个栅格。图2a-图2e是生成待搬移障碍物对应的引力势场栅格图的方法示意图。其中,图2a描述了机械臂在XrYr平面的工作空间Srm,由8×6个虚线栅格组成;在图2b中,包括3×2个实线栅格的矩形表示障碍物Obs_mov在XrYr平面上投影得到的外接矩形,该外接矩形中每个栅格被赋值为障碍物沿Zr轴方向的高度,本例中为0.1米;在图2c中,包括5×4个实线栅格的矩形表示经过第1次扩展以后得到的扩展矩形,用该扩展矩形的扩展次数1来表征该扩展矩形,本次扩展出来的栅格赋值为1×ka;在图2d中,包括7×6个实线栅格的矩形表示经过第2次扩展以后得到的扩展矩形,用该扩展矩形的扩展次数2来表征该扩展矩形,本次扩展出来的栅格有一部分已经到了工作空间Srm之外,但是我们并不关心这部分栅格,只对位于工作空间Srm之内的栅格赋值为2×ka;在图2e中,包括9×8个实线栅格的矩形表示经过第3次扩展以后得到的扩展矩形,用该扩展矩形的扩展次数3来表征该扩展矩形,对于本次扩展出来的栅格,我们同样只对位于工作空间Srm之内的栅格赋值,赋值为3×ka。可以看出,经过3次扩展以后,最***的扩展矩形已经将机械臂在XrYr平面的工作空间Srm内的所有栅格全部包含,图2e中所有赋过值的栅格共同组成了本例中待搬移障碍物对应的引力势场栅格图。
步骤S826,对于每一个其他物体(包括除待搬移障碍物Obs_mov之外的其他障碍物以及目标物体),将机械臂在XrYr平面的工作空间Srm中的每一个栅格重新赋值,生成该物体对应的斥力势场栅格图。具体赋值方法为:将该物体在XrYr平面上投影得到的外接矩形内所有栅格的数值,赋值为该物体沿Zr轴方向的高度;基于该物体在XrYr平面上投影得到的外接矩形,以栅格为单位向外扩展,每次在Xr轴正方向、Yr轴正方向、Xr轴负方向、Yr轴负方向上同时扩展一个栅格,从而产生一系列扩展矩形,直到最***的扩展矩形将机械臂在XrYr平面的工作空间Srm内的所有栅格全部包含为止,其中,每个扩展矩形用产生该扩展矩形时的扩展次数进行表征,位于投影得到的外接矩形之外且在Srm之内的栅格Gre的数值Fre按照公式(5)进行赋值:
其中,kr为预设的斥力场系数,Oh为物体的高度,nre为包含栅格Gre的最小面积的扩展矩形所对应的扩展次数,nre=1,2,3,…,μ是给定的高斯分布的均值,σ是给定的高斯分布的标准差;
步骤S827,将待搬移障碍物Obs_mov对应的引力势场栅格图和其他障碍物及目标物体各自生成的斥力势场栅格图叠加起来(即将上述栅格图中同一栅格的数值相加),生成最终的势场栅格图,选取该势场栅格图中数值最小的栅格所对应的位置作为障碍物Obs_mov的待放置位置,如果数值最小的栅格不止一个,则选取到障碍物Obs_mov在XrYr平面上投影得到的外接矩形的中心点距离最近的栅格所对应的位置作为障碍物Obs_mov的待放置位置;
步骤S828,基于待搬移障碍物Obs_mov的当前位置和尺寸信息,以及待搬移障碍物的待放置位置、其他障碍物和目标物体的位置和尺寸信息,采用MoveIt!功能包进行机械臂的运动规划,完成对障碍物Obs_mov的搬移(至此,前面提到的“待放置位置”改称为该障碍物的“新位置”,障碍物Obs_mov也由“待搬移障碍物”改称为“被搬移障碍物”);将障碍物Obs_mov移出障碍物影响序列,该障碍物在XrYr平面上投影得到的外接矩形的中心点的位置用该障碍物被搬移后的新位置加以更新;
步骤S829,基于目标物体的位置和尺寸信息,以及被搬移障碍物Obs_mov的新位置和尺寸信息、其他障碍物的当前位置和尺寸信息,通过MoveIt!功能包判断目标物体是否可以被直接抓取,如果目标物体不能被直接抓取,则返回步骤S824;否则,通过MoveIt!功能包进行机械臂的运动规划,控制机械臂完成对目标物体的抓取。
在一个具体实施例中,Kinect传感器倾斜向下安装,其倾斜角度为45°,ng=15,xmin=0.1米,xmax=0.5米,ymin=-0.25米,ymax=0.25米,dh=0.01米,λ=0.5,MK=100,dth=0.02米,Nmin=80,zh=0.05米,cx=0.01米,ka=0.015,kr=0.1,μ=0,σ=10。
采用本发明的抓取方法,当存在阻碍目标物体被直接抓取的障碍物时,能够先进行障碍物搬移操作,而后完成对目标物体的抓取,为服务机器人在家庭、办公、医护等环境下的抓取作业提供技术支持,能够实现较好的技术效果。
上述实施例中虽然将各个步骤按照上述先后次序的方式进行了描述,但是本领域技术人员可以理解,为了实现本实施例的效果,不同的步骤之间不必按照这样的次序执行,其可以同时(并行)执行或以颠倒的次序执行,这些简单的变化都在本发明的保护范围之内。
本领域技术人员应该能够意识到,结合本文中所公开的实施例描述的各示例的方法步骤,能够以电子硬件、计算机软件或者二者的结合来实现,为了清楚地说明电子硬件和软件的可互换性,在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。
至此,已经结合附图所示的优选实施方式描述了本发明的技术方案,但是,本领域技术人员容易理解的是,本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下,本领域技术人员可以对相关技术特征做出等同的更改或替换,这些更改或替换之后的技术方案都将落入本发明的保护范围之内。
Claims (10)
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中,同时将该点在Do中所对应的数据标记为已处理状态;重复执行,直到所述队列Q中所有的点都遍历完成而且没有新的点加入到所述队列Q为止;其中,dth为预设的阈值;
步骤S625,若所述队列Q中点的数目大于Nmin,则将所述队列Q中所有的点记为一个点云簇并放入所述聚类集C中;转至步骤S622;其中,Nmin为预设的阈值;
步骤S626,判断所述聚类集C是否为空集,若不是空集,则计算每个点云簇所对应障碍物的位置和尺寸信息。
7.根据权利要求1所述的方法,其特征在于,步骤S8中“基于所述目标物体的位置和尺寸信息,以及各障碍物的位置和尺寸信息,先对阻碍所述目标物体被直接抓取的障碍物进行搬移,而后完成对所述目标物体的抓取”,具体包括:
步骤S81,基于所述目标物体的位置和尺寸信息,以及各障碍物的位置和尺寸信息,通过MoveIt!功能包判断所述目标物体是否能够被直接抓取,若是,则根据MoveIt!功能包的规划结果实施对所述目标物体的抓取;否则,执行步骤S82;其中,机械臂末端的抓取方向为沿Zr轴竖直向下抓取;
步骤S82,对阻碍所述目标物体被直接抓取的障碍物进行搬移,进而完成对所述目标物体的抓取。
8.根据权利要求7所述的方法,其特征在于,步骤S82中“对阻碍所述目标物体被直接抓取的障碍物进行搬移,进而完成对所述目标物体的抓取”,具体包括:
步骤S821,在所述目标物体的第三点云数据Dtr中,选取在Zr轴方向上数值位于[zmax-zh,zmax]内的所有数据,作为所述目标物体抓取区的点云数据Dgr;其中,zmax为Dtr中各数据在Zr轴方向上的最大值,zh为预设的阈值;
步骤S822,对于每个障碍物,求取所述目标物体抓取区的点云数据Dgr所对应的点集合中的各点与该障碍物相应点云簇中各点之间距离的最小值,该最小值记为该障碍物到所述目标物体的距离,将该障碍物到所述目标物体的距离以及该障碍物的ID号组合起来形成障碍物影响序列;
步骤S823,对机械臂在XrYr平面的工作空间Srm进行栅格划分,得到所述工作空间Srm的栅格图;分别将所述目标物体的第三点云数据Dtr对应的所有点以及各障碍物所对应的点云簇,投影到所述机械臂坐标系OrXrYrZr的XrYr平面上,从而获得相应的外接矩形;
步骤S824,根据所述障碍物影响序列中各障碍物所对应的到所述目标物体的距离,选定到所述目标物体距离最小的障碍物作为待搬移障碍物Obs_mov;
步骤S825,对所述工作空间Srm中的每一个栅格进行赋值,生成所述待搬移障碍物Obs_mov对应的引力势场栅格图;
步骤S826,对于每一个其他物体,将所述工作空间Srm中的每一个栅格重新赋值,生成该物体对应的斥力势场栅格图;其中,所述其他物体包括:除所述待搬移障碍物Obs_mov之外的其他障碍物以及所述目标物体;
步骤S827,将所述待搬移障碍物Obs_mov对应的引力势场栅格图和所述其他物体的斥力势场栅格图叠加起来,生成最终的势场栅格图;选取该势场栅格图中数值最小的栅格所对应的位置作为所述待搬移障碍物Obs_mov的待放置位置;若数值最小的栅格数量大于1,则选取距离所述待搬移障碍物Obs_mov在XrYr平面上投影得到的外接矩形的中心点最近的栅格所对应的位置作为所述待搬移障碍物Obs_mov的待放置位置;
步骤S828,基于所述待搬移障碍物Obs_mov的当前位置和尺寸信息,以及所述待搬移障碍物Obs_mov的待放置位置、其他障碍物和所述目标物体的位置和尺寸信息,采用MoveIt!功能包进行机械臂的运动规划,完成对所述待搬移障碍物Obs_mov的搬移;将所述待搬移障碍物Obs_mov改称为被搬移障碍物Obs_mov,将该障碍物的所述待放置位置改称为新位置;将所述被搬移障碍物Obs_mov移出所述障碍物影响序列,该障碍物在XrYr平面上投影得到的外接矩形的中心点的位置用该障碍物被搬移后的新位置加以更新;
步骤S829,基于所述目标物体的位置和尺寸信息,以及所述被搬移障碍物Obs_mov的新位置和尺寸信息、其他障碍物的当前位置和尺寸信息,通过MoveIt!功能包判断所述目标物体是否能够被直接抓取,若是,则通过MoveIt!功能包进行机械臂的运动规划,进而对所述目标物体进行抓取;否则,转至步骤S824。
9.根据权利要求8所述的方法,其特征在于,步骤S825中“对所述工作空间Srm中的每一个栅格进行赋值,生成所述待搬移障碍物Obs_mov对应的引力势场栅格图”具体包括:
将所述待搬移障碍物Obs_mov在XrYr平面上投影得到的外接矩形内所有栅格的数值,赋值为该障碍物沿Zr轴方向的高度;
基于所述待搬移障碍物Obs_mov在XrYr平面上投影得到的外接矩形,以栅格为单位向外扩展,每次在Xr轴正方向、Yr轴正方向、Xr轴负方向、Yr轴负方向上同时扩展一个栅格,从而产生一系列扩展矩形,直到最***的扩展矩形将所述工作空间Srm内的所有栅格全部包含为止,其中,每个扩展矩形用产生该扩展矩形时的扩展次数进行表征,位于所述待搬移障碍物Obs_mov在XrYr平面上投影得到的外接矩形之外且在所述工作空间Srm之内的栅格Gat的数值根据下式进行赋值:
Fat=nat·ka
其中,Fat为栅格Gat的数值,ka为预设的引力场系数,nat为包含栅格Gat的最小面积的扩展矩形所对应的扩展次数,nat=1,2,3,…。
10.根据权利要求8所述的方法,其特征在于,步骤S826中“对于每一个其他物体,将所述工作空间Srm中的每一个栅格重新赋值,生成该物体对应的斥力势场栅格图”具体包括:
将该物体在XrYr平面上投影得到的外接矩形内所有栅格的数值,赋值为该物体沿Zr轴方向的高度;
基于该物体在XrYr平面上投影得到的外接矩形,以栅格为单位向外扩展,每次在Xr轴正方向、Yr轴正方向、Xr轴负方向、Yr轴负方向上同时扩展一个栅格,从而产生一系列扩展矩形,直到最***的扩展矩形将所述工作空间Srm内的所有栅格全部包含为止,其中,每个扩展矩形用产生该扩展矩形时的扩展次数进行表征,位于该物体在XrYr平面上投影得到的外接矩形之外且在所述工作空间Srm之内的栅格Gre的数值根据下式进行赋值:
其中,Fre为栅格Gre的数值,kr为预设的斥力场系数,Oh为物体的高度,nre为包含栅格Gre的最小面积的扩展矩形所对应的扩展次数,nre=1,2,3,…,μ是给定的高斯分布的均值,σ是给定的高斯分布的标准差。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810841533.0A CN108858199B (zh) | 2018-07-27 | 2018-07-27 | 基于视觉的服务机器人抓取目标物体的方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810841533.0A CN108858199B (zh) | 2018-07-27 | 2018-07-27 | 基于视觉的服务机器人抓取目标物体的方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108858199A true CN108858199A (zh) | 2018-11-23 |
CN108858199B CN108858199B (zh) | 2020-04-07 |
Family
ID=64305734
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810841533.0A Active CN108858199B (zh) | 2018-07-27 | 2018-07-27 | 基于视觉的服务机器人抓取目标物体的方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108858199B (zh) |
Cited By (26)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109767452A (zh) * | 2018-12-24 | 2019-05-17 | 深圳市道通智能航空技术有限公司 | 一种目标定位方法和装置、无人机 |
CN109910011A (zh) * | 2019-03-29 | 2019-06-21 | 齐鲁工业大学 | 一种基于多传感器的机械臂避障方法和机械臂 |
CN109934871A (zh) * | 2019-02-18 | 2019-06-25 | 武汉大学 | 一种面向高危环境的智能无人机抓取目标的***和方法 |
CN109978949A (zh) * | 2019-03-26 | 2019-07-05 | 南开大学 | 基于计算机视觉的农作物识别与特征点三维坐标提取的一种方法 |
CN110000783A (zh) * | 2019-04-04 | 2019-07-12 | 上海节卡机器人科技有限公司 | 机器人的视觉抓取方法和装置 |
CN110116067A (zh) * | 2019-05-26 | 2019-08-13 | 天津大学 | 一种车轴自动喷涂装置和方法 |
CN110182527A (zh) * | 2019-04-11 | 2019-08-30 | 上海快仓智能科技有限公司 | 用于货架阵列的出入库控制方法和搬运*** |
CN110182529A (zh) * | 2019-04-11 | 2019-08-30 | 上海快仓智能科技有限公司 | 用于货架阵列的出入库控制方法和搬运*** |
CN110231035A (zh) * | 2019-06-27 | 2019-09-13 | 北京克莱明科技有限公司 | 攀爬移动机器人路径引导方法 |
CN110298885A (zh) * | 2019-06-18 | 2019-10-01 | 仲恺农业工程学院 | 一种非光滑类球体目标的立体视觉识别方法和定位夹持检测装置及其应用 |
CN110405730A (zh) * | 2019-06-06 | 2019-11-05 | 大连理工大学 | 一种基于rgb-d图像的人机物交互机械臂示教*** |
CN110455189A (zh) * | 2019-08-26 | 2019-11-15 | 广东博智林机器人有限公司 | 一种大尺寸物料的视觉定位方法和搬运机器人 |
CN110568866A (zh) * | 2019-08-23 | 2019-12-13 | 成都新西旺自动化科技有限公司 | 一种三维立体曲面视觉引导对位***及对位方法 |
CN110706288A (zh) * | 2019-10-10 | 2020-01-17 | 上海眼控科技股份有限公司 | 目标检测的方法、装置、设备及可读存储介质 |
CN110744544A (zh) * | 2019-10-31 | 2020-02-04 | 昆山市工研院智能制造技术有限公司 | 服务机器人视觉抓取方法及服务机器人 |
CN110989631A (zh) * | 2019-12-30 | 2020-04-10 | 科沃斯机器人股份有限公司 | 自移动机器人控制方法、装置、自移动机器人和存储介质 |
CN111882610A (zh) * | 2020-07-15 | 2020-11-03 | 中国科学院自动化研究所 | 基于椭圆锥人工势场的服务机器人抓取目标物体的方法 |
CN112053398A (zh) * | 2020-08-11 | 2020-12-08 | 浙江大华技术股份有限公司 | 物体抓取方法、装置、计算设备和存储介质 |
CN112060087A (zh) * | 2020-08-28 | 2020-12-11 | 佛山隆深机器人有限公司 | 一种用于机器人抓取场景的点云碰撞检测方法 |
WO2021025960A1 (en) * | 2019-08-02 | 2021-02-11 | Dextrous Robotics, Inc. | A robotic system for picking and placing objects from and into a constrained space |
CN112508933A (zh) * | 2020-12-21 | 2021-03-16 | 航天东方红卫星有限公司 | 一种基于复杂空间障碍物定位的柔性机械臂运动避障方法 |
CN112914727A (zh) * | 2021-03-19 | 2021-06-08 | 联仁健康医疗大数据科技股份有限公司 | 非目标障碍物分离方法、***、医疗机器人及存储介质 |
CN113084817A (zh) * | 2021-04-15 | 2021-07-09 | 中国科学院自动化研究所 | 扰流环境下水下仿生机器人的物体搜索及抓取控制方法 |
CN113325832A (zh) * | 2020-02-28 | 2021-08-31 | 杭州萤石软件有限公司 | 一种可移动机器人避障方法、以及可移动机器人 |
CN113925742A (zh) * | 2021-10-20 | 2022-01-14 | 南通大学 | 一种目标驱动式的上肢外骨骼康复机器人的控制方法及其控制*** |
US11845184B2 (en) | 2022-04-18 | 2023-12-19 | Dextrous Robotics, Inc. | System and/or method for grasping objects |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CA2928645A1 (en) * | 2013-10-25 | 2015-04-30 | Aleksandar VAKANSKI | Image-based robot trajectory planning approach |
WO2016155787A1 (en) * | 2015-03-31 | 2016-10-06 | Abb Technology Ltd | A method for controlling an industrial robot by touch |
CN106407947A (zh) * | 2016-09-29 | 2017-02-15 | 百度在线网络技术(北京)有限公司 | 用于无人驾驶车辆的目标物体识别方法和装置 |
CN107053173A (zh) * | 2016-12-29 | 2017-08-18 | 芜湖哈特机器人产业技术研究院有限公司 | 机器人抓取***及抓取工件的方法 |
-
2018
- 2018-07-27 CN CN201810841533.0A patent/CN108858199B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CA2928645A1 (en) * | 2013-10-25 | 2015-04-30 | Aleksandar VAKANSKI | Image-based robot trajectory planning approach |
WO2016155787A1 (en) * | 2015-03-31 | 2016-10-06 | Abb Technology Ltd | A method for controlling an industrial robot by touch |
CN106407947A (zh) * | 2016-09-29 | 2017-02-15 | 百度在线网络技术(北京)有限公司 | 用于无人驾驶车辆的目标物体识别方法和装置 |
CN107053173A (zh) * | 2016-12-29 | 2017-08-18 | 芜湖哈特机器人产业技术研究院有限公司 | 机器人抓取***及抓取工件的方法 |
Cited By (34)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109767452A (zh) * | 2018-12-24 | 2019-05-17 | 深圳市道通智能航空技术有限公司 | 一种目标定位方法和装置、无人机 |
CN109934871A (zh) * | 2019-02-18 | 2019-06-25 | 武汉大学 | 一种面向高危环境的智能无人机抓取目标的***和方法 |
CN109978949A (zh) * | 2019-03-26 | 2019-07-05 | 南开大学 | 基于计算机视觉的农作物识别与特征点三维坐标提取的一种方法 |
CN109978949B (zh) * | 2019-03-26 | 2023-04-28 | 南开大学 | 基于计算机视觉的农作物识别与特征点三维坐标提取的一种方法 |
CN109910011A (zh) * | 2019-03-29 | 2019-06-21 | 齐鲁工业大学 | 一种基于多传感器的机械臂避障方法和机械臂 |
CN110000783A (zh) * | 2019-04-04 | 2019-07-12 | 上海节卡机器人科技有限公司 | 机器人的视觉抓取方法和装置 |
CN110182527A (zh) * | 2019-04-11 | 2019-08-30 | 上海快仓智能科技有限公司 | 用于货架阵列的出入库控制方法和搬运*** |
CN110182529A (zh) * | 2019-04-11 | 2019-08-30 | 上海快仓智能科技有限公司 | 用于货架阵列的出入库控制方法和搬运*** |
CN110116067A (zh) * | 2019-05-26 | 2019-08-13 | 天津大学 | 一种车轴自动喷涂装置和方法 |
CN110405730A (zh) * | 2019-06-06 | 2019-11-05 | 大连理工大学 | 一种基于rgb-d图像的人机物交互机械臂示教*** |
CN110298885A (zh) * | 2019-06-18 | 2019-10-01 | 仲恺农业工程学院 | 一种非光滑类球体目标的立体视觉识别方法和定位夹持检测装置及其应用 |
CN110298885B (zh) * | 2019-06-18 | 2023-06-27 | 仲恺农业工程学院 | 一种非光滑类球体目标的立体视觉识别方法和定位夹持检测装置及其应用 |
CN110231035A (zh) * | 2019-06-27 | 2019-09-13 | 北京克莱明科技有限公司 | 攀爬移动机器人路径引导方法 |
US11548152B2 (en) | 2019-08-02 | 2023-01-10 | Dextrous Robotics, Inc. | Systems and methods for robotic control under contact |
WO2021025960A1 (en) * | 2019-08-02 | 2021-02-11 | Dextrous Robotics, Inc. | A robotic system for picking and placing objects from and into a constrained space |
CN110568866A (zh) * | 2019-08-23 | 2019-12-13 | 成都新西旺自动化科技有限公司 | 一种三维立体曲面视觉引导对位***及对位方法 |
CN110455189A (zh) * | 2019-08-26 | 2019-11-15 | 广东博智林机器人有限公司 | 一种大尺寸物料的视觉定位方法和搬运机器人 |
CN110706288A (zh) * | 2019-10-10 | 2020-01-17 | 上海眼控科技股份有限公司 | 目标检测的方法、装置、设备及可读存储介质 |
CN110744544A (zh) * | 2019-10-31 | 2020-02-04 | 昆山市工研院智能制造技术有限公司 | 服务机器人视觉抓取方法及服务机器人 |
CN110989631A (zh) * | 2019-12-30 | 2020-04-10 | 科沃斯机器人股份有限公司 | 自移动机器人控制方法、装置、自移动机器人和存储介质 |
CN110989631B (zh) * | 2019-12-30 | 2022-07-12 | 科沃斯机器人股份有限公司 | 自移动机器人控制方法、装置、自移动机器人和存储介质 |
CN113325832A (zh) * | 2020-02-28 | 2021-08-31 | 杭州萤石软件有限公司 | 一种可移动机器人避障方法、以及可移动机器人 |
CN113325832B (zh) * | 2020-02-28 | 2023-08-11 | 杭州萤石软件有限公司 | 一种可移动机器人避障方法以及可移动机器人 |
CN111882610B (zh) * | 2020-07-15 | 2022-09-20 | 中国科学院自动化研究所 | 基于椭圆锥人工势场的服务机器人抓取目标物体的方法 |
CN111882610A (zh) * | 2020-07-15 | 2020-11-03 | 中国科学院自动化研究所 | 基于椭圆锥人工势场的服务机器人抓取目标物体的方法 |
CN112053398B (zh) * | 2020-08-11 | 2021-08-27 | 浙江大华技术股份有限公司 | 物体抓取方法、装置、计算设备和存储介质 |
CN112053398A (zh) * | 2020-08-11 | 2020-12-08 | 浙江大华技术股份有限公司 | 物体抓取方法、装置、计算设备和存储介质 |
CN112060087A (zh) * | 2020-08-28 | 2020-12-11 | 佛山隆深机器人有限公司 | 一种用于机器人抓取场景的点云碰撞检测方法 |
CN112508933A (zh) * | 2020-12-21 | 2021-03-16 | 航天东方红卫星有限公司 | 一种基于复杂空间障碍物定位的柔性机械臂运动避障方法 |
CN112914727A (zh) * | 2021-03-19 | 2021-06-08 | 联仁健康医疗大数据科技股份有限公司 | 非目标障碍物分离方法、***、医疗机器人及存储介质 |
CN113084817A (zh) * | 2021-04-15 | 2021-07-09 | 中国科学院自动化研究所 | 扰流环境下水下仿生机器人的物体搜索及抓取控制方法 |
CN113084817B (zh) * | 2021-04-15 | 2022-08-19 | 中国科学院自动化研究所 | 扰流环境下水下机器人的物体搜索及抓取控制方法 |
CN113925742A (zh) * | 2021-10-20 | 2022-01-14 | 南通大学 | 一种目标驱动式的上肢外骨骼康复机器人的控制方法及其控制*** |
US11845184B2 (en) | 2022-04-18 | 2023-12-19 | Dextrous Robotics, Inc. | System and/or method for grasping objects |
Also Published As
Publication number | Publication date |
---|---|
CN108858199B (zh) | 2020-04-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108858199A (zh) | 基于视觉的服务机器人抓取目标物体的方法 | |
CN106200679B (zh) | 基于多模态自然交互的单操作员多无人机混合主动控制方法 | |
Zou et al. | Particle swarm optimization-based source seeking | |
CN107688342B (zh) | 机器人的避障控制***及方法 | |
EP2835792B1 (en) | Method and system for selecting position and orientation for a monitoring camera | |
JP6069589B2 (ja) | コンポーネントを操作する、動かす、および/または姿勢変更する移動エージェント | |
CN109253729A (zh) | 一种无人机航线规划方法、装置及电子设备 | |
CN106647771B (zh) | 多移动机器人的最小步编队方法 | |
CN106774314A (zh) | 一种基于行走轨迹的家庭服务机器人路径规划方法 | |
Tarcai et al. | Patterns, transitions and the role of leaders in the collective dynamics of a simple robotic flock | |
CN107291072B (zh) | 一种移动机器人路径规划***及方法 | |
CN107065859A (zh) | 多移动机器人的轨迹预测方法 | |
CN107009357B (zh) | 一种基于nao机器人抓取物体的方法 | |
Zhang et al. | Path planning based quadtree representation for mobile robot using hybrid-simulated annealing and ant colony optimization algorithm | |
Auerbach et al. | Dynamic resolution in the co-evolution of morphology and control | |
Zhang et al. | Building metric-topological map to efficient object search for mobile robot | |
CN110162094A (zh) | 一种基于视觉测量信息的密集编队控制方法 | |
Chen et al. | Computationally efficient trajectory planning for high speed obstacle avoidance of a quadrotor with active sensing | |
Taylor | Bioinspired magnetoreception and navigation using magnetic signatures as waypoints | |
Ongaro et al. | Evaluation of an electromagnetic system with haptic feedback for control of untethered, soft grippers affected by disturbances | |
Liu et al. | Deep Q-learning for dry stacking irregular objects | |
CN116214522B (zh) | 基于意图识别的机械臂控制方法、***及相关设备 | |
Jia et al. | Autonomous tactile localization and mapping of objects buried in granular materials | |
Chiang | Vision-based obstacle avoidance system with fuzzy logic for humanoid robots | |
Krautmacher et al. | AIS based robot navigation in a rescue scenario |
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 |