CN105751196A - 一种基于主从式的工业机器人协作作业方法 - Google Patents

Abstract

本发明公开了一种基于主从式的工业机器人协作作业方法，其包括协作控制、轨迹规划、作业实施三个部分，目的是为了克服单个工业机器人作业的局限性，使主从式工业机器人能够协作地完成较为复杂的作业，从而具有更高的自主性、鲁棒性、适应性。根据主从式工业机器人之间的位置和姿态约束关系，采用所述的协作控制方法、轨迹规划方法、作业实施方法，可以控制主从式工业机器人各自的运动轨迹，使它们相互协作地完成单调、重复、繁重的抓取、搬运、装配等作业。本发明的基于主从式的工业机器人协作作业方法可用于先进制造行业及其流水生产线。

Description

一种基于主从式的工业机器人协作作业方法

技术领域

本发明涉及机器人作业的技术领域，尤其是指一种基于主从式的工业机器人协作作业方法。

背景技术

目前，人们对工业机器人的要求越来越高，希望工业机器人能够代替人类做更多和更复杂的工作。为此，人们渴望研制出功能更为复杂和强大的工业机器人。但是，工业机器人本身非常复杂，如果还要在其中再增加其它更多的功能，这无疑会大大地增加工业机器人的复杂度，而且把众多功能集成到一台工业机器人中，则其可靠性会降低，效率也不一定会显著提升，反而会加大工业机器人在设计和制造及维护上的困难。更何况很多作业靠单个工业机器人根本无法完成，需要多个工业机器人协作才能完成。

多个工业机器人协作，是指通过多个工业机器人相互配合完成某些作业。由多个工业机器人组成的协作系统并不是将多个工业机器人简单的组合或叠加，而是每台工业机器人除了要完成自身的控制目标之外，还要通过相互之间的动作协作和动作组合来完成更为复杂的作业。多个工业机器人协作时，每台工业机器人采取行动时，要考虑其它工业机器人的行动，行动要同步化，以达到所有工业机器人之间协作一致地完成作业。多个工业机器人协作比单个工业机器人具有更多的优势，因为多个工业机器人协作在空间上是分布式的，在时间上是并行的，且灵活性更好，容错能力强，资源利用率高，开发成本更低。

多个工业机器人协作研究始于20世纪70年代。从那时起国外许多机构和大学对多个工业机器人协作进行了大量的研究。1997年，欧盟专门设立了一个用于搬运的多自主机器人系统项目，在该项目中对多个工业机器人协作研究给予了大力的支持。目前，德国KUKA机器人公司已经成功地在奔驰汽车生产车间里实现了由15台工业机器人共同作业的工业机器人协作系统，而且参加协作的工业机器人数量还有望不断增加。在车身焊装车间里，有大量由8～12台工业机器人组成的工业机器人协作系统，每台工业机器人被有组织地安排在一个小区域内，对车身或板件进行焊接、传递、装配和搬运等，图2所示为工业机器人协作系统。其中，每台工业机器人可以单独工作，也可以和其它工业机器人协作。并且所有的工业机器人都采用标准的KUKA机器人，它们通过100Mbit/s的高速以太网进行通信连接和数据交换。因为KUKA机器人控制系统KRC2是基于微软WindowsXP操作系统的，这也使得互联网及技术作为通信端口，在工业机器人协作系统中的应用得到了充分的发挥。

日本对多个工业机器人协作研究一直没有间断过，许多机构和大学都进行了大量的研究。迄今已经建立了一系列比较具有代表性的工业机器人协作系统。美国也采用自下而上的路线，建立了许多工业机器人协作系统。图3所示为美国Robai公司生产的Cyton双臂机器人，它的每条机械臂有8个自由度(包括机械手有一个自由度)，跟人的手臂结构类似，共有16个自由度。中国科学院沈阳自动化所针对制造装配作业，采用了分散与集中相结合的分层体系结构建立了一个工业机器人协作装配实验系统MRCAS(Multi-robotcooperativeassemblysystem)。该系统由一台普通计算机构成协作组织级，协作作业由AGV、PUMA760、PUMA562和AdeptI等工业机器人完成。MRCAS为日后深入研究多个工业机器人协作理论和方法提供了一个非常好的实验平台。清华大学智能技术与系统国家重点实验室针对存在障碍物的空间中两台工业机器人协作搬运物体的规划问题，提出了基于快速探索随机树的改进算法，并且完全按照真实的工业机器人模型，建立了工业机器人协作仿真平台，以及设计完成了运动规划仿真实验。

一般而言，多个工业机器人协作大致上可以分为主从式、分布式和混合式。目前，多个工业机器人协作研究主要围绕着控制方法、路径规划、作业方法等领域展开。控制方法是工业机器人协作系统的基础，它包括各台工业机器人之间在逻辑上和物理上的信息和控制总和。路径规划是生成工业机器人各个关节运动轨迹的方法，通过主工业机器人(主机械手)与从工业机器人(从机械手)之间的约束关系，计算出从工业机器人(从机械手)的运动轨迹。作业方法包括每台工业机器人之间的协作关系，主要是针对不同的协作任务，设计相应的约束条件及实施具体的作业分解和完成方法，这也是完成协作任务的关键所在。

总的来说，相比单个工业机器人，工业机器人协作系统有以下显著特性：①应用领域更广泛。对于有些作业，单个工业机器人根本无法完成，需要靠多个工业机器人协作去完成。②容错能力更强大。通常在工业机器人协作系统中的工业机器人具有相同的结构和功能，当有某台工业机器人出现故障时，就可以由其它工业机器人代替它完成作业。③鲁棒性能更好。工业机器人协作系统中各成员之间的相互协作可以增加系统的自由度和冗余度，消除失效点，从而使系统的鲁棒性更好。④开发成本更加低。对于一些复杂的作业，开发单个功能复杂的工业机器人远比开工业机器人协作系统的经济成本更高。⑤资源的利用率高。工业机器人协作系统不但可协作完成复杂的大型作业，而且可以在没有复杂作业时，每台工业机器人执行其工作范围内的其他作业。有复杂作业时，只需要开发一个针对特定任务的协作系统就可以使它们协作。⑥系统具有并行性。对于有些复杂的作业，可以把它们分解为一些子作业，工业机器人协作系统中的单个工业机器人就可以并行地完成不同的子作业，这样对整个工业机器人协作系统的效率也有很大的提高。

工业机器人协作系统已经成为工业机器人技术的一个主要发展方向和必然趋势，它可以更好地代替人类完成更复杂的作业，从而将人类从繁重和危险的工作中解放出来。总之，将工业机器人协作系统引入到流水生产线中，会使工业机器人具有更大的灵活性。并且工业机器人协作系统在时间和空间上都具有分布性，在信息处理、资源利用、功能实现上都具有互补性，相比单个工业机器人有许多明显的优势。因此，开发和应用工业机器人协作系统将有助于进一步地提高工业机器人的智能水平。

发明内容

本发明的目的在于克服现有技术的不足与缺点，提供一种基于主从式的工业机器人协作作业方法，克服单个工业机器人作业的局限性，使主从式工业机器人能协作地完成较为复杂的作业，从而具有较高的自主性、鲁棒性、适应性。

为实现上述目的，本发明所提供的技术方案为：一种基于主从式的工业机器人协作作业方法，通过主从式工业机器人的协作控制，以模拟人类两只手的运动，实现协作搬运、零件装配类作业，在主从式协作控制过程中，主工业机器人的运动要事先规划好，也即事先给定主工业机器人及主机械手的运动轨迹，再通过它们与从工业机器人及从机械手的位置和姿态约束关系，计算出从工业机器人及从机械手的运动轨迹；其中，主从工业机器人及主从机械手的运动约束关系根据不同的作业而不同，要针对不同的作业设计相应的约束条件，并且还要使用三维仿真软件对主从式工业机器人及主从机械手模型的所有运动轨迹进行精确的仿真控制，以确保实现主从式工业机器人及主从机械手的安全控制；所述主从式工业机器人的协作作业，包括以下三个部分：

1)协作控制

根据主从式工业机器人之间的位置和姿态约束关系来控制它们各自的运动轨迹，也即主/从工业机器人的运动要考虑到从/主工业机器人的运动，它们要相互配合以期达到高效协作完成某一特定作业的目的；针对所述的抓取、搬运、装配类作业，主从式工业机器人相互之间采取的行动要同步进行，以便它们能够协作地完成作业；

2)轨迹规划

事先给定主工业机器人的运动轨迹，再通过主工业机器人与从工业机器人之间的约束关系，计算出从工业机器人的运动轨迹，并且改进多项式运动轨迹规划，以减缓主从式工业机器人的磨损程度和简化编程；此外，主从式工业机器人及机械手的运动约束关系根据不同的协作作业而不同，针对不同的协作作业要设计相应的约束条件；

3)作业实施

先使用三维仿真软件对主从式工业机器人模型进行各种精确的三维仿真控制，验证各种协作作业的可行性，以便使主从式工业机器人控制更安全；操作者对主工业机器人操作时，要将运动指令通过通信端口传送给从工业机器人，从工业机器人接收到命令后跟踪主工业机器人运动，完成抓取、搬运、装配相关作业，同时将自己的工作状态返回给操作者。

所述协作控制包括以下三部分：

1)协作作业分解

主从式协作时，操作者对主工业机器人进行操作，并将运动指令通过通信端口传送给从工业机器人，从工业机器人接收到命令后跟踪主工业机器人运动，同时从工业机器人将自己的工作状态返回给操作者；基于主从式的工业机器人协作作业方式分为松耦合作业和紧耦合作业两种：(a)松藕合作业，分为两类：①子作业有先后顺序的作业，这种类型的作业是指主从式工业机器人执行的子作业在时间上有明显的先后顺序，有些子作业必须在某个或某些作业完成后再执行；②子作业无先后顺序的作业，这种类型的作业较常见，它们的特点是将作业分解为许多相互独立的子作业，主从式工业机器人之间不需要任何配合，只需要跟单个工业机器人控制一样去完成每个独立的子作业；(b)紧耦合作业，分为两类：①不可分紧耦合作业，这类作业无法分解成由单个工业机器人完成的一串子作业，必须由主从式工业机器人一起共同完成；②可分紧耦合作业，是指将作业分解为若干个相互关联和相互依赖的子作业；紧耦合作业的特点是，要求主从式工业机器人相互配合，紧密协作共同完成同一个作业，主从式工业机器人不能独立地执行作业，它们都受到彼此之间的运动约束；

2)虚拟现实模式

根据主从式工业机器人的协作作业要求，从工业机器人通常需要完成抓取、移动、放置作业；考虑将虚拟现实模式下，主工业机器人对从工业机器人的控制和自主抓取结合起来完成抓取作业；在作业空间里，先通过虚拟现实技术由主工业机器人引导从工业机器人靠近被抓物体，形成预抓取姿态，然后在被抓物体的局部小空间里，由从工业机器人自主地完成抓取作业；主从式工业机器人的虚拟现实模式，是指操作者在直接控制主工业机器人的运动，主工业机器人的位置和姿态运动信息通过通信端口传递给从工业机器人，使从工业机器人完成与主工业机器人相匹配的运动；同时从工业机器人与环境相交互产生的作用力也通过通信端口反馈给主工业机器人，并通过主工业机器人传递给操作者，使操作者能感知从工业机器人的操作信息，产生身临其境的感觉；整个主从式工业机器人是一个力反馈的闭环系统；

3)协作抓取规划

主从机械手抓取策略包括抓取操作定性分析、孩童抓取、基于知识库自主抓取，如下：

(a)抓取操作定性分析；影响抓取稳定性有六个因素，其中前三个因素：物体表面局部特征、手指表面局部特征、接触点间距离都会直接影响抓取稳定性；前两者影响接触点的相关曲率，因此相对而言，更小的物体表面曲率，更小的手指指面曲率，更小的接触点距离，均会有更稳定的抓取；在抓取作业时，①考虑抓取物体较平坦的部位，称其为大接触区域优先原则；②考虑使得手指张开距离更小的操作，称其为接近币心优先原则；另外三个因素：物体和手指的表面材料特性、物体质量、夹持力大小，它们与抓取稳定性的关系在于影响不稳定抓取的不稳定程度；前两者在机械手执行抓取前已经确定，所以只考虑抓取时所施加的夹持力控制；当抓取物体时，由于条件限制，无法保证稳定的抓取，但又能进行抓取作业，这时控制所施加的夹持力就显得必要，称其为不稳定夹持力控制原则；对于刚体而言，在稳定抓取时，手指施加的力不会破坏物体本身；但对于易碎或易变形物体，不仅要考虑稳定抓取，更要考虑夹持力大小控制，避免破坏物体本身，称其为具有破坏性的夹持力控制原则；因此，在对机械手抓取规划时，必须遵循以下原则：①大接触区域优先原则：为保证抓取稳定性，应优先保证手指和物体有较大的接触面积；②接近币心优先原则：为保证抓取稳定性，应优先保证两接触点与币心距离和较小；③不稳定夹持力控制原则：为了不使不稳定抓取的不稳定性恶化，在保证能完成抓取的条件下，应尽量不增加夹持力；④具有破坏性的夹持力控制原则：当夹持力增加对物体本身具有破坏性时，应保证在完成抓取的情祝下，尽量不增加夹持力；

(b)孩童抓取；机械手的智能抓取其关键在于对于被抓物体的识别；从工业机器人中加入大量的传感器，包括视觉、触觉、力觉传感器，通过多传感器融合技术，建立被抓物体的数学模型，将作业环境中的物体模块化；当从工业机器人开始自主操作时，通过定位摄像机对被抓物体进行扫描，确定目标物的位置、形状信息，然后模仿人类抓取未知物体的模式，进行“孩童抓取”；这里的“孩童抓取”是相对人类知识抓取的一种初级抓取方法，在开始时，他并不知道自己要抓的东西是什么，应该如何抓，脑子里没有该物体成熟的抓取方案；但是，通过不断尝试后是能够实现抓取作业；对于物体进行初次抓取时，必须充分利用主动感知技术获取物体的形状，材质以及质量信息，把这些信息模块化，再与抓取模式一起存入知识库中；当更换物体后，先对目标物进行识别，然后首先搜索库中的资料，如有类似，就进行知识抓取，否则，新进行“孩童抓取”，建立新的抓取模式，存入知识库中。

(c)基于知识库的从工业机器人自主抓取；建立知识库是实现从工业机器人自主抓取的必要步骤，如前所述，知识库包括抓取物的特征参数和从工业机器人实现抓取作业的动作指令；然而要使从工业机器人能实现知识抓取则必须结合抓取原则，通过推理机根据抓取物的变化推导出相应的抓取策略：①建立知识库，根据机械手所能夹持的实际情况，初步把物体分成规则形状物体和特殊物体，把规则形状物体再分成刚体和非刚体；这样通过“孩童抓取”，就能针对何种基本形状的物体对应有其特征参数及恰当抓取模式；对特殊物体则必须单独训练，作为一个独立的情况，只有满足这一特殊情祝，才运用这样的抓取模式；②产生有效抓取的推理过程，当由主工业机器人引导从工业机器人靠近被抓物体后，就转为从工业机器人自主抓取模式，由传感器获取物体几何形状、物体质量和物体表面刚度物性后，与知识库中存有的所有情况进行比较，判断是否存在有类似的情况，存在则进行从工业机器人的自主抓取，不存在则重新进行“孩童抓取”，然后判断是否是规则物体，是则进一步判断是否是刚性物体，不是规则物体则进行特殊物体抓取模式，对于规则刚性物体，则根据知识库中存在的对应抓取模式，结合尺寸变化，推导出相应的抓取模式；对应规则非刚性物体，不仅要根据物体尺寸变化，还要结合物体材质来推导相应的抓取办法；对特殊物体，则完全根据特殊物体的抓取方式，以“对号入座”方式查找其特有的抓取模式。

所述轨迹规划就是在机器人运动学的基础上，求解工业机器人各个关节的运动轨迹和机械手的运动轨迹以及这些轨迹的生成，轨迹是指工业机器人在执行作业的整个过程中，工业机器人各个关节的速度、加速度及机械手的速度和加速度，轨迹规划就是根据工业机器人执行的作业要求，计算出工业机器人的运动轨迹；通过主工业机器人就能够生成主机器人的运动轨迹，或直接给出主机器人的运动轨迹，然后根据具体的协作作业约束条件计算出从工业机器人的运动轨迹；工业机器人协作控制需要保证主从工业机器人及其机械手的运动轨迹连续且满足它们之间的约束关系；由于同时涉及关节空间轨迹规划和直角坐标系空间轨迹规划，因为在主从式工业机器人协作执行作业之前，它们需要分别抓持它们的目标和运动到规定的目标位置，即需要关节空间轨迹规划，另外，它们在协作搬运、协作装配作业中需要按照特定的路径和姿态运动，即需要直角坐标系空间轨迹规划，如下：

①关节空间轨迹规划——首先在工作空间中，对期望的路径点，用逆运动学计算将路径点转换成关节矢量角度值，然后对每个关节拟合一个光滑函数，从初始点依次通过所有路径点到达目标点，并使每一路径的各关节运动时间均相同。

②直角坐标系空间轨迹规划——直角坐标空间的路径点，指的是机械手的作业坐标相对于基坐标的位置和姿态，每一个点由6个量组成，其中3个量描述位置，另外3个量描述姿态，这些量直接由用户给定，然后根据这些量在直角坐标空间规划出要求的运动轨迹。

所述作业实施包括：

(1)协作搬运实施

1)协作搬运作业分解

主从式工业机器人协作搬运刚体，主工业机器人和从工业机器人能够同时去抓取目标刚体，抓取到刚体之后，把刚体从起始位姿搬运到目标位姿，在搬运过程中主工业机器人及主机械手与刚体之间、刚体与从工业机器人及从机械手之间不发生相对运动；

2)协作搬运运动学方程

当主从式工业机器人及主从机械手协作抓持一个刚体运动时，两台工业机器及其机械手与刚体不发生相对运动，因此，两台工业机器人及其机械手与被抓持刚体形成一个闭合的运动链，两台工业机器人及其机械手的运动要受到相应的位姿约束关系；

主工业机器人的基坐标系在基座处，用{B}表示，从工业机器人的基坐标系用{B’}表示，它们的机械手坐标系分别用{T}和{T’}表示，目标刚体质心坐标系用{G}表示；

①主工业机器人与刚体位姿约束，如下：

主工业机器人的基坐标系到目标刚体的变换矩阵^B _GT为

^B _GT＝^B _TT^T _GT(1)

式中，^B _TT为主工业机器人基坐标系{B}到机械手坐标系{T}的变换矩阵，能够通过正向运动学求得，^T _GT为机械手坐标系{T}到目标刚体质心坐标系{G}的变换，因为主工业机器人及机械手与目标刚体之间没有相对运动，所以^T _GT保持不变；

②主工业机器人与从工业机器人的位姿约束，如下：

同理能够看出主从工业机器人之间的位姿约束方程为：

^B _TT^T _GT＝^B _B’T^B’ _T’T^T’ _GT(2)

由式(2)可得

(^B _B’T^–1)^B _TT^T _GT(^T’ _GT^–1)＝^B’ _T’T(3)

因为主从工业机器人和目标刚体之间都没有相对运动，所以{T}、{T’}和{G}之间存在固定的变换关系，从而可知^T _T’T＝^T _GT(^T’ _GT^–1)是一个常数矩阵，在整个协作搬运过程中都保持不变；这个常数矩阵根据搬运开始时主从机械手的初始位姿确定；而^B _B’T^–1是主工业机器人机器人基坐标系到从工业机器人基坐标系变换的逆，即从工业机器人基坐标系到主工业机器人基坐标系的变换矩阵，也是个常数矩阵；所以运动过程中^B’ _T’T能够根据主工业机器人末端的位姿来确定，得出从工业机器人末端位姿后，通过逆向运动学求解把从工业机器人各个关节的旋转角度计算出来；

(2)协作装配实施

1)协作装配作业分解

轴孔装配是装配作业中较常见的一种，它要求主从式工业机器人在特定的时间内，以某种姿态到达指定位置；因此需要考虑工业机器人到达这些点的位置和姿态的约束关系；主从式工业机器人协作装配作业，首先需要主从机械手各自分别去抓去轴和孔，然后运动到预装配位置，最后根据运动学约束关系进行装配作业；

2)协作装配作业运动学方程

首先，根据具体的作业确定主工业机器人运行轨迹，然后再根据主从式工业机器人的位置约束关系得到从工业机器人的运行轨迹；(x₀,y₀,z₀)是主机械手坐标系，记为{T}，(x₁,y₁,z₁)是从机械手坐标系，记为{T’}，v₀、v₁分别为两个工业机器人及其机械手的运动速度，这里规定两者值相等方向相反，d₀为两个机械手坐标系原点之间的距离，l为两个被装配的零件相对一端之间的距离，主工业机器人基坐标系记为{B}；而从工业机器人基坐标系记为{B’}，以下假定：^B _TT为坐标系{B}到坐标系{T}的变换矩阵，^T _T’T为坐标系{T}到坐标系{T’}的变换矩阵，以此类推；

零件装配作业主要分为以下过程：

(a)主从工业机器人分别抓取两个被装配的零件，移动到使两个被装配的零件的轴心对齐位置，主机械手坐标系{T}到从机械手坐标系{T’}的变换关系，只需要将坐标系{T}沿y轴移动距离d₀，然后绕x轴旋转90°，就能够得到坐标系{T’}之间的变换矩阵^T _T’T

$T_{T^{\′}}^T=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& -1& d_0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]---\left(4\right)$

(b)主从式工业机器人分别以v₀和v₁的速度夹持两个被装配的零件运动到两者接触为止；坐标系{B}到{T’}的转换^B _T’T，表示为^B _B’T^B’ _T’T和^B _TT^T _T’T，于是存在下列等式关系

^B _B’T^B’ _T’T＝^B _TT^T _T’T(5)

将式(5)左右两边左乘^B _B’T^–1就能够得到从工业机器人基坐标系到从机械手坐标之间的转换关系^B’ _T’T为：

^B’ _T’T＝^B _B’T^–1B _TT^T _T’T(6)

式中，^B _B’T^–1为主从式工业机器人基坐标系之间的变换矩阵，在协作装配过程中不会改变，是已知的；^B _TT在给定主机械手坐标时，利用逆运动学计算各关节的旋转度数，然后由运动学方程⁰ _NT＝⁰ ₁T¹ ₂T² ₃T…^N–1 _NT求得^T _T’T如下

$T_{T^{\′}}^T=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& -1& 0& d\\ 0& 0& -1& 0\\ 0& 0& 0& 1\end{array}\right]---\left(7\right)$

式中，d＝d₀－(v₀+v₁)t，即每一时刻主从机械手作业坐标系原点的距离；

(c)主工业机器人夹持一个零件以v₀’的速度继续运动，从工业机器人夹持另一个零件原地旋转，从工业机器人只需要夹持零件将第6个关节旋转相应的角度就行，其它关节并不需要运动，根据此约束关系，从工业机器人关节6的旋转角度为

Δθ₆＝(v₀’t/dis)×360(8)

式中，dis是螺丝旋转一周的距离，当从工业机器人第6个关节旋转到了最大角度时，主工业机器人暂停移动，从机械手松开并旋转到起始位置，再抓紧零件重复前面的动作直到零件装配好为止；根据式(6)能够求出^B’ _T’T，由于零件位置相对从机械手的坐标系{T’}保持不变，则由^B’ _T’T能够求出零件相对于从工业机器人基坐标系{B’}的位置；因此，给定主工业机器人的运动轨迹及参数就能够根据式(6)和式(8)确定从工业机器人及其机械手的运动轨迹。

本发明与现有技术相比，具有如下优点与有益效果：

本发明涉及一种基于主从式的工业机器人协作作业方法，其包括协作控制、轨迹规划、作业实施三个部分，目的是为了克服单个工业机器人作业的局限性，使主从式工业机器人能够协作地完成较为复杂的作业，从而具有更高的自主性、鲁棒性、适应性。根据主从式工业机器人之间的位置和姿态约束关系，采用所述的协作控制方法、轨迹规划方法、作业实施方法，可以控制主从式工业机器人各自的运动轨迹，使它们相互协作地完成单调、重复、繁重的抓取、搬运、装配等作业。本发明的基于主从式的工业机器人协作作业方法可用于先进制造行业及其流水生产线。

附图说明

图1-1是主从式工业机器人协作搬运作业仿真实物示意图。

图1-2是主从式工业机器人协作抓取作业仿真实物示意图。

图1-3是主从式工业机器人协作装配作业仿真实物示意图。

图2是车身焊接机器人协同作业示意图。

图3是美国CytonII双臂机器人。

图4是虚拟现实模式系统原理图。

图5是虚拟现实模式下主工业机器人对从工业机器人的控制流程图。

图6是从工业机器人抓取策略框图。

图7是抓取推理机的推理方式图。

图8是工业机器人及机械手轨迹规划原理图。

图9是协作搬运作业流程图。

图10是两台工业机器人抓持同一刚体。

图11是协作装配作业流程图。

图12是主从式工业机器人协作装配零件示意图。

图13是控制操作界面。

图14a是主从工业机器人协作搬运作业实验仿真实物示意图之一。

图14b是主从工业机器人协作搬运作业实验仿真实物示意图之二。

图14c是主从工业机器人协作搬运作业实验仿真实物示意图之三。

图14d是主从工业机器人协作搬运作业实验仿真实物示意图之四。

图15是主工业机器人各个关节角度变化图。

图16是从工业机器人各个关节角度变化图。

图17a是主从工业机器人协作装配作业实验仿真实物示意图之一。

图17b是主从工业机器人协作装配作业实验仿真实物示意图之二。

图17c是主从工业机器人协作装配作业实验仿真实物示意图之三。

图17d是主从工业机器人协作装配作业实验仿真实物示意图之四。

图18是主工业机器人各个关节角度变化图。

图19是从工业机器人各个关节角度变化图。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

本发明所述的基于主从式的工业机器人协作作业方法，具体是通过(两台)主从式工业机器人的协作控制(如图1-1和图1-2及图1-3所示)，以模拟人类两只手的运动，实现协作搬运、零件装配等作业。在主从式协作控制方法中，主工业机器人的运动要事先规划好，也即事先给定主工业机器人及主机械手的运动轨迹，再通过它们与从工业机器人及从机械手的位置和姿态约束关系，计算出从工业机器人及从机械手的运动轨迹。其中，主从工业机器人及主从机械手的运动约束关系根据不同的作业而不同，要针对不同的作业设计相应的约束条件。并且还要使用三维仿真软件对主从式工业机器人及主从机械手模型的所有运动轨迹进行精确的仿真控制，以确保实现主从式工业机器人及主从机械手的安全控制。

所述主从式工业机器人的协作作业，包括以下三个部分：

1)协作控制

主从式工业机器人系统由操作者、主工业机器人(包含主机械手)、从工业机器人(包含从机械手)、通信端口、工作环境组成。协作控制方法根据主从式工业机器人之间的位置和姿态约束关系来控制它们各自的运动轨迹，也即主(从)工业机器人的运动要考虑到从(主)工业机器人的运动，它们要相互配合以期达到高效协作完成某一特定作业的目的。针对所述的抓取、搬运、装配等作业，主从式工业机器人相互之间采取的行动要同步进行，以便它们能够协作地完成作业。

2)轨迹规划

事先给定主工业机器人(主机械手)的运动轨迹，再通过主工业机器人(主机械手)与从工业机器人(从机械手)之间的约束关系，计算出从工业机器人(从机械手)的运动轨迹。并且改进了多项式运动轨迹规划，以减缓主从式工业机器人的磨损程度和简化编程。此外，主从式工业机器人及机械手的运动约束关系根据不同的协作作业而不同，针对不同的协作作业要设计相应的约束条件。

3)作业实施

先使用三维仿真软件3DSMax、OpenGL、VC++等对主从式工业机器人模型进行各种精确的三维仿真控制，验证各种协作作业的可行性，以便使主从式工业机器人控制更安全。操作者对主工业机器人操作时，要将运动指令通过通信端口传送给从工业机器人，从工业机器人接收到命令后跟踪主工业机器人运动，在复杂或危险环境下完成抓取、搬运、装配等相关作业，同时将自己的工作状态返回给操作者。

所述的协作控制包括以下三部分：

(1)协作作业分解

基于主从式的工业机器人协作系统一般由操作者、主工业机器人(主机械手)、通信端口、从工业机器人(从机械手)和工作环境组成。主从式协作时，操作者对主工业机器人进行操作，并将运动指令通过通信端口传送给从工业机器人，从工业机器人接收到命令后跟踪主工业机器人运动，在复杂或危险环境下完成相关作业，同时从工业机器人将自己的工作状态返回给操作者。一般来说，基于主从式的工业机器人协作作业方式可以分为松耦合作业和紧耦合作业两种：(a)松藕合作业，大致可以分为两类：①子作业有先后顺序的作业。这种类型的作业是指主从式工业机器人执行的子作业在时间上有明显的先后顺序，有些子作业必须在某个或某些作业完成后再执行。②子作业无先后顺序的作业。这种类型的作业较常见，它们的特点是可以将作业分解为许多相互独立的子作业，主从式工业机器人之间不需要任何配合，只需要跟单个工业机器人控制一样去完成每个独立的子作业就行了。这时主要应解决的问题是主从式工业机器人之间的避免碰撞和路径规划问题。(b)紧耦合作业，也可以分为两类：①不可分紧耦合作业，这类作业无法分解成可由单个工业机器人完成的一串子作业，须由主从式工业机器人一起共同完成。②可分紧耦合作业，是指可以将作业分解为若干个相互关联和相互依赖的子作业。紧耦合作业的特点是，要求主从式工业机器人相互配合，紧密协作共同完成同一个作业，主从式工业机器人不能独立地执行作业，它们都受到彼此之间的运动约束。

(2)虚拟现实模式

根据主从式工业机器人的协作作业要求，通常从工业机器人(从机械手)需要完成抓取、移动、放置作业。对于抓取而言，由于机械手本身的特点局限了它只能进行简单的抓取，但仍存在抓取接触点的选择和抓取稳定性问题。就机械手抓取本身来说动作是单一的，可是由于所抓取的对象是多种多样的，故这又决定了抓取控制过程的复杂性。受工业机器人智能化程度的限制，在一个未知的环境里，如果让从工业机器人完全自主，往往无法完成作业。所以在主从式工业机器人中，可以通过主工业机器人控制从工业机器人而实现协作作业。但是，时延问题常常会影响协作作业的可靠性和稳定性。由于时延影响，一方面主从式工业机器人在输入信号的作用下要经过时延时间系统的输出端才出现反应，此时的反馈量不是现时一刻所要求的反馈量，而是某个时延时间以前的状态量。这样就使得操作者不能实时地感受到机械手与环境的相互作用情况，从而影响了操作性能；另一方面时延造成相位滞后，对主从式工业机器人的稳定性极为不利。因此，又希望从工业机器人能够实现自主操作，以提高操作的实时性和可靠性，降低人们的劳动强度。

因此，考虑将虚拟现实模式下，主工业机器人对从工业机器人的控制和自主抓取结合起来完成抓取作业。在作业空间里，可先通过虚拟现实技术由主工业机器人(主机械手)引导从工业机器人(从机械手)靠近被抓物体，形成预抓取姿态，然后在被抓物体的局部小空间里，由从工业机器人自主地完成抓取作业。主从式工业机器人的虚拟现实模式，是指操作者在直接控制主工业机器人的运动，主工业机器人的位置和姿态等运动信息通过通信端口传递给从工业机器人，使从工业机器人完成与主工业机器人相匹配的运动；同时从工业机器人与环境相交互产生的作用力也通过通信端口反馈给主工业机器人，并通过主工业机器人传递给操作者，使操作者能感知从工业机器人的操作信息，产生身临其境的感觉。整个主从式工业机器人是一个力反馈的闭环系统，如图4所示。

当摄像机将被抓物体和作业环境的图像信息反馈到显示器上，操作者根据被抓物体所处位置控制主机械手运动，控制计算机循环检测主机械手的位置和姿态以及手指闭合位置，并通过通信端口发送给从工业机器人，从工业机器人通过对主机械手的位姿(位置和姿势)跟踪而逐渐靠近物体，并形成预抓取模式；另一方面从工业机器人在操作过程中由传感器检测到的与作业环境交互所产生的力也通过通信端口反馈给主机械手。图5所示给出了虚拟现实模式下主从式工业机器人的协作流程图。

(3)协作抓取规划

主从机械手抓取策略包括如下的抓取操作定性分析、孩童抓取、基于知识库自主抓取。

(a)抓取操作定性分析。影响抓取稳定性有六个因素，其中前三个因素：物体表面局部特征、手指表面局部特征、接触点间距离等都会直接影响抓取稳定性。前两者影响接触点的相关曲率，因此相对而言，更小的物体表面曲率，更小的手指指面曲率，更小的接触点距离，均会有更稳定的抓取。在抓取作业时，①考虑抓取物体较平坦的部位，称其为大接触区域优先原则；②考虑使得手指张开距离更小的操作，称其为接近币心优先原则。另外三个因素：物体和手指的表面材料特性、物体质量、夹持力大小，它们与抓取稳定性的关系在于影响不稳定抓取的不稳定程度。前两者在机械手执行抓取前已经确定，所以只考虑抓取时所施加的夹持力控制。当抓取物体时，由于条件限制，无法保证稳定的抓取，但又能进行抓取作业，这时控制所施加的夹持力就显得必要了，称其为不稳定夹持力控制原则。对于刚体而言，在稳定抓取时，手指施加的力不会破坏物体本身。但对于易碎或易变形物体，如玻璃杯、纸盒、鸡蛋等，不仅要考虑稳定抓取，更要考虑夹持力大小控制，避免破坏物体本身，称其为具有破坏性的夹持力控制原则。因此，在对机械手抓取规划时，必须遵循以下原则：①大接触区域优先原则：为保证抓取稳定性，应优先保证手指和物体有较大的接触面积；②接近币心优先原则：为保证抓取稳定性，应优先保证两接触点与币心距离和较小；③不稳定夹持力控制原则：为了不使不稳定抓取的不稳定性恶化，在保证能完成抓取的条件下，应尽量不增加夹持力。④具有破坏性的夹持力控制原则：当夹持力增加对物体本身具有破坏性时，应保证在完成抓取的情祝下，尽量不增加夹持力。

(b)孩童抓取。人们用手完成抓取作业时，往往是凭经验进行的。同时又通过不断的学习，不断积累使经验更加丰富，从而使双手能快捷高效地完成各种抓取操作。但是，同样的事，小孩难以做到的，大人却能做到就是这个道理。这给规划工业机器人及机械手的抓取很大的启发。机械手的智能抓取其关键在于对于被抓物体的识别。从工业机器人中加入了大量的传感器，如视觉，触觉，力觉等等之后，就可以通过多传感器融合技术，建立被抓物体的数学模型，将作业环境中的物体模块化。当从工业机器人开始自主操作时，通过定位摄像机对被抓物体进行扫描，确定目标物的位置、形状等必要信息，然后模仿人类抓取未知物体的模式，进行“孩童抓取”。这里的“孩童抓取”是相对人类知识抓取的一种初级抓取方法，犹如小孩抓物。在开始时，他并不知道自己要抓的东西是什么，应该如何抓，脑子里没有该物体成熟的抓取方案。但是，通过不断尝试后是能够实现抓取作业。对于物体进行初次抓取时，必须充分利用主动感知技术获取物体的形状，材质以及质量信息，把这些信息模块化，再与抓取模式一起存入知识库中。当更换了物体后，先对目标物进行识别，然后首先搜索库中的资料，如有类似，就进行知识抓取，否则，新进行“孩童抓取”，建立新的抓取模式，存入知识库中，该策略框图如图6所示。

通常把“孩童抓取”分成接触点选择(抓取位置选择)和夹持力大小控制两个阶段：(a)接触点选择。抓取位置选择要遵循所述的大接触区域优先原则和接近币心优先原则。假设，被抓取物是一个(x²/a²)+(y²/b²)+(z²/c²)＝1的椭圆体，手指指面是扁平的，抓取接触点为(a,0,0)和(-a,0,0)。所以主相关曲率为k_1a＝k_2a＝1/b和k_1b＝k_2b＝1/c，接触点间距离d＝2a。因此，抓取稳定性界线为b≥a和c≥a。当遵循大接触区域优先原则时，则选取椭圆体扁平部分作为接触区域，即满足了b≥a和c≥a，根据实用判别法，这样就实现了稳定抓取。又如抓取一个边长为l的立方体，立方体表面和指面都是扁平的，那么主相关曲率为：k_1a＝k_1b＝k_2a＝k_2b＝0。所以只要抓取接触点在相对面的中心上，即满足接近币心优先原则，必然满足稳定性要求。(b)夹持力大小控制。夹持力大小控制要遵循两个原则：①不稳定夹持力控制原则；②具有破坏性的夹持力控制原则。把抓取的物体分成刚体与非刚体两类，把抓取操作分成稳定抓取和不稳定抓取，这样就有四类抓取过程。稳定性的好坏在于接触点选择，由于对于一个新的物体，如果抓取没有充分的认识，只能进行“孩童抓取”，这个过程中允许失败：①对刚体稳定性抓取来说比较容易，因为对夹持力大小控制，不受两原则限制，可用尽量大的力去抓起物体；②对刚体不稳定抓取则要遵循不稳定夹持力控制原则，不能把力加得过大；③对非刚体稳定抓取须遵循具有破坏性夹持力控制原则，充分利用传感器信息，控制好力的大小；④对非刚体不稳定抓取，则要同时遵循这两个原则，但具有破坏性的夹持力控制原则优先于不稳定夹持力控制原则，要保证不抓坏物体，再进行下一步才有意义。

由主工业机器人引导从工业机器人靠近被抓物体后，就转为从工业机器人自主抓取模式。首先摄像机扫描被抓物体的侧面，根据获得的图像信息结合图像处理技术判别物体各侧面的表面情况，估算曲率大小，辨认物体的基本形状；同时计算物体的基本尺寸大小等。根据这两个特点来选择接触点位置。在抓取时，可以由触觉传感器进一步地确认抓取位置选择。然后可通过在夹持过程，力信息和位置信息结合来判断物体是否为刚体。即在物体表面施加一定力，观测物体变形情况，由此来初步判断物体的性质。总之，针对不同的情况，尝试用不同的抓取模式去实现抓取，再把相应的物体特征和成功的抓取模式记录下来存入知识库中。其中物体的特征主要表现在由视觉传感器、位移传感器和触觉传感器所得到的信息，包括物体表面形状、机械手手指张开的距离及手指与物体接触的情况；抓取模式就是实现对从工业机器人的控制，包括夹持力大小控制和工业机器人控制。

(c)基于知识库的从工业机器人自主抓取。建立知识库是实现从工业机器人自主抓取的必要步骤，如前所述，知识库包括抓取物的特征参数和从工业机器人实现抓取作业的动作指令。然而要使从工业机器人能实现知识抓取则必须结合抓取原则，通过推理机根据抓取物的变化推导出相应的抓取策略。①建立知识库。根据机械手所能夹持的实际情况，初步把物体分成规则形状物体和特殊物体。规则形状物体又可分成球体、圆柱体、立方体等，特殊物体可以是任意多面体、鸡蛋、工艺品等，把规则形状物体再分成刚体和非刚体。这样通过“孩童抓取”，就能针对何种基本形状的物体对应有其特征参数及恰当抓取模式；对特殊物体则必须单独训练，作为一个独立的情况，只有满足这一特殊情祝，才运用这样的抓取模式。②产生有效抓取的推理过程。自主抓取推理过程如图7所示，当由主工业机器人引导从工业机器人靠近被抓物体后，就转为从工业机器人自主抓取模式。由传感器获取物体几何形状、物体质量和物体表面刚度物性等后，与知识库中存有的所有情况进行比较，判断是否存在有类似的情况，存在则进行从工业机器人的自主抓取，不存在则重新进行“孩童抓取”；然后判断是否是规则物体，是则进一步判断是否是刚性物体，不是规则物体则进行特殊物体抓取模式；对于规则刚性物体，则根据知识库中存在的对应抓取模式，结合尺寸变化，推导出相应的抓取模式。对应规则非刚性物体，不仅要根据物体尺寸变化，还要结合物体材质来推导相应的抓取办法。对特殊物体，则完全根据特殊物体的抓取方式，以“对号入座”方式查找其特有的抓取模式。

本发明工业机器人协作作业的轨迹规划，如图8所示，可以通过主工业机器人(主机械手)与从工业机器人(从机械手)之间的约束关系，计算出主从式工业机器人及其机械手的运动轨迹。所述工业机器人的轨迹规划就是在机器人运动学的基础上，求解工业机器人各个关节的运动轨迹和机械手的运动轨迹以及这些轨迹的生成方法。轨迹是指工业机器人在执行作业的整个过程中，工业机器人各个关节的速度、加速度及机械手的速度和加速度。轨迹规划就是根据工业机器人执行的作业要求，计算出工业机器人的运动轨迹。首先，需要对工业机器人执行的作业及运动的路径进行描述。轨迹规划可以使工业机器人的使用更加简单方便，操作者只需要给定工业机器人的初始位姿、目标位姿或运动路线就可以，轨迹规划单元可以根据这些已知条件驱动工业机器人按照要求的轨迹运动和完成作业。

工业机器人(机械手)的轨迹规划像是一个黑箱子，如图8所示是轨迹规划框图，只需要根据作业的要求输入约束条件，就可以得到工业机器人及机械手的位姿序列，然后只需要根据这些位姿序列去计算每个关节的转动角度。规迹规划大致可以分为关节空间轨迹规划和直角坐标系空间(笛卡尔空间)轨迹规划。关节空间轨迹规划通常是将关节角度用时间函数来表示，根据工业机器人的起始和目标位姿计算出它的运动轨迹，基本上不需要考虑工业机器人及机械手运动的中间位姿。而直角坐标系空间通常是针对工业机器人及机械手的运动，要求工业机器人及机械手按照特定姿态和特定路径运动，把工业机器人及机械手运动过程的位姿、速度和加速度用时间函数来表示。

本专利申请是控制(两台)主从式工业机器人协作作业，通过主工业机器人就可以生成主机器人的运动轨迹，或直接给出主机器人的运动轨迹，然后根据具体的协作作业约束条件计算出从工业机器人的运动轨迹。工业机器人协作控制需要保证主从工业机器人及其机械手的运动轨迹连续且满足它们之间的约束关系。本专利申请将同时涉及关节空间轨迹规划和直角坐标系空间轨迹规划，因为在主从式工业机器人协作执行作业之前，它们需要分别抓持它们的目标和运动到规定的目标位置，即需要关节空间轨迹规划。另外，它们在协作搬运、协作装配等作业中需要按照特定的路径和姿态运动，即需要直角坐标系空间轨迹规划。

①关节空间法。关节空间法首先在工作空间中，对期望的路径点，用逆运动学计算将路径点转换成关节矢量角度值，然后对每个关节拟合一个光滑函数，从初始点依次通过所有路径点到达目标点，并使每一路径的各关节运动时间均相同。关节轨迹同时要满足一组约束条件，如位姿、速度、加速度与连续性等。在满足约束条件下，可选取不同类型的关节插值函数。这种方法确定的轨迹在直角坐标空间(即工作空间)中可以保证经过路径点，但是在路径点之间的轨迹形状则可能是很复杂的。这种规划轨迹方法计算比较简单，各关节函数之间是相互独立的，且不会发生机构的奇异性问题。在关节空间常用的规划方法有三次多项式函数插值法、高阶多项式插值法以及抛物线连接的线性函数插值法等。

(a)三次多项式轨迹规划算法。通常初始关节角度或初始位姿都是已知的，而且机械手的期望位姿也是已知的，可以根据机械手的位姿用逆向运动学计算与其对应的各个关节角度值。根据这些已知条件，可以用一个三次多项式来表示每个关节随时间变化的函数。确定了三次多项式的四个参数后，就可以计算出工业机器人在任意时刻的关节角度值，只需要根据这些角度去控制相应的关节旋转。所以三次多项式轨迹规划只需要为每一个关节都规划一个运动轨迹，至于机械手是如何运动的并不关心。这里求得的特解是初始位姿和目标位姿的速度都为零的情况，对于初始位姿和目标位置速度不为零，也可以计算出它们各个关节随时间变换的函数。这种方法求解出的轨迹不要求中间点经过特定的位置，如果要求工业机器人及机械手经过多个点(两个以上)时，可以将每两个点作为一段，每一段的目标位姿作为下一段的起始位姿，然后每一段的轨迹用三次多项式进行规划。三次多项式规划虽然规划后的关节速度和位置是连续的，但是它们的加速度并不一定连续。

(b)五次多项式轨迹规划算法。前面的三次多项式轨迹规划算法虽然可以使关节角的位置和速度连续，但不能使工业机器人的关节角加速度连续。为了克服三次多项式轨迹规划的这个缺点，可以把加速度作为一个约束条件，使用五次多项式轨迹规划算法。和三次多项式规划类似，可以得到五次多项式的通项公式。对于工业机器人的一般抓取作业，通常起始和目标位姿的速度和加速度都为零，这种情况可以得到五次多项式的一组特解。和三次多项式轨迹规划一样，都是根据起始和目标位姿计算出的轨迹，工业机器人及机械手的中间位姿可能会很复杂。如果要经过多个点，需要计算多个五次多项式来完成。

(c)具有中间点的关节空间轨迹规划。假如工业机器人及机械手有多个运动段，即工业机器人及机械手除了指定了起始和终点位姿外，还有很多中间点的位姿也指定了，每相邻的两个点划为一段，那么这样就构成多个运动段。为了防止工业机器人的关节在这些运动段之间时走时停，需要采用一些运动段之间的过渡性方法。通常可以利用起始点和终点以及中间点的信息，采用高次多项式来规划工业机器人的轨迹是其可以经过所有指定的点。但是这种方法需要进行大量的计算求解高次多项式方程。为了减少计算量可以使用一些低次多项式来替代高次多项式，为每相邻的两个点设计一个低次多项式，然后将这些低次多项式平滑的连在一起。通常工业机器人初始位姿的关节角度是已知的，中间点和终点的关节角可以根据逆向运动学原理计算出来，这样就得到了整个运动过程中各个点的关节角度值。为了在各运动段之间进行过渡，用每一点的边界条件来计算抛物线的系数。每一运动段的末端点作为下一运动段的初始端点，速度保持连续，这样重复计算每相邻两点之间的抛物线系数。对于每一个运动段必须检查它的加速度和速度是否超过工业机器人的限定值。

②直角坐标空间法。在直角坐标空间，路径点之间的轨迹形状往往十分复杂，它取决于机械手的运动学机构特性。在有些情况下，对机械手的轨迹形状也有一定要求，例如要求它在两点之间走一条直线，或者沿着一个圆弧运动以绕过障碍物等。这时便需要在直角坐标空间内规划机械手的运动轨迹。直角坐标空间的路径点，指的是机械手的作业坐标相对于基坐标的位置和姿态。每一个点由6个量组成，其中3个量描述位置，另外3个量描述姿态。这些量可直接由用户给定，然后根据这些量在直角坐标空间规划出要求的运动轨迹，因此它不需要首先进行逆运动学计算。但是在实际运行时，由于需要将规划好的直角坐标空间的运动轨迹转换到关节空间，这个计算量是相当大的。因此，总的来说，基于直角坐标空间的规划法，其计算量要远远大于关节空间法。

对于电弧焊或电切割等加工，不仅要求工业机器人在终点准确定位，而且要求运动轨迹具有一定的精度，即要求对工业机器人进行连续轨迹(ContinuousPath，CP)控制。即使是搬运、上下料，也要求有一些准备动作，以保证可靠的抓取与卸除，因此需要增加逼近与脱离两个位姿。同时为了避免多余的抖动与停顿，常常也要求机械手在通过这些空间点时运动是连续的。在直角坐标空间进行规划的方法有线性函数插值、圆弧插值方法等。CP运动方式有空间直线运动、空间圆弧运动及间椭圆运动等运动方式。

虽然关节空间轨迹规划可以保证工业机器人的各个关节在运动过程中保持连续和平滑，但是机械手的轨迹形状却很复杂。与关节空间轨迹规划不同的是，直角空间坐标系轨迹规划关注的是工业机器人及机械手运动的轨迹形状，往往要求工业机器人及机械手按照特定的轨迹形状运动。实际上可以用关节空间轨迹规划来实现坐标系空间轨迹规划。两种方法的主要区别在于，关节空间轨迹规划直接是对每个关节的运动进行规划，而直角坐标系空间轨迹规划需要先生成一系列的工业机器人及机械手的位姿，然后根据这些位姿用逆向运动学求解关节角，所以计算量会远远大于关节空间轨迹规划。(a)直线插补算法。空间直线轨迹规划就是给定机器人手的起始和目标位姿，要求工业机器人从起始点沿着直线运动到目标点，通常运动过程中姿态可以保持不变。所以只需要根据这两点确定的直线，求出这两点间并且在直线上其它点(插补点)的位姿。(b)圆弧插补算法。三维空间中三个不共线的点可以确定一个圆弧，而且这段圆弧在这三个点确定的平面内。所以通常需要先将空间圆弧转化为平面圆弧，再用平面圆弧插补算法求出插补点的坐标值，然后再将这些平面圆弧的坐标转换为三维空间的坐标值。因此，只需要将圆弧平面坐标系插补点的坐标值左乘变换矩阵就可以得到所有插补点在工业机器人参考坐标系的坐标值。最后根据这些坐标值用逆向运动学算法计算工业机器人各个关节的角度值。每两个相邻的插补点再用三次多项式或五次多项式轨迹规划计算每个关节的运动情况。(c)样条轨迹规划算法。工业机器人执行某些作业时，工业机器人整个运动的路径都按照作业要求给定好，如焊接、目标跟踪、喷涂等作业，要求工业机器人及机械手按照给定的曲线运动。这种情况下只需要根据给定的曲线和精度要求，在曲线上选择插补点。然后在每两个相邻的插补点之间用关节空间轨迹规划算法规划每两个插补点之间的轨迹。

对于工业机器人轨迹规划算法改进问题，虽然三次或五次多项式轨迹规划可以使工业机器人的运动轨迹平滑，但是工业机器人的各个关节可能一直在做加速或减速运动，这样一方面会使工业机器人的磨损程度比匀速运动的磨损程度大，另一方面也会加大工业机器人编程的工作量，因为需要为每一个时间点设定工业机器人的速度、加速度和运动角度等。因此，如果工业机器人的关节以匀速运动，即可以减缓工业机器人的磨损程度，又可以简化编程。本专利申请通过对三次多项式轨迹规划进行改进，使工业机器人能够在大部分时间里匀速运动以及各个关节不会急停和急加速，以减缓工业机器人的磨损程度和简化编程。具体的做法是根据起始点和终止点的位姿，计算工业机器人关节的角度变化量，再计算在规定时间内该关节的平均速度v，然后选取开始后和结束前的各一小段时间作为过渡点，分别记为A点和B点，在A、B之间以速度v匀速运动，在起点和A点之间用三次多项式轨迹规划，在B点和终点之间也用三次多项式轨迹规划。A点和B点的速度为v，加速度为0。

本发明工业机器人协作作业的作业实施，如图1-1、图1-2、图1-3所示，可以使主从工业机器人完成协作搬运、零件装配等作业，使主从工业机器人有更高的自主性和适应性。

(1)协作搬运实施方法。以两台6自由度工业机器人搬运刚体为例验证其协作策略。

1)协作搬运作业分解。(两台)主从式工业机器人协作搬运刚体，主工业机器人和从工业机器人可以同时去抓取目标刚体，抓取到刚体之后，可以把刚体从起始位姿搬运到目标位姿，在搬运过程中主工业机器人及主机械手与刚体之间、刚体与从工业机器人及从机械手之间不发生相对运动。如图9所示是协作搬运作业流程图。搬运作业属于紧耦合作业，最主要的部分是运动学计算。主从工业机器人之间有严格的约束条件，需要根据约束条件规划好它们的运动轨迹。

2)协作搬运运动学方程。如图10所示，当主从式工业机器人及主从机械手协作抓持一个刚体运动时，两台工业机器及其机械手与刚体不发生相对运动。因此，两台工业机器人及其机械手与被抓持刚体形成一个闭合的运动链，两台工业机器人及其机械手的运动要受到相应的位姿约束关系。

主工业机器人的基坐标系在基座处，用{B}表示，从工业机器人的基坐标系用{B’}表示，它们的机械手坐标系分别用{T}和{T’}表示，目标刚体质心坐标系用{G}(坐标系原点一般为刚体的质心)表示。

①主工业机器人的基坐标系到目标刚体的变换矩阵^B _GT为

^B _GT＝^B _TT^T _GT(1)

式中，^B _TT为主工业机器人基坐标系{B}到机械手坐标系{T}的变换矩阵，能够通过正向运动学求得，^T _GT为机械手坐标系{T}到目标刚体质心坐标系{G}的变换，因为主工业机器人及机械手与目标刚体之间没有相对运动，所以^T _GT保持不变；

②主工业机器人与从工业机器人的位姿约束。同理可以看出主从工业机器人之间的位姿约束方程为：

^B _TT^T _GT＝^B _B’T^B’ _T’T^T’ _GT(2)

由式(2)可得

(^B _B’T^–1)^B _TT^T _GT(^T’ _GT^–1)＝^B’ _T’T(3)

因为主从工业机器人和目标刚体之间都没有相对运动，所以{T}、{T’}和{G}之间存在固定的变换关系，从而可知^T _T’T＝^T _GT(^T’ _GT^–1)是一个常数矩阵，在整个协作搬运过程中都保持不变；这个常数矩阵根据搬运开始时主从机械手的初始位姿确定；而^B _B’T^–1是主工业机器人机器人基坐标系到从工业机器人基坐标系变换的逆，即从工业机器人基坐标系到主工业机器人基坐标系的变换矩阵，也是个常数矩阵；所以运动过程中^B’ _T’T能够根据主工业机器人末端的位姿来确定，得出从工业机器人末端位姿后，通过逆向运动学求解把从工业机器人各个关节的旋转角度计算出来；

(2)协作装配实施方法。

1)协作装配作业分解。轴孔装配是装配作业中较常见的一种，它要求主从式工业机器人在特定的时间内，以某种姿态到达指定位置。因此需要考虑工业机器人到达这些点的位置和姿态的约束关系。目前，对轴孔装配作业多采用单个工业机器人完成，对于多机器人协作装配作业并不多见，本专利申请分别以钢棒和棒套来代替轴孔模拟轴孔协作装配作业。

主从式工业机器人协作装配作业，首先需要主从机械手各自分别去抓去轴(钢棒)和孔(棒套)，然后运动到预装配位置，最后根据运动学约束关系进行装配作业。其工作流程如图11所示。从图11可以看出装配之前需要把轴(钢棒)和孔(棒套)移动到一个合适位置，使它们的轴心线重合，这需要根据具体的轴孔尺寸来确定它们的具体位置。开始装配后，主从工业机器人的机械手分别朝着对方平移，姿态不改变，也可以其中一台工业机器人保持不动，另一台平移，本专利申请采用了主从机械手都以相同的速度相向而行。

2)协作装配作业运动学方程。单个工业机器人不能高效地完成零件装配作业，所以采用主从式协作控制方式进行零件装配作业。首先，根据具体的作业确定主工业机器人运行轨迹，然后再根据主从式工业机器人的位置约束关系得到从工业机器人的运行轨迹。图12是主从式工业机器人协作装配零件的示意图。图中的(x₀,y₀,z₀)是主机械手坐标系，记为{T}，(x₁,y₁,z₁)是从机械手坐标系，记为{T’}，v₀、v₁分别为两个工业机器人及其机械手的运动速度，这里规定两者值相等方向相反，d₀为两个机械手坐标系原点之间的距离，l为被装配的零件(假定为螺丝和螺母)相对一端之间的距离。主工业机器人基坐标系记为{B}；而从工业机器人基坐标系记为{B’}。以下假定：^B _TT为坐标系{B}到坐标系{T}的变换矩阵，^T _T’T为坐标系{T}到坐标系{T’}的变换矩阵，以此类推。

零件装配作业主要分为以下过程：

(a)主从工业机器人分别抓取螺母和螺丝，移动到使螺母和螺丝的轴心对齐位置，如图12所示。可以看出主机械手坐标系{T}到从机械手坐标系{T’}的变换关系，只需要将坐标系{T}沿y轴移动距离d₀，然后绕x轴旋转90°，就可以得到坐标系{T’}之间的变换矩阵^T _T’T

$T_{T^{\′}}^T=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& -1& d_0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]---\left(4\right)$

(b)主从式工业机器人分别以v₀和v₁的速度夹持两个被装配的零件运动到两者接触为止；坐标系{B}到{T’}的转换^B _T’T，表示为^B _B’T^B’ _T’T和^B _TT^T _T’T，于是存在下列等式关系

^B _B’T^B’ _T’T＝^B _TT^T _T’T(5)

将式(5)左右两边左乘^B _B’T^–1就能够得到从工业机器人基坐标系到从机械手坐标之间的转换关系^B’ _T’T为：

^B’ _T’T＝^B _B’T^–1B _TT^T _T’T(6)

式中，^B _B’T^–1为主从式工业机器人基坐标系之间的变换矩阵，在协作装配过程中不会改变，是已知的；^B _TT在给定主机械手坐标时，利用逆运动学计算各关节的旋转度数，然后由运动学方程⁰ _NT＝⁰ ₁T¹ ₂T² ₃T…^N–1 _NT求得^T _T’T如下

$T_{T^{\′}}^T=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& -1& 0& d\\ 0& 0& -1& 0\\ 0& 0& 0& 1\end{array}\right]---\left(7\right)$

式中，d＝d₀－(v₀+v₁)t，即每一时刻主从机械手作业坐标系原点的距离；

(c)主工业机器人夹持螺母以v₀’的速度继续运动，从工业机器人夹持螺丝原地旋转。从工业机器人只需要夹持零件将第6个关节旋转相应的角度就行，其它关节并不需要运动，根据此约束关系，从工业机器人关节6的旋转角度为

Δθ₆＝(v₀’t/dis)×360(8)

式中，dis是螺丝旋转一周的距离，当从工业机器人第6个关节旋转到了最大角度时，主工业机器人暂停移动，从机械手松开并旋转到起始位置，再抓紧零件重复前面的动作直到零件装配好为止；根据式(6)能够求出^B’ _T’T，由于零件位置相对从机械手的坐标系{T’}保持不变，则由^B’ _T’T能够求出零件相对于从工业机器人基坐标系{B’}的位置；因此，给定主工业机器人的运动轨迹及参数就能够根据式(6)和式(8)确定从工业机器人及其机械手的运动轨迹。

实验所使用两台6自由度工业机器人，以及PC机和摄像头等。在Windows7操作系统下，用VC++、3DSMax和OpenGL开发仿真和控制软件。图13所示是控制操作界面。

实施例1——协作搬运实验

协作搬运实验是先分别控制主从式工业机器人去抓取目标刚体，然后控制主工业机器人运动，并且驱使从工业机器人协调与主工业机器人一起搬运刚体，具体步骤如下：①把主从式工业机器人的所有硬件连接好；②启动仿真和控制软件；③选择控制模式(控制模式和仿真模式)和协作方式；④控制主工业机器人去抓取钢棒；⑤控制从工业机器人去抓取钢棒；⑥控制主从工业机器人搬运钢棒到目标位置；⑦主工业机器人和从工业机器人分别松开机械手；⑧主从工业机器人均复位；⑨重复步骤④～⑧多次，并且主从式工业机器人合作搬运刚体时，每次选择不同的路径到达目标位置。按照所述步骤对主从式工业机器人进行协作控制，就可以完成协作搬运作业。图14所示是其中一次协作搬运作业实验效果图，图14(a)为主从工业机器人初始状态，图14(b)为主从工业机器人都抓取到刚体时的效果图，图14(c)为搬运过程中某个位姿的效果图，图14(d)为将钢棒搬到目的地的效果图。

协作搬运刚体整个过程中，主从工业机器人的各个关节角度变化如图15和图16所示，前面50秒是主从工业机器人去抓取刚体的运动，采用了改进的轨迹规划算法，主从工业机器人的关节基本都为匀速运动，可见改进的轨迹规划算法对工业机器人的磨损和编程都带来了利益。50秒之后是合作搬运的关节运动轨迹，其中在130秒到150秒对钢棒的位姿进行了一些微小调整。从实验结果和各个关节的运动情况可以看出，主从式工业机器人协作搬运可以实现。在工业机器人在搬运过程中，主从机械手和钢棒之间都无相对运动。而且可以看到主从工业机器人的协作搬运比单个工业机器人具有更大优势，因为单个工业机器人无法搬运大型重物，尤其是在工业应用中，主从工业机器人协作搬运有广阔的应用空间。

实施例2——协作装配实验

协作装配实验根据其任务分解大致分为两步，一是抓取目标，二是装配。首先，分别控制两台工业机器人先去抓取轴和孔(这里用钢棒和帮套代替)，并分别将轴和孔移动到预装配位姿，然后按照轴孔尺寸和装配速度完成轴孔装配任务，具体实验步骤如下：①选择控制模式(控制模式和仿真模式)和协作方式。②控制主工业机器人去抓取钢棒。③控制从工业机器人去抓取棒套。④主从工业机器人分别运动到预装配位姿。⑤开始装配轴和孔。⑥将装配好的轴孔放到目标位姿。⑦主从工业机器人均复位。⑧重复步骤②～⑦多次，并且在装配时，选择不同的速度和从工业机器人不动等多种方式进行装配。

如图17所示是协作装配实验结果，实验效果图清晰地展示了整个协作装配过程。其中图17(a)为主从工业机器人分别抓取轴和孔的过程，图17(b)为协作装配的预装配位姿，图17(c)为协作装配过程，图17(d)为协作装配任务已经完成，得到了一个装配好的轴孔零件。

在整个协作装配过程中，主从工业机器人的各个关节角度变化情况如图18和图19所示。前20秒为抓取目标的运动轨迹，20～40秒为运动到预装配位姿的轨迹，40秒以后为协作装配的运动轨迹，因为在实验时，主从工业机器人均放在一个面对面的对称位置，所以装配时它们的关节运动轨迹也几乎相同。

在实验中，还跟单个工业机器人进行轴孔装配作了比较，在单个工业机器人轴孔装配中，必须要先把孔固定好，然后工业机器人根据空的位置进行装配。而在协作装配作业中，不需要先把孔固定好，只需要主从式工业机器人分别去抓取轴和孔并协作，就能完成装配任务。而且主从式工业机器人协作装配的速度要比单个工业机器人的装配要快一倍，因为协作装配的主从式工业机器人都可以运动。从实验结果和主从式工业机器人的各个关节运动情况可以看出，协作装配实施方法可以实现协调装配。

以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (4)

1.一种基于主从式的工业机器人协作作业方法，其特征在于：通过主从式工业机器人的协作控制，以模拟人类两只手的运动，实现协作搬运、零件装配类作业，在主从式协作控制过程中，主工业机器人的运动要事先规划好，也即事先给定主工业机器人及主机械手的运动轨迹，再通过它们与从工业机器人及从机械手的位置和姿态约束关系，计算出从工业机器人及从机械手的运动轨迹；其中，主从工业机器人及主从机械手的运动约束关系根据不同的作业而不同，要针对不同的作业设计相应的约束条件，并且还要使用三维仿真软件对主从式工业机器人及主从机械手模型的所有运动轨迹进行精确的仿真控制，以确保实现主从式工业机器人及主从机械手的安全控制；所述主从式工业机器人的协作作业，包括以下三个部分：

1)协作控制

根据主从式工业机器人之间的位置和姿态约束关系来控制它们各自的运动轨迹，也即主/从工业机器人的运动要考虑到从/主工业机器人的运动，它们要相互配合以期达到高效协作完成某一特定作业的目的；针对所述的抓取、搬运、装配类作业，主从式工业机器人相互之间采取的行动要同步进行，以便它们能够协作地完成作业；

2)轨迹规划

事先给定主工业机器人的运动轨迹，再通过主工业机器人与从工业机器人之间的约束关系，计算出从工业机器人的运动轨迹，并且改进多项式运动轨迹规划，以减缓主从式工业机器人因运动轨迹规划曲线不平滑而造成关节和减速机齿轮抖动导致的磨损程度和简化编程；此外，主从式工业机器人及机械手的运动约束关系根据不同的协作作业而不同，针对不同的协作作业要设计相应的约束条件；

3)作业实施

先使用三维仿真软件对主从式工业机器人模型进行各种精确的三维仿真控制，验证各种协作作业的可行性，以便使主从式工业机器人控制更安全；操作者对主工业机器人操作时，要将运动指令通过通信端口传送给从工业机器人，从工业机器人接收到命令后跟踪主工业机器人运动，完成抓取、搬运、装配相关作业，同时将自己的工作状态返回给操作者。

2.根据权利要求1所述的一种基于主从式的工业机器人协作作业方法，其特征在于，所述协作控制包括以下三部分：

1)协作作业分解

主从式协作时，操作者对主工业机器人进行操作，并将运动指令通过通信端口传送给从工业机器人，从工业机器人接收到命令后跟踪主工业机器人运动，同时从工业机器人将自己的工作状态返回给操作者；基于主从式的工业机器人协作作业方式分为松耦合作业和紧耦合作业两种：(a)松藕合作业，分为两类：①子作业有先后顺序的作业，这种类型的作业是指主从式工业机器人执行的子作业在时间上有明显的先后顺序，有些子作业必须在某个或某些作业完成后再执行；②子作业无先后顺序的作业，这种类型的作业较常见，它们的特点是将作业分解为许多相互独立的子作业，主从式工业机器人之间不需要任何配合，只需要跟单个工业机器人控制一样去完成每个独立的子作业；(b)紧耦合作业，分为两类：①不可分紧耦合作业，这类作业无法分解成由单个工业机器人完成的一串子作业，必须由主从式工业机器人一起共同完成；②可分紧耦合作业，是指将作业分解为若干个相互关联和相互依赖的子作业；紧耦合作业的特点是，要求主从式工业机器人相互配合，紧密协作共同完成同一个作业，主从式工业机器人不能独立地执行作业，它们都受到彼此之间的运动约束；

2)虚拟现实模式

根据主从式工业机器人的协作作业要求，从工业机器人通常需要完成抓取、移动、放置作业；考虑将虚拟现实模式下，主工业机器人对从工业机器人的控制和自主抓取结合起来完成抓取作业；在作业空间里，先通过虚拟现实技术由主工业机器人引导从工业机器人靠近被抓物体，形成预抓取姿态，然后在被抓物体的局部小空间里，由从工业机器人自主地完成抓取作业；主从式工业机器人的虚拟现实模式，是指操作者在直接控制主工业机器人的运动，主工业机器人的位置和姿态运动信息通过通信端口传递给从工业机器人，使从工业机器人完成与主工业机器人相匹配的运动；同时从工业机器人与环境相交互产生的作用力也通过通信端口反馈给主工业机器人，并通过主工业机器人传递给操作者，使操作者能感知从工业机器人的操作信息，产生身临其境的感觉；整个主从式工业机器人是一个力反馈的闭环系统；

3)协作抓取规划

主从机械手抓取策略包括抓取操作定性分析、孩童抓取、基于知识库自主抓取，如下：

(a)抓取操作定性分析；影响抓取稳定性有六个因素，其中前三个因素：物体表面局部特征、手指表面局部特征、接触点间距离都会直接影响抓取稳定性；前两者影响接触点的相关曲率，因此相对而言，更小的物体表面曲率，更小的手指指面曲率，更小的接触点距离，均会有更稳定的抓取；在抓取作业时，①考虑抓取物体较平坦的部位，称其为大接触区域优先原则；②考虑使得手指张开距离更小的操作，称其为接近币心优先原则；另外三个因素：物体和手指的表面材料特性、物体质量、夹持力大小，它们与抓取稳定性的关系在于影响不稳定抓取的不稳定程度；前两者在机械手执行抓取前已经确定，所以只考虑抓取时所施加的夹持力控制；当抓取物体时，由于条件限制，无法保证稳定的抓取，但又能进行抓取作业，这时控制所施加的夹持力就显得必要，称其为不稳定夹持力控制原则；对于刚体而言，在稳定抓取时，手指施加的力不会破坏物体本身；但对于易碎或易变形物体，不仅要考虑稳定抓取，更要考虑夹持力大小控制，避免破坏物体本身，称其为具有破坏性的夹持力控制原则；因此，在对机械手抓取规划时，必须遵循以下原则：①大接触区域优先原则：为保证抓取稳定性，应优先保证手指和物体有较大的接触面积；②接近币心优先原则：为保证抓取稳定性，应优先保证两接触点与币心距离和较小；③不稳定夹持力控制原则：为了不使不稳定抓取的不稳定性恶化，在保证能完成抓取的条件下，应尽量不增加夹持力；④具有破坏性的夹持力控制原则：当夹持力增加对物体本身具有破坏性时，应保证在完成抓取的情祝下，尽量不增加夹持力；

(b)孩童抓取；机械手的智能抓取其关键在于对于被抓物体的识别；从工业机器人中加入大量的传感器，包括视觉、触觉、力觉传感器，通过多传感器融合技术，建立被抓物体的数学模型，将作业环境中的物体模块化；当从工业机器人开始自主操作时，通过定位摄像机对被抓物体进行扫描，确定目标物的位置、形状信息，然后模仿人类抓取未知物体的模式，进行“孩童抓取”；这里的“孩童抓取”是相对人类知识抓取的一种初级抓取方法，在开始时，他并不知道自己要抓的东西是什么，应该如何抓，脑子里没有该物体成熟的抓取方案；但是，通过不断尝试后是能够实现抓取作业；对于物体进行初次抓取时，必须充分利用主动感知技术获取物体的形状，材质以及质量信息，把这些信息模块化，再与抓取模式一起存入知识库中；当更换物体后，先对目标物进行识别，然后首先搜索库中的资料，如有类似，就进行知识抓取，否则，新进行“孩童抓取”，建立新的抓取模式，存入知识库中；

(c)基于知识库的从工业机器人自主抓取；建立知识库是实现从工业机器人自主抓取的必要步骤，如前所述，知识库包括抓取物的特征参数和从工业机器人实现抓取作业的动作指令；然而要使从工业机器人能实现知识抓取则必须结合抓取原则，通过推理机根据抓取物的变化推导出相应的抓取策略：①建立知识库，根据机械手所能夹持的实际情况，初步把物体分成规则形状物体和特殊物体，把规则形状物体再分成刚体和非刚体；这样通过“孩童抓取”，就能针对何种基本形状的物体对应有其特征参数及恰当抓取模式；对特殊物体则必须单独训练，作为一个独立的情况，只有满足这一特殊情祝，才运用这样的抓取模式；②产生有效抓取的推理过程，当由主工业机器人引导从工业机器人靠近被抓物体后，就转为从工业机器人自主抓取模式，由传感器获取物体几何形状、物体质量和物体表面刚度物性后，与知识库中存有的所有情况进行比较，判断是否存在有类似的情况，存在则进行从工业机器人的自主抓取，不存在则重新进行“孩童抓取”，然后判断是否是规则物体，是则进一步判断是否是刚性物体，不是规则物体则进行特殊物体抓取模式，对于规则刚性物体，则根据知识库中存在的对应抓取模式，结合尺寸变化，推导出相应的抓取模式；对应规则非刚性物体，不仅要根据物体尺寸变化，还要结合物体材质来推导相应的抓取办法；对特殊物体，则完全根据特殊物体的抓取方式，以“对号入座”方式查找其特有的抓取模式。

3.根据权利要求1所述的一种基于主从式的工业机器人协作作业方法，其特征在于：所述轨迹规划就是在机器人运动学的基础上，求解工业机器人各个关节的运动轨迹和机械手的运动轨迹以及这些轨迹的生成，轨迹是指工业机器人在执行作业的整个过程中，工业机器人各个关节的速度、加速度及机械手的速度和加速度，轨迹规划就是根据工业机器人执行的作业要求，计算出工业机器人的运动轨迹；通过主工业机器人就能够生成主机器人的运动轨迹，或直接给出主机器人的运动轨迹，然后根据具体的协作作业约束条件计算出从工业机器人的运动轨迹；工业机器人协作控制需要保证主从工业机器人及其机械手的运动轨迹连续且满足它们之间的约束关系；由于同时涉及关节空间轨迹规划和直角坐标系空间轨迹规划，因为在主从式工业机器人协作执行作业之前，它们需要分别抓持它们的目标和运动到规定的目标位置，即需要关节空间轨迹规划，另外，它们在协作搬运、协作装配作业中需要按照特定的路径和姿态运动，即需要直角坐标系空间轨迹规划，如下：

①关节空间轨迹规划——首先在工作空间中，对期望的路径点，用逆运动学计算将路径点转换成关节矢量角度值，然后对每个关节拟合一个光滑函数，从初始点依次通过所有路径点到达目标点，并使每一路径的各关节运动时间均相同；

②直角坐标系空间轨迹规划——直角坐标空间的路径点，指的是机械手的作业坐标相对于基坐标的位置和姿态，每一个点由6个量组成，其中3个量描述位置，另外3个量描述姿态，这些量直接由用户给定，然后根据这些量在直角坐标空间规划出要求的运动轨迹。

4.根据权利要求1所述的一种基于主从式的工业机器人协作作业方法，其特征在于，所述作业实施包括：

(1)协作搬运实施

1)协作搬运作业分解

主从式工业机器人协作搬运刚体，主工业机器人和从工业机器人能够同时去抓取目标刚体，抓取到刚体之后，把刚体从起始位姿搬运到目标位姿，在搬运过程中主工业机器人及主机械手与刚体之间、刚体与从工业机器人及从机械手之间不发生相对运动；

2)协作搬运运动学方程

当主从式工业机器人及主从机械手协作抓持一个刚体运动时，两台工业机器及其机械手与刚体不发生相对运动，因此，两台工业机器人及其机械手与被抓持刚体形成一个闭合的运动链，两台工业机器人及其机械手的运动要受到相应的位姿约束关系；

主工业机器人的基坐标系在基座处，用{B}表示，从工业机器人的基坐标系用{B’}表示，它们的机械手坐标系分别用{T}和{T’}表示，目标刚体质心坐标系用{G}表示；

①主工业机器人与刚体位姿约束，如下：

主工业机器人的基坐标系到目标刚体的变换矩阵^B _GT为

^B _GT＝^B _TT^T _GT(1)

式中，^B _TT为主工业机器人基坐标系{B}到机械手坐标系{T}的变换矩阵，能够通过正向运动学求得，^T _GT为机械手坐标系{T}到目标刚体质心坐标系{G}的变换，因为主工业机器人及机械手与目标刚体之间没有相对运动，所以^T _GT保持不变；

②主工业机器人与从工业机器人的位姿约束，如下：

同理能够看出主从工业机器人之间的位姿约束方程为：

^B _TT^T _GT＝^B _B’T^B’ _T’T^T’ _GT(2)

由式(2)可得

(^B _B’T^–1)^B _TT^T _GT(^T’ _GT^–1)＝^B’ _T’T(3)

因为主从工业机器人和目标刚体之间都没有相对运动，所以{T}、{T’}和{G}之间存在固定的变换关系，从而可知^T _T’T＝^T _GT(^T’ _GT^–1)是一个常数矩阵，在整个协作搬运过程中都保持不变；这个常数矩阵根据搬运开始时主从机械手的初始位姿确定；而^B _B’T^–1是主工业机器人机器人基坐标系到从工业机器人基坐标系变换的逆，即从工业机器人基坐标系到主工业机器人基坐标系的变换矩阵，也是个常数矩阵；所以运动过程中^B’ _T’T能够根据主工业机器人末端的位姿来确定，得出从工业机器人末端位姿后，通过逆向运动学求解把从工业机器人各个关节的旋转角度计算出来；

(2)协作装配实施

1)协作装配作业分解

轴孔装配是装配作业中较常见的一种，它要求主从式工业机器人在特定的时间内，以某种姿态到达指定位置；因此需要考虑工业机器人到达这些点的位置和姿态的约束关系；主从式工业机器人协作装配作业，首先需要主从机械手各自分别去抓去轴和孔，然后运动到预装配位置，最后根据运动学约束关系进行装配作业；

2)协作装配作业运动学方程

首先，根据具体的作业确定主工业机器人运行轨迹，然后再根据主从式工业机器人的位置约束关系得到从工业机器人的运行轨迹；(x₀,y₀,z₀)是主机械手坐标系，记为{T}，(x₁,y₁,z₁)是从机械手坐标系，记为{T’}，v₀、v₁分别为两个工业机器人及其机械手的运动速度，这里规定两者值相等方向相反，d₀为两个机械手坐标系原点之间的距离，l为两个被装配的零件相对一端之间的距离，主工业机器人基坐标系记为{B}；而从工业机器人基坐标系记为{B’}，以下假定：^B _TT为坐标系{B}到坐标系{T}的变换矩阵，^T _T’T为坐标系{T}到坐标系{T’}的变换矩阵，以此类推；

零件装配作业主要分为以下过程：

(a)主从工业机器人分别抓取两个被装配的零件，移动到使两个被装配的零件的轴心对齐位置，主机械手坐标系{T}到从机械手坐标系{T’}的变换关系，只需要将坐标系{T}沿y轴移动距离d₀，然后绕x轴旋转90°，就能够得到坐标系{T’}之间的变换矩阵^T _T’T

(b)主从式工业机器人分别以v₀和v₁的速度夹持两个被装配的零件运动到两者接触为止；坐标系{B}到{T’}的转换^B _T’T，表示为^B _B’T^B’ _T’T和^B _TT^T _T’T，于是存在下列等式关系

^B _B’T^B’ _T’T＝^B _TT^T _T’T(5)

将式(5)左右两边左乘^B _B’T^–1就能够得到从工业机器人基坐标系到从机械手坐标之间的转换关系^B’ _T’T为：

^B’ _T’T＝^B _B’T^–1B _TT^T _T’T(6)

式中，^B _B’T^–1为主从式工业机器人基坐标系之间的变换矩阵，在协作装配过程中不会改变，是已知的；^B _TT在给定主机械手坐标时，利用逆运动学计算各关节的旋转度数，然后由运动学方程⁰ _NT＝⁰ ₁T¹ ₂T² ₃T…^N–1 _NT求得^T _T’T如下

式中，d＝d₀－(v₀+v₁)t，即每一时刻主从机械手作业坐标系原点的距离；

(c)主工业机器人夹持一个零件以v₀’的速度继续运动，从工业机器人夹持另一个零件原地旋转，从工业机器人只需要夹持零件将第6个关节旋转相应的角度就行，其它关节并不需要运动，根据此约束关系，从工业机器人关节6的旋转角度为

Δθ₆＝(v₀’t/dis)×360(8)

式中，dis是螺丝旋转一周的距离，当从工业机器人第6个关节旋转到了最大角度时，主工业机器人暂停移动，从机械手松开并旋转到起始位置，再抓紧零件重复前面的动作直到零件装配好为止；根据式(6)能够求出^B’ _T’T，由于零件位置相对从机械手的坐标系{T’}保持不变，则由^B’ _T’T能够求出零件相对于从工业机器人基坐标系{B’}的位置；因此，给定主工业机器人的运动轨迹及参数就能够根据式(6)和式(8)确定从工业机器人及其机械手的运动轨迹。