CN113146620B - 基于双目视觉的双臂协作机器人系统和控制方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉的双臂协作机器人系统，包括第一机械臂装置、第二机械臂装置、双目检测装置、工作区域装置、底盘装置和控制装置；通过双目检测装置识别工作区域内待双臂协作抓取与组合的工件的点云数据，完成基于双目摄像头观测模型的手眼关系建模，将待识别工件的像素坐标的重投影误差最小化；将经逆运动学解算后的结果，选取最优解，反馈至机械臂控制装置，由其控制第一机械臂先行运动至工件I所在区域，完成抓取任务；随后，控制第二机械臂仿照第一机械臂经逆解后的闭环舵机运动参数至工件II所在区域；由两个机械臂协作完成任务。本发明能够有效提高机器人的作业精度，真正意义上达到拟人化操作，大大提升了工业生产的自动化程度。

Description

基于双目视觉的双臂协作机器人系统和控制方法

技术领域

本发明涉及机器视觉技术领域，具体而言涉及一种基于双目视觉的双臂协作机器人系统和控制方法。

背景技术

近年来，随着机器人技术的发展，仿人双臂机器人协调运动的研究日益增多。双臂机器人不是单臂机器人在数量上的简单叠加，而是通过一定的协调控制算法去共同完成工作任务。因为从事单调、重复性技术劳动的场合仍然需要大量的技术工人，但是随着劳动力成本的上升，这些技术工人也面临着巨大的缺口，给目前制造业企业带来发展的瓶颈和困惑。仿人双臂机器人比传统的机器人具有更高的灵活性和更好的适应能力，更适合于复杂多变的工作环境和繁杂的操作任务。当今，机器视觉已广泛应用于社会生产生活实践中，尤其在工业生产领域基于双目视觉的双臂协作机器人正在逐步得到推广，为进一步提高机器人精度，需要对末端执行机构运动时与实际工件所产生的距离偏差进行补偿，目前尚缺针对此类误差的控制方法。

在现代化视觉检测自动加工生产中，工件目标识别与定位是必不可少的一步。传统的目标识别常常使用迭代最近点的点云配准方法，这种方法一般需要提供一个较好的初值，也就是需要粗配准，但是由于算法本身缺陷，最终迭代结果可能会陷入局部最优，导致配准失败，往往达不到我们想要的效果。

因此，亟待提出一种基于双目视觉的双臂协作机器人装置，并提出一种能够实现双臂协作机器人协调装配工件的控制方法，能够较好地提高双臂机器人的协作效率，并使得机器人适用于复杂的生产生活领域。

发明内容

本发明针对现有技术中的不足，提供一种基于双目视觉的双臂协作机器人系统和控制方法，能够实现工件的协调装配，并有效提高双臂机器人的协作精度。

为实现上述目的，本发明采用以下技术方案：

一种基于双目视觉的双臂协作机器人系统，所述双臂协作机器人装置包括第一机械臂装置(1)、第二机械臂装置(3)、双目检测装置(2)、工作区域装置(4)、底盘装置(7)和控制装置；

所述底盘装置(7)呈扁平长方体状，沿长边的延伸方向设置有第一端部和第二端部；所述工作区域装置(4)水平安装在底盘装置(7)上表面的中心区域；结构相同的第一机械臂装置(1)和第二机械臂装置(3)镜像安装在底盘装置(7)的第一端部和第二端部，用于对放置在工作区域装置(4)上方的工件I(402)和工件II(403)进行装配作业；所述双目检测装置(2)包括L型连接杆(201)与双目摄像头(202)，双目摄像头(202)通过固定在底盘装置(7)一侧的L型连接杆(201)安装在工作区域装置(4)的正上方，用于识别工件I(402)、工件II(403)、第一机械臂装置(1)末端和第二机械臂装置(3)末端的点云数据；

所述控制装置分别以工作区域装置(4)、第一机械臂装置(1)、第二机械臂装置(3)和双目检测装置(2)的中心点为原点构建基坐标系OXYZ、第一机械臂工作坐标系O₁X₁Y₁Z₁、第二机械臂工作坐标系O₂X₂Y₂Z₂和测量坐标系O₃X₃Y₃Z₃；

所述控制装置实时跟踪第一机械臂装置(1)末端和第二机械臂装置(3)末端的空间位姿，计算得到测量坐标系与基坐标系、第一机械臂工作坐标系和第二机械臂工作坐标系的转换关系，建立基于双目检测装置(2)的手眼关系模型，通过将测量坐标系下任意位置处坐标的重投影误差最小化，得到手眼关系模型的最优模型参数，使第一机械臂装置(1)、第二机械臂装置(3)协作抓取过程中两个机械臂末端中心点与待抓取工件中心点之间的误差以及协作组合过程中工件I(402)和工件II(403)中心点之间的误差最小；再在测量坐标系下控制第一机械臂装置(1)和第二机械臂装置(3)对工件I(402)和工件II(403)进行双臂协作抓取与组合，完成对工件I(402)和工件II(403)的装配任务。

为优化上述技术方案，采取的具体措施还包括：

进一步地，所述控制装置根据机械臂DH模型几何参数值及其末端的期望位姿，求解逆运动学方程获得末端到达期待位姿时各个关节变量值，根据第一机械臂装置运动范围最小以及行程最短到达的关节角，确定一组第一机械臂装置最优解关节角；并且根据第一机械臂装置最优解关节角，控制第一机械臂装置(1)先行运动至工件I(402)所在区域，完成抓取任务，再根据第二机械臂装置运动范围最小以及行程最短到达的关节角，确定一组第二机械臂装置最优解关节角，并根据第二机械臂装置最优解关节角控制第二机械臂装置(3)运动至第二工件所在区域，最后，由第一机械臂装置(1)和第二机械臂装置(3)协作完成装配任务；

在第一机械臂装置(1)和第二机械臂装置(3)的移动和装配过程中，双目检测装置(2)不断对双臂末端位姿进行采集，在重投影误差最小化的前提下，对其机械臂末端中心点坐标与待抓取工件I(402)、工件II(403)进行接近反馈。

进一步地，所述控制装置构建基坐标系OXYZ、第一机械臂工作坐标系O₁X₁Y₁Z₁、第二机械臂工作坐标系O₂X₂Y₂Z₂和测量坐标系O₃X₃Y₃Z₃的过程包括以下步骤：

以工作区域中心点O为坐标原点，以第一机械臂装置(1)和第二机械臂装置(3)的底盘中心点O₁、O₂连线

为X轴，垂直于

连线的方向为Y轴，根据右手法则确立Z轴，建立基坐标系OXYZ；

以第一机械臂装置(1)底盘中心点O₁为坐标原点，以

方向为X₁轴，与Y轴平行方向为Y₁轴，与Z轴平行方向为Z₁轴建立第一机械臂工具坐标系O₁X₁Y₁Z₁；

以第二机械臂装置底盘中心点O₂为坐标原点，以

方向为X₂轴，与Y轴平行方向为Y₂轴，与Z轴平行方向为Z₂轴建立第二机械臂工具坐标系O₂X₂Y₂Z₂；

以双目检测装置的双目摄像头视角中心O₃为坐标原点，与X轴平行方向为X₃轴，与Y轴平行同向的方向为Y₃轴，以

轴方向且与Z重合为Z₃轴建立测量坐标系O₃X₃Y₃Z₃。

进一步地，所述第一机械臂装置(1)包括第一机械臂底盘(111)、第一固定铜柱(110)、第一多孔圆盘(109)、第一肩关节总线舵机(108)、第一工字型连接杆(107)、第一机械臂末端(106)、第一掌关节总线舵机(105)、第一腕关节总线舵机(104)、第一肘关节总线舵机(103)、第一底部连接机构(102)，所述第一机械臂底盘(111)固定于底盘装置(7)上；

所述第二机械臂装置(3)包括第二机械臂底盘(311)、第二固定铜柱(310)、第二多孔圆盘(309)、第二肩关节总线舵机(308)、第二工字型连接杆(307)、第二机械臂末端(306)、第二掌关节总线舵机(305)、第二腕关节总线舵机(304)、第二肘关节总线舵机(303)、第二底部连接机构(302)，所述第二机械臂底盘(311)固定于底盘装置(7)上。

进一步地，所述控制装置包括用于控制第一机械臂装置(1)的第一机械臂舵控板(101)和用于控制第二机械臂装置(3)的第二机械臂舵控板(301)，分别通过信号线一(5)和信号线二(6)与双目检测装置(2)连接。

基于前述机器人系统，本发明还提及一种基于双目视觉的双臂协作机器人系统的控制方法，包括以下步骤：

S1，建立基坐标系OXYZ，第一、第二机械臂工具坐标系O₁X₁Y₁Z₁、O₂X₂Y₂Z₂及测量坐标系O₃X₃Y₃Z₃；

S2，对工件I(402)、工件II(403)的点云数据进行采集，经双目摄像头(202)取重心点作为中心点的方法，输出工件I(402)、工件II(403)与两个机械臂末端中心点在测量坐标系下的坐标；

S3，双目检测装置(2)实时跟踪第一、第二机械臂末端的空间位姿，建立基于双目摄像头的手眼关系模型，通过将测量坐标系下任意位置处坐标的重投影误差最小化的方法，减小第一、第二机械臂协作抓取过程中机械臂末端中心点与待抓取工件中心点之间的误差以及协作组合过程中工件I(402)与工件II(403)中心点之间的误差；

S4，控制第一机械臂装置(1)和第二机械臂装置(3)对工件I(402)、工件II(403)进行双臂协作抓取与组合，完成对工件I(402)、工件II(403)的装配任务。

进一步地，步骤S2中，对工件I(402)、工件II(403)的点云数据进行采集，经双目摄像头(202)取重心点作为中心点的方法，输出工件I(402)、工件II(403)与两个机械臂末端中心点在测量坐标系下的坐标的过程包括以下步骤：

在工作区域处放置标定板，在保证工作区域与标定板的大小与安装位置一致的前提下，采用张正友标定法标定双目相机的内参数，确定工作区域上的点在基坐标系与测量坐标系之间的一一对应关系；

获取处于工作区域的工件I(402)和工件II(403)的点云数据，由双目摄像头(202)输出测量坐标系下工件I(402)、工件II(403)中心点与机械臂末端中心点坐标。

进一步地，步骤S3中，所述双目检测装置(2)实时跟踪第一、第二机械臂末端的空间位姿，建立基于双目摄像头的手眼关系模型的过程包括以下步骤：

S31，将双目摄像头采集的图像以M×N的矩阵形式存储，其矩阵中元素的坐标为像素坐标(m,n)，其m轴、n轴互相垂直且分别与基坐标系的X轴、Y轴平行，原点O在m-n像素坐标系中的坐标为(m₀,n₀)，每一个像素在X轴、Y轴方向上的物理尺寸分别为d_x、d_y，则基坐标系与像素坐标系之间的关系表示为：

用齐次坐标系表示为：

S32，根据双目摄像头的透视投影模型，得到如下关系：

其中双目摄像头视角中心到基坐标原点

的长为有效焦距f，(x₃,y₃,z₃)为测量坐标系上的点，则测量坐标系与基坐标系的转换关系用齐次坐标表示为：

S33，通过机械臂末端和标定板的关系推导出标定板相对于双目摄像头的位姿即双目摄像头的外参数，设O₃、O、O₁、O₂、O₁′和O₂′分别为双目摄像头、工作区域、第一机械臂末端、第二机械臂末端、第一机械臂底盘和第二机械臂底盘中心点，得到以下等式：

其中，式(5.1)对应第一机械臂装置，式(5.2)对应第二机械臂装置，T表示旋转平移矩阵，包括旋转和平移两部分，旋转部分包含3个未知数，即分别绕X、Y、Z轴的三个转角(θ_x,θ_y,θ_z)，以欧拉角表示的旋转矩阵为：

平移部分包含3个未知数(t_x,t_y,t_z)；

S34，建立基于双目摄像头观测模型的手眼关系模型：

其中，式(7.1)是第一机械臂装置对应的手眼关系模型，式(7.2)是第二机械臂装置对应的手眼关系模型，G是双目摄像头的内参数，[m n]^T、

[x₃ y₃ z₃]^T是观测数据，[m n]^T通过双目摄像头得到，

由第一机械臂末端和第二机械臂末端执行返回，

和

是待求解的模型参数；

S35，将n组观测数据作为输入数据，将第一机械臂装置第k次机械臂末端与双目摄像头之间的模型参数

与第二机械臂装置第k次机械臂末端与双目摄像头之间的模型参数

作为已知参数代入待求解的目标方程，将第一机械臂装置第k-1次机械臂底座与双目摄像头之间模型参数

与第二机械臂装置第k-1次机械臂底座与双目摄像头之间模型参数

作为优化的初始值，采用非线性最小二乘求解

与

优化的目标函数为：

其中，

分别代表像素坐标系上的点q以及测量坐标系下的点P；当该目标函数最小时，得到最终优化结果

否则置k＝k+1，在n组数据中筛选出最精确的观测数据作为最好的双目摄像头定位结果即测量坐标系下的坐标值；采用Levenberg-Marquardt算法求解每一步中的非线性优化问题。

本发明通过双目检测装置识别工作区域内待双臂协作抓取与组合的工件的点云数据；完成基于双目摄像头观测模型的手眼关系建模，将待识别工件的像素坐标的重投影误差最小化，减小机械臂末端中心点与待抓取工件中心点之间距离的偏差；将经逆运动学解算后的结果，选取最优解，反馈至机械臂控制装置，由其控制第一机械臂先行运动至工件I所在区域，完成抓取任务；随后，控制第二机械臂仿照第一机械臂经逆解后的闭环舵机运动参数至工件II所在区域；最后，由第一、第二机械臂协作完成组合、装配任务。

本发明的有益效果是：

(1)本发明能够有效提高机器人的作业精度，真正意义上达到拟人化操作，大大提升了工业生产的自动化程度。

(2)本发明的双臂协作机器人装置相比较国外成品的机械臂装置价格低廉，且在控制过程中能对机械臂末端抓取过程中产生的偏差进行补偿，从而完成一整套的协作抓取与组合任务。

(3)本发明所述的双臂机器人协调控制方法，相比较以往的迭代最近点的点云配准方法中由于算法本身缺陷，最终迭代结果可能会陷入局部最优，导致配准失败的问题，建立基于双目摄像头的手眼关系模型，通过将测量坐标系下任意位置处坐标的重投影误差最小化的方法，减小第一、第二机械臂协作抓取过程中机械臂末端中心点与待抓取工件中心点之间的误差以及协作组合过程中工件I与II中心点之间的误差，提供更为精确的运动规划方案。

附图说明

图1是本发明的基于双目视觉的双臂协作机器人系统的结构示意图。

图2是本发明的第一机械臂结构示意图。

图3是本发明的第二机械臂结构示意图。

图4是本发明的双目检测装置结构示意图。

图5是本发明的工作区域装置结构示意图。

图6是本发明的协调控制方法示意图。

图7是基于双目摄像头观测模型的外参数的转换关系示意图。

具体实施方式

现在结合附图对本发明作进一步详细的说明。

需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

如图1所示，在本发明的其中一个实施例中，提出了一种基于双目视觉的双臂协作机器人系统，包括第一机械臂装置1、第二机械臂装置3、双目检测装置2、工作区域装置4、信号线一5、信号线二6和底盘装置7，所述第一机械臂装置1和第二机械臂装置3置于工作区域装置4两端，均固定于底盘装置7。控制装置首先建立基坐标系OXYZ，第一、第二机械臂工具坐标系O₁X₁Y₁Z₁、O₂X₂Y₂Z₂及测量坐标系O₃X₃Y₃Z₃。第一机械臂装置1与第二机械臂装置3分别置于工作区域两端，其基坐标系X轴与工具坐标系X₁、X₂轴重合，Y轴与Y₁、Y₂轴平行，Z轴与Z₁、Z₂轴平行。双目检测装置2包括L型连接杆201和双目摄像头202，L型连接杆201垂直固定于底盘装置7上，其测量坐标系Z₃轴与基坐标系Z轴重合，X₃轴与X轴平行，Y₃轴与Y、Y₁、Y₂平行。所述双目检测装置2用于检测工作区域装置4中待组合的工件I402与工件II403及第一机械臂末端106与第二机械臂末端306。

在本实施例中，控制装置包括用于控制第一机械臂装置1的第一机械臂舵控板101和用于控制第二机械臂装置3的第二机械臂舵控板301，分别通过信号线一5和信号线二6与双目检测装置2连接，由双目摄像头202输出的测量坐标系下工件I402、II403中心点与机械臂末端中心点坐标，机械臂控制装置负责在测量坐标系下控制机械臂末端向任意位置坐标接近。

如图2和图3所示，在本发明的其中一个实施例中，第一机械臂装置1包括第一底盘111、第一固定铜柱110、第一多孔圆盘109、第一肩关节总线舵机108、第一工字型连接杆107、第一机械臂末端106、第一掌关节总线舵机105、第一腕关节总线舵机104、第一肘关节总线舵机103、第一底部连接机构102，所述第一机械臂底盘111固定于底盘装置7上。所述第二机械臂装置3包括第二底盘311、第二固定铜柱310、第二多孔圆盘309、第二肩关节总线舵机308、第二工字型连接杆307、第二机械臂末端306、第二掌关节总线舵机305、第二腕关节总线舵机304、第二肘关节总线舵机303、第二底部连接机构302，所述第二机械臂底盘311固定于底盘装置7上。所述双目检测装置2包括L型连接杆201与双目摄像头202。所述工作区域装置4包括底板401，工件402与工件403位于底板401上。

如图6所示，在本发明的其中一个实施例中，一种基于双目视觉的双臂协作机器人控制方法具体包括以下步骤：

S1，建立基坐标系OXYZ，第一、第二机械臂工作坐标系O₁X₁Y₁Z₁、O₂X₂Y₂Z₂及测量坐标系O₃X₃Y₃Z₃。

S2，对工件I402、工件II403的点云数据进行采集，经双目摄像头202取重心点作为中心点的方法，输出工件I402、工件II403与机械臂末端中心点在测量坐标系下的坐标。

S3，双目摄像头实时跟踪第一、第二机械臂末端的空间位姿，建立基于双目摄像头的手眼关系模型，通过将测量坐标系下任意位置处坐标的重投影误差最小化的方法，减小第一、第二机械臂协作抓取过程中机械臂末端中心点与待抓取工件中心点之间的误差以及协作组合过程中工件I402和工件II403中心点之间的误差。

S4，通过机械臂控制装置控制第一、第二机械臂对工件I402和工件II403进行双臂协作抓取与组合，完成对工件I402和工件II403的装配任务。

如图6所示，在本发明的其中一个实施例中，基于双目视觉的双臂协作机器人装置及控制方法中建立基坐标系OXYZ，第一、第二机械臂工作坐标系O₁X₁Y₁Z₁、O₂X₂Y₂Z₂及测量坐标系O₃X₃Y₃Z₃具体包括以下步骤：

以工作区域中心点O为坐标原点，以第一、第二机械臂底盘中心点O₁、O₂连线

为X轴，垂直于

连线的方向为Y轴，根据右手法则确立Z轴，建立基坐标系OXYZ；以第一机械臂底盘中心点O₁为坐标原点，以

方向为X₁轴，与Y轴平行方向为Y₁轴，与Z轴平行方向为Z₁轴建立第一机械臂工具坐标系O₁X₁Y₁Z₁，以第二机械臂装置底盘中心点O₂为坐标原点，以

方向为X₂轴，与Y轴平行方向为Y₂轴，与Z轴平行方向为Z₂轴建立第二机械臂工具坐标系O₂X₂Y₂Z₂；以双目检测装置的双目摄像头视角中心O₃为坐标原点，与X轴平行方向为X₃轴，与Y轴平行同向的方向为Y₃轴，以

轴方向且与Z重合为Z₃轴建立测量坐标系O₃X₃Y₃Z₃。

如图6所示，在本发明的其中一个实施例中，基于双目视觉的双臂协作机器人装置及控制方法中对工件I402和工件II403的点云数据进行采集，经双目摄像头202取重心点作为中心点的方法，输出工件I402、工件II403与机械臂末端中心点在测量坐标系下的坐标具体包括以下步骤：

在工作区域处放置标定板，在保证工作区域与标定板的大小与安装位置一致的前提下，采用张正友标定法标定双目相机的内参数，确定工作区域上的点在基坐标系与测量坐标系之间的一一对应关系。获取的处于工作区域的工件I402和工件II403的点云数据，由双目摄像头202输出的测量坐标系下工件I402、工件II403中心点与机械臂末端中心点坐标，机械臂控制装置负责在测量坐标系下控制机械臂末端接近工件中心点。

如图6所示，在本发明的其中一个实施例中，基于双目视觉的双臂协作机器人装置及控制方法中双目摄像头实时跟踪第一、第二机械臂末端的空间位姿，建立基于双目摄像头的手眼关系模型，通过将测量坐标系下任意位置处坐标的重投影误差最小化的方法，减小第一、第二机械臂协作抓取过程中机械臂末端中心点与待抓取工件中心点之间的误差以及协作组合过程中工件I402和工件II403中心点之间的误差，具体包括以下步骤：

步骤一、基于双目摄像头的手眼关系建模

双目摄像头采集的图像以M×N的矩阵形式存储，其矩阵中元素的坐标为像素坐标(m,n)，其m轴、n轴互相垂直且分别与基坐标系的X轴、Y轴平行，原点O在m-n像素坐标系中的坐标为(m₀,n₀)，每一个像素在X轴、Y轴方向上的物理尺寸分别为d_x、d_y，则基坐标系与像素坐标系之间的关系可以表示为：

其中，该步骤用于将双目摄像头所采集到的图像上的像素点，尤其是工件上与机械臂末端上的点所对应的像素坐标转化为基坐标系上的点(x,y,z)。

用齐次坐标系可表示为：

进一步的根据双目摄像头的透视投影模型，可以得到如下关系：

其中双目摄像头视角中心到基坐标原点

的长为有效焦距f，(x₃,y₃,z₃)为测量坐标系上的点。则测量坐标系与基坐标系的转换关系用齐次坐标可以表示为：

其中，该步骤完成了将基坐标系的点向测量坐标系下的映射，这也是基于双目视觉的双臂协作机器人控制方法中步骤二：对工件I402和工件II403的点云数据进行采集，经双目摄像头202取重心点作为中心点的方法，输出工件I402、工件II403与机械臂末端中心点在测量坐标系下的坐标的基础。

在确定好工作区域上的点所在基坐标系以及测量坐标系之间的一一对应关系后，我们可以通过机械臂末端和标定板的关系推导出标定板相对于双目摄像头的位姿即双目摄像头的外参数，其中手眼模型的外参数的转换关系如图7所示，其中O₃、O、O₁、O₂、O₁′和O₂′分别为双目摄像头、工作区域、第一机械臂末端、第二机械臂末端、第一机械臂底盘和第二机械臂底盘中心点。因此可以得到以下等式：

其中，式(5.1)对应第一机械臂装置，式(5.2)对应第二机械臂装置，T表示旋转平移矩阵，包括旋转和平移两部分。其中旋转部分包含3个未知数，也就是分别绕X、Y、Z轴的三个转角(θ_x,θ_y,θ_z)。以欧拉角表示的旋转矩阵为：

平移部分包含3个未知数(t_x,t_y,t_z)。该步骤通过工作区域与第一、第二机械臂末端的位姿关系，第一、第二机械臂末端与其底盘的位姿关系，手眼变换关系以及双目摄像头的透视投影变换关系，映射到像素坐标。由此建立起基于双目摄像头观测模型的手眼关系模型：

其中，式(7.1)是第一机械臂装置对应的手眼关系模型，式(7.2)是第二机械臂装置对应的手眼关系模型，[m n]^T、

[x₃ y₃ z₃]^T是观测数据，[m n]^T通过双目摄像头得到，

由第一机械臂末端和第二机械臂末端执行返回，

和

是待求解的模型参数。

步骤二、基于最小化重投影误差的手眼关系求解

公式(7.1)与(7.2)的优化的方法是最小化重投影误差。当模型参数使得所有特征点的重投影误差最小时，就得到了当前观测下的最优的模型参数。

将n组观测数据作为输入数据，将第一机械臂装置第k次机械臂末端与双目摄像头之间模型参数

和第二机械臂装置第k次机械臂末端与双目摄像头之间模型参数

作为已知参数代入待求解的目标方程，将第一机械臂装置第k-1次机械臂底座与双目摄像头之间模型参数

与第二机械臂装置第k-1次机械臂底座与双目摄像头之间模型参数

作为优化的初始值，采用非线性最小二乘求解

与

优化的目标函数为：

其中，式(8.1)为第一机械臂装置优化的目标函数，式(8.2)为第二机械臂装置优化的目标函数

分别代表像素坐标系上的点q以及测量坐标系下的点P，由于第一机械臂与第二机械臂的机械结构相同，安装位置也镜像的位于工作区域两侧。最终的目标函数可以合二为一为：

当该目标函数最小时，得到最终优化结果

否则置k＝k+1。其含义是在n组数据中筛选出最精确的观测数据作为最好的双目摄像头定位结果即测量坐标系下的坐标值。

同时采用Levenberg-Marquardt算法(简称LM算法)求解每一步中的非线性优化问题，当优化的初始值距离最终的优化目标较远时，使用LM算法也能获得一个相对较好的解。该标定方法把最小化重投影误差作为优化目标，在一定程度上抑制标定数据采集时带来的误差影响，可以得到更高精度的标定结果。采用该方法将有利的减小第一、第二机械臂协作抓取过程中机械臂末端中心点与待抓取工件中心点之间的误差以及协作组合过程中工件I402和工件II403中心点之间的误差。

如图6所示，在本发明的其中一个实施例中，基于双目视觉的双臂协作机器人装置及控制方法中通过机械臂控制装置控制第一、第二机械臂对工件I402和工件II403进行双臂协作抓取与组合，完成对工件I402和工件II403的装配任务具体包括：

根据机械臂DH模型几何参数值及其末端的期望位姿，利用现有技术求解逆运动学方程获得末端到达该位姿时各个关节变量值，共有8组解，根据第一机械臂装置运动范围最小以及行程最短到达的关节角，确定一组第一机械臂装置最优解关节角。根据此最优解，控制第一机械臂先行运动至工件I402所在区域，完成抓取任务；随后，控制第二机械臂仿照第一机械臂末端中心点的运动轨迹至工件II403所在区域；最后，由第一、第二机械臂协作完成装配这一任务要求。同时，双目检测装置2将不断对双臂末端位姿进行采集，在重投影误差最小化的前提下，获取其机械臂末端中心点坐标与待抓取工件I402和工件II403进行接近反馈，保证其能正确的抓取到工件I402和工件II403，并完成装配任务。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (2)

1.一种基于双目视觉的双臂协作机器人系统的控制方法，其特征在于，实现基于双目视觉的双臂协作机器人系统的控制方法的双臂协作机器人系统包括第一机械臂装置(1)、第二机械臂装置(3)、双目检测装置(2)、工作区域装置(4)、底盘装置(7)和控制装置；

所述底盘装置(7)呈扁平长方体状，沿长边的延伸方向设置有第一端部和第二端部；所述工作区域装置(4)水平安装在底盘装置(7)上表面的中心区域；结构相同的第一机械臂装置(1)和第二机械臂装置(3)镜像安装在底盘装置(7)的第一端部和第二端部，用于对放置在工作区域装置(4)上方的工件I(402)和工件II(403)进行装配作业；所述双目检测装置(2)包括L型连接杆(201)与双目摄像头(202)，双目摄像头(202)通过固定在底盘装置(7)一侧的L型连接杆(201)安装在工作区域装置(4)的正上方，用于识别工件I(402)、工件II(403)、第一机械臂装置(1)末端和第二机械臂装置(3)末端的点云数据；

所述控制装置分别以工作区域装置(4)、第一机械臂装置(1)、第二机械臂装置(3)和双目检测装置(2)的中心点为原点构建基坐标系OXYZ、第一机械臂工作坐标系O₁X₁Y₁Z₁、第二机械臂工作坐标系O₂X₂Y₂Z₂和测量坐标系O₃X₃Y₃Z₃；

所述控制装置实时跟踪第一机械臂装置(1)末端和第二机械臂装置(3)末端的空间位姿，计算得到测量坐标系与基坐标系、第一机械臂工作坐标系和第二机械臂工作坐标系的转换关系，建立基于双目检测装置(2)的手眼关系模型，通过将测量坐标系下任意位置处坐标的重投影误差最小化，得到手眼关系模型的最优模型参数，使第一机械臂装置(1)、第二机械臂装置(3)协作抓取过程中两个机械臂末端中心点与待抓取工件中心点之间的误差以及协作组合过程中工件I(402)和工件II(403)中心点之间的误差最小；再在测量坐标系下控制第一机械臂装置(1)和第二机械臂装置(3)对工件I(402)和工件II(403)进行双臂协作抓取与组合，完成对工件I(402)和工件II(403)的装配任务；

所述控制装置根据机械臂DH模型几何参数值及其末端的期望位姿，求解逆运动学方程获得末端到达期待位姿时各个关节变量值，根据第一机械臂装置运动范围最小以及行程最短到达的关节角，确定一组第一机械臂装置最优解关节角；并且根据第一机械臂装置最优解关节角，控制第一机械臂装置(1)先行运动至工件I(402)所在区域，完成抓取任务，再根据第二机械臂装置运动范围最小以及行程最短到达的关节角，确定一组第二机械臂装置最优解关节角，并根据第二机械臂装置最优解关节角控制第二机械臂装置(3)运动轨迹至第二工件所在区域，最后，由第一机械臂装置(1)和第二机械臂装置(3)协作完成装配任务；

在第一机械臂装置(1)和第二机械臂装置(3)的移动和装配过程中，双目检测装置(2)不断对双臂末端位姿进行采集，在重投影误差最小化的前提下，对其机械臂末端中心点坐标与待抓取工件I(402)、工件II(403)进行接近反馈；

所述控制装置构建基坐标系OXYZ、第一机械臂工作坐标系O₁X₁Y₁Z₁、第二机械臂工作坐标系O₂X₂Y₂Z₂和测量坐标系O₃X₃Y₃Z₃的过程包括以下步骤：

以工作区域中心点O为坐标原点，以第一机械臂装置(1)和第二机械臂装置(3)的底盘中心点O₁、O₂连线

为X轴，垂直于

连线的方向为Y轴，根据右手法则确立Z轴，建立基坐标系OXYZ；

以第一机械臂装置(1)底盘中心点O₁为坐标原点，以

方向为X₁轴，与Y轴平行方向为Y₁轴，与Z轴平行方向为Z₁轴建立第一机械臂工具坐标系O₁X₁Y₁Z₁；

以第二机械臂装置底盘中心点O₂为坐标原点，以

方向为X₂轴，与Y轴平行方向为Y₂轴，与Z轴平行方向为Z₂轴建立第二机械臂工具坐标系O₂X₂Y₂Z₂；

以双目检测装置的双目摄像头视角中心O₃为坐标原点，与X轴平行方向为X₃轴，与Y轴平行同向的方向为Y₃轴，以

轴方向且与Z重合为Z₃轴建立测量坐标系O₃X₃Y₃Z₃；

所述第一机械臂装置(1)包括第一机械臂底盘(111)、第一固定铜柱(110)、第一多孔圆盘(109)、第一肩关节总线舵机(108)、第一工字型连接杆(107)、第一机械臂末端(106)、第一掌关节总线舵机(105)、第一腕关节总线舵机(104)、第一肘关节总线舵机(103)、第一底部连接机构(102)，所述第一机械臂底盘(111)固定于底盘装置(7)上；

所述第二机械臂装置(3)包括第二机械臂底盘(311)、第二固定铜柱(310)、第二多孔圆盘(309)、第二肩关节总线舵机(308)、第二工字型连接杆(307)、第二机械臂末端(306)、第二掌关节总线舵机(305)、第二腕关节总线舵机(304)、第二肘关节总线舵机(303)、第二底部连接机构(302)，所述第二机械臂底盘(311)固定于底盘装置(7)上；

所述控制装置包括用于控制第一机械臂装置(1)的第一机械臂舵控板(101)和用于控制第二机械臂装置(3)的第二机械臂舵控板(301)，分别通过信号线一(5)和信号线二(6)与双目检测装置(2)连接；

所述基于双目视觉的双臂协作机器人系统的控制方法，包括以下步骤：

S1，建立基坐标系OXYZ，第一、第二机械臂工具坐标系O₁X₁Y₁Z₁、O₂X₂Y₂Z₂及测量坐标系O₃X₃Y₃Z₃；

S2，对工件I(402)、工件II(403)的点云数据进行采集，经双目摄像头(202)取重心点作为中心点的方法，输出工件I(402)、工件II(403)与两个机械臂末端中心点在测量坐标系下的坐标；

S3，双目检测装置(2)实时跟踪第一、第二机械臂末端的空间位姿，建立基于双目摄像头的手眼关系模型，通过将测量坐标系下任意位置处坐标的重投影误差最小化的方法，减小第一、第二机械臂协作抓取过程中机械臂末端中心点与待抓取工件中心点之间的误差以及协作组合过程中工件I(402)与工件II(403)中心点之间的误差；步骤S3中，所述双目检测装置(2)实时跟踪第一、第二机械臂末端的空间位姿，建立基于双目摄像头的手眼关系模型的过程包括以下步骤：

S31，将双目摄像头采集的图像以M×N的矩阵形式存储，其矩阵中元素的坐标为像素坐标(m,n)，其m轴、n轴互相垂直且分别与基坐标系的X轴、Y轴平行，原点O在m-n像素坐标系中的坐标为(m₀,n₀)，每一个像素在X轴、Y轴方向上的物理尺寸分别为d_x、d_y，则基坐标系与像素坐标系之间的关系表示为：

用齐次坐标系表示为：

S32，根据双目摄像头的透视投影模型，得到如下关系：

其中双目摄像头视角中心到基坐标原点

的长为有效焦距f，(x₃,y₃,z₃)为测量坐标系上的点，则测量坐标系与基坐标系的转换关系用齐次坐标表示为：

S33，通过机械臂末端和标定板的关系推导出标定板相对于双目摄像头的位姿即双目摄像头的外参数，设O₃、O、O₁、O₂、O₁′和O₂′分别为双目摄像头、工作区域、第一机械臂末端、第二机械臂末端、第一机械臂底盘和第二机械臂底盘中心点，得到以下等式：

其中，式(5.1)对应第一机械臂装置，式(5.2)对应第二机械臂装置，T表示旋转平移矩阵，包括旋转和平移两部分，旋转部分包含3个未知数，即分别绕X、Y、Z轴的三个转角(θ_x,θ_y,θ_z)，以欧拉角表示的旋转矩阵为：

平移部分包含3个未知数(t_x,t_y,t_z)；

S34，建立基于双目摄像头观测模型的手眼关系模型：

其中，式(7.1)是第一机械臂装置对应的手眼关系模型，式(7.2)是第二机械臂装置对应的手眼关系模型，G是双目摄像头的内参数，[m n]^T、

[x₃ y₃ z₃]^T是观测数据，[m n]^T通过双目摄像头得到，

由第一机械臂末端和第二机械臂末端执行返回，

和

是待求解的模型参数；

S35，将n组观测数据作为输入数据，将第一机械臂装置第k次机械臂末端与双目摄像头之间的模型参数

与第二机械臂装置第k次机械臂末端与双目摄像头之间的模型参数

作为已知参数代入待求解的目标方程，将第一机械臂装置第k-1次机械臂底盘与双目摄像头之间模型参数

与第二机械臂装置第k-1次机械臂底盘与双目摄像头之间模型参数

作为优化的初始值，采用非线性最小二乘求解

与

优化的目标函数为：

其中，

分别代表像素坐标系上的点q以及测量坐标系下的点P；当该目标函数最小时，得到最终优化结果

否则置k＝k+1，在n组数据中筛选出最精确的观测数据作为最好的双目摄像头定位结果即测量坐标系下的坐标值；采用Levenberg-Marquardt算法求解每一步中的非线性优化问题；

S4，控制第一机械臂装置(1)和第二机械臂装置(3)对工件I(402)、工件II(403)进行双臂协作抓取与组合，完成对工件I(402)、工件II(403)的装配任务。

2.根据权利要求1所述的基于双目视觉的双臂协作机器人系统的控制方法，其特征在于，步骤S2中，对工件I(402)、工件II(403)的点云数据进行采集，经双目摄像头(202)取重心点作为中心点的方法，输出工件I(402)、工件II(403)与两个机械臂末端中心点在测量坐标系下的坐标的过程包括以下步骤：

在工作区域处放置标定板，在保证工作区域与标定板的大小与安装位置一致的前提下，采用张正友标定法标定双目相机的内参数，确定工作区域上的点在基坐标系与测量坐标系之间的一一对应关系；

获取处于工作区域的工件I(402)和工件II(403)的点云数据，由双目摄像头(202)输出测量坐标系下工件I(402)、工件II(403)中心点与机械臂末端中心点坐标。

Images