CN110640748A - 基于双目视觉的双臂机器人协调控制方法 - Google Patents

Abstract

本发明提供了一种基于双目视觉的双臂机器人协调控制方法，所述方法包括：构建双机械臂的数学模型；通过双目视觉获取待加工工件在双机械臂的坐标系中的初始位姿；设定双机械臂组合待加工工件的运动模式；根据双机械臂的数学模型、初始位姿和运动模式控制双机械臂对待加工工件进行组合；实时获取双机械臂在组合过程中受到的作用力并处理；根据处理后的作用力调整双机械臂在组合过程中的位姿。本发明能够协调控制双机械臂抓取、组合不规则物体，具有较好的实施性和协调性。

Description

基于双目视觉的双臂机器人协调控制方法

技术领域

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

背景技术

机器人被广泛应用于汽车零部件、3C电子、金属机械、五金卫浴、食品饮料、服务行业、科研试验、医疗制药等行业。而近年来，对让机器人在人类环境中对物体的操作得到了广泛的关注，随着机器人应用范围的不断扩大，各种新的操作任务和工作环境对机器人的操作能力提出了更高的要求。比如，让机器人与人类协同完成工业任务，顺应市场需求诞生了轻便灵巧的“协作机器人”。

目前，随着协作机器人变得越来越轻便和灵活，它们也开始更多的应用于服务机器人行业。但对于单臂协作机器人来说，需要搬运大型物体或不易用一只手臂持有的物体时特别困难。与单臂协作机器人相比，双臂协作机器人拥有非常多的自由度和复杂的结构，提高了机器人的灵活性和通用性。

然而，双臂协作机器人难以有效地对不规则物体进行抓取、组合动作，并且随着不规则物体形状的变化也会增加抓取、组合的难度。

发明内容

本发明旨在至少在一定程度上解决上述技术中的技术问题之一。为此，本发明的目的在于提出一种基于双目视觉的双臂机器人协调控制方法，能够协调控制双机械臂抓取、组合不规则物体，具有较好的实施性和协调性。

为达到上述目的，本发明实施例提出了一种基于双目视觉的双臂机器人协调控制方法，所述双臂机器人包括双机械臂，所述方法包括：构建所述双机械臂的数学模型；通过双目视觉获取待加工工件在所述双机械臂的坐标系中的初始位姿；设定所述双机械臂组合所述待加工工件的运动模式；根据所述双机械臂的数学模型、所述初始位姿和所述运动模式控制所述双机械臂对所述待加工工件进行组合；实时获取所述双机械臂在组合过程中受到的作用力并处理；根据处理后的所述作用力调整所述双机械臂在组合过程中的位姿。

根据本发明实施例的基于双目视觉的双臂机器人协调控制方法，通过构建双机械臂的数学模型，并通过双目视觉获取待加工工件在双机械臂的坐标系中的初始位姿，以及设定双机械臂组合待加工工件的运动模式，然后根据双机械臂的数学模型、初始位姿和运动模式控制双机械臂对待加工工件进行组合，实时获取双机械臂在组合过程中受到的作用力并处理，并根据处理后的作用力调整双机械臂在组合过程中的位姿，由此，能够协调控制双机械臂抓取、组合不规则物体，具有较好的实施性和协调性。

另外，根据本发明上述实施例提出的基于双目视觉的双臂机器人协调控制方法还可以具有如下附加的技术特征：

根据本发明的一个实施例，构建所述双机械臂的数学模型包括：通过D-H表示法建立所述双机械臂各关节的参考坐标系；确定所述双机械臂的D-H参数；根据所述D-H参数和所述参考坐标系得到所述双机械臂中相邻关节间的相对位置关系。

进一步地，所述相对位置关系的表达式为：

其中，A_i为关节i和i-1之间的相对位置关系，θ_i为关节i的旋转角，β_i为关节i和关节i-1间的扭转角，α_i为关节i的长度；d_i为关节i的偏距。

根据本发明的一个实施例，通过双目视觉获取待加工工件在所述双机械臂的坐标系中的初始位姿包括：通过单目视觉获取在世界坐标系中的所述待加工工件的顶面圆心和底面圆心在像平面上的平面坐标；通过双目视觉根据所述平面坐标得到所述顶面圆心和所述底面圆心在世界坐标系中的三维坐标并计算所述待加工工件的倾斜角度；根据所述三维坐标得到所述顶面圆心和所述底面圆心之间的高度差，以及所述顶面圆心和所述底面圆心在所述像平面上的投影距离；根据所述三维坐标、所述高度差、所述投影距离和所述倾斜角度得到所述待加工工件在所述双机械臂的坐标系中的初始位姿。

根据本发明的一个实施例，设定所述双机械臂组合所述待加工工件的运动模式包括：通过单机械臂预组合所述待加工工件并得到预组合算法；根据所述预组合算法对所述双机械臂中的第一机械臂进行示教编程，以得到所述第一机械臂的运动控制程序；通过实验得到所述双机械臂中的第二机械臂的运动控制程序，以及所述第一机械臂和所述第二机械臂的预接触位置；根据所述第一机械臂的运动控制程序、第二机械臂的运动控制程序和所述预接触位置设定所述双机械臂组合所述待加工工件的运动模式。

根据本发明的一个实施例，获取所述双机械臂在所述组合过程中受到的作用力并处理包括：通过力传感器获取所述双机械臂在组合过程中受到的作用力，并对所述作用力进行标定和解耦。

进一步地，进行标定的表达式为：

V×D＝F

F＝(F_x F_y F_z M_x M_y M_z)

V＝(v₁ v₂ v₃ v₄ v₅ v₆)

其中，F为给定力和力矩组成矩阵，V为测得的电压矩阵，D为标定矩阵。

根据本发明的一个实施例，根据处理后的所述作用力调整所述双机械臂在组合过程中的位姿包括：设定所述双机械臂接触时期望保持的作用力的阈值；根据处理后的所述作用力和所述作用力的阈值得到所述双机械臂位姿的调整值；根据所述调整值调整所述双机械臂的位姿。

附图说明

图1为本发明实施例的基于双目视觉的双臂机器人协调控制方法的流程图；

图2为本发明一个实施例的构建双机械臂的数学模型的方法的流程图；

图3为本发明一个实施例的机械臂的D-H表示法的示意图；

图4为本发明一个实施例的机械臂的D-H参数的图表；

图5为本发明一个实施例的通过双目视觉获取待加工工件初始位姿的方法的流程图；

图6为本发明一个实施例的待加工工件的倾斜角度的分解示意图；

图7为本发明一个实施例的倾斜的待加工工件的示意图；

图8为本发明一个实施例的待加工工件在双机械臂的坐标系中位姿的示意图；

图9为本发明一个实施例的设定双机械臂组合待加工工件的运动模式的方法的流程图；

图10为本发明一个实施例的根据处理后的作用力调整双机械臂在组合过程中的位姿的方法的流程图；

图11为本发明一个实施例的闭环控制系统的示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明实施例的基于双目视觉的双臂机器人协调控制方法的流程图。

如图1所示，本发明实施例的基于双目视觉的双臂机器人协调控制方法，包括步骤：

S1，构建双机械臂的数学模型。

具体地，如图2所示，步骤S1包括：

S101，通过D-H表示法建立双机械臂各关节的参考坐标系。

举例而言，当双机械臂中的任一机械臂均包括六个关节时，如图3所示，可通过D-H表示法分别为机械臂中关节1、2、3、4、5、6分别建立参考坐标系。

S102，确定双机械臂的D-H参数。

举例而言，当双机械臂中的任一机械臂均包括六个关节时，如图4所示，可分别确定每个关节的旋转角θ、扭转角β、长度α、偏角d和关节变量的范围。

S103，根据D-H参数和参考坐标系得到双机械臂中相邻关节间的相对位置关系。

其中，相对位置关系的表达式为：

其中，A_i为关节i和i-1之间的相对位置关系，θ_i为关节i的旋转角，β_i为关节i和关节i-1间的扭转角，α_i为关节i的长度；d_i为关节i的偏距。

进一步地，根据上述双机械臂的数学模型，可计算双臂机器人的运动学正逆解。

其中，运动学正解是通过机械臂各轴的角度计算机械臂末端位姿，例如，当双臂机器人任一机械臂包括六个轴，可将图4中的参数代入上述相对位置关系的表达式中，从而得到机械臂末端位姿矩阵：

其中，A₁、A₂、A₃、A₄、A₅、A₆分别为各个所述关节的相对位置关系。

其中，运动学的逆解是正解的逆过程，即通过机械臂末端的位姿得到机械臂各轴角度，具体可通过解析法，并引入双变量正切函数求出逆解。例如，可将i＝1，2，3，4，5，6分别代入上述相对位置关系的表达式中，得出A₁～A₆，然后代入上述机械臂末端位姿矩阵中，以得到机械臂末端位姿矩阵，同时已知机械臂末端的位置坐标，通过矩阵对应位置各项相等的原则推导出机械臂各轴的角度，即参数θ₁～θ₆，最后通过最短行程准则选择最优解。

S2，通过双目视觉获取待加工工件在双机械臂的坐标系中的初始位姿。

具体地，如图5所示，步骤S2包括：

S201，通过单目视觉获取在世界坐标系中的待加工工件的顶面圆心和底面圆心在像平面上的平面坐标并计算待加工工件的倾斜角度。

S202，通过双目视觉根据平面坐标得到顶面圆心和底面圆心在世界坐标系中的三维坐标。

S203，根据三维坐标得到顶面圆心和底面圆心之间的高度差，以及顶面圆心和底面圆心在像平面上的投影距离。

S204，根据三维坐标、高度差、投影距离和倾斜角度得到待加工工件在双机械臂的坐标系中的初始位姿。

举例而言，当待加工工件为螺丝和螺母时，可先通过单目视觉求取其中螺丝的顶面圆和底面圆的相对位置，进而计算出螺丝的倾斜角度，并如图6所示，将螺丝的倾斜角度进行分解。

具体地，可通过图像处理得到螺丝的顶面圆和底面圆的圆心坐标，然后通过下列公式求出螺丝的两个圆心投影在底面上的距离：

其中，(x₁，y₁)为螺丝的顶面圆圆心，(x₂，y₂)为螺丝的底面圆圆心，L为螺丝两个圆心投影到底面上的距离。

其中，L可通过对应比例换算为螺丝的实际尺寸，例如通过下列公式进行换算：

其中，L₁为螺丝两个圆心的实际距离。

进一步地，若螺丝的高度为5cm，则可通过下列公式得到角度：

其中，θ为螺丝与z轴所成角度值。

进一步地，若机械臂可绕y轴和z轴做旋转动作，则如图6所示，将倾斜角度θ分解到xoz面和yoz面上，对应得到分解角θ_x和θ_y，同时将螺丝投影至xoz平面和yoz平面上，然后对xoz平面和yoz平面上的螺丝投影分解到x轴和y轴上，可通过下列公式进行表示：

其中，L_x为螺丝在xoy面上的投影在x轴上的分量，L_y为螺丝在yoz面上的投影在y轴上的分量。

进一步地，可通过下列公式分别求出分解角θ_x和θ_y的角度：

进一步地，求解θ_x和θ_y在xoz平面和yoz平面上的偏转方向，具体地，若x₁＞x₂，θ_x偏向轴x正方向；若x₁＜x₂，θ_x偏向x轴负方向；若y₁＞y₂，θ_y偏向y轴正方向；若y₁＜y₂，θ_y偏向y轴负方向。

进一步地，通过双目视觉基于视差原理计算螺丝的三维坐标。具体通过张氏标定法得到用于拍摄螺丝的相机的内外参数，并结合上述单目视觉得到的螺丝的顶面圆心和底面圆心的平面坐标得到螺丝的顶面圆心和底面圆心的三维坐标。通过螺丝的顶面圆心和底面圆心的三维坐标可得到图7所示倾斜的螺丝的意图。

如图7所示，其中，螺丝的倾斜角度θ以及倾斜方向的计算方法可参照上述在单目视觉中计算螺丝倾斜角度与倾斜方向的计算方法，顶面圆心P₁和底面圆心P₂在像平面上的投影距离L和顶面圆心P₁和底面圆心P₂的高度差H可分别通过下列公式进行计算：

H＝z₁-z₂

其中，(x₁，y₁，z₁)为顶面圆心P₁的三维坐标，(x₂，y₂，z₂)为底面圆心P₂的三维坐标。

进一步地，根据上述螺丝的顶面圆心P₁和底面圆心P₂在像平面上的投影距离L，顶面圆心P₁和底面圆心P₂的高度差H，顶面圆心P₁和底面圆心P₂三维坐标，以及倾斜角度得到图8所示的螺丝在双机械臂的坐标系中的位姿。

S3，设定双机械臂组合待加工工件的运动模式。

具体地，如图9所示，步骤S3包括：

S301，通过单机械臂预组合待加工工件并得到预组合算法。

S302，根据预组合算法对双机械臂中的第一机械臂进行示教编程，以得到第一机械臂的运动控制程序。

S303，通过实验得到双机械臂中的第二机械臂的运动控制程序，以及第一机械臂和第二机械臂的预接触位置。

S304，根据第一机械臂的运动控制程序、第二机械臂的运动控制程序和预接触位置设定双机械臂组合待加工工件的运动模式。

举例而言，当待加工工件为配套的螺丝和螺母时，可先通过双目视觉定位螺母并控制单机械臂抓取螺母，具体通过求取螺母质心的方式定位螺母在空间中的三维位置，确定位置后引导单机械臂抓取螺母。

进一步地，可通过双目视觉定位螺丝，具体通过求取螺丝质心的方式定位螺丝在空间中的三维位置，并引导单机械臂抓取螺母运动到螺丝的正上方，例如，可引导单机械臂抓取螺母运动到螺丝的正上方40mm处。其中，若螺丝质心的三维坐标为(x，y，z)，则单机械臂抓取螺母运动到达的位置为(x，y，z+40)，并将该位置设定为预接触位置。通过预留40mm能够避免单机械臂抓取螺母快速到达螺丝位置时的直接接触，从而造成螺母与螺丝的碰撞。

进一步地，单机械臂抓取螺母从预接触位置慢速运动至接触位置，即控制螺母与螺丝接触，具体可控制单机械臂抓取螺母从预接触位置垂直向下运动，例如垂直向下运动40mm到达与螺丝接触的位置，同时还需要对设置于单机械臂末端的力传感器进行零点校准，当力传感器在单机械臂末端的运动方向上，即垂直向下的运动方向上检测到大于3N的力，则判断螺母与螺丝相互接触，同时控制单机械臂停止垂直向下的运动。

进一步地，控制单机械臂末端旋转以带动螺母转动，从而使得螺丝上扣。

进一步地，在螺丝上扣完成后，将接触力设置为零，并通过调整以保证接触力动态保持为零，直至螺母和螺丝之间的组合完成。

进一步地，验证组合任务是否完成，具体可通过控制单机械臂垂直向上运动部分距离，若螺丝已经达到上扣并旋进一部分的状态，则力传感器的示数将突增，此时表示组合任务完成。

综上所述，可得到单机械臂预组合螺丝和螺母的预组合算法，进而可将预组合算法移植到双机械臂组合螺丝和螺母的控制中。但是，单机械臂在组合螺丝和螺母的过程中，螺丝可静置在加工平面上，双机械臂组合螺丝和螺母的过程中，需要通过第一机械臂，即从机械臂抓取螺丝和第二机械臂，即主机械臂抓取螺母协调动作以完成组合，因此，在双机械臂组合螺丝和螺母的过程中，不是在垂直方向运动，而是在平行于加工平面的方向运动，即主机械臂需要一边在世界坐标系中左右移动，一边控制主机械臂的末端，即T轴在主机械臂的坐标系中运动。

具体地，可通过在控制主机械臂运动的函数的合适位置加入相应的语句，例如，当通过GALIL运动控制器控制主机械臂运动时，可通过寻找GALIL运动控制器的代码，在GALIL运动控制器调用RobotMotion()，即控制主机械臂运动的函数时，在合适的位置加入m_DMCWin.Command语句，同时设置控制主机械臂的末端，即T轴运动的参数，例如，运动步长、速度、加速度和减速度参数并通过实验进行调整，从而得到控制主机械臂运动的运动控制程序。

进一步地，通过上述单机械臂组合螺丝和螺母的过程，可知在组合螺丝和螺母的过程中，需要控制螺母到达预接触位置，而预接触位置并不在加工平面，因此无法通过双目视觉引导双机械臂到达预接触位置。

进一步地，可通过实验得到双机械臂的预接触位置，并通过示教确定预接触位置并保存。具体地，可先将双机械臂示教调整到相互对准的状态，并记录此时双机械臂各轴的角度，进而通过双机械臂的数学模型得到双机械臂对准状态的坐标，然后将双机械臂中的主机械臂向远离从机械臂的方向平移一定距离，例如40mm，并记录此时双机械臂的位置，此时双机械臂的位置即为双机械臂的预接触位置，同时记录此时双机械臂各轴的角度，然后通过运动学逆解根据各轴的角度得到各轴的位姿并保存在.jbi文件中，利用双机械臂的再现功能可直接控制双机械臂运动到预接触位置，不需要每次重新调整。

S4，根据运动模式和双机械臂的数学模型控制双机械臂对待加工工件进行组合。

S5，实时获取双机械臂在组合过程中受到的作用力并处理。

具体地，可通过力传感器实时获取双机械臂在组合过程中受到的作用力，并对作用力进行标定和解耦。

其中，标定的表达式为：

V×D＝F

F＝(F_x F_y F_z M_x M_y M_z)

V＝(v₁ v₂ v₃ v₄ v₅ v₆)

其中，F为给定力和力矩组成矩阵，V为测得的电压矩阵，D为标定矩阵。

需要说明的是，标定和解耦是一个整体过程，通过标定和解耦可减少因力传感器内部弹性结构造成的误差。

S6，根据处理后的作用力调整双机械臂在组合过程中的位姿。

具体地，如图10所示，步骤S6包括：

S601，设定双机械臂接触时期望保持的作用力的阈值。

S602，根据处理后的作用力和作用力的阈值得到双机械臂位姿的调整值。

S603，根据调整值调整双机械臂的位姿。

举例而言，在通过双机械臂组合螺丝和螺母时，可将力传感器设置于主机械臂上，如图11所示，可给定双机械臂接触时期望保持的作用力，即给定F，与力传感器实时获取并处理后反馈的作用力，即检测F’进行比较，得到差值ΔF，并将ΔF作为力控制器的输入，力控制器同时输出螺丝和螺母的相对位置ΔX作为位置控制器的输入，若位置控制器通过ΔX检测出螺丝和螺母的位置出现偏差，则将ΔX转换为机械臂各轴应调整的角度，以调整机械臂的位姿，从而使得ΔX趋向于零，在调整后通过力传感器实时检测主机械臂末端的作用力进行反馈，以形成闭环控制系统。同时，主机械臂的末端，即T轴的转动作为扰动输入到闭环控制系统。

根据本发明实施例提出的基于双目视觉的双臂机器人协调控制方法，通过构建双机械臂的数学模型，并通过双目视觉获取待加工工件在双机械臂的坐标系中的初始位姿，以及设定双机械臂组合待加工工件的运动模式，然后根据双机械臂的数学模型、初始位姿和运动模式控制双机械臂对待加工工件进行组合，实时获取双机械臂在组合过程中受到的作用力并处理，并根据处理后的作用力调整双机械臂在组合过程中的位姿，由此，能够协调控制双机械臂抓取、组合不规则物体，具有较好的实施性和协调性。

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

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (8)

1.一种基于双目视觉的双臂机器人协调控制方法，其特征在于，所述双臂机器人包括双机械臂，所述方法包括：

构建所述双机械臂的数学模型；

通过双目视觉获取待加工工件在所述双机械臂的坐标系中的初始位姿；

设定所述双机械臂组合所述待加工工件的运动模式；

根据所述双机械臂的数学模型、所述初始位姿和所述运动模式控制所述双机械臂对所述待加工工件进行组合；

实时获取所述双机械臂在组合过程中受到的作用力并处理；

根据处理后的所述作用力调整所述双机械臂在组合过程中的位姿。

2.根据权利要求1所述的基于双目视觉的双臂机器人协调控制方法，其特征在于，构建所述双机械臂的数学模型包括：

通过D-H表示法建立所述双机械臂各关节的参考坐标系；

确定所述双机械臂的D-H参数；

根据所述D-H参数和所述参考坐标系得到所述双机械臂中相邻关节间的相对位置关系。

3.根据权利要求2所述的基于双目视觉的双臂机器人协调控制方法，其特征在于，所述相对位置关系的表达式为：

其中，A_i为关节i和i-1之间的相对位置关系，θ_i为关节i的旋转角，β_i为关节i和关节i-1间的扭转角，α_i为关节i的长度；d_i为关节i的偏距。

4.根据权利要求3所述的基于双目视觉的双臂机器人协调控制方法，其特征在于，通过双目视觉获取待加工工件在所述双机械臂的坐标系中的初始位姿包括：

通过单目视觉获取在世界坐标系中的所述待加工工件的顶面圆心和底面圆心在像平面上的平面坐标；

通过双目视觉根据所述平面坐标得到所述顶面圆心和所述底面圆心在世界坐标系中的三维坐标并计算所述待加工工件的倾斜角度；

根据所述三维坐标得到所述顶面圆心和所述底面圆心之间的高度差，以及所述顶面圆心和所述底面圆心在所述像平面上的投影距离；

根据所述三维坐标、所述高度差、所述投影距离和所述倾斜角度得到所述待加工工件在所述双机械臂的坐标系中的初始位姿。

5.根据权利要求4所述的基于双目视觉的双臂机器人协调控制方法，其特征在于，设定所述双机械臂组合所述待加工工件的运动模式包括：

通过单机械臂预组合所述待加工工件并得到预组合算法；

根据所述预组合算法对所述双机械臂中的第一机械臂进行示教编程，以得到所述第一机械臂的运动控制程序；

通过实验得到所述双机械臂中的第二机械臂的运动控制程序，以及所述第一机械臂和所述第二机械臂的预接触位置；

根据所述第一机械臂的运动控制程序、第二机械臂的运动控制程序和所述预接触位置设定所述双机械臂组合所述待加工工件的运动模式。

6.根据权利要求5所述的基于双目视觉的双臂机器人协调控制方法，其特征在于，获取所述双机械臂在所述组合过程中受到的作用力并处理包括：通过力传感器获取所述双机械臂在组合过程中受到的作用力，并对所述作用力进行标定和解耦。

7.根据权利要求6所述的基于双目视觉的双臂机器人协调控制方法，其特征在于，进行标定的表达式为：

V×D＝F

F＝(F_x F_y F_z M_x M_y M_z)

V＝(v₁ v₂ v₃ v₄ v₅ v₆)

其中，F为给定力和力矩组成矩阵，V为测得的电压矩阵，D为标定矩阵。

8.根据权利要求7所述的基于双目视觉的双臂机器人协调控制方法，其特征在于，根据处理后的所述作用力调整所述双机械臂在组合过程中的位姿包括：

设定所述双机械臂接触时期望保持的作用力的阈值；

根据处理后的所述作用力和所述作用力的阈值得到所述双机械臂位姿的调整值；

根据所述调整值调整所述双机械臂的位姿。

Images