CN112476434A - 一种基于协作机器人的视觉3d取放方法及系统 - Google Patents

Abstract

本发明提供一种基于协作机器人的视觉3D取放方法及系统，方法包括：标定双目结构光三维扫描仪的相机的内外参数；标定协作机器人的手眼，获得标定结果矩阵；采集待取放目标物三维数字模型三维数字模型；采用标定后的双目结构光三维扫描仪获取散乱堆叠的待取放目标物的点云数据，点云分割得到多个待取放目标物的场景点云；根据多个待取放目标物的场景点云选择抓取成功率最高的待取放目标物作为抓取目标；通过抓取目标的三维数字模型三维数字模型与场景点对特征进行配准，将预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为抓取目标的抓取位姿；规划出协作机器人初步的抓取路径轨迹。可准确识别目标物，抓取定位精度高。

Description

一种基于协作机器人的视觉3D取放方法及系统

技术领域

本发明涉及视觉3D取放技术领域，尤其涉及一种基于协作机器人的视觉3D取放方法及系统。

背景技术

随着工业制造与物流运输的自动化、智能化，多传感器融合的工业机器人取放系统是未来自动化智能制造、智慧物流领域的核心主力。目前工业机器人取放系统主要运用于产线工件装配、物料上料、产品搬运、目标分拣、缺陷检测以及包装等领域。传统结构化环境中通过离线编程进行单一重复作业的的机器人取放系统无法完成散乱堆叠目标物对象，同时也无法估计场景抓取目标物的位姿，只是单纯机械式重复视角定义的抓取过程。因此，为了更加锲合智能制造及智慧物流领域，如何从散乱堆叠目标物场景中准确识别定位目标物，并获得正确的抓取位姿，是现阶段机器人智能取放系统所要面临的一个十分严峻的考验。

自动化视觉取放系统可以分为视觉感知部分与机器人抓取操作部分；视觉感知部分在机器人抓取作业中的作用就是识别、定位目标物体，为机器人提供目标物体的类型与位姿信息，机器人抓取操作部分用于精确的完成目标物取放任务；然而，现有的机器人抓取操作部分大多基于传统工业机器人，操作复杂，无碰撞检测，不注重人机协作，对环境的可操作性差，大多只能用于结构化环境中；而视觉感知部分则主要针对二维图像的机器人平面单目标物体的识别，但对于现实复杂环境中的三维物体，仅使用二维信息对三维目标进行表征，会不可避免的造成信息损失，从而难以实现非结构化环境中机器人对于多目标物体的高精度抓取操作。因此，如何提升机器人的视觉感知能力，并基于此在复杂环境中自主完成对目标物的识别、定位、抓取等操作是动化智能视觉3D取放系统要解决的关键问题。

已有的视觉感知过程大多只能用于平面单目标物体的2D位姿估计，易受光照、背景环境等因素影响，难以应对场景光照变化、遮挡堆叠等实际情况。而目前取得较大进展的基于深度学习方法的视觉感知系统，其网络训练数据集制作周期长，制作难度高；同时网络模型泛化能力弱，鲁棒性低，不利于实际应用，多在实验室场景下进行。

而对于已有的机器人系统，安装使用上限高，编程过程繁琐，不注重人机协作，运动轨迹多为人工示教，未进行轨迹规划，运行效率低，可靠性差，机器人目标位置发生变化便会作业失败，只能运用于结构化环境中。

现有技术中缺乏一种基于协作机器人及高精度视觉算法的自动化、智能化取放系统。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

发明内容

本发明为了解决现有的问题，提供一种基于协作机器人的视觉3D取放方法及系统。

为了解决上述问题，本发明采用的技术方案如下所述：

一种基于协作机器人的视觉3D取放方法，包括如下步骤：S1：标定双目结构光三维扫描仪的相机的内外参数；S2：标定协作机器人的手眼，获得所述双目结构光三维扫描仪与所述协作机器人末端执行件夹爪的空间相对位置关系的标定结果矩阵；S3：采集待取放目标物三维数字模型；S4：采用标定后的所述双目结构光三维扫描仪获取散乱堆叠的所述待取放目标物的点云数据，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云；S5：根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选，选择抓取成功率最高的所述待取放目标物作为抓取目标；S6：通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿；S7：根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹。

优选地，标定双目结构光三维扫描仪的相机的内外参数包括如下步骤：S11：从不同角度和位置对标定板进行拍摄得到标定图片，其中，所述标定板配置有编码标志点和非编码标志点；S12：对所述标定图片进行识别获得所述编码标志点和所述非编码标志点的二维图像坐标，对所述编码标志点和所述非编码标志点进行三维重建获得准确的三维空间坐标；S13：基于所述编码标志点和所述非编码标志点的二维图像坐标、三维空间坐标对所述相机的内外参数进行整体的光束平差迭代优化调整，以最小化所述相机的重投影误差为目标损失函数，并加入比例尺参数进行约束获得所述相机的内外参数。

优选地，标定协作机器人的手眼，获得所述双目结构光三维扫描仪与所述协作机器人末端执行件夹爪的空间相对位置关系的标定结果矩阵包括如下步骤：S21：配置有编码与非编码标志点的标准标定板，所述双目结构光三维扫描仪与所述协作机器人的机械臂底座，所述机器人的工具坐标系与所述标准标定板的空间相对位置关系在整个标定过程固定不变；S22：控制所述协作机器人移动所述标准标定板到空间不同位置，使用所述双目结构光三维扫描仪观察所述标准标定板采集标定图像；重复这一过程，采集10～15帧标定图像；S23：对所述标定图像进行双目匹配、三维重建，计算重建标志点三维坐标点与已知的标准标定板世界坐标系全局点相对外参数；S24：从所述协作机器人的示教器中读取预先存储的所述协作机器人的当前空间标定位置，与所述相对外参数作为手眼标定过程的数据输入，利用手眼标定算法进行求解，获得所述标定结果矩阵。

优选地，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云包括如下步骤：S41：去除所述待取放目标物的场景点云中的背景点云；S42：去除待取放目标物的场景点云中的离散点；S43：采用三维区域生长分割算法，根据所述点云数据的特征分割聚类成不同的目标物假设，获得多个所述待取放目标物的场景点云。

优选地，根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选包括如下规则：计算分割获得的散乱堆叠的多个所述待取放目标物的场景点云的点云簇质心，选择所述点云簇质心的高度最高的待取放目标物视为抓取成功率最高的待取放目标物；计算选择的所述待取放目标物的场景点云与剩余所述待取放目标物的场景点云的重合度，平均重合度最小的为抓取成功率最高的待取放目标物；比较转换至所述协作机器人的机械臂坐标系下的所述待取放目标物的场景点云的抓取位姿与所述待取放目标物的场景点云的抓取位姿的相似程度，相似程度最大的为抓取成功率最高的待取放目标物；所述待取放目标物的场景点云的点云数量最多的为抓取成功率最高的待取放目标物。

优选地，在所述三维数字模型上预先定义所述取放位姿点，具体包括：根据所述三维数字模型，基于3-2-1坐标系定义方法，预先定义所述待取放目标物的抓取位置及抓取姿态确定所述待取放目标物的取放位姿点；或，使用主成分分析方法建立坐标系，确定抓取位姿并使用质心点作为抓取点确定所述待取放目标物的取放位姿点；通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿包括如下步骤：S61：分别将所述抓取目标的三维数字模型和所述抓取目标的场景点云数据进行前处理，所述前处理包括对点云的下采样和法向估计；S62：对前处理后的所述抓取目标的三维数字模型进行稠密点对特征提取，构建所述抓取目标的三维数字模型的点对特征哈希表；对前处理后的所述抓取目标的场景点云数据选取参考点进行稀疏点对特征提取，构建参考点点对特征哈希表；S63：识别所述抓取目标的场景点云数据对应的点云位姿，基于所述参考点点对特征哈希表从所述抓取目标的三维数字模型的点对特征哈希表获取相对应的特征以及相对应的位姿；S64：采用霍夫投票数从所述相对应的位姿中筛选出候选位姿；S65：根据如下公式从所述候选位姿中选出匹配质量Q最高的候选位姿：

其中，已匹配的目标点数是所述抓取目标的场景点云数据与所述抓取目标的三维数字模型中已匹配的目标点数；目标点总数是所述抓取目标的三维数字模型中三维点总数；S66：使用迭代最近点配准算法，以所述匹配质量Q最高的候选位姿为初始值，通过最小二乘法迭代计算最优的坐标变换矩阵，获得配准后的位姿估计矩阵为配准后的位姿估计结果；S67：将所述三维数字模型上预先定义的所述取放位姿点配准到场景中，得到配准后的抓取位姿估计结果，作为所述抓取目标的抓取位姿。

优选地，将所述三维数字模型上预先定义的所述取放位姿点配准到场景中，得到配准后的抓取位姿估计结果，作为所述待取放目标物的抓取位姿包括：利用所述标定结果矩阵，左乘所述配准后的位姿估计矩阵获得所述协作机器人可执行的所述抓取目标的抓取位姿。

优选地，根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹包括：采用所述协作机器人各关节轴旋转角度表示路径点，以所述协作机器人各轴旋转角度的绝对值之和作为机器人的运动代价，根据实际抓取过程，将路径分段，对同一路径段内路径点进行全排列，计算所有排列方式的机器人运动代价总和，以运动代价最小的路径作为该路径段的局部最优路径轨迹，直至所有路径段均完成局部最优路径计算，按照取放动作流程顺序连接所述局部最优运动路径得到所述协作机器人初步的抓取路径轨迹；其中，运动代价函数计算公式为：

其中，q_i代表关节旋转角度表示的路径点，q₀代表机械臂的机械零点关节轴旋转角度。

优选地，还包括：S81：仿真运行所述协作机器人初步的抓取路径轨迹，基于包围盒碰撞检测算法检测所述协作机器人末端执行件夹爪与周围固定场景及机械臂自身的碰撞，记录碰撞点；S82：根据所述碰撞点，基于运动学逆解和关节空间的规划方法添加中间避障过渡点生成避障轨迹路径，补充到所述协作机器人初步的抓取路径轨迹，得到最终的抓取路径轨迹。

本发明还提供一种基于协作机器人的视觉3D取放系统，包括：第一单元：用于标定双目结构光三维扫描仪的相机的内外参数；第二单元：用于标定协作机器人的手眼，获得所述双目结构光三维扫描仪与所述协作机器人末端执行件夹爪的空间相对位置关系的标定结果矩阵；第三单元：用于采集待取放目标物三维数字模型；第四单元：用于采用标定后的所述双目结构光三维扫描仪获取散乱堆叠的所述待取放目标物的点云数据，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云；第五单元：用于根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选，选择抓取成功率最高的所述待取放目标物作为抓取目标；第六单元：用于通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿；第七单元：用于根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹。

本发明的有益效果为：提供一种基于协作机器人的视觉3D取放方法及系统，通过使用双目结构光高精度三维扫描仪，成像精度高，点云获取速度快，获取质量高，抗环境光干扰能力强，对黑色、反光物体具有高适应性，单幅扫描精度可达0.01mm，单幅扫描速度小于1s；结合2D图像及3D点云数据，通过高精度点云注册算法，可准确识别目标物，抓取定位精度高。

进一步地，引入协作机器人，其本体更加轻量化，易于安装与移动，增强了对环境的可操作性。

进一步地，使用基于摄影测量的双目相机系统标定方法，准确获得双目结构光高精度三维扫描仪和协作机器人的标定参数，解决了传统标定计算过程不收敛、鲁棒性差的问题；同时极大的简化了标定过程，标定方法重投影误差在0.05个像素内，满足系统测量精度。

再进一步地，拥有碰撞检测功能，对实际去放过程进行仿真验证，对运行轨迹进行优化调整及碰撞检测，保障了取放过程的可靠性及高效性。

附图说明

图1是本发明实施例中第一种基于协作机器人的视觉3D取放方法的示意图。

图2是本发明实施例中标定双目结构光三维扫描仪的相机的内外参数的流程示意图。

图3是本发明实施例中对所述双目结构光三维扫描仪的相机的内外参数进行标定的方法示意图。

图4是本发明实施例中配置有编码与非编码标志点的标准标定板及用于重建的比例尺Scale示意图。

图5是本发明实施例中对所述协作机器人的手眼标定的方法示意图。

图6是本发明实施例中对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云的方法示意图。

图7是本发明实施例中取放目标物识别与定位的流程示意图。

图8是本发明实施例中得到配准后的位姿估计结果的方法示意图。

图9是本发明实施例中得到配准后的位姿估计结果的流程示意图。

图10是本发明实施例中第二种基于协作机器人的视觉3D取放方法的示意图。

图11是本发明实施例中第一种基于协作机器人的视觉3D取放系统的示意图。

图12是本发明实施例中第二种基于协作机器人的视觉3D取放系统的示意图。

具体实施方式

为了使本发明实施例所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是用于电路连通作用。

需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

如图1所示，本发明提供一种基于协作机器人的视觉3D取放方法，包括如下步骤：

S1：标定双目结构光三维扫描仪的相机的内外参数；

S2：标定协作机器人的手眼，获得所述双目结构光三维扫描仪与所述协作机器人末端执行件夹爪的空间相对位置关系的标定结果矩阵；

S3：采集待取放目标物三维数字模型；

S4：采用标定后的所述双目结构光三维扫描仪获取散乱堆叠的所述待取放目标物的点云数据，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云；

S5：根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选，选择抓取成功率最高的所述待取放目标物作为抓取目标；

S6：通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿；

S7：根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹。

在本发明的一种实施例中，三维数字模型是CAD注册点云模型，CAD(ComputerAided Design)是计算机辅助设计，CAD注册点云模型是数据存储模型。

本发明通过使用双目结构光高精度三维扫描仪，成像精度高，点云获取速度快，获取质量高，抗环境光干扰能力强，对黑色、反光物体具有高适应性，单幅扫描精度可达0.01mm，单幅扫描速度小于1s；结合2D图像及3D点云数据，通过高精度点云注册算法，可准确识别目标物，抓取定位精度高。

具体地，双目结构光三维扫描仪的相机的内外参数标定是指确定其内、外方位参数的过程，其精度直接影响三维扫描仪点云重建精度。本发明使用配置有编码与非编码标志点的标准标定板，从不同角度和位置对标定板进行拍摄，得到一定数量的标定图片；接着识别图像中的标志点图像u-v坐标，基于摄影测量原理，对标志特征点进行三维重建，获得其准确的三维空间坐标；接着基于标志点二维图像u-v坐标以及三维空间坐标，对小孔成像相机模型中的所有相机参数进行整体的光束平差迭代优化调整，以最小化相机重投影误差为目标损失函数，并加入比例尺(图像与真实维度缩放参数)参数进行约束最终获得精确的三维扫描仪相机的内外参数。

该方法无需高精度的标定板，只需将标定板上任意一对标识点的准确距离作为比例尺就可以准确地计算出相机的内、外方位参数，从而实现相机的标定。

如图2所示，是本发明中标定双目结构光三维扫描仪的相机的内外参数的流程示意图。

如图3所示，对所述双目结构光三维扫描仪的相机的内外参数进行标定包括如下步骤：

S11：从不同角度和位置对标定板进行拍摄得到标定图片，其中，所述标定板配置有编码标志点和非编码标志点；

S12：对所述标定图片进行识别获得所述编码标志点和所述非编码标志点的二维图像坐标，对所述编码标志点和所述非编码标志点进行三维重建获得准确的三维空间坐标；

S13：基于所述编码标志点和所述非编码标志点的二维图像坐标、三维空间坐标对所述相机的内外参数进行整体的光束平差迭代优化调整，以最小化所述相机的重投影误差为目标损失函数，并加入比例尺参数进行约束获得所述相机的内外参数。

如图4所示，是本发明中配置有编码与非编码标志点的标准标定板及用于重建的比例尺Scale示意图。

协作机器人手眼标定是为了获得三维扫描仪(Eye)与协作机器人末端执行器的空间相对位置关系，本质上是两个坐标系之间转换关系的标定，其精度直接影响整个取放系统的抓取精度。可以理解的是，三维扫描仪获得场景中待抓取物抓取位姿，协作机器人手眼标定结果矩阵获得从三维扫描仪变换到机械臂底座基坐标系的变换矩阵，将三维扫描仪坐标系下的抓取位姿转化至机械臂底座基坐标系下；机械臂再利用这转换后的抓取位姿，控制协作机器人末端执行器进行抓取；简而言之，三维扫描仪获得实时抓取位姿，利用手眼标定结果矩阵转换抓取位姿至机械臂，机械臂利用转换后的位姿控制末端执行器夹爪进行抓取。

如图5所示，对所述协作机器人的手眼标定包括如下步骤：

S21：配置有编码与非编码标志点的标准标定板，所述双目结构光三维扫描仪与所述协作机器人的机械臂底座，所述机器人的工具坐标系与所述标准标定板的空间相对位置关系在整个标定过程固定不变；

S22：控制所述协作机器人移动所述标准标定板到空间不同位置，使用所述双目结构光三维扫描仪观察所述标准标定板采集标定图像；重复这一过程，采集10～15帧标定图像；

S23：对所述标定图像进行双目匹配、三维重建，计算重建标志点三维坐标点与已知的标准标定板世界坐标系全局点相对外参数；

S24：从所述协作机器人的示教器中读取预先存储的所述协作机器人的当前空间标定位置，与所述相对外参数作为手眼标定过程的数据输入，利用手眼标定算法进行求解，获得所述标定结果矩阵。

在本发明中步骤S1和步骤S2的标定的整个标定过程无需制作成本较高的标定参考物，只使用配置有标志点的标准标定板，从不同角度及位置采集一定数量的标定板标定图像，通过识别标志特征点，重建特征点三维坐标，光束平差整体优化，加入比例参数及相机内外参数等步骤，迭代最小化重投误差，从而获得准确的双目结构光相机及手眼标定相对位置参数，保证点云采集质量以及取放系统的抓取精度。

在完成步骤S1和步骤S2的标定以后，采集待取放目标物三维数字模型三维数字模型。待取放目标物三维数字模型数据采集步骤为：

1.对于有对应图纸，型面特征简的单目标物，基于设计图纸，通过三维CAD建模软件获得三维数字模型；

2.对于型面特征复杂，无对应图纸的目标物，首先取出单个目标抓取物，置于取放系统中的扫描转台工装上，装夹稳固；

3.综合考虑三维扫描仪测量幅面、机器人运动范围、运行姿态轨迹等因素，通过人工拖拽示教，生成机器人最优扫描视点运动路径；

4.进行机器人自动扫描，从多角度、多方位扫描目标物，获得抓取目标物高精度三维数字模型。

然后，通过双目结构光三维扫描仪采集场景中待抓取目标物的三维点云数据。点云数据是对所拍摄目标物的表面采样数据，是三维坐标的无序集合。通过控制双目结构光三维扫描仪对物料框内待抓取目标物进行扫描拍摄，获得高质量的三维点云数据及2D图像数据。本发明采用蓝光窄域波长投影技术，可以有效避免环境光干扰，受光照强度变化、成像距离以及视点变化的影响小；同时相机分辨率采用900万及更高像素，在1～2s内即可获得高细节特征的三维点云数据。

取放目标物场景点云模型数据采集步骤为：

1.调整3D扫描仪焦距及投影亮度，保证目标物在相机幅面内；

2.控制3D扫描仪开始扫描场景目标物；

3.存储输出场景目标物点云模型数据；

扫描得到的场景目标物点云只有表面层，残缺程度较高，表面信息不完整，而零件的CAD模型则十分完整，因此，本发明使用CAD模型点云数据代替扫描点云进行后续抓取位姿规划。

使用标定后的高精度双目结构光三维扫描仪，采集当前堆叠目标物场景点云数据后需要对点云数据进行分割。

如图6所示，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云包括如下步骤：

S41：去除所述待取放目标物的场景点云中的背景点云；

具体地，在获取堆叠待取放目标物表面扫描点云时，其放置平面及周围场景会不可避免地被扫描到，这些背景点云属于冗余点，不仅干扰后续目标物识别，还会降低数据处理效率，故须将其去除

(1)基于与结构光扫描仪相机光心距离，分割滤除大部分场景点云干扰；

(2)基于双目结构光扫描仪测量体积，仅保留测量体积内的点云(只有测量体积内的点云才可以保证重建准确)。

S42：去除待取放目标物的场景点云中的离散点；

具体地，离散点属于噪声点，指的是离主体点云较远的离散点，这些点会对点云配准带来干扰，故须滤除，本文将在指定半径中含有较少近邻点的目标点视为离群点滤除；

(1)目标点指定半径下邻域点搜索，检索近邻点数量；

(2)选择近邻点数量低于预先设定阈值点的目标点进行滤除。

S43：采用三维区域生长分割算法，根据所述点云数据的特征分割聚类成不同的物体假设，获得多个所述待取放目标物的场景点云。

由于是堆叠目标物场景，故须从多个堆叠目标物点云中，根据其特征分割聚类成不同的物体假设，获得多个单一目标物点云类簇；本文采用三维区域生长分割算法：

(1)选择种子点或种子单元；

(2)区域生长，选择区域单元为增长点；

(3)增长结束；

(4)基于欧式距离聚类分割；

对散乱堆叠目标物点云进行处理后，获得多个抓取目标，需要进行评估与筛选，选择抓取成功率最高的目标物进行抓取。根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选包括如下规则：

计算分割获得的散乱堆叠的多个所述待取放目标物的场景点云的点云簇质心，选择所述点云簇质心的高度最高的待取放目标物视为抓取成功率最高的待取放目标物；

计算选择的所述待取放目标物的场景点云与剩余所述待取放目标物的场景点云的重合度，平均重合度最小的为抓取成功率最高的待取放目标物；

比较转换至所述协作机器人的机械臂坐标系下的所述待取放目标物的场景点云的抓取位姿与所述待取放目标物的场景点云的抓取位姿的相似程度，相似程度最大的为抓取成功率最高的待取放目标物；

所述待取放目标物的场景点云的点云数量最多的为抓取成功率最高的待取放目标物。

在本发明的一种实施例中，采用基于投票的三维目标识别算法，直接匹配模型点云与场景目标点云间的固有特性，生成有限的候选姿态集后，利用先验条件构造支持函数与罚函数并对每一个姿态进行投票，进而得出最优的变换矩阵M_moduleHscene。

如图7所示，是本发明实施例中取放目标物识别与定位的流程示意图。

在本发明的一种实施例中，在所述三维数字模型上预先定义所述取放位姿点，具体包括：

根据所述三维数字模型，基于3-2-1坐标系定义方法，预先定义所述待取放目标物的抓取位置及抓取姿态确定所述待取放目标物的取放位姿点；或，使用主成分分析方法建立坐标系，确定抓取位姿并使用质心点作为抓取点确定所述待取放目标物的取放位姿点。

如图8所示，通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿包括如下步骤：

S61：分别将所述抓取目标的三维数字模型和所述抓取目标的场景点云数据进行前处理，所述前处理包括对点云的下采样和法向估计；

对待取放目标物的场景点云数据进行体素下采样，利用体素栅格重心来近似的表示体素中其他点，能够在保障点云特征的同时，减少点云数目，降低运算量；法向估计主要用于计算PPF点对特征计算，正确的法向量是提取PPF点对特征的前提，本发明基于当前查询点的k邻域元素，估计其准确的法向量。

S62：对前处理后的所述抓取目标的三维数字模型进行稠密点对特征提取，构建所述抓取目标的三维数字模型的点对特征哈希表；对前处理后的所述抓取目标的场景点云数据选取参考点进行稀疏点对特征提取，构建参考点点对特征哈希表；

PPF(Point Pair Feature)点对特征提取包括目标物三维数字模型PPF点对特征与待取放目标物的场景点云数据PPF点对特征。三维数字模型模型PPF点对特征是对完整的CAD点云模型的PPF点对特征计算，属于线下训练阶段，用于构建一个完整模型描述的哈希表，耗时较长。

场景点云PPF点对特征提取过程包括两部分：首先是场景稀疏点云中按比例提取采样点，减少线上匹配运算量；接着计算所提取采样点的PPF点对特征，构建场景点云的特征哈希表。

线上匹配的主要目的是识别场景中的目标点云位姿。在完成场景点云点对特征哈希表提取后，在线下训练的模型PPF点对特征中进行搜索，提取与场景点对特征近似的模型点对特征。

S63：识别所述抓取目标的场景点云数据对应的点云位姿，基于所述参考点点对特征哈希表从所述抓取目标的三维数字模型的点对特征哈希表获取相对应的特征以及相对应的位姿；

在本发明的一种实施例中，一个场景参考点能够算出一个可靠位姿，n个场景参考点有n个可能位姿，这些位姿是不尽相同的，甚至差异很大，衡量其差异的方式是霍夫投票数。对这些候选位姿进行聚类，同一个类中位姿的t向量差异和R矩阵的差异小于特定阈值。当前类的分数定义为：类中各个位姿在上节投票中获得的票数的总和。将投票数较高几类位姿作为候选位姿。

S64：采用霍夫投票数从所述相对应的位姿中筛选出候选位姿；

匹配质量评估的基本思想是判断目标点云与模型点云的重合度，本文通过空间聚类来判断，即对目标中的所有点，在进行转换后的模型点云中存在一点，与该点的距离小于预设阈值，将该点视为正确匹配点。

S65：根据如下公式从所述候选位姿中选出匹配质量Q最高的候选位姿：

其中，已匹配的目标点数是所述待取放目标物的场景点云数据与所述目标物三维数字模型中已匹配的目标点数；目标点总数是所述待取放目标物的三维数字模型中三维点总数；

S66：使用迭代最近点配准算法，以所述匹配质量Q最高的候选位姿为初始值，通过最小二乘法迭代计算最优的坐标变换矩阵，获得配准后的位姿估计矩阵为配准后的位姿估计结果；

S67：将所述三维数字模型上预先定义的所述取放位姿点配准到场景中，得到配准后的抓取位姿估计结果，作为所述待取放目标物的抓取位姿。

如图9所示，得到配准后的位姿估计结果的流程示意图。

得到配准后的位姿估计结果以后进行目标识别与定位，其的目的是在点云场景中，对待抓取模型进行三维目标识别以及对应的姿态估计，本发明主要基于模板匹配的算法。

在本发明的一种实施例中，将所述三维数字模型上预先定义的所述取放位姿点配准到场景中，得到配准后的抓取位姿估计结果，作为所述待取放目标物的抓取位姿包括：

利用所述标定结果矩阵X，左乘所述配准后的位姿估计矩阵M_moduleHscene获得所述协作机器人可执行的所述抓取目标的抓取位姿M_baseHtcp。

根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹包括：

采用所述协作机器人各关节轴旋转角度表示路径点，以所述协作机器人各轴旋转角度的绝对值之和作为机器人的运动代价，根据实际抓取过程，将路径分段，对同一路径段内路径点进行全排列，计算所有排列方式的机器人运动代价总和，以运动代价最小的路径作为该路径段的局部最优路径轨迹，直至所有路径段均完成局部最优路径计算，按照取放动作流程顺序连接所述局部最优运动路径得到所述协作机器人初步的抓取路径轨迹；其中，运动代价函数计算公式为：

其中，q_i代表关节旋转角度表示的路径点，q₀代表机械臂的机械零点关节轴旋转角度。

每次取放过程中，目标物抓取点位姿都会发生变化，同时利用机械手将抓取目标从物料箱中取出放置到上料台的过程中也会发生机械臂与物料箱及周边场景的碰撞，有的抓取点也可能未处于机械臂工作空间范围中。考虑到抓取过程中各路径点(包括运动起始与最终点及中间过渡点)高度自由，彼此连续性低，易造成不同视点间机器人各轴转动角度过大，针对这一特性，本发明以不同路径点协作机器人关节轴转动角度为评价标准，不再以空间中的坐标值表示路径点，转而使用机器人各轴旋转角度表示路径点。

在本发明的一种实施例中，抓取路径轨迹规划算法步骤如下：

(1)根据实际抓取过程，将路径分段，分为取放等待(3D扫描仪采集场景点云数据)、开始取放(机械手开始抓取)、上料(机械手成功抓取工件，并向上料台运送)、取放复位(复位至初始位置，等待下次抓取)；

(2)对同一路径段内路径点进行全排列，计算所有排列方式的协作机器人运动代价总和，以机器人运动代价最小的路径作为该路径段的局部最优路径轨迹；

(3)重复步骤(2)，直至所有路径段均完成局部最优路径计算；

(4)按照取放动作流程，顺序连接上述局部最优运动路径，最为初步的取放路径轨迹。

上述取放路径轨迹是保证机械臂以最佳姿态及最高效运行轨迹的运动方式，但实际运行时会遇到夹爪与机械臂自身以及夹爪与周围固定场景(如工作台)的碰撞，导致取放过程无法进行，因此需要预先进行碰撞检测及避障轨迹规划，本文通过虚拟仿真完成这一过程，具体步骤为：

(1)在仿真可视化窗口中导入当前取放场景元素，如机械手、物料箱、工作台，上料传送带等；

(2)导入当前时刻采集的场景目标物点云及其抓取位姿；

(3)导入初步的取放运动路径轨迹；

(4)基于编写的机器人运动学可视化模块，仿真运行(3)中取放运动路径轨迹，基于包围盒碰撞检测算法，检测机械手夹爪与周围固定场景及机械臂自身的碰撞，记录碰撞点；

(5)对于碰撞点，基于运动学逆解模块，基于关节空间的规划方法，添加中间避障过渡点，生成避障轨迹路径，补充到S33取放运动轨迹中；

(6)出相应的机器人示教器运动控制程序，用于实际抓取。

如图10所示，本发明的方法还包括：

S81：仿真运行所述协作机器人初步的抓取路径轨迹，基于包围盒碰撞检测算法检测所述协作机器人末端执行件夹爪与周围固定场景及机械臂自身的碰撞，记录碰撞点；

S82：根据所述碰撞点，基于运动学逆解和关节空间的规划方法添加中间避障过渡点生成避障轨迹路径，补充到所述协作机器人初步的抓取路径轨迹，得到最终的抓取路径轨迹。

如图11所示，基于上述方法，本发明还提供一种基于协作机器人的视觉3D取放系统，包括：

数据采集模块：用于获取待取放目标物和场景的2D图像及3D点云数据，采用双目结构光三维扫描仪进行数据采集；该模块包括扫描仪标定单元与数据采集单元；

视觉感知模块：所用于堆叠的待取放目标物的识别与定位，协作机器人通过该模块感知环境并实现对待取放目标物的识别与定位，识别结果直接决定了后续协作机器人的抓取精度；该模块包括模型与场景表征单元，目标识别与定位单元；

机器人取放操作模块：用于完成协作机器人对待取放目标物的取放操作，该模块包括系统标定单元、取放策略规划单元、运动轨迹规划单元以及虚拟仿真单元；

取放执行模块：用于控制协作机器人完成实际的物料取放任务。

在本发明的一种实施例中，通过数据采集模块提供的任务窗口界面，首先对三维扫描仪进行相机标定，保证双目重建点云的高精度；接着控制双目结构光高精度三维扫描仪，采集取放工件及场景的2D图像及3D点云数据，用于进一步的视觉感知模块；视觉感知模块基于数据采集模块获得的3D点云数据，首先通过模型与场景表征单元，对取放目标物点云进行点云特征描述以及点云特征提取；点云特征描述指的是将模型与场景对应的无序点集通过特定的算法编码为低维的特征向量，用词来表征对象的局部或者全局信息。接着基于目标识别与定位单元，通过获得的点云特征向量，在场景点云中对待抓取模型进行三维目标识别以及对应的姿态估计，方便进行进一步的机器人取放。基于机器人取放操作模块中的系统标定单元，利用手眼标定，确定机器人与3D扫描仪空间相对位置关系，用于精确抓取；取放策略规划单元主要用于保障较高的抓取成功率，规划目标物抓取策略，保证能够稳固的抓取目标物，同时还应确保兼容性及对新物体的适应性，最后还拥有抓取质量评估参数对物体接触点的位置及末端夹爪的配置进行评价；运动轨迹规划单元主要用于规划从机器人运动起始位置到目标物抓取点的路径轨迹，进行避障规划及保障机器人以最高运行效率完成取放动作；最后的虚拟仿真单元是对轨迹规划单元生成的取放轨迹进行虚拟仿真运行，验证规划轨迹的可行性及可靠性。基于取放执行模块的控制窗口界面，将取放运动轨迹转换为机器人可执行程序，并控制机器人、夹爪按照仿真后的取放运动轨迹，完成实际的物料取放任务。

如图12所示，是本发明中另一种基于协作机器人的视觉3D取放系统，包括：

第一单元：用于标定双目结构光三维扫描仪的相机的内外参数；

第二单元：用于标定协作机器人的手眼，获得所述双目结构光三维扫描仪与所述协作机器人末端执行件夹爪的空间相对位置关系的标定结果矩阵；

第三单元：用于采集待取放目标物三维数字模型；

第四单元：用于采用标定后的所述双目结构光三维扫描仪获取散乱堆叠的所述待取放目标物的点云数据，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云；

第五单元：用于根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选，选择抓取成功率最高的所述待取放目标物作为抓取目标；

第六单元：用于通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿；

第七单元：用于根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹。

本申请实施例还提供一种控制装置，包括处理器和用于存储计算机程序的存储介质；其中，处理器用于执行所述计算机程序时至少执行如上所述的方法。

本申请实施例还提供一种存储介质，用于存储计算机程序，该计算机程序被执行时至少执行如上所述的方法。

本申请实施例还提供一种处理器，所述处理器执行计算机程序，至少执行如上所述的方法。

所述存储介质可以由任何类型的易失性或非易失性存储设备、或者它们的组合来实现。其中，非易失性存储器可以是只读存储器(ROM，Read Only Memory)、可编程只读存储器(PROM，Programmable Read-Only Memory)、可擦除可编程只读存储器(EPROM，ErasableProgrammable Read-Only Memory)、电可擦除可编程只读存储器(EEPROM，Electrically Erasable Programmable Read-Only Memory)、磁性随机存取存储器(FRAM，Ferromagnetic Random Access Memory)、快闪存储器(Flash Memory)、磁表面存储器、光盘、或只读光盘(CD-ROM，Compact Disc Read-Only Memory)；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器(RAM，Random Access Memory)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(SRAM，Static Random Access Memory)、同步静态随机存取存储器(SSRAM，Synchronous Static Random Access Memory)、动态随机存取存储器(DRAM，DynamicRandom AccessMemory)、同步动态随机存取存储器(SDRAM，Synchronous Dynamic RandomAccess Memory)、双倍数据速率同步动态随机存取存储器(DDRSDRAM，Double Data RateSynchronous Dynamic Random Access Memory)、增强型同步动态随机存取存储器(ESDRAM，Enhanced Synchronous Dynamic Random Access Memory)、同步连接动态随机存取存储器(SLDRAM，SyncLink Dynamic Random Access Memory)、直接内存总线随机存取存储器(DRRAM，Direct Rambus Random Access Memory)。本发明实施例描述的存储介质旨在包括但不限于这些和任意其它适合类型的存储器。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。

上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。

另外，在本发明各实施例中的各功能单元可以全部集成在一个处理单元中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

或者，本发明上述集成的单元如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

本申请所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。

本申请所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。

本申请所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。

以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (10)

1.一种基于协作机器人的视觉3D取放方法，其特征在于，包括如下步骤：

S1：标定双目结构光三维扫描仪的相机的内外参数；

S2：标定协作机器人的手眼，获得所述双目结构光三维扫描仪与所述协作机器人末端执行件夹爪的空间相对位置关系的标定结果矩阵；

S3：采集待取放目标物三维数字模型；

S4：采用标定后的所述双目结构光三维扫描仪获取散乱堆叠的所述待取放目标物的点云数据，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云；

S5：根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选，选择抓取成功率最高的所述待取放目标物作为抓取目标；

S6：通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿；

S7：根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹。

2.如权利要求1所述的基于协作机器人的视觉3D取放方法，其特征在于，标定双目结构光三维扫描仪的相机的内外参数包括如下步骤：

S11：从不同角度和位置对标定板进行拍摄得到标定图片，其中，所述标定板配置有编码标志点和非编码标志点；

S12：对所述标定图片进行识别获得所述编码标志点和所述非编码标志点的二维图像坐标，对所述编码标志点和所述非编码标志点进行三维重建获得准确的三维空间坐标；

S13：基于所述编码标志点和所述非编码标志点的二维图像坐标、三维空间坐标对所述相机的内外参数进行整体的光束平差迭代优化调整，以最小化所述相机的重投影误差为目标损失函数，并加入比例尺参数进行约束获得所述相机的内外参数。

3.如权利要求2所述的基于协作机器人的视觉3D取放方法，其特征在于，标定协作机器人的手眼，获得所述双目结构光三维扫描仪与所述协作机器人末端执行件夹爪的空间相对位置关系的标定结果矩阵包括如下步骤：

S21：配置有编码与非编码标志点的标准标定板，所述双目结构光三维扫描仪与所述协作机器人的机械臂底座，所述机器人的工具坐标系与所述标准标定板的空间相对位置关系在整个标定过程固定不变；

S22：控制所述协作机器人移动所述标准标定板到空间不同位置，使用所述双目结构光三维扫描仪观察所述标准标定板采集标定图像；重复这一过程，采集10～15帧标定图像；

S23：对所述标定图像进行双目匹配、三维重建，计算重建标志点三维坐标点与已知的标准标定板世界坐标系全局点相对外参数；

S24：从所述协作机器人的示教器中读取预先存储的所述协作机器人的当前空间标定位置，与所述相对外参数作为手眼标定过程的数据输入，利用手眼标定算法进行求解，获得所述标定结果矩阵。

4.如权利要求3所述的基于协作机器人的视觉3D取放方法，其特征在于，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云包括如下步骤：

S41：去除所述待取放目标物的场景点云中的背景点云；

S42：去除待取放目标物的场景点云中的离散点；

S43：采用三维区域生长分割算法，根据所述点云数据的特征分割聚类成不同的目标物假设，获得多个所述待取放目标物的场景点云。

5.如权利要求4所述的基于协作机器人的视觉3D取放方法，其特征在于，根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选包括如下规则：

计算分割获得的散乱堆叠的多个所述待取放目标物的场景点云的点云簇质心，选择所述点云簇质心的高度最高的待取放目标物视为抓取成功率最高的待取放目标物；

计算选择的所述待取放目标物的场景点云与剩余所述待取放目标物的场景点云的重合度，平均重合度最小的为抓取成功率最高的待取放目标物；

比较转换至所述协作机器人的机械臂坐标系下的所述待取放目标物的场景点云的抓取位姿与所述待取放目标物的场景点云的抓取位姿的相似程度，相似程度最大的为抓取成功率最高的待取放目标物；

所述待取放目标物的场景点云的点云数量最多的为抓取成功率最高的待取放目标物。

6.如权利要求5所述的基于协作机器人的视觉3D取放方法，其特征在于，在所述三维数字模型上预先定义所述取放位姿点，具体包括：

根据所述三维数字模型，基于3-2-1坐标系定义方法，预先定义所述待取放目标物的抓取位置及抓取姿态确定所述待取放目标物的取放位姿点；或，使用主成分分析方法建立坐标系，确定抓取位姿并使用质心点作为抓取点确定所述待取放目标物的取放位姿点；

通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿包括如下步骤：

S61：分别将所述抓取目标的三维数字模型和所述抓取目标的场景点云数据进行前处理，所述前处理包括对点云的下采样和法向估计；

S62：对前处理后的所述抓取目标的三维数字模型进行稠密点对特征提取，构建所述抓取目标的三维数字模型的点对特征哈希表；对前处理后的所述抓取目标的场景点云数据选取参考点进行稀疏点对特征提取，构建参考点点对特征哈希表；

S63：识别所述抓取目标的场景点云数据对应的点云位姿，基于所述参考点点对特征哈希表从所述抓取目标的三维数字模型的点对特征哈希表获取相对应的特征以及相对应的位姿；

S64：采用霍夫投票数从所述相对应的位姿中筛选出候选位姿；

S65：根据如下公式从所述候选位姿中选出匹配质量Q最高的候选位姿：

其中，已匹配的目标点数是所述抓取目标的场景点云数据与所述抓取目标的三维数字模型中已匹配的目标点数；目标点总数是所述抓取目标的三维数字模型中三维点总数；

S66：使用迭代最近点配准算法，以所述匹配质量Q最高的候选位姿为初始值，通过最小二乘法迭代计算最优的坐标变换矩阵，获得配准后的位姿估计矩阵为配准后的位姿估计结果；

S67：将所述三维数字模型上预先定义的所述取放位姿点配准到场景中，得到配准后的抓取位姿估计结果，作为所述抓取目标的抓取位姿。

7.如权利要求6所述的基于协作机器人的视觉3D取放方法，其特征在于，将所述三维数字模型上预先定义的所述取放位姿点配准到场景中，得到配准后的抓取位姿估计结果，作为所述待取放目标物的抓取位姿包括：

利用所述标定结果矩阵，左乘所述配准后的位姿估计矩阵获得所述协作机器人可执行的所述抓取目标的抓取位姿。

8.如权利要求7所述的基于协作机器人的视觉3D取放方法，其特征在于，根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹包括：

采用所述协作机器人各关节轴旋转角度表示路径点，以所述协作机器人各轴旋转角度的绝对值之和作为机器人的运动代价，根据实际抓取过程，将路径分段，对同一路径段内路径点进行全排列，计算所有排列方式的机器人运动代价总和，以运动代价最小的路径作为该路径段的局部最优路径轨迹，直至所有路径段均完成局部最优路径计算，按照取放动作流程顺序连接所述局部最优运动路径得到所述协作机器人初步的抓取路径轨迹；其中，运动代价函数计算公式为：

其中，q_i代表关节旋转角度表示的路径点，q₀代表机械臂的机械零点关节轴旋转角度。

9.如权利要求8所述的基于协作机器人的视觉3D取放方法，其特征在于，还包括：

S81：仿真运行所述协作机器人初步的抓取路径轨迹，基于包围盒碰撞检测算法检测所述协作机器人末端执行件夹爪与周围固定场景及机械臂自身的碰撞，记录碰撞点；

S82：根据所述碰撞点，基于运动学逆解和关节空间的规划方法添加中间避障过渡点生成避障轨迹路径，补充到所述协作机器人初步的抓取路径轨迹，得到最终的抓取路径轨迹。

10.一种基于协作机器人的视觉3D取放系统，其特征在于，包括：

第一单元：用于标定双目结构光三维扫描仪的相机的内外参数；

第二单元：用于标定协作机器人的手眼，获得所述双目结构光三维扫描仪与所述协作机器人末端执行件夹爪的空间相对位置关系的标定结果矩阵；

第三单元：用于采集待取放目标物三维数字模型；

第四单元：用于采用标定后的所述双目结构光三维扫描仪获取散乱堆叠的所述待取放目标物的点云数据，对所述点云数据进行点云分割得到多个所述待取放目标物的场景点云；

第五单元：用于根据多个所述待取放目标物的场景点云对多个所述待取放目标物进行评估与筛选，选择抓取成功率最高的所述待取放目标物作为抓取目标；

第六单元：用于通过所述抓取目标的三维数字模型的点对特征与所述抓取目标的场景点对特征进行配准，将所述三维数字模型上预先定义的取放位姿点配准到场景中，得到配准后的位姿估计结果，作为所述抓取目标的抓取位姿；

第七单元：用于根据所述抓取目标的抓取位姿规划出所述协作机器人初步的抓取路径轨迹。

Images