一种基于物体平面特征的复合机器人3D抓取方法及系统与流程

一种基于物体平面特征的复合机器人3d抓取方法及系统
技术领域
1.本发明涉及机器视觉技术，尤其涉及基于物体平面特征的复合机器人3d抓取方法及系统。


背景技术：

2.目前现有固定式机械臂无法满足人们对于低成本多工位柔性化分析的需求，因此将机械臂与agv（automated guided vehicle自动引导车）相结合的移动式复合机器人在工业领域有着越来越重要的应用价值。由于移动式复合机器人定位精度低，目前一些物料的抓取、摆放等工作主要依赖agv导航定位和机器视觉补偿技术相结合的方式。
3.中国专利公布号为cn113084808a的发明专利公布了一种基于单目视觉的移动机械臂2d平面抓取方法。该方法主要使用基于形状的模板匹配方法识别标志物，计算agv移动后机械臂的新抓取位姿。但是通过标志物来间接定位抓取物，要求工件与标志物相对位姿固定，一旦放置有偏差，则容易造成后期抓取失败。另外该技术在抓取过程中，需多次移动机械臂拍摄图片以供计算抓取位姿，这样会导致整体抓取节拍过于缓慢。
4.在3d视觉定位方面，bsel和mckay提出了迭代最近点(iterative closest point ,icp)算法，它重复进行“确定对应点关系点集——计算最优刚体变换”的过程，直到两片点云对应点的均方误差最小。但是对于传统的icp配准算法及其改进形式，它们对于点云的初始位姿要求极高，容易陷入局部最优；算法在搜索对应点的过程中，计算代价很大，对于实际测量的海量数据，无法直接使用。
5.另一方面，rusu提出的基于点特征直方图(point feature histograms ,pfh)的三维点云配准算法，其通过参数化查询点与邻域点之间的空间差异，并形成一个多维直方图对点的k邻域集合属性进行描述，他是基于点与其 k邻域之间的关系以及它们的估计法线，考虑估计法线方向之间所有的相互作用，试图捕获最好的样本表面变化情况，以描述样本的几何特征，通过寻找最为相似的点特征直方图实现了点云的配准。
6.此外，在pfh的基础上，2009年rusu提出了基于快速点特征直方图(fast point feature histograms ,fpfh)三维点云配准算法，降低了算法的计算复杂度，保留了 pfh大部分的识别特性，提高了点云配准的效率。基于特征的点云配准算法本质上是将二维图像的特征推广到三维，如：sift-3d、 harris-3d、iss关键点的提取，还有pfh、fpfh特征直方图的计算。
7.然而对于识别工业场景中平面特征较为丰富的目标物体的点云，当局部特征较为相似时，容易导致点云匹配效果较差。


技术实现要素：

8.为此本发明的主要目的在于提供一种基于物体平面特征的复合机器人3d抓取方法及系统，以实现无需借助额外标志物，即可识别工业场景中平面特征较为丰富的目标物体，并准确解得机械臂的抓取位姿。
9.为了实现上述目的，根据本发明的一个方面，提供了一种基于物体平面特征的复合机器人3d抓取方法，其步骤包括：s1标定机械臂与相机之间的手眼关系；s2设定标准拍照位姿，并采集场景点云，以分离出目标物体点云来制作第一模板；s3 agv在保持不动的条件下获取场景点云，以供与第一模板进行模板匹配，进而获取场景中目标物体相对于第一模板的第一变换矩阵；s4 在agv和目标物体保持不动的条件下，示教机械臂，直至获取可抓取目标物体的第一位姿以结合步骤s3的第一变换矩阵，计算出标准抓取位姿变换矩阵；s5机械臂保持标准拍照位姿，移动agv到新的位置后获取场景点云，以供与第一模板进行模板匹配，进而获取场景中目标物体相对于第一模板的第二变换矩阵；s6根据第二变换矩阵与步骤s4标准抓取位姿变换矩阵，解得机械臂抓取位姿。
10.在可能的优选实施方式中，其中该步骤s2中制作第一模板的步骤包括：s21在目标物体点云的平面特征区域创建roi，将roi内点云拟合平面后；s22建立旋转矩阵r来转换roi内点云，以旋转roi内点云使其所在平面法向与z轴平行；s23计算旋转后roi内点云的中心，移动roi内点云使其中心与参考坐标系原点重合后，将该roi内点云定为第一模板。
11.在可能的优选实施方式中，其中该步骤s3中模板匹配步骤包括：s31对场景点云使用区域增长算法分割出各初始曲面点集；s32使用随机抽样一致性算法在各初始曲面点集中提取其所包含的平面模型及其对应的平面点云；s33将第一模板以及步骤s32中获得的各平面点云分别在参考坐标系内映射成相同分辨率的2d栅格图；s34将平面点云的2d栅格图与第一模板的2d栅格图进行重合匹配，若匹配成功，记录平面点云在参考坐标系内的旋转角度及偏移量，籍以建立目标物体到相机的第一变换矩阵。
12.在可能的优选实施方式中，其中该步骤s33中平面点云的2d栅格图建立步骤包括：s331建立旋转矩阵r1来转换平面点云，以旋转平面点云使其平面法向与z轴平行；将旋转矩阵r1转换为齐次变换矩阵；s332 为根据步骤s331转换后的平面点云建立包围盒；s333根据预设尺寸等距栅格化该包围盒，并将存在点云的栅格进行填充标记，以转换为平面点云的2d栅格图，并记录对应二值变换矩阵为。
13.在可能的优选实施方式中，其中该步骤s33中第一模板的2d栅格图建立步骤包括：s334 建立第一模板点云包围盒；s335根据预设尺寸等距栅格化第一模板点云包围盒，并将存在点云的栅格进行填充标记，以转换为第一模板的2d栅格图。
14.在可能的优选实施方式中，其中该步骤s34中所述重合匹配步骤包括，粗匹配步骤：
s341以预设步长对平面点云的2d栅格图进行旋转、偏移操作，直至与第一模板的2d栅格图匹配后，记录匹配旋转角度及偏移量；s342 根据匹配旋转角度反算第一模板的2d栅格图的相对旋转角度，以建立变换矩阵。
15.在可能的优选实施方式中，其中该步骤s34中所述重合匹配步骤还包括，精匹配步骤：s343 将第一模板的点云根据步骤s342获得的变换矩阵进行转换，得到第二模板点云；以建立第二模板点云包围盒；s344 根据预设尺寸等距栅格化第二模板点云包围盒，并将存在点云的栅格进行填充标记，以转换为第二模板的2d栅格图；并建立对应的二值变换矩阵为；s345以预设步长对平面点云的2d栅格图进行旋转、偏移操作，直至与第二模板的2d栅格图匹配后，记录匹配旋转角度及偏移量；s346根据匹配旋转角度及偏移量反算第二模板的2d栅格图的相对旋转角度及偏移量，以建立3d空间变换矩阵，进而反算出目标物体在3d空间内的姿态得出第一变换矩阵。
16.在可能的优选实施方式中，其中该步骤s4中所述标准抓取位姿变换矩阵计算步骤包括：将示教机械臂后获得的第一位姿记为pose，其中将示教机械臂后获得的第一位姿记为pose，其中其中是预设的目标物体到机械臂基座的变换矩阵；为第一变换矩阵；是预设的相机到机械臂末端的变换矩阵；是预设的机械臂末端到基座的标准拍照位姿变换矩阵；进而结合第一变换矩阵，计算出标准抓取位姿变换矩阵。
17.在可能的优选实施方式中，其中该步骤s6中计算机械臂抓取位姿pose1的步骤包括：计算，其中为第二变换矩阵。
18.为了实现上述目的，根据本发明的另一个方面，还提供了一种基于物体平面特征的复合机器人3d抓取系统，其包括：存储单元，其存有实现如权利要求1至9任一所述3d抓取方法步骤的程序，以供控制单元及处理单元适时调取执行包括；控制单元操控机械臂及相机以标准拍照位姿，采集场景点云以向处理单元发送；处理单元从场景点云中分离出目标物体点云以制作第一模板；控制单元操控agv在保持不动的条件下，令相机获取场景点云，以并向处理单元发
送供其与第一模板进行模板匹配计算，进而获取场景中目标物体相对于第一模板的第一变换矩阵；控制单元操控agv保持静止，此时若目标物体也静止时，示教机械臂，直至获取可抓取目标物体的第一位姿，以向处理单元发送，处理单元根据第一位姿及第一变换矩阵，计算出标准抓取位姿变换矩阵；控制单元操控机械臂保持标准拍照位姿，移动agv到新的位置后，令相机获取场景点云，以向处理单元传输以供其与第一模板进行模板匹配计算，进而获取场景中目标物体相对于第一模板的第二变换矩阵；处理单元进一步根据第二变换矩阵与标准抓取位姿变换矩阵，解得机械臂抓取位姿后，发送至控制单元令其据此控制机械臂对目标物体进行抓取。
19.通过本发明提供的该基于物体平面特征的复合机器人3d抓取方法及系统，使得复合机器人无需借助额外标志物，即可识别工业场景中平面特征较为丰富的目标物体，相比icp、fpfh等3d匹配算法，本案的计算代价相对更小，识别速度更快、稳定性更高，可实现精确定位和快速抓取，同时其定位抓取精度不受avg精度影响。
20.另一方面由于本发明提供的方案中，机械臂不需要进行工具坐标系标定，因此方案部署难度较低，步骤较少，有利于快速部署。
附图说明
21.构成本技术的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：图1为本发明中示例的复合机器人结构示意图；图2为本发明的抓取方法的基本步骤流程示意图；图3为本发明的抓取方法中计算目标物体到相机的第一变换矩阵的步骤流程示意图；图4为本发明的抓取方法中相机所采集的场景点云的示意图；图5为本发明的抓取方法中从场景点云中分离出的目标物体点云第一模板示意图；图6为本发明的抓取方法中agv保持不动时相机重新获取的一帧场景点云示意图；图7为本发明的抓取方法中第一模板与场景点云匹配示意图。
22.图8为本发明的抓取方法中平面点云的2d栅格图与第一模板的2d栅格图匹配过程图，其中左侧为平面点云的2d栅格图，右侧为第一模板的2d栅格图，下侧为两者最佳匹配结果图；图9为本发明的抓取方法中的源图像；图10为本发明的抓取方法中的源图像旋转10
°
后被裁切的示意图；图11为本发明的抓取方法中对旋转后的源图像进行补偿后的示意图；图12为本发明的抓取系统结构示意图。
23.附图标记说明agv 1，机械臂2，相机3，夹爪4，目标物体5，工作台6。
具体实施方式
24.为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的具体技术方案进行清楚、完整地描述，以助于本领域的技术人员进一步理解本发明。显然，本案所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思及相互不冲突的前提下，本技术中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的揭露及保护范围。
25.此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“s1”、“s2”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。除非另有明确的规定和限定，术语“设置”、“布设”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本案中的具体含义。
26.如图1所示，本发明所称的复合机器人包括：具有多自由度的机械臂，agv移动机器人，具有三维数据采集功能的相机，夹爪，其中该相机固定在机械臂上，而机械臂的基座固定在agv上，因此基于该复合机器人的结构，本发明构思上，通过将机械臂视觉抓取问题等价为，为了获得相机中拍到的目标物体在机械臂基座标系下的位姿问题，因此本发明的下述示例中皆使用眼在手上方式进行定义：其中是目标物到机械臂基座的变换矩阵，是目标物体到相机的变换矩阵，是相机到机械臂末端的变换矩阵，是机械臂末端到基座的变换矩阵。
27.具体来说，如图1至图3所示，该复合机器人若要抓取到工作台上的目标物体，则根据本发明提供的该基于物体平面特征的复合机器人3d抓取方法，其步骤包括：步骤s1预先标定机械臂与相机之间的手眼关系，使用眼在手上标定方式，获得相机到械臂末端的变换矩阵。
28.步骤s2设定标准拍照位姿，以建立机械臂末端到基座的变换矩阵，之后通过相机采集一帧包含场景点云（如图4所示），以分离出所要抓取的目标物体点云来制作第一模板（如图5所示）。
29.具体的，该制作第一模板的步骤包括：步骤s21：在目标物体点云的平面特征区域创建roi（region of interest感兴趣区域），并对roi内点云使用最小二乘法拟合平面方程，使其平面朝向参考坐标系的 法向。
30.例如对roi内有n个点的点云集合，i＝0 ,1 ,
…
，n-1，n≥3，用最小二乘法拟合所述平面方程， 其中平面方程一般式为ax+by+cz+d=0，假设c≠0， 那么按最小二乘思路使误差平方和最小，即偏导数。通过解方程组，可以获得平面方程参数。
31.步骤s22：然后计算法向与参考坐标系的z轴方向夹角，即 ；以及参考坐标系a轴方向的旋转轴，即 ，之后根据罗德里格公式（rodrigues' rotation formula），带入，,建立并计算3d旋转矩阵r，由此将roi内点云绕旋转轴旋转角度，即左乘矩阵r后，即可使其平面法向与z轴平行。
32.步骤s23：计算旋转后roi内点云的中心，并移动点云使其中心与参考坐标系原点重合后，将此刻该roi内点云定为第一模板。
33.步骤s3令agv保持不动，相机重新获取一帧如图6所示的场景点云，并如图7所示以供与第一模板进行模板匹配，进而获取场景中目标物体相对于第一模板的第一变换矩阵。
34.具体来说，该模板匹配步骤包括：s31对场景点云使用区域增长算法进行分割，以形成各初始曲面点集。
35.例如对场景点云p中的任一采样点pi(x，y，z) ,借助k-d树数据结构查找与其距离最邻近的k个采样点，并组成其k-邻域nk(pi)；确定曲面上一点的法线的问题近似为估计平面切线到曲面的法线的问题，这转而成为一个最小二乘平面拟合估计问题。其公式如下，，对于上式，k是最邻近点的个数，表示最近邻点集的中心， 是协方差矩阵的第j个特征值，是第j个特征向量。由以上公式可知，估计表面法线的解决方案可以被简化为查询点的最近邻点创建的协方差矩阵的特征向量和特征值(或pca主成分分析)的分析。
36.对于每个查询点，都可利用其最邻近点创建出协方差矩阵c。计算得到特征值λ0≥λ1≥λ2和特征向量 ，对最小特征λ2对应的特征向量 进行单位化，并由其近似表示采样点pi处的法向；利用公式c＝λ2/(λ0+λ1+λ2)即可计算pi处的近似曲率。
37.进一步的，在执行区域增长算法分割初始曲面ri时，先从p中选择一个还未分割、且曲率最小的采样点为初始种子点，并添加到集合s中；然后，对s中的每个种子点si，查找其 k-邻域，并判断其k-邻域中的任一点pi与si的共面性情况；如果pi和si的法向间夹角小于共面点法向阈值kn，则将pi添加到初始曲面ri的采样点集合p(ri)中；同时，如果pi点的
曲率还小于共面点曲率阈值kc，则将pi点看作是一个新的种子点，并添加到s；依此方式不断地向外增长，直到不再产生新的种子的点，并生成一个初始曲面ri；迭代执行上述区域增长算法，直到p中没有未分割点云。判断每个初始曲面ri，如果p(ri)中的采样点数量小于ks，则认为ri中不可能存在目标区域，并将其去除，最终得到初始曲面点集r0；步骤s32：使用随机抽样一致性算法在各初始曲面点集中提取其所包含的平面模型及其对应的平面点云。
38.例如，对于r0中任意初始曲面ri都进行如下操作：首先，设置平面点集的点数阈值kp（kp≥3），当点集中点数大于kp，随机抽样一致性算法提取平面，其次，设置随机抽样一致性算法提出的模型为空间平面模型、判断一个采样点属于平面模型内部点距离阈值d＝kd；，借助随机抽样一致性算法从ri中提取一个小平面ri_n，包括ri_n的平面模型mi和内部点集合p(ri_n)，对于内部点集合p(ri_n) 进行主成分分析，计算得到特征值λ0≥λ1≥λ2和特征向量 ，对最小特征λ2对应的特征向量 进行单位化，并由其近似表示平面模型mi的法向，p(ri_n)的中心点ci表示为平面上确定一点，为了防止计算到的出现180
°
反转，统一规定与ci所表示的向量的夹角应＜90
°
，否则法向为，平面模型mi_refine由ci和确定；再次，把ri_n在ri中去除，获得余下点集ri_remain; 对ri_remain重复上述步骤，获得ri中所有包含的平面模型mi_refine和其内部点集p(ri_n)。
39.对于任意两个平面模型和内部点云，检验他们的共面性和连接性，合并属于同一实际平面的点云。上述步骤提取出的为平面点云集合ra，planecloud_i为ra中任意一个平面点云，之后判断其与ra中余下的每个平面点云planecloud_j是否共面且相连，若是，则说明planecloud_i与planecloud_j属于目标物的同一实际平面，标记planecloud_i与planecloud_j，并将其合并成一个新的平面点云planecloud_new，将planecloud_new与ra中未标记过点云继续重复上述判断；否则，将planecloud_i作为一个单独的平面；对ra中每一个点云进行上述判断操作；在此，判断planecloud_i与planecloud_j是否共面且相连时，如采用的表达式为：planecloud_i∩planecloud_j≠
∅
。并且acos(n_i
·
n_j)＜ε，其中n_n和n_m为planecloud_i与planecloud_j对应平面方程模型的法向。
40.最终得到r0中所有的平面模型plane_i和对应的内部平面点云points_i。
41.步骤s33：将第一模板以及步骤s32中提取的各平面点云分别在参考坐标系内映射成相同分辨率的2d栅格图。
42.具体来说，其中步骤s33中平面点云的2d栅格图建立步骤包括：步骤s331：对于场景中的平面点云points_i，根据其所在平面法向ni，结合步骤s22中的方法对其进行旋转，使其所在平面法向为（0，0，1），以建立旋转矩阵r1，从而通过计算旋转矩阵r1来转换平面点云，以旋转平面点云使其平面法向与z轴平行，同时可将旋转矩阵r1写为齐次变换矩阵，最终得出转换后的平面点云points_ir。；步骤s332： 为转换后的平面点云points_ir建立包围盒min1（xmin1，ymin1，zmin1）、max1（xmax1，ymax1，zmax1）后进行平移，将该包围盒左下角min1移动到参考坐标系的原点o（0，0，0）；
步骤s333：根据点云的点间距确定栅格的分辨率reslution（a，a），将xy平面等距栅格化该包围盒，若栅格内存在点，则将灰度值设置为255，反之设置为0，从而得到各点云的二值图像，此时可能得到的图像存在残缺或孔洞，因此可进行一次开操作，并计算连通区域，设置孔洞面积阈值hole，连通区域面积si《hole则用255填充，将残缺或孔洞补全，得到最终的点云二值图，即平面点云的2d栅格图p_points_ir，并记录对应二值变换矩阵为：其中zmid为转换后的平面点云中所有点的z值的中位数。
43.其中步骤s33中第一模板的2d栅格图建立步骤包括：步骤s334：对第一模板点云pc2，计算点云包围盒min2（xmin2，ymin2，zmin2）、max2（xmax2，ymax2，zmax2），平移点云，将包围盒左下角min2移动到原点o（0，0，0）。
44.步骤s335：根据点云的点间距确定栅格的分辨率reslution（a，a），将xy平面等距栅格化，若栅格内存在点，则将灰度值设置为255，反之设置为0，得到各点云的二值图像，得到的图像存在残缺或孔洞，需要进行一次开操作，并计算连通区域，设置孔洞面积阈值hole，连通区域面积si《hole则用255填充，将残缺或孔洞补全，得到最终的点云二值图，即第一模板的2d栅格图p_modelr。
45.步骤s34：将平面点云的2d栅格图与第一模板的2d栅格图进行重合匹配，若匹配成功，记录平面点云在2d空间中参考坐标系内的旋转角度及偏移量，籍以建立目标物体到相机的第一变换矩阵。
46.具体来说，步骤s34中所述重合匹配步骤示例包括：步骤s341：如图8所示，记平面点云的2d栅格图为s，其栅格宽为w，高为h；第一模板的2d栅格图为t，其宽为w_t，高为h_t；设置匹配角度区间[minangle,maxangle]，以进行粗匹配。
[0047]
步骤s342：以预设角度步长对s进行旋转，旋转中心为s的中点c(w/2,h/2)，计算出旋转θ角度时的t的变换矩阵的变换矩阵同时计算出旋转后新的s图的宽高（new_w，new_h），计算如下：new_w= h
∗
abs()+w
∗
abs()，new_h= w
∗
abs()+h
∗
abs()，此时，由于旋转矩阵是以源图像（如图9所示）自身的图像中心c计算的的，为了不让输出图像被裁切掉一部分（如图10所示），那么需要在t的变换矩阵的第三列上增加一个补偿值（如图11所示进行补足），从而形成最终仿射变换矩阵t1；；s旋转θ角度后，进行基于平方差的2d模板匹配，将t(w_t*h_t)矩阵和s(w*h)矩阵计算匹配度，如调用opencv函数计算，即用该t的矩阵
大小作为滑窗窗口在s上滑动，以计算t矩阵中每个像素和s对应的像素差异，从而得到匹配结果矩阵r_mat，其中匹配度越高，平方差的和就越小，匹配结果矩阵r_mat中最小值元素位置就是t在s中的偏移量，重复上述步骤，获得最佳旋转角度angle1，和偏移量d1（x1，y1）；此时根据s旋转的angle1转而得到t旋转的anglet，并作为粗匹配结果，以获得绕z轴旋转anglet角度的变换矩阵。
[0048]
其中步骤s34中所述重合匹配步骤中还包括精匹配步骤：步骤s343： 将第一模板的点云根据步骤s342获得的变换矩阵进行变换，得到第二模板点云；建立第二模板点云包围盒min3（xmin3，ymin3，zmin3）、max3（xmax3，ymax3，zmax3）后进行平移，将第二模板点云包围盒左下角min3移动到参考坐标系的原点；步骤s344：根据预设尺寸等距栅格化第二模板点云包围盒，并参照步骤s335将存在点云的栅格进行填充标记，以转换为第二模板的2d栅格图t2，其宽为w_t2，高为h_t2；并记录对应二值变换矩阵为；步骤s345：设置旋转角度步长匹配区间[-step,step],以预设角度步长对s图进行旋转，旋转中心为图像中点c(w/2,h/2), 将t2(w_t2*h_t2)矩阵和s(w*h)矩阵进行基于平方差的2d模板匹配，以当得到匹配结果矩阵r_mat2时，记录s最佳旋转角度angle2，并取r_mat中最小值元素位置为t2在s中的偏移量d2（x2，y2）；其中，需要要注意的是，粗匹配和精匹配步骤所使用的角度区间和步长的设置一般不同，即粗匹配会给出一个相较精匹配更较大的角度区间及步长step。
[0049]
步骤s346：计算出旋转θ角度时的t2的变换矩阵的变换矩阵同时计算出旋转后新的s图的宽高（new_w，new_h），计算如下：new_w= h
∗
abs()+w
∗
abs()，new_h= w
∗
abs()+h
∗
abs()，此时，由于旋转矩阵是以源图像（如图9所示）自身的图像中心c计算的的，为了不让输出图像被裁切掉一部分（如图10所示），那么需要在t2的变换矩阵的第三列上增加一个补偿值（如图11所示进行补足），从而形成最终变换矩阵t3；；由于旋转的是s，相对于t的匹配结果转换成矩阵形式：由于旋转的是s，相对于t的匹配结果转换成矩阵形式：当转换到3d空间后
,最终结合上述步骤中的变换，反算目标物体在3d空间姿态，如下：以mi（xm，ym，zm）为第一模板的点云中任意一点，场景点云中对应点si（xs，ys，zs），因此第一变换矩阵因此第一变换矩阵。
[0050]
步骤s4在agv和目标物体保持不动的条件下，示教机械臂，直至获取可抓取目标物体的第一位姿以结合步骤s3的第一变换矩阵，计算出标准抓取位姿变换矩阵。
[0051]
具体来说，步骤s4中所述标准抓取位姿变换矩阵计算步骤包括：步骤s41：根据此前定义的 获取相关计算参数，其中是目标物体到机械臂基座的变换矩阵，为第一变换矩阵，是相机到机械臂末端的变换矩阵，是机械臂末端到基座的变换矩阵；步骤s42：将示教机械臂后获得的第一位姿记为pose，其中进而结合步骤s3的第一变换矩阵，计算出标准抓取位姿变换矩阵。
[0052]
步骤s5机械臂保持标准拍照位姿，移动agv到新的位置后获取场景点云，以供与第一模板进行模板匹配，并重复上述步骤s3中计算第一变换矩阵的相关步骤，进而获取场景中目标物体相对于第一模板的第二变换矩阵。
[0053]
步骤s6根据第二变换矩阵与步骤s4标准抓取位姿变换矩阵，解得机械臂最终抓取位姿，即计算。
[0054]
另一方面，如图12所示，本发明还提供了一种基于物体平面特征的复合机器人3d抓取系统，其包括：存储单元，其存有实现如上述实施例任一所述3d抓取方法步骤的程序，以供控制单元及处理单元适时调取执行包括； 控制单元操控机械臂及相机以标准拍照位姿，采集场景点云以向处理单元发送；处理单元从场景点云中分离出目标物体点云以制作第一模板。
[0055]
控制单元操控agv在保持不动的条件下，令相机获取场景点云，以并向处理单元发
送供其与第一模板进行模板匹配计算，进而获取场景中目标物体相对于第一模板的第一变换矩阵。
[0056]
控制单元操控agv保持静止，此时若目标物体也静止时，示教机械臂，直至获取可抓取目标物体的第一位姿，以向处理单元发送，处理单元根据第一位姿及第一变换矩阵，计算出标准抓取位姿变换矩阵。
[0057]
控制单元操控机械臂保持标准拍照位姿，移动agv到新的位置后，令相机获取场景点云，以向处理单元传输以供其与第一模板进行模板匹配计算，进而获取场景中目标物体相对于第一模板的第二变换矩阵。
[0058]
处理单元进一步根据第二变换矩阵与标准抓取位姿变换矩阵，解得机械臂抓取位姿后，发送至控制单元令其据此控制机械臂对目标物体进行抓取。
[0059]
综上所述，通过本发明提供的该基于物体平面特征的复合机器人3d抓取方法及系统，使得复合机器人无需借助额外标志物，即可识别工业场景中平面特征较为丰富的目标物体，相比icp、fpfh等3d匹配算法，本案的计算代价相对更小，识别速度更快、稳定性更高，可实现精确定位和快速抓取，同时其定位抓取精度不受avg精度影响。
[0060]
另一方面由于本发明提供的方案中，机械臂不需要进行工具坐标系标定，因此方案部署难度较低，步骤较少，有利于快速部署。
[0061]
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
[0062]
本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0063]
此外实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器（rom，read-only memory）、随机存取存储器（ram，random access memory）、磁碟或者光盘等各种可以存储程序代码的介质。
[0064]
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。