CN112720487B - 基于自适应动态力平衡的机械臂抓取方法和系统 - Google Patents

Abstract

本申请属于工业机器人技术领域，具体涉及一种基于自适应动态力平衡的机械臂抓取方法和系统。其中的方法包括：获取目标物体的目标点云信息，通过凹包轮廓检测算法检测得到边缘轮廓信息，基于边缘轮廓信息和机械臂末端执行器两指间距离，建立目标物体的可抓取区域模型；按照预设的规则生成矩形滑框，利用最小二乘直线拟合对每一矩形滑框内左右两边的凸顶点集合进行直线拟合，基于拟合得到的两条直线间的夹角确定目标物体的可抓取位置；确定目标物体左侧边缘、右侧边缘在第一坐标轴上坐标的平均值，当平均值之差小于预设阈值时，机械臂根据可抓取位置对目标物体进行抓取。本申请方法实施简单快捷，具有较强的实时性，可准确可靠地抓取目标物体。

Description

基于自适应动态力平衡的机械臂抓取方法和系统

技术领域

本申请属于工业机器人技术领域，具体涉及一种基于自适应动态力平衡的机械臂抓取方法和系统。

背景技术

近年来，基于多自由度机器臂的智能机器人被广泛应用于工业、服务等领域，以完成复杂且重复的物体抓取操作。不同于机器人在结构化环境下对工件的抓取，非结构环境往往具有高度动态性以及不确定性，其中的目标物体也往往是未知的，由于未知物体在模型数据库中没有相应的几何模型和外观信息，无法利用先验知识，因此非结构化环境下的自主抓取在精度和稳定性等方面还存在很大的提升空间。

提供目标物体的详细信息用于抓取操作将是一项非常耗时的任务。现有方法中采用相似模型的抓取方法可以简化机械臂控制，但很容易导致机器人选错相似的物体，从而使抓取变得不可靠或者不准确；而采用机器学习的方法对目标物体进行识别，需要通过足够多的抓取实验得到的大量数据，作为机械臂抓取模型的训练集，对网络模型进行训练得到抓取模型，整个过程不仅耗时较长，而且复杂不易实施。

发明内容

(一)要解决的技术问题

鉴于现有技术的上述缺点、不足，本申请提供一种基于自适应动态力平衡的机械臂抓取方法和系统。

(二)技术方案

为达到上述目的，本申请采用如下技术方案：

第一方面，本申请实施例提供一种基于自适应动态力平衡的机械臂抓取方法，该方法包括：

S10、获取图像传感器在目标位置上采集的目标物体的目标点云信息，所述图像传感器设置于机械臂上部设定位置；

S20、通过凹包轮廓检测算法对所述目标点云信息进行检测，得到所述目标物体在第一平面上投影的边缘轮廓信息；所述第一平面基于对所述目标点云信息采用主成分分析法提取得到的第一主成分信息确定；

S30、基于所述边缘轮廓信息和机械臂末端执行器两指间距离，建立所述目标物体的可抓取区域模型；

S40、基于可抓取区域模型，按照预设的规则生成矩形滑框，利用最小二乘直线拟合对每一矩形滑框内左右两边的凸顶点集合进行直线拟合，基于拟合得到的两条直线间的夹角确定所述目标物体的可抓取位置；

S50、基于所述可抓取位置，确定所述目标物体左侧边缘、右侧边缘在第一坐标轴上坐标的平均值，所述第一坐标轴垂直于所述第一平面；

S60、当平均值之差小于预设阈值时，机械臂根据所述可抓取位置对所述目标物体进行抓取。

可选地，在S10之前还包括：

S01、图像传感器在初始位置采集目标物体的初始点云信息；

S02、通过主成分分析法提取所述初始点云信息的第二主成分信息，基于所述第二主成分信息对所述初始位置进行变换，得到所述图像传感器的目标位置。

可选地，步骤S02包括：

S021、将所述初始点云信息进行去中心化，得到第一点云数据；

S022、基于所述第一点云数据的协方差矩阵，计算所述协方差矩阵的特征值和特征向量；

S023、将计算得到的特征值最大的三个特征向量作为第二主成分信息；

S024、将所述第二主成分信息中的三个特征向量按特征值大小进行排序，得到位置变换矩阵；

S025、通过所述位置变换矩阵对所述初始位置进行变换，得到所述目标位置。

可选地，所述第一主成分信息包括基于所述目标点云信息得到的特征值最大的三个特征向量，所述三个特征向量包括第一特征向量、第二特征向量、第三特征向量；其中，所述第一特征向量的特征值最大，所述第三特征向量的特征值最小；

可选地，所述第一平面基于对所述目标点云信息采用主成分分析法提取得到的第一主成分信息确定，包括：

以所述目标物体的几何中心作为目标物体坐标系的原点，以所述第一特征向量的方向作为所述目标物体坐标系的Y轴方向，以所述第二特征向量的方向作为所述目标物体坐标系的X轴方向，以所述第三特征向量的方向作为所述目标物体坐标系的Z轴方向，建立目标物体坐标系；

将所述目标物体坐标系的XOY平面作为第一平面。

可选地，通过凹包轮廓检测算法对所述目标点云信息进行检测，包括：

S21、基于所述目标点云信息，得到目标点云投影于XOY平面的轮廓点集合；

S22、提取所述轮廓点集合中Y值最小的点作为凹包边界轮廓的第一边缘轮廓点，将所述第一边缘轮廓点作为待检测点；

S23、选取距离待检测点最近的K个点作为候选点，测量从水平线到各个候选点与所述待检测顶点连线的角度，将最大向右旋转角度的候选点作为第二边缘轮廓点；将所述第二边缘轮廓点作为待检测点；

S24、选取距离待检测点最近的K个点作为候选点，将所述待检测点与上一个待检测点的连线作为当前线段，将所述待检测点与候选点的连线作为候选线段，测量当前线段与候选线段之间的角度，将最大向右旋转角度的候选线段对应的候选点作为第三边缘轮廓点；

S25、将所述第三边缘轮廓点作为待检测点；

S26、循环执行步骤S24-S25，直至作为待检测点的第三边缘轮廓点与第一边缘轮廓点重合，将所有待检测点的点云信息作为检测结果输出。

可选地，所述可抓取区域模型的建立方法，包括：

S31、提取Y轴方向上最远的点作为初始点，以预设长度为间隔，得到平行于X轴的直线组；

S32、针对直线组中的每条直线，确定区域模型点，包括：

基于边缘轮廓信息确定直线与所述目标物体的边缘轮廓的交叉点；

将交叉点中最左边与最右边的交叉点之间的距离作为待判断距离；

将所述待判断距离小于机械臂末端执行器左右两指间距离的两个交叉点作为区域模型点；

S33、基于所有的区域模型点确定所述可抓取区域模型。

可选地，按照预设的规则生成矩形滑框，包括：

S41、通过向量积法确定所述可抓取区域模型中的凹顶点和凸顶点；

S42、将凹顶点按y值大小进行升序排列，从y值最小的顶点开始，顺次取点，直到生成的滑框高度大于等于2cm；

S43、对滑框外剩余的凹顶点，循环执行步骤S42，直到最后一个顶点。

可选地，基于拟合得到的两条直线间的夹角确定可抓取位置的方法包括：

比较所有矩形滑框内的两条直线间的夹角；

将最小夹角对应的矩形滑框确定的区域作为可抓取位置。

第二方面，本申请实施例提供一种基于自适应动态力平衡的机械臂抓取系统，该系统包括：图像传感器、处理器、机械臂；

所述图像传感器设置于所述机械臂上部设定位置，用于采集目标物体的点云信息；

所述处理器，用于基于所述图像传感器采集的目标物体的点云信息，通过上述的基于自适应动态力平衡的机械臂抓取方法生成所述机械臂的抓取指令；

所述机械臂，用于基于所述处理器输出的抓取指令抓取所述目标物体。

(三)有益效果

本申请的有益效果是：本申请提出了一种基于自适应动态力平衡的机械臂抓取方法和系统，其中的方法包括：获取图像传感器在目标位置上采集的目标物体的目标点云信息，通过凹包轮廓检测算法检测得到边缘轮廓信息，基于边缘轮廓信息和机械臂末端执行器两指间距离，建立目标物体的可抓取区域模型；基于可抓取区域模型，按照预设的规则生成矩形滑框，利用最小二乘直线拟合对每一矩形滑框内左右两边的凸顶点集合进行直线拟合，基于拟合得到的两条直线间的夹角确定目标物体的可抓取位置；基于所述可抓取位置，确定目标物体左侧边缘、右侧边缘在第一坐标轴上坐标的平均值，当平均值之差小于预设阈值时，机械臂根据可抓取位置对目标物体进行抓取。本申请中的基于自适应动态力平衡的机械臂抓取方法，只需利用目标物体的点云即可检测出最优抓取位置，无需离线训练或构建物体的3D点云模型库，该方法实施时简单快捷，具有较强的实时性，可实现对目标物体的准确可靠的抓取。

附图说明

本申请借助于以下附图进行描述：

图1为本申请一个实施例中的基于自适应动态力平衡的机械臂抓取方法流程示意图；

图2为本申请另一个实施例中的基于自适应动态力平衡的机械臂抓取方法流程示意图；

图3为本申请另一个实施例中的正常比例与比例失衡的目标点云示例图；

图4为本申请另一个实施例中的3D相机初始位置、目标位置及矫正后目标点云示例图；

图5为本申请另一个实施例中的凹包轮廓检测算法示例图；

图6为本申请另一个实施例中的计算可抓取区域流程示例图；

图7为本申请另一个实施例中的凹凸性判断示意图；

图8为本申请另一个实施例中的计算XOY平面力平衡过程示例图；

图9为本申请另一个实施例中的XOY平面力平衡的计算结果示例图；

图10为本申请另一个实施例中的抓取配置示例图；

图11为本申请另一个实施例中的计算XOZ平面力平衡示例图；

图12为本申请再一个实施例中的机械臂末端执行器结构示例图；

图13为本申请再一个实施例中的基于自适应动态力平衡的未知物体最优抓取结果示例图；

图14为本申请再一个实施例中的最优抓取的平均运行时间测试结果统计图；

图15为本申请再一个实施例中的最优抓取的成功率统计图。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。可以理解的是，以下所描述的具体的实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合；为了便于描述，附图中仅示出了与发明相关的部分。

针对非结构环境下目标物体难以确定最优抓取区域并实现快速抓取，本申请提出一种基于自适应动态力平衡的机械臂抓取方法，对形状和轮廓未知的物体进行抓取。下面将参考附图并结合实施例来详细说明本申请。需要说明的是，实施例附图中，所有坐标系的横轴为X轴，纵轴为Y轴，垂直纸面的轴为Z轴。

实施例一

图1示出了本申请一个实施例中的基于自适应动态力平衡的机械臂抓取方法流程示意图。如图所示，本实施例的基于自适应动态力平衡的机械臂抓取方法包括：

S10、获取图像传感器在目标位置上采集的目标物体的目标点云信息，图像传感器设置于机械臂上部设定位置；

S20、通过凹包轮廓检测算法对目标点云信息进行检测，得到目标物体在第一平面上投影的边缘轮廓信息；第一平面基于对目标点云信息采用主成分分析法提取得到的第一主成分信息确定；

S30、基于边缘轮廓信息和机械臂末端执行器两指间距离，建立目标物体的可抓取区域模型；

S40、基于可抓取区域模型，按照预设的规则生成矩形滑框，利用最小二乘直线拟合对每一矩形滑框内左右两边的凸顶点集合进行直线拟合，基于拟合得到的两条直线间的夹角确定目标物体的可抓取位置；

S50、基于可抓取位置，确定目标物体左侧边缘、右侧边缘在第一坐标轴上坐标的平均值，第一坐标轴垂直于第一平面；

S60、当平均值之差小于预设阈值时，机械臂根据可抓取位置对目标物体进行抓取。

在一些实施例中，在S10之前还包括：

S01、图像传感器在初始位置采集目标物体的初始点云信息；

S02、通过主成分分析法提取初始点云信息的第二主成分信息，基于第二主成分信息对初始位置进行变换，得到图像传感器的目标位置。

在一些实施例中，步骤S02包括：

S021、将初始点云信息进行去中心化，得到第一点云数据；

S022、基于第一点云数据的协方差矩阵，计算协方差矩阵的特征值和特征向量；

S023、将计算得到的特征值最大的三个特征向量作为第二主成分信息；

S024、将第二主成分信息中的三个特征向量按特征值大小进行排序，得到位置变换矩阵；

S025、通过位置变换矩阵对初始位置进行变换，得到目标位置。

在一些实施例中，第一主成分信息包括基于目标点云信息得到的特征值最大的三个特征向量，三个特征向量包括第一特征向量、第二特征向量、第三特征向量；其中，第一特征向量的特征值最大，第三特征向量的特征值最小；

在一些实施例中，第一平面基于对目标点云信息采用主成分分析法提取得到的第一主成分信息确定，包括：

以目标物体的几何中心作为目标物体坐标系的原点，以第一特征向量的方向作为目标物体坐标系的Y轴方向，以第二特征向量的方向作为目标物体坐标系的X轴方向，以第三特征向量的方向作为目标物体坐标系的Z轴方向，建立目标物体坐标系；

将目标物体坐标系的XOY平面作为第一平面。

在一些实施例中，通过凹包轮廓检测算法对目标点云信息进行检测，包括：

S21、基于目标点云信息，得到目标点云投影于XOY平面的轮廓点集合；

S22、提取轮廓点集合中Y值最小的点作为凹包边界轮廓的第一边缘轮廓点，将第一边缘轮廓点作为待检测点；

S23、选取距离待检测点最近的K个点作为候选点，测量从水平线到各个候选点与待检测顶点连线的角度，将最大向右旋转角度的候选点作为第二边缘轮廓点；将第二边缘轮廓点作为待检测点；

S24、选取距离待检测点最近的K个点作为候选点，将待检测点与上一个待检测点的连线作为当前线段，将待检测点与候选点的连线作为候选线段，测量当前线段与候选线段之间的角度，将最大向右旋转角度的候选线段对应的候选点作为第三边缘轮廓点；

S25、将第三边缘轮廓点作为待检测点；

S26、循环执行步骤S24-S25，直至作为待检测点的第三边缘轮廓点与第一边缘轮廓点重合，将所有待检测点的点云信息作为检测结果输出。

在一些实施例中，可抓取区域模型的建立方法，包括：

S31、提取Y轴方向上最远的点作为初始点，以预设长度为间隔，得到平行于X轴的直线组；

S32、针对直线组中的每条直线，确定区域模型点，包括：

基于边缘轮廓信息确定直线与目标物体的边缘轮廓的交叉点；

将交叉点中最左边与最右边的交叉点之间的距离作为待判断距离；

将待判断距离小于机械臂末端执行器左右两指间距离的两个交叉点作为区域模型点；

S33、基于所有的区域模型点确定可抓取区域模型。

在一些实施例中，按照预设的规则生成矩形滑框，包括：

S41、通过向量积法确定可抓取区域模型中的凹顶点和凸顶点；

S42、将凹顶点按y值大小进行升序排列，从y值最小的顶点开始，顺次取点，直到生成的滑框高度大于等于2cm；

S43、对滑框外剩余的凹顶点，循环执行步骤S42，直到最后一个顶点。

在一些实施例中，基于拟合得到的两条直线间的夹角确定可抓取位置的方法包括：

比较所有矩形滑框内的两条直线间的夹角；

将最小夹角对应的矩形滑框确定的区域作为可抓取位置。

本发明方法以突破未知物体难以获取最优抓取区域并实现快速抓取为出发点，将机械臂末端执行器对物体的受力分析分解为两个二维平面轮廓的受力分析，从三维信息降至二维，提高了实时性。当两平面的受力均平衡时，即可得到可靠的抓取，提高了准确性。利用3D传感器的旋转调节，获取目标物体无比例失调的点云信息，可适用于随意摆放的物体，提高了抓取算法的鲁棒性。

实施例二

图2为本申请另一个实施例中的基于自适应动态力平衡的机械臂抓取方法流程示意图，如图2所示，该方法包括：

步骤1、调整视觉传感器主轴方向。

本实施例利用Kinect传感器获取场景点云，通过背景滤波和平面识别与分割提取目标物体，作为整个系统的初始点云。

本实施例使用的是eye-in-hand系统，图3为本申请另一个实施例中的正常比例与比例失衡的目标点云示例图，图3中(a)为比例失衡的目标点云，(b)为正常比例的点云。视觉传感器将会随机械臂而移动，极易出现初始点云的比例失衡的情况，如图3中(a)所示。如果采集到的初始点云满足原始比例条件，如图3中(b)所示，可直接用以建立物体坐标系。为了避免比例失衡情况的出现，需执行两次获取初始点云及主成分分析(PrincipalComponent Analysis，PCA)的步骤，分别用于初始旋转调节3D相机光轴方向和建立物体坐标系。

假设在传感器坐标系下的初始物体点云为^cam0C，使用PCA计算物体点云的长轴方向的具体流程为：

(1)通过公式(1)计算目标点云^cam0C的几何中心

并将^cam0C去中心化。

其中，P_i为目标点云的i点坐标，P_i'为去中心化后的i点坐标，^cam0C'为去中心化后的目标点云集，N为点云的数量。

将所有去中心化后点的坐标排列成3×N矩阵A，如公式(2)所示。

(2)令M＝A·A^T，根据公式(3)计算M的特征值与特征向量：

λ_i·V_i＝M·V_i,i＝1,2,3 (3)

其中，M为协方差矩阵，λ_i为M的特征值，V_i为特征向量。

将特征向量正则化||V_i||＝1，则对应最大特征值的特征向量即为物体的长轴方向。

令λ₁＜λ₂＜λ₃，可知物体坐标系相对世界坐标系的旋转

由此，可通过式(4)调整3D相机的坐标系方向与物体的坐标系方向一致。

其中，P_t为视觉传感器的目标位置，P_o为视觉传感器的初始位置。

图4为本申请另一个实施例中的3D相机初始位置、目标位置及矫正后目标点云示例图，如图4所示，图4中(a)为视觉传感器的初始位置，(c)为视觉传感器的目标位置，(b)为调整视觉传感器坐标系后重新获取的目标点云，与图3中的(a)相较，已减小了由比例失调带来的影响。

步骤2、建立物体坐标系。

为了方便后续对视野内点云轮廓进行受力平衡分析，需建立以物体几何中心为原点的物体坐标系，并将点云转换到该坐标系下。再次使用PCA算法建立物体坐标系，使坐标系的Y轴对齐到物体的长轴方向，以进行XOY平面的力平衡分析。具体流程在步骤1已详细阐述，在此不再赘述，但这一次是由调整3D传感器坐标系后重新获取的目标初始点云经PCA计算而得物体坐标系相对于世界坐标系的旋转

物体坐标系原点在世界坐标系下的坐标即为^cam0C的几何中心：

由

和

可知，物体坐标系在3D传感器坐标系下的位姿的表示如公式(5)所示。

通过公式(6)将^cam0C各点转换到物体坐标系下。

请继续参阅图(4)，由公式(6)可得物体坐标系下的目标点云，如图4(d)所示。

步骤3、边缘轮廓检测。

本实施例中首先将物体坐标系下的目标点云投影于XOY平面，并利用空洞填补算法获取更为精确的边界轮廓。轮廓检测算法包括凸包轮廓检测算法和凹包轮廓检测算法，前者求取的是覆盖所有点的凸多边形，不能表征目标物体的真实边界轮廓，因此本实施例采用凹包轮廓检测算法检测目标物体的轮廓。请参阅图5，图5为本申请另一个实施例中的凹包轮廓检测算法示例图，图5中(a)、(b)为部分目标点云示例图，图5中(c)为检测得到的边缘轮廓示例图。以下结合图5具体说明凹包轮廓检测算法步骤。

步骤3.1：提取投影面轮廓点集合中Y值最小的点作为凹包边界轮廓的第一个顶点(图5中(a)中的A点)。

步骤3.2：选取距离当前顶点A最近的K个点作为候选点(图5(a)中的B，C和D点，K＝3)，测量从水平线到各个点的角度，获得最大向右旋转角度的点即为凹包轮廓的下一顶点(图5(a)中的C点)。

步骤3.3：再选取距离当前顶点C最近的K个点作为候选点(图5(b)中的B，D和E点，K＝3)，测量上一条线段AC与候选线段CD、CB及CE之间的角度，获得最大向右旋转角度的点即为凹包轮廓的下一顶点(图5(b)中的E点)；循环step3，直至多边形与第一个顶点A闭合，退出循环。

这里需要说明三种特殊情况：(1)如果K值大于剩余的点数，算法会自动考虑所有剩余的点，无需手动干预；(2)当候选点与顶点之间的连线与确定的边缘线相交时，将会导致多边形交叉封闭，所以需舍弃此候选点；(3)为了解决密度不均匀导致某些点集不能被设为候选点，可适当调大K值。

由于目标点云集中点数以万计，不可将K值取得过小(即小于3)，否则得不到完整的二维轮廓信息。针对本实施例中的目标物体，当K＝20时，经凹包轮廓检测算法提取到的目标点云轮廓效果最佳，如图5(c)所示。

步骤4、生成可抓取区域模型。

为了减少用于动态力平衡计算的候选分析区域以提高实时性，建立关于目标物体尺寸与机械臂末端执行器两指间距离的可抓取区域生成模型。提取Y轴方向上最远的点y_min作为初始点，以Δy(本实施例中Δy＝1)为间隔，按照公式(7)计算平行于X轴的直线y，并获取y与凹包边缘轮廓的交叉点。图6为本申请另一个实施例中的计算可抓取区域过程示例图，图6中(a)为目标物体局部计算得到的平行于X轴的直线y示例图，(b)为获取y与凹包边缘轮廓的交叉点示例图，其中，y₁、y₂、y₃、y₄为平行于X轴的直线，直线y₃与凹包边缘轮廓有四个交叉点，(c)为可抓取区域模型局部示例图。针对四个交叉点，保留最左边与最右边的交叉点，舍弃中间点，然后计算两个边缘交叉点之间的距离，如果两个边缘交叉点之间的距离小于机械臂末端执行器左右两指间距离，则将两个交叉点添加到可抓取区域模型中，如图6(c)所示。

y＝y_min+nΔy (7)

步骤5、检测边缘轮廓凹凸点。

判断多边形顶点凹凸性的常见算法有凸壳法、角度法、左右点法、矢量面积法、向量法、射线法、斜率法以及极点顺序法等，除了凸壳法的时间复杂度为非线性之外，其他算法的时间复杂度均为线性。虽然角度法、左右点法、矢量面积法、射线法和斜率法的几何形状表征方式不同，但其本质是等同的，都可归结于向量积法，而且向量积法表征比较简洁且容易实现。因此，本实施例使用向量积法计算物体的凹凸性，并与动态力平衡分析相融合。

首先在物体轮廓中找到一个凸顶点，也就是顶点数据集中y坐标值最大的点，若此顶点不唯一，则在这些y坐标值最大的点中再次寻找x坐标值最大的点，根据该凸顶点的向量积标量的正负可确定顶点序列的顺逆性，然后再计算出所有顶点的向量积，与之方向相同的顶点必为凸顶点，与之方向相反的顶点必为凹顶点。

为了说明凹凸性判断的步骤，这里仅仅取边缘轮廓中的四个顶点组成一个多边形进行说明。图7为本申请另一个实施例中的凹凸性判断示意图，如图7所示，图7中(a)为边缘轮廓中部分顶点示意图，从中选取四个顶点，V₁顶点为x坐标值最大的顶点，其余三个顶点为V₀、V₂和V₃。图7中(b)为四个顶点组成向量示意图，两个向量之间的箭头方向表示顶点序列为逆时针排列，图7中(c)为目标物体凹点示意图，图中的黑色点即为得到部分凹点。计算V₁前后两个相邻边矢量的向量积

此值为负表征顶点序列为逆时针排列，再计算其他三个顶点V₀、V₂和V₃的向量积，发现顶点V₀、V₃的向量积为负，与V₁的向量积方向相同，则顶点V₀和V₃为凸点，而顶点V₂的向量积为正，与V₁的向量积方向相反，则顶点V₂为凹点，物体中的所有凹点如图7中的(c)中黑色圆点所示。

大部分物体中一般凹点数少于凸点数。为了提高未知物体抓取的实时性，同时又可将物体分割为多凹段和多凸段区域，本实施例仅保留了凹点用以制作自适应滑框。

步骤6、检测边缘轮廓凹凸点。

本实施例通过生成可抓取区域模型，初步估算出机械臂末端执行器可完成抓取操作的位置，但为了能够获得更为稳定快速的抓取，在此模型中寻找相对动态平衡的位置，即为此物体的最优抓取位置。

本实施例中的机械臂末端执行器为三指型手爪，可近似简化为底边为width_hand的向下倾斜三角形，但为了方便计算物体的动态力平衡，假设机械臂末端执行器为具有width_hand宽度的矩形框，其高度在2cm-15.5cm之间(2cm为单一手指的高度，15.5cm为上下两指间距离与两指高度之和)，所以此高度可由依据目标物体自身的凹凸性确定的矩形滑框来确定。从y_min开始移动矩形框直到y_max-50(mm)(y_max为点集合中y的最大值，50mm是为了保证机械臂末端执行器不会触碰到桌面以防损坏)的位置，其中每一个矩形框滑过的区域都是可抓取区域，因为此区域中的点云都在执行器的可抓取范围内，但并不是所有的可抓取区域都是最优的，还需通过力平衡分析进行遴选。另外，针对家庭真实场景而言，物体摆放各异，如果单纯只利用XOY平面动态力平衡分析获取最优抓取位置，并不能保证此次抓取稳定，仍然会有失败的可能，所以需要再结合另一平面(XOZ平面)的动态力平衡分析，以确保抓取鲁棒性。

步骤6.1XOY平面动态力平衡分析

本实施例利用最小二乘直线拟合对每一可抓取区域内左右两边轮廓的点集合进行直线拟合，并求取两直线间的夹角θ，由此选取使执行器在XOY平面上获得最佳动态力平衡的区域。

将步骤5获得的凹凸点坐标按y值大小进行升序排列，从y_min开始制作矩形滑框，y_min到凹凸点集中的第一个点为初始滑框，此后每两个凹凸点间区域为下一滑框，直到最后一个凹凸点到y_max-50(mm)为末端滑框，如图7(a)所示。但如果矩形滑框的高度小于2cm，将会自动并入到下一滑框当中。然后，对每一自适应滑框内左右两边的凸顶点集合进行直线拟合，拟合表达式如公式(8)所示。

y＝kx+b (8)

其中，拟合直线的系数k和b由公式(9)计算而得。

其中，n为滑框中的一边的点的数量。

图8为本申请另一个实施例中的计算XOY平面力平衡过程示例图，图8中(a)为目标问题局部生成的矩形滑框示例图,(b)为最优抓取位置示例图，(c)为直线夹角示例图。利用左右两边轮廓的拟合直线求取夹角θ，如图8中(c)所示。夹角θ越小，表示在此区域的抓取越稳定，越适于机械臂末端执行器的抓取。当拟合直线平行时，此时θ＝0，表示此抓取区域为最优抓取区域，图8中(b)显示了此物体中的最优抓取位置，即两条实线的位置。

图9为本申请另一个实施例中的XOY平面力平衡的计算结果示例图，如图9所示，将从每一自适应滑框中获取的夹角θ绘制成曲线，可以很直观的看出在第5个滑框处取得最小夹角θ＝0.305619，所以在此处可获得可靠抓取。

步骤6.2XOZ平面动态力平衡分析

图10为本申请另一个实施例中的抓取配置示例图，图10中(a)为机械臂和目标物体的初始配置示例图，显示了机械臂和目标物体的初始配置，图10中(b)为由XOY平面动态力平衡分析获得的抓取配置示例图，显示了由XOY平面动态力平衡分析获得的抓取配置，如图10所示，经XOY平面的动态力平衡分析，获得物体的最优抓取位置，但此区域无法保证XOZ平面的受力依旧平衡，所以抓取操作同样会失败。

图11为本申请另一个实施例中的计算XOZ平面力平衡示例图；图11中(a)为抓取时对物体的受力分析图，F₁与F₂为机械臂末端执行器对物体施加的接触力。如果机械臂按照此抓取配置进行抓取目标物体，物体将会绕Y轴旋转，导致抓取失败。因此，还需进行XOZ平面动态力平衡分析以保证更为可靠的抓取操作。

在XOZ平面上，本实施例使用最优抓取配置中物体两侧边缘的Z轴坐标平均值Z₁、Z₂替代F₁、F₂进行受力分析，如图11中(b)所示。依据点云的尺度，设定阈值Z_dif，判断|Z₁-Z₂|是否小于Z_dif，若|Z₁-Z₂|＜Z_dif，表征此抓取配置为最优抓取，否则，表征在整个物体中未有比较可靠的抓取配置，需移动机械臂重新寻找最优抓取配置。

机械臂需要移动的角度需要依据目标物体的主平面而定，并将主平面投影于XOZ平面，投影线的距离为d。当d大于夹持器的开口宽度时，机械臂将从3D传感器的初始位置的切线方向移动至目标位置。为了确保3D传感器能够得到有效的数据，需考虑3D传感器与目标物体之间的距离，如Kinect的有效测量距离为0.5m，这意味着机械臂在距离物体小于0.5m的地方将找不到物体。因此，这里引入R表示三维传感器到目标物体的距离。机械臂的移动实际上是一个从初始位置绕Y轴旋转β角度的过程。α为主平面与Z轴之间的夹角，显然有β＝α。当d小于夹持器的开口宽度时，机械臂将从3D传感器的初始位置的垂直方向移动至目标位置，在这种情况下，β＝π/2-α。

假定P_i为3D传感器的初始投影位置，P_t为目标位置，可根据公式(10)将3D传感器从P_i旋转至P_t处。

其中，x、y和z为旋转轴，c表示cosβ，s表示sinβ。

在移动机械臂至新的位置后，需重新获取物体的部分点云，按照前述的方法和力平衡分析获取最佳抓取区域。

本实施例提出了一种基于自适应动态力平衡的机械臂抓取方法(AdaptiveDynamic Force Balance，ADFB)，与现有方法相比，本实施例中，首先，利用主元分析法分析并调整视觉传感器的主轴方向，使其与物体坐标系保持一致；采用多面投影技术和凹包轮廓检测算法提取未知物体点云的二维轮廓信息，降低计算复杂度；其次，建立关于目标物体尺寸与机械臂末端执行器两指间距离的可抓取区域生成模型，通过对边缘轮廓凹凸点的检测自适应地生成若干个未知物体的候选可抓取区域；最后，通过最小化力平衡角度系数确定最优区域，并根据轮廓与点云的对应关系计算最优抓取位置及方向，完成自主抓取操作。本实施例的创新之处在于：(1)快速调整视觉传感器的主轴方向，使其与物体坐标系的方向保持一致，有效解决了比例失衡导致无法获取抓取位置的问题，适用性较强；(2)根据物体的自身形状，自适应地调节力平衡分析的滑动窗口，有效提高了评估准确性和抓取成功率。

为了更好地验证基于自适应动态力平衡的机械臂抓取方法的有效性，将本实施例中的方法与现有力平衡评估抓取算法进行了动态力平衡分析对比实验，其中对比评测的4种算法按提出时间先后的顺序排列，分别是FGFB(利用力平衡优化快速抓取未知物体Fastgrasping of unknown objects using force balance optimization)、UOGC(利用凹性抓取未知物体Unknown object grasping by using concavity)、FGCS(在单点云上使用圆柱搜索快速抓取未知物体Fast grasping of unknown objects using cylinder searchingon a single point cloud)、FGPC(利用主成分分析快速抓取未知物体Fast grasping ofunknown objects using principal component analysis)。

本实施例方法和现有力平衡评估抓取算法的关键之处均在于经XOY平面以及XOZ平面力平衡分析以评估最优抓取区域的精确度。由于标准的benchmark测试集存放的是家庭中常用物品在某一视角下的3D点云模型，但本实施例需要在初始状态进行一步视觉传感器的调整，获取其无失真的点云进行分析，所以本实施例利用自制数据集(如家庭中常见物体喷壶、维尼熊、花瓶、洗发水瓶以及高脚杯等物品)对所提ADFB算法与其它4种力平衡分析算法进行了五组力平衡分析对比实验，其实验结果如表1、表2所示。

表1

表2

表1为XOY平面力平衡仿真实验中得到的最优抓取区域拟合角度实验结果，如表1所示，不同物品在XOY平面上最优抓取区域内拟合角度越小，代表最优抓取区域越精确，抓取操作越可靠。表2为XOZ平面力平衡仿真实验中得到的最优抓取区域边缘之间的距离实验结果，如表2所示，不同物品在XOZ平面上最优抓取区域边缘之间的距离对比实验结果，可由此推测目标物体发生旋转的可能性，间距越大，发生旋转的可能性越高。实验结果表明，对于凹凸关系较小的花瓶、洗发水瓶等物品，五种算法的分析结果相差不大，但对于喷壶、维尼熊等具有明显凹凸关系的物品，本实施例算法ADFB、以及UOGC与FGPC算法的力平衡计算结果更为准确，使得抓取更为牢靠。因此，引入物体自身的凹凸关系可获得更为精确的最优抓取区域，使得抓取成功率更高。

对于物体比例相差较大的高脚杯，由于外形原因，导致FGFB、UOGC、FGCS算法的XOZ平面力平衡分析都不是很理想；而XOY平面的力平衡分析却未受影响，表现比较理想。这种情况将导致抓取操作不稳定，甚至失败。因为可通过XOY平面力平衡分析获取得到结果，表明目标物体存在最优抓取区域，但由于XOZ平面力平衡分析不理想，将会导致物体发生旋转，致使抓取失败。而本实施例算法ADFB与FGPC算法在XOY平面以及XOZ平面上的力平衡分析均未受物体外形的影响，表现良好。综上所述，本实施例算法ADFB优于其他四种算法。

实施例三

本实施例利用ROS环境结合Gazebo仿真平台以及MoveIt！运动规划插件对五种物品进行了模拟抓取，其仿真实验平台设置为：将Asus Xtion PRO Live传感器固定于机械臂“JACO2 6DOF-S”末端，采用eye-in-hand配置方式，可快速且准确地采集到不同视野下的点云信息；机器人末端执行器采用三指型手爪，图12为本申请再一个实施例中的机械臂末端执行器结构示例图，如图12所示，其左右两指最大开口宽度W＝21.5cm，上下两指间距离h_hand＝9.5cm，为了避免出现由3D传感器、视觉算法等带来的误差使实际宽度大于开口宽度导致机械臂末端执行器损坏的情况，将指间开口宽度设为w_hand＝0.8*W。

为了完成抓取，构建五个ROS节点，分别为Gazebo模拟器、点云采集、点云处理、MoveIt！机器人控制及执行器控制。在ROS平台下，利用Gazebo模拟器设置仿真环境，将机械臂JACO固定于桌面，并在其0.6m的地方放置不同的待抓取物体，再由虚拟环境中机械臂末端的RGB-D传感器采集场景点云，作为点云处理节点的输入。随后经本发明抓取算法进一步处理得最优抓取位置，并结合此未知物体的深度图，将其映射到三维空间，并求取最优抓取区域的中心点(x₀,y₀,z₀)，得末端执行器需移动到的位置。然而，只求得抓取点位置信息并不足以使机械臂完成抓取操作，还需向机械臂提供目标物体抓取点的3维位姿信息，才可成功抓取目标物体。本文定义目标物体在笛卡尔空间下的位姿为(x,y,z,α,β,γ,w)^T，其中(x,y,z)为抓取点坐标，即最优抓取区域的中心点坐标(x₀,y₀,z₀)，(α,β,γ)分别为绕X、Y、Z轴的旋转角度，w为机械手的开口宽度，即指间开口宽度。由于本文在初始状态通过调整视觉传感器的主轴方向将传感器坐标系的XOY平面转换到视野平面，所以α和β均为0度，γ为抓取区域法向量与视觉传感器坐标系Z轴的夹角，一般为0度或180度，即可获得目标物体在深度传感器坐标系下的位姿信息，而后通过深度相机与机械臂的坐标关系，将其转换为实际抓取点在机械臂基座坐标系下的位姿信息。然后再利用MoveIt！机器人控制节点为机械臂规划从初始点到目标点的轨迹，并将其送入Gazebo模拟器以驱动机械臂到达目标位置，控制执行器闭合抓取物体，图13为本申请再一个实施例中的基于自适应动态力平衡的未知物体最优抓取结果示例图，机械臂最优抓取结果如图13所示。

为了证明本实施例算法的准确性，图13分别使用维尼熊、花瓶、高脚杯、洗发水瓶、喷壶作为待抓取物品，以不同姿态估计物体的合理抓取位置。图13中(a₁)-(a₄)、(a_5-1)分别显示了目标物体以及机械臂的初始状态。图13中(b₁)-(b₅)分别显示了目标物体初始点云(标准的或失调的)，当出现比例失衡的初始点云时，如图13中(b₅)所示，可利用PCA算法旋转调整3D传感器的坐标系方向，重新获取标准的目标点云。(a_5-2)显示了经旋转调节3D传感器后目标物体以及机械臂的状态。图13中(c₁)-(c₅)分别显示了通过垂直于Y轴的直线与(b₁)-(b₅)中凹形轮廓之间的交叉点计算出的可抓取范围内的点集。图13中(d₁)-(d₅)显示了经动态力平衡分析后得到目标物体的抓取位置，其中维尼熊是脖子部位的浅色部分，花瓶、高脚杯、洗发水瓶、喷壶是浅色部分。

为了衡量各未知物体抓取算法在实际应用过程中的实时性，在硬件配置为InterXeon E5-2650 2.0GHz八核CPU 8G RAM，编程环境为Visual Studio 2013的计算机上，对本实施例所提算法以及其它4种基于力平衡优化的未知物体抓取算法的处理速度进行了评测，即对喷壶、维尼熊、花瓶、洗发水瓶以及高脚杯等五组具有代表性物品进行每组40次最优抓取仿真实验所需的平均处理时间，请参阅图14，图14为本申请再一个实施例中的最优抓取的平均运行时间测试结果统计图。

其中四条虚线曲线分别标注了FGFB、UOGC、FGCS、FGPC算法对每一物体的平均处理时间，实线为本实施例算法、由此可以看出，在点云密度相同的情况下，本实施例算法较现有4种算法所耗费时间较少，执行效率更优。虽然UOGC算法与本文算法相比运行时间相差无几，甚至优于本文算法，但这仅限于具有凹凸特性的物体，如维尼熊、喷壶等，而且UOGC算法评价最优抓取区域的精确度低于本实施例算法。FGFB、FGCS、FGPC等算法的处理速度明显慢于本实施例所提算法，但FGPC算法的最优抓取区域评估精确度却与其相差无几。FGFB算法的评估精度明显低于ADFB，FGCS算法对凹凸不平的物品其评估精确度才会明显低于ADFB，而对于其它物品，其精确度接近于ADFB。同时需要说明的是，其抓取算法的处理速度与点云的密集程度关系密切，点云越密集，数量越多，处理速度越慢，其用于测评的五组模型的点数均为18000。

图15为本申请再一个实施例中的最优抓取的成功率统计图，将这五组物品在Gazebo仿真平台以任意姿态摆放，并对每组物品进行40次最优抓取仿真实验，测得其最优抓取成功率如图14所示。可以看出，本实施例算法的最优抓取成功率明显高于FGFB、UOGC、FGCS等算法，与FGPC算法相差无几，但综合最优抓取区域评估精确度以及平均处理速度等因素，ADFB算法的性能要优于其他基于力平衡优化的抓取算法。

实施例四

本申请第二方面提出了一种基于自适应动态力平衡的机械臂抓取系统，该系统包括：图像传感器、处理器、机械臂；

图像传感器设置于机械臂上部设定位置，用于采集目标物体的点云信息；

处理器，用于基于图像传感器采集的目标物体的点云信息，通过基于自适应动态力平衡的机械臂抓取方法生成所述机械臂的抓取指令；

机械臂，用于基于处理器输出的抓取指令抓取所述目标物体。

上述系统可执行本申请实施例所提供的基于自适应动态力平衡的机械臂抓取方法，并可产生同样的有益效果，具体工作过程可参照前述方法部分的描述，此处不再赘述。

应当注意的是，在权利要求中，不应将位于括号之间的任何附图标记理解成对权利要求的限制。词语“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的词语“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。

此外，需要说明的是，在本说明书的描述中，术语“一个实施例”、“一些实施例”、“实施例”、“示例”、“具体示例”或“一些示例”等的描述，是指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管已描述了本发明的优选实施例，但本领域的技术人员在得知了基本创造性概念后，则可对这些实施例作出另外的变更和修改。所以，权利要求应该解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种修改和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也应该包含这些修改和变型在内。

Claims (7)

1.一种基于自适应动态力平衡的机械臂抓取方法，其特征在于，该方法包括：

S10、获取图像传感器在目标位置上采集的目标物体的目标点云信息，所述图像传感器设置于机械臂上部设定位置；

S20、通过凹包轮廓检测算法对所述目标点云信息进行检测，得到所述目标物体在第一平面上投影的边缘轮廓信息；

所述第一平面基于对所述目标点云信息采用主成分分析法提取得到的第一主成分信息确定，所述第一主成分信息包括基于所述目标点云信息得到的特征值最大的三个特征向量，所述三个特征向量包括第一特征向量、第二特征向量、第三特征向量，其中，所述第一特征向量的特征值最大，所述第三特征向量的特征值最小；包括：

以所述目标物体的几何中心作为目标物体坐标系的原点，以所述第一特征向量的方向作为所述目标物体坐标系的Y轴方向，以所述第二特征向量的方向作为所述目标物体坐标系的X轴方向，以所述第三特征向量的方向作为所述目标物体坐标系的Z轴方向，建立目标物体坐标系；将所述目标物体坐标系的XOY平面作为第一平面；

S30、基于所述边缘轮廓信息和机械臂末端执行器两指间距离，建立所述目标物体的可抓取区域模型；

S40、基于可抓取区域模型，按照预设的规则生成矩形滑框，利用最小二乘直线拟合对每一矩形滑框内左右两边的凸顶点集合进行直线拟合，基于拟合得到的两条直线间的夹角确定所述目标物体的可抓取位置；其中，按照预设的规则生成矩形滑框，包括：

S41、通过向量积法确定所述可抓取区域模型中的凹顶点和凸顶点；

S42、将凹顶点按y值大小进行升序排列，从y值最小的顶点开始，顺次取点，直到生成的滑框高度大于等于2cm；

S43、对滑框外剩余的凹顶点，循环执行步骤S42，直到最后一个顶点；

S50、基于所述可抓取位置，确定所述目标物体左侧边缘、右侧边缘在第一坐标轴上坐标的平均值，所述第一坐标轴垂直于所述第一平面；

S60、当平均值之差小于预设阈值时，机械臂根据所述可抓取位置对所述目标物体进行抓取。

2.根据权利要求1所述的基于自适应动态力平衡的机械臂抓取方法，其特征在于，在S10之前还包括：

S01、图像传感器在初始位置采集目标物体的初始点云信息；

S02、通过主成分分析法提取所述初始点云信息的第二主成分信息，基于所述第二主成分信息对所述初始位置进行变换，得到所述图像传感器的目标位置。

3.根据权利要求2所述的基于自适应动态力平衡的机械臂抓取方法，其特征在于，步骤S02包括：

S021、将所述初始点云信息进行去中心化，得到第一点云数据；

S022、基于所述第一点云数据的协方差矩阵，计算所述协方差矩阵的特征值和特征向量；

S023、将计算得到的特征值最大的三个特征向量作为第二主成分信息；

S024、将所述第二主成分信息中的三个特征向量按特征值大小进行排序，得到位置变换矩阵；

S025、通过所述位置变换矩阵对所述初始位置进行变换，得到所述目标位置。

4.根据权利要求1所述的基于自适应动态力平衡的机械臂抓取方法，其特征在于，通过凹包轮廓检测算法对所述目标点云信息进行检测，包括：

S21、基于所述目标点云信息，得到目标点云投影于XOY平面的轮廓点集合；

S22、提取所述轮廓点集合中Y值最小的点作为凹包边界轮廓的第一边缘轮廓点，将所述第一边缘轮廓点作为待检测点；

S23、选取距离待检测点最近的K个点作为候选点，测量从水平线到各个候选点与所述待检测顶点连线的角度，将最大向右旋转角度的候选点作为第二边缘轮廓点；将所述第二边缘轮廓点作为待检测点；

S24、选取距离待检测点最近的K个点作为候选点，将所述待检测点与上一个待检测点的连线作为当前线段，将所述待检测点与候选点的连线作为候选线段，测量当前线段与候选线段之间的角度，将最大向右旋转角度的候选线段对应的候选点作为第三边缘轮廓点；

S25、将所述第三边缘轮廓点作为待检测点；

S26、循环执行步骤S24-S25，直至作为待检测点的第三边缘轮廓点与第一边缘轮廓点重合，将所有待检测点的点云信息作为检测结果输出。

5.根据权利要求1所述的基于自适应动态力平衡的机械臂抓取方法，其特征在于，所述可抓取区域模型的建立方法，包括：

S31、提取Y轴方向上最远的点作为初始点，以预设长度为间隔，得到平行于X轴的直线组；

S32、针对直线组中的每条直线，确定区域模型点，包括：

基于边缘轮廓信息确定直线与所述目标物体的边缘轮廓的交叉点；

将交叉点中最左边与最右边的交叉点之间的距离作为待判断距离；

将所述待判断距离小于机械臂末端执行器左右两指间距离的两个交叉点作为区域模型点；

S33、基于所有的区域模型点确定所述可抓取区域模型。

6.根据权利要求1所述的基于自适应动态力平衡的机械臂抓取方法，其特征在于，基于拟合得到的两条直线间的夹角确定可抓取位置的方法包括：

比较所有矩形滑框内的两条直线间的夹角；

将最小夹角对应的矩形滑框确定的区域作为可抓取位置。

7.一种基于自适应动态力平衡的机械臂抓取系统，其特征在于，该系统包括：图像传感器、处理器、机械臂；

所述图像传感器设置于所述机械臂上部设定位置，用于采集目标物体的点云信息；

所述处理器，用于基于所述图像传感器采集的目标物体的点云信息，通过权利要求1-6任一项所述的基于自适应动态力平衡的机械臂抓取方法生成所述机械臂的抓取指令；

所述机械臂，用于基于所述处理器输出的抓取指令抓取所述目标物体。

Images