一种基于多维度特征融合的零件识别装配方法和装置与流程

【技术领域】

本发明涉及智能机械设备技术领域，特别是涉及一种基于多维度特征融合的零件识别装配方法和装置。

背景技术：

传统的工业机械臂只能按照事先规划好的轨迹移动，不能够获取外界信息，限制了应用范围。为了增强机械臂的灵活性，现在已经有研究将机器视觉应用在机械臂控制中，但是现阶段很多机械臂控制中应用的机器视觉是单目的，并且是固定在传送带上某一个确定位置，采用这种形式的机器视觉系统对目标的定位不够精确，灵活性差。

鉴于此，克服该现有技术所存在的缺陷是本技术领域亟待解决的问题。

技术实现要素：

本发明要解决的技术问题是现阶段很多机械臂控制中应用的机器视觉是单目的，并且是固定在传送带上某一个确定位置，采用这种形式的机器视觉系统对目标的定位不够精确，灵活性差。

本发明采用如下技术方案：

第一方面，本发明提供了一种基于多维度特征融合的零件识别装配方法，预先对相机、结构光系统分别进行标定，确定相机的参数，并对机械臂和相机进行结构光系统标定或者手眼标定，以及，将不同形状的工件作为待检测目标，通过计算机视觉中的图像处理算法将工件特征提取，所述方法包括：

根据待识别目标的特征，进行目标识别；

在感兴趣区域对图像进行处理，提取出轮廓，然后利用三角测量法确定出圆环工件相对于相机的三维坐标；

根据手眼标定的结果，将该三维坐标转换到机械臂坐标系下，控制机械臂进行抓取。

优选的，所述相机的参数包括：焦距、光轴、径向畸变和切向畸变中的一项或者多项。

优选的，所述待识别目标的特征包括：

三维特征、拓扑特征、二维表面特征点中的一种或者多种，并将包含所述待识别目标的特征区域范围，作为图像的感兴趣区域。

优选的，所述三维特征中，具体包括：

半径、高程差、高程标准差和点密度中的一项或者多项三维几何特征；

线性特征、平面特征、散乱特征、总方差、各向异性、特征熵、特征值和曲率中的一项或者多项三维局部形状特征。

优选的，所述二维表面特征点，具体包括：

将二维滤波器对应的矩阵水平翻转，竖直翻转；

把这个滤波器矩阵，覆盖到图像上方；将滤波器矩阵的每一个元素，同下方图像的每一个元素相乘，再把所有的乘积加起来，并不断移动这个滤波器矩阵；

每移动一格，对应得到一个值；

当滤波器矩阵扫过原来的图像的所有位置的时候，我们就得到了一个新的图像；这个图像就是原来的图像经过该二维滤波器的输出，从而获得二维表面特征点。

优选的，预先对相机进行标定，具体包括：

设定模板平面在世界坐标系z＝0的平面上；

h＝[h1h2h3]＝λk[r1r2t]，

其中，k为摄像机的内参数矩阵，r1和r2是旋转向量，h1、h2和h3为转化参数，由上式中的λk[r1r2t]求解得到，λ代表相机离标定板距离，为模板平面上点的齐次坐标，x和y是世界坐标，是像素平面上点的齐次坐标，u和v分别代表x轴和y轴的像素坐标，为模板平面上点投影到图象平面上对应点的齐次坐标，[r1r2r3]和t分别是摄像机坐标系相对于世界坐标系的旋转矩阵和平移向量；

根据旋转矩阵的性质：r1^tr2＝0和||r1||＝||r2||＝1，每幅图象可以获得以下两个对内参数矩阵的基本约束；

其中，摄像机有5个未知内参数，当所摄取得的图象数目大于等于3时，就可以线性唯一求解出k。

优选的，所述结构光系统标定包括：

在平台上放置标定用对象，测量标定用对象到激光器和摄像头连线的一系列实际距离值qi，以及每个距离值对应的由摄像头采集的标定图像；

获取所述标定图像中预设标定点位的间距pxi，带入由相似三角形得到公式中，计算得到各相关参数的标定数据；

其中，f为摄像头中镜头到图像传感器距离，s为摄像头与激光器的间距，pixelsize为图像像素的物理尺寸，offset为图像原点相对于三角测距中图像边缘的偏置距离。

优选的，所述结构光系统标定包括利用九点标定直接建立相机和机械手之间的坐标变换关系，具体包括：

控制机械手的末端抵达预设的9个点，得到在机器人坐标系中的坐标，同时还要用相机识别9个点得到像素坐标，从而得到了9组对应的坐标，求出标定的矩阵。

优选的，所述机械臂具体为6自由度机械臂，其舵机0、舵机1、舵机2、舵机3和舵机4依次相连，为水平旋转舵机，舵机5为控制机械手抓取的舵机，其中，舵机0为可在水平面上旋转的的舵机，舵机1、舵机2、舵机3和舵机4为竖直旋转舵机，具体的：

机械臂三连杆长度分别为l1、l2、l3，舵机0的水平旋转角度为θ0，舵机1的竖直面旋转角度为θ1，舵机2的竖直面旋转角度为θ2，舵机3的竖直面旋转角度为θ3，舵机4的竖直面旋转角度为θ4，舵机5的竖直面旋转角度为θ5；

以机械臂底座旋转中心为三维坐标系原点，根据目标点三维坐标p(x，y，z)与机械臂原点o(0，0，0)距离|po|判定，若|po|≤l1+l2+l3，则机械臂可达；若|po|＞l1+l2+l3，则机械臂不可达；

若可达，则根据目标点三维坐标p(x，y，z)计算出舵机0的旋转角度为arctan(m/n)，其中m和n为舵机1所在点坐标。

第二方面，本发明还提供了一种基于多维度特征融合的零件识别装配装置，用于实现第一方面所述的基于多维度特征融合的零件识别装配方法，所述装置包括：

至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被程序设置为执行第一方面所述的基于多维度特征融合的零件识别装配方法。

第三方面，本发明还提供了一种非易失性计算机存储介质，所述计算机存储介质存储有计算机可执行指令，该计算机可执行指令被一个或多个处理器执行，用于完成第一方面所述的基于多维度特征融合的零件识别装配方法。

本发明对使用双目视觉很好的识别、定位圆环工件并通过上位机将圆环工件的位置信息发送给机械臂控制机械臂进行抓取。针对计算机视觉和机械臂相结合的相关技术进行结合，将三维结构光技术和机械臂相结合，实现了机械臂对目标的识别、定位和抓取。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图作简单地介绍。显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种基于多维度特征融合的零件识别装配方法流程示意图；

图2是本发明实施例提供的一种基于多维度特征融合的零件识别装配方法中提取二维表面特征点的流程示意图；

图3是本发明实施例提供的一种基于多维度特征融合的零件识别装配方法中结构光系统定标方法流程图；

图4是本发明实施例提供的一种基于多维度特征融合的零件识别装配方法中结构光系统定标架构示意图；

图5是本发明实施例提供的一种基于多维度特征融合的零件识别装配方法中结构光成像中像素距离与实际距离关系图；

图6是本发明实施例提供的一种结构光成像中灰度质心获取方法流程图；

图7是本发明实施例提供的一种结构光成像中步进电机控制方法流程图；

图8是本发明实施例提供的一种结构光成像中原理示意图；

图9是本发明实施例提供的另一种结构光成像中原理示意图；

图10是本发明实施例提供的一种滤波器处理对应的矩阵的示意图；

图11是本发明实施例提供的另一种滤波器处理对应的矩阵的示意图；

图12是本发明实施例提供的还一种滤波器处理对应的矩阵的示意图；

图13是本发明实施例提供的一种机械臂装置的结构示意图；

图14是本发明实施例提供的一种机械臂装置的角度关系示意图；

图15是本发明实施例提供的一种基于多维度特征融合的零件识别装配装置结构示意图。

【具体实施方式】

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

在本发明的描述中，术语“内”、“外”、“纵向”、“横向”、“上”、“下”、“顶”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明而不是要求本发明必须以特定的方位构造和操作，因此不应当理解为对本发明的限制。

首先是系统标定，包括对相机、结构光系统进行标定，确定相机的参数，以及对机械臂和相机进行手眼标定。再次根据待识别目标的特征，进行目标识别。将不同形状的工件作为待检测目标。通过计算机视觉中的图像处理算法将工件特征提取，如三维特征、拓扑特征、二维表面特征点等等，将这一区域范围作为图像的感兴趣区域(roi)，之后在感兴趣区域对图像进行处理，提取出轮廓，然后利用三角测量法确定出圆环工件相对于相机的三维坐标，最后根据手眼标定的结果，将该三维坐标转换到机械臂坐标系下，控制机械臂进行抓取。实验结果表明使用双目视觉很好的识别、定位圆环工件并通过上位机将圆环工件的位置信息发送给机械臂，控制机械臂进行抓取。

此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

实施例1:

本发明实施例1提供了一种基于多维度特征融合的零件识别装配方法，预先对相机、结构光系统分别进行标定，确定相机的参数(包括：焦距、光轴、径向畸变和切向畸变中的一项或者多项，其为相机的固有参数)，并对机械臂和相机进行结构光系统标定或者手眼标定，以及，将不同形状的工件作为待检测目标，通过计算机视觉中的图像处理算法将工件特征提取，如图1所示，所述方法包括：

在步骤201中，根据待识别目标的特征，进行目标识别。

在本发明实施例中，所述待识别目标的特征包括：三维特征、拓扑特征、二维表面特征点中的一种或者多种，并将包含所述待识别目标的特征区域范围，作为图像的感兴趣区域。

在步骤202中，在感兴趣区域对图像进行处理，提取出轮廓，然后利用三角测量法确定出圆环工件相对于相机的三维坐标。

在步骤203中，根据手眼标定的结果，将该三维坐标转换到机械臂坐标系下，控制机械臂进行抓取。

本发明对使用双目视觉很好的识别、定位圆环工件并通过上位机将圆环工件的位置信息发送给机械臂，控制机械臂进行抓取。针对计算机视觉和机械臂相结合的相关技术进行结合，将三维结构光技术和机械臂相结合，实现了机械臂对目标的识别、定位和抓取。

在本发明实施例具体实现方式中，所述三维特征中，具体包括：

半径、高程差、高程标准差和点密度中的一项或者多项三维几何特征；

线性特征、平面特征、散乱特征、总方差、各向异性、特征熵、特征值和曲率中的一项或者多项三维局部形状特征。

在本发明实施例具体实现方式中，提取二维表面特征点，如图2所示，具体包括：

在步骤301中，将二维滤波器对应的矩阵水平翻转，竖直翻转。

在步骤302中，把这个滤波器矩阵，覆盖到图像上方。最后，将滤波器矩阵的每一个元素，同下方图像的每一个元素相乘，再把所有的乘积加起来，并不断移动这个滤波器矩阵。

在步骤303中，每移动一格，对应得到一个值。

在步骤304中，当滤波器矩阵扫过原来的图像的所有位置的时候，我们就得到了一个新的图像；这个图像就是原来的图像经过该二维滤波器的输出，从而获得二维表面特征点。

结合本发明实施例，预先对相机进行标定，存在一种可选的实现方案，具体包括：

设定模板平面在世界坐标系z＝0的平面上；

h＝[h1h2h3]＝λk[r1r2t]，

其中，k为摄像机的内参数矩阵，r1和r2是旋转向量，h1、h2和h3为转化参数，由上式中的λk[r1r2t]求解得到，λ代表相机离标定板距离，为模板平面上点的齐次坐标，x和y是世界坐标，是像素平面上点的齐次坐标，u和v分别代表x轴和y轴的像素坐标，为模板平面上点投影到图象平面上对应点的齐次坐标，[r1r2r3]和t分别是摄像机坐标系相对于世界坐标系的旋转矩阵和平移向量；

根据旋转矩阵的性质：r1^tr2＝0和||r1||＝||r2||＝1，每幅图象可以获得以下两个对内参数矩阵的基本约束；

其中，摄像机有5个未知内参数(由3个径向畸变参数和2切向畸变参数构成)，，当所摄取得的图象数目大于等于3时，就可以线性唯一求解出k。

传统标定方法比较笨拙，需人工放置标定板，依次调整标定板的距离，拍照，通过图像处理手段获取系统参数，最终通过仿真软件拟合出系统参数。本发明实施例提出一种基于伺服系统的全自动标定系统，从初始位置到标定结束位置，全程由伺服系统控制标定板位姿与距离，同步拍摄、图像处理、数据存储、系统标定算法，直至完成标定流程获取标定结果。具体的，在本发明实施中，对于所述结构光系统标定，给予了一种优选的实现方案，如图3是，包括：

在步骤401中，通过摄像头采集激光器投射到待扫描对象上的帧图像，将所述帧图像转换为灰度图。其中，所述激光器设置在机械臂上，用于表征所述机械臂的抓取方向。

本发明实施例正式利用激光具有良好的单色性，很少受到物体表面纹理或颜色的影响特性，进一步，通过灰度处理采集导的各帧图像，简化整个计算过程。

在步骤402中，计算得到各帧图像中，对应一个或者多个激光扫描点的灰度质心。

其中，单帧图像只能得到待扫描对象垂直方向上一条线的位置，并且，所述一条线通常也并非连续的线，而是由一个或者多个离散的激光扫描点构成的，在本发明实施例中，对于每一个激光扫描点在灰度处理后的图像中会呈现一个灰度区域，而所述灰度质心便是根据各灰度区域计算得到的，用于计算该帧图片中有效采集内容的对象。

在步骤403中，根据所述灰度质心在图像中的像素距离和所述标定数据，计算出灰度质心的三维坐标，记录成为第一成像数据；其中，所述像素距离是通过所述数据模型生成的激光器在图像中的映射位置与图像中采集到的对应激光扫描点之间的距离。

而在本发明实施例中，所述点源数据库不一定仅仅是离散的携带三维坐标信息的灰度质心。在本发明实施例后续的扩展方案中，将进一步介绍对所述点源数据库的存储内容的扩展实现。

本发明实施例利用定标后的数据模型，并结合灰度质心的计算手段，达到了高精度三维模型的生成；由于对于每一帧图像的处理简化到线结构光的扫描区域，提高了整个过程中采集图像的处理效率。

在平台(如图4所示的平移平台，通常由步进电机驱动)上放置标定用对象，测量标定用对象到激光器和摄像头连线的一系列实际距离值qi，以及每个距离值对应的由摄像头采集的标定图像；

获取所述标定图像中预设标定点位的间距pxi，带入由相似三角形得到公式中，计算得到各相关参数的标定数据；其中，优选的，所述预设标定点位(以两个点，包括第一标定点位和第二标定点位为例)要求其中第一标定点位与激光器的连线，以及第二标定点位与摄像头镜头的连线平行。上述优选的标定点设置方式，能够很大程度上提高等式的建立速度，从而快速的完成相应标定数据计算过程。

其中，f为摄像头中镜头到图像传感器距离，s为摄像头与激光器的间距，pixelsize为图像像素的物理尺寸，offset为图像原点相对于三角测距中图像边缘的偏置距离。

在实际结合计算机实现所述标定数据的获取方式中，还提供了一种可选的实现思路，具体实现如下：

通过人为的设置一组(如图5所示，对应y轴坐标的为20个)所述第一标定点到激光器和摄像头连接线的距离qi(如图5所示的y轴坐标)；而对应每一个所述距离qi，在图像中表现的像素距离pxi(如图5所示的x轴坐标)，从而拟合出相应的关系曲线；并根据所述关系曲线和公式(1)求解得到标定数据。所述标定数据包括所述公式(1)中的f、s、pixelsize、offset等等。以便在后续实际3d扫描过程中，能够针对每一灰度质心点，均可用上述公式(1)通过像素点计算得到实际距离值qi。

在本发明实施例中，除了提供如图4和图5所示的结构光系统标定方法以外，还提供了另一种可选的结构光系统标定方法，具体如下：

九点标定直接建立相机和机械手之间的坐标变换关系。

让机械手的末端去走这就9个点得到在机器人坐标系中的坐标，同时还要用相机识别9个点得到像素坐标。这样就得到了9组对应的坐标。

由下面的式子可知至少需要3个点才能求出标定的矩阵。

(1)、标定，halcon中进行9点标定的算子

％前面求出图像坐标。

area_center(sortedregions，area,row,column)％

column_robot:＝[275,225,170,280,230,180,295,240,190]；

％机器人末端运动到9点的列坐标。

row_robot:＝[55,50,45,5,0,-5,-50,-50,-50]；

％机器人末端运动到9点的行坐标。

vector_to_hom_mat2d(row,column,row_robot,column_robot,hommat2d)；

％求解变换矩阵，hommat2d是图像坐标和机械手坐标之间的关系。

(2)、求解

affine_trans_point_2d(hommat2d,row2,column2,qx,qy)；

％由像素坐标和标定矩阵求出机器人基础坐标系中的坐标。

接下来，将对典型的两种相机固定方式进行介绍：

方式一、相机固定在一个地方，然后拍照找到目标，控制机械手去抓取，这种就很好理解，这种方式也叫eye-to-hand。

方式二、相机固定在机械手上面，这种情况的标定过程实际上和相机和机械手分离的标定方法是一样的，因为相机拍照时，机械手会运动到相机标定的时候的位置，然后相机拍照，得到目标的坐标，再控制机械手，所以简单的相机固定在末端的手眼系统很多都是采用这种方法，标定的过程和手眼分离系统的标定是可以相同对待的，这种方式也叫eye-in-hand。

在本发明实施例中，计算得到各帧图像中，对应一个或者多个激光扫描点的灰度质心，如图6所示，具体包括：

在步骤501中，根据预设的灰度阈值，逐一筛选图像中的各像素点，从而确认对应于所述一个或者多个激光扫描点的一个或者多个灰度区域。

在步骤502中，通过公式和计算得到相应的灰度质心；其中，xi和yi是像素坐标，fij是相应坐标点的灰度值。

结合本发明实施例，还存在一种优选的实现方案，考虑到相机拍摄出来的图片会因为相机的性能差异而存在畸变，因此，在本发明实施例基础上对于各图像进行灰度处理前，优选的，所述方法还包括相机内参的标定,具体的：

通过对所述摄像头进行多次标定得到内参数矩阵和畸变向量；其中，所述内参数矩阵和畸变向量用于对摄像头拍摄下来的帧图像做畸变矫正；所述畸变矫正后的帧图像用于所述转换灰度图处理。

结合本发明实施例，还提供了一种驱动步进电机进行扫描的具体实现方式，可以配合本发明实施例所提出线结构光扫描方案，起到更有效的扫描结果。所述通过摄像头采集激光器投射到待扫描对象上的激光扫描点，如图7所示，具体包括：

在步骤601中，通过串口设定步进电机的工作参数；其中步进电机的工作参数包括：电机运动的加速度、减速度、圆周脉冲数和角速度中的一项或者多项。

其中，设定的步进电机的工作参数，通常是根据采购的步进电机的允许工作范围内选配的，相应的通过串口设定步进电机的工作参数，具体是由构造函数完成数据输入，所述构造函数示例如下：

其中，setacc为设定加速度时间，200是脉冲数；setdec为设定减速的脉冲数；setsubdivision为设定的转一圈所需要的脉冲数；setspeed为速度，其参数含义是一秒5度；setangle为工作角度范围，具体为360°。

在步骤602中，根据所述步进电机的工作参数和待扫描对象的纹理细节分辨率，设定步进电机的工作模式，以便步进电机驱动转盘带动待扫描对象和/或激光器-摄像头组件，完成摄像头采集激光器投射到待扫描对象上的激光扫描点过程。

由于本发明实施例采用的是根据灰度质心来生成扫描结果的方式，因此，实际理论的精度是很高的，但是，在具体操作过程中也需要考虑效果和效率上的均衡，因此，在步骤602中提出了待扫描对象的纹理细节分辨率的概念，并结合步进电机的转速和加速度性能，设置一套与当前待扫描对象的纹理细节分辨率相匹配的，最优的扫描旋转速度。其中，电机运动的加速度和减速度，用于系统在涉及需要补充扫描位置时，进行差异性控制；例如：加速转过不需要补充扫描区域，而对于需要补充扫描区域则进行减速操作。

在本发明实施例中，摄像头可以为普通usbrgb摄像头，最高帧率30fps；分辨率大小为640x480；物理焦距为4.3mm；像素大小3um。激光器可以是功率100mw，激光波长650m,最小线宽0.4mm，且可以调节。

实施例2：

本发明实施例是针对实施例1中所描述的标定方法和获得的标定数据，对于步骤303中所涉及的所述根据所述灰度质心在图像中的像素距离和所述标定数据，计算出灰度质心的三维坐标，给予具体的一种实现实例的阐述。其中，所述数据模型包括平面模型(如图8所示)和垂直模型(如图9所示)，所述平面模型用于计算激光扫描点a按照其投射角度延伸到转轴时(如图9中标识p所示)与待扫描对象上的所述激光扫描点的间离(具体为图9中所示的pa)，并根据偏斜角度θ转换为三维坐标中的x和y坐标值；所述垂直模型用于为所述平面模型提供相应激光扫描点的光路相对于水平光路的偏斜角度θ，以便平面模型计算相应激光扫描点抵达转轴时(如图8和图9中标识的p所示)与发射点的间离(即线段bp的距离)，以及计算出z轴坐标，则所述根据所述灰度质心在图像中的像素距离和所述标定数据，计算出灰度质心的三维坐标，具体包括：

根据公式计算相应激光扫描点抵达待扫描对象时与发射点的间离。

根据公式(d-qi*cosθ)得到待扫描对象上的激光扫描点a到转轴的垂直距离ap’；其中，d为摄像头和激光器所在直线与转轴的垂直距离，θ为投射相应激光扫描点相对于水平面的偏斜角度；

结合待测目标的旋转角度γ，计算得到激光扫描点的三维坐标为[(d-qi*cosθ)*sinγ，(d-qi*cosθ)*cosnγ，(d-qi*cosθ)*tanθ]。换一种描述，所述三维坐标也可以表述为(ap’*sinγ，ap’*cosnγ，ap’*tanθ)，其中，初始状态开始扫描时，相应的旋转角度为初始值0，所述旋转角度即步进电机带动所述待扫描目标旋转的角度值。在上述三维坐标中，原点坐标为转轴与垂直于摄像头和激光器所在平面的交底。

实施例3：

本发明实施例结合具体的滤波器将上述步骤301-304方法内容做具体场景下的描述，把滤波器对应的矩阵翻转后，扣到图像对应的矩阵上。然后，先从左上角开始，将这两个矩阵重叠的元素相乘相加，再不断移动滤波器对应的矩阵，直到所有的点都扫完为止。

左边是输入图像对应的矩阵，经过中间的滤波器后，得到的输出是右边的样子，其过程如图10所示。

(1)先将滤波器上下左右翻转，因为这个滤波器是对称的，所以翻转后和之前一样。

(2)把翻转后的滤波器，对准输入图像的左上角(灰色区域，如图10所示)，再把对应的元素相乘相加。这样就得到了105*0+91*(-1)+100*0+105*(-1)+77*8+70*(-1)+101*0+120*(-1)+89*0＝230这个值。我们把这个值，放入此刻滤波器中心所在的位置，作为输出图像在该位置的值。

如图11所示，然后按同样的计算方法，得到了91*0+100*(-1)+97*0+77*(-1)+70*8+104*(-1)+120*0+89*(-1)+99*0＝190。190就应该位于此刻滤波器的中心位置。然后我们继续从左向右移动滤波器，每移动一格，就得到输出图像的一个值，如图11所示。当移动滤波器到最右边的时候，滤波器返回左边下一行，继续重复，直到滤波器到达右下角。

在计算最边缘的一行和一列的输出结果时，对原矩阵外围补零，再进行矩阵的卷和操作，如图12所示，得到了0*0+0*(-1)+0*0+102*(-1)+100*5+97*(-1)+99*0+103*(-1)+101*0＝198。

实施例4：

本发明实施例提供了产品设计中的一种机械臂，具体为6自由度机械臂，如图13所示，其舵机0、舵机1、舵机2、舵机3和舵机4依次相连，为水平旋转舵机，舵机5为控制机械手抓取的舵机，其中，舵机0为可在水平面上旋转的的舵机，舵机1、舵机2、舵机3和舵机4为竖直旋转舵机，具体的：

机械臂三连杆长度分别为l1、l2、l3，舵机0的水平旋转角度为θ0，舵机1的竖直面旋转角度为θ1，舵机2的竖直面旋转角度为θ2，舵机3的竖直面旋转角度为θ3，舵机4的竖直面旋转角度为θ4，舵机5的竖直面旋转角度为θ5；

以机械臂底座旋转中心为三维坐标系原点，根据目标点三维坐标p(x，y，z)与机械臂原点o(0，0，0)距离|po|判定，若|po|≤l1+l2+l3，则机械臂可达；若|po|＞l1+l2+l3，则机械臂不可达；

若可达，则根据目标点三维坐标p(x，y，z)计算出舵机0的旋转角度为arctan(m/n)，其中m和n为舵机1所在点坐标。

在本发明实施例中，旋转舵机0，将机械臂连杆机构与目标点位于同一平面上，设该平面为x-y平面，使得机械臂逆运动学问题进一步简化，将空间路径规划问题简化为同一平面的三连杆路径规划问题。如图14所示，在x-y平面中，设物体在该坐标系下坐标为p′(a，b)，机械臂下臂旋转中心坐标为坐标原点o′(0，0)。以o′为圆心，l1为半径作圆o′，该圆方程为x²+y²＝l²1；在其上取一点m(m，n)，使得m、p′两点间距离|mp′|≤l2+l3，以m(m，n)为圆心，l2为半径作圆m，该圆方程为：(x-m)²+(y-n)²＝l²2；以p′(a，b)为圆心，l3为半径作圆p′，该圆方程为：(x-a)²+(y-b)²＝l²3；m、p′两点间距离|mp′|≤l2+l3，故至少存在1个、至多存在2个点，圆m与圆p′相交，设交点坐标为n(i，j)。

由θ3≤π/2可知，|mp′|≤(l2+l3)^1/2,

即((a-m)²+(b-n)²)^1/2≥(l2+l3)^1/2；

由机械臂臂长参数l1、l2、l3值可知：(l2+l3)^1/2>l1，得|mp′|≥l1。

在以o′为圆心，l1+l2+l3为半径的解空间内，除了原点o′以外，对任意一点p′(a，b)，都至少存在一组解θ1、θ2、θ3，使得机械臂的三连杆末端可达。

θ1＝arcsin(m/l1)；|θ2|＝π-arccos((l²1+l²2-|mj|²)/2l1l2)；|θ3|＝π-arccos((l²2+l²3-|p′m|²)/2l2l3)。

θ2的正负取值可根据n点与直线o′m的相对关系确定，若n点在直线o′m上或在其上半部，θ2取负值；若m点在直线o′m下半部，θ2取正值。θ3的正负取值可根据p′点与直线mn的相对关系确定，若p′点在直线mn上或在其上半部，θ3取负值；若p′点在直线mn下半部，θ3取正值。由机械臂参数规定，θ1、θ2、θ3取值均在空-90度到90度内。

本发明中可以求解物体位置与姿态、已知机械臂与姿态，通过位姿反求，路径规划，机械臂可通过每一节机械臂的自由度限制，规划出用时最短且路径最优的方案。

实施例5：

如图15所示，是本发明实施例的机械臂控制装置的架构示意图。本实施例的机械臂控制装置包括一个或多个处理器21以及存储器22。其中，图15中以一个处理器21为例。

处理器21和存储器22可以通过总线或者其他方式连接，图15中以通过总线连接为例。

存储器22作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序和非易失性计算机可执行程序，如实施例1中的基于多维度特征融合的零件识别装配方法。处理器21通过运行存储在存储器22中的非易失性软件程序和指令，从而执行基于多维度特征融合的零件识别装配方法。

存储器22可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器22可选包括相对于处理器21远程设置的存储器，这些远程存储器可以通过网络连接至处理器21。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

所述程序指令/模块存储在所述存储器22中，当被所述一个或者多个处理器21执行时，执行上述实施例1中的基于多维度特征融合的零件识别装配方法，例如，执行以上描述的图1、图2、图6、图7所示的各个步骤。

值得说明的是，上述装置和系统内的模块、单元之间的信息交互、执行过程等内容，由于与本发明的处理方法实施例基于同一构思，具体内容可参见本发明方法实施例中的叙述，此处不再赘述。

本领域普通技术人员可以理解实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(rom，readonlymemory)、随机存取存储器(ram，randomaccessmemory)、磁盘或光盘等。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。