本发明涉及多机器人协同的路径规划技术领域，尤其是涉及一种双机器人协同旋转的路径规划方法和装置。
背景技术：
多机器人系统是机器人学研究的一个重要方向，相比单机器人系统能实现更复杂工况且鲁棒性更强。双机器人协同作业是多机器人系统的一个重要分支，作为双机器人协同的重要运动工况，双机器人绕中心点协同旋转可实现工件在空间中的灵巧多姿态变换，满足各种复杂作业需求。如钢板弧形弯折加工，空间三维复杂焊缝的焊接加工等。
串联机器人路径规划包含大量机器人末端执行器的姿态变换，机器人姿态描述有四元数，旋转矩阵和欧拉角三种，使用四元数表示旋转可灵活匹配标定坐标系内的任意旋转轴和旋转角度，有效避免万向节锁问题。旋转矩阵与四元数之间存在简洁的等效变换关系，在插补计算中广泛应用。
技术实现要素：
有鉴于此，有必要针对上述的问题，提供一种无差别适用的双机器人协同旋转的路径规划方法和装置，能够在双机器人协同作业的实际工况下，精确控制双机器人末端执行器相对于路径圆弧中心点执行协调旋转工况，且具有普遍适用性。
一种双机器人协同旋转的路径规划方法，应用于一双机器人协同系统，所述系统包括二机器人，所述二机器人均包含用以共同夹持待加工工件的末端执行器，所述方法用于对所述机器人末端第六轴进行空间中多角度绕中心点协同旋转运动的路径规划，所述方法包括：
在单个机器人独立建模的基础上，建立机器人平台的相互位置关系，构造具有相对坐标变换的运动学系统；
依据机器人平台布置策略，设置世界坐标系，标定两机器人的基坐标系与末端坐标系；
以两机器人各基坐标系为中心坐标，依据标准d-h参数为基本运动学参数，求取机器人末端坐标的齐次变换矩阵基本形式；
在所述机器人系统协同旋转工况上建立路径圆弧模型，设对称中心坐标系为路径基作为原点；
依据已知路径偏转角和路径圆弧半径，计算路径初始控制点的齐次变换矩阵；
在所述路径初始控制点的齐次变换矩阵中，提取三维旋转矩阵进行基于四元数的坐标旋转插补；
在所述路径初始控制点的齐次变换矩阵中，提取初始坐标向量进行基于空间几何的控制点坐标向量插补；
基于逆动力学的齐次变换矩阵转换，获取最终控制点的关节控制角度。
进一步地，在三位旋转坐标的插补过程中考虑圆弧轴向偏移量的影响，依据圆弧轴向偏移量的限制求解控制点的最大间隔角度值。
进一步地，由初始控制点的齐次变换矩阵提取三维旋转矩阵
(1)将t3转化为四元数形式的q0＝q0+q1i+q2j+q3k，其中i、j、k为虚数单位，q0、q1、q2、q3是具体变量值，设置控制点的四元数表达式为qi＝[wi,(xi,yi,zi)]t，wi是实部变量值，xi,yi,zi是虚部变量值，依据qi＝q′*q0*q′-1及四元数乘法的计算公式，代入控制点的最大圆弧最大角度，得到各旋转后控制点的四元数，转化为任意控制点处的旋转矩阵；
(2)控制点相对坐标系p的坐标偏移量同样可由各坐标轴方向上的偏移向量px，py，pz表示,将控制点的坐标向量表示为
进一步地，初始控制齐次变换矩阵的求解方法为：旋转路径相对xpzp平面偏转角度为α，路径圆弧半径为r，路径起始点坐标p0先由yp顺时针旋转90°，然后相对z轴顺时针旋转(α+90°)，再沿z轴轴向平移-rm，(0≤m<360)；p0相对p点坐标系的齐次变换矩阵矩阵表示为：
进一步地，圆弧控制点的最大角度值满足：
进一步地，初始控制点q0的四元数表示一般形式为：
四元数绕轴旋转角度为n，旋转轴的相关四元数表示为：
四元数qi＝[wi,(xi,yi,zi)]t姿态旋转的一般形式表示为：
四元数转换为旋转矩阵的公式为：
进一步地，控制点相对点p坐标系坐标向量的各元素满足关系：
进一步地，两机器人控制点关于机器人基座标的齐次变换矩阵表示为：
l为两机器人x轴方向相距，b为y轴方向相距，h1为左侧机器人的底座高度为，h2为右侧机器人的底座高度。
一种双机器人协同旋转的路径规划装置，包括机器人坐标标定模块、控制点插补模块以及逆动力学求解模块；
所述机器人坐标标定模块，用于标定单位机器人的基坐标与末端轴坐标属性，并在机器人协同系统中建立相对坐标关系；
所述控制点插补模块，用于依据机器人末端齐次变换矩阵，针对路径圆弧的插补需求，执行对二机器人的圆弧路径插补；
逆动力学求解模块，用于合并各控制点的齐次变换矩阵，求解对应六轴的关节角度变化量。
进一步地，所述控制点插补模块包括初始控制点单元、坐标旋转插补单元以及坐标向量位置插补单元；
所述初始控制点单元包括路径圆弧建模模块和初始控制点坐标系的齐次变换矩阵矩阵求解模块，用于对路径圆弧进行建模并求解初始控制点的齐次变换矩阵；
所述坐标旋转插补求单元包括圆弧轴向偏差求解模块、旋转矩阵绕轴旋转的四元数插补模块，用于计算满足圆弧轴向偏差要求的最大控制点间隔角，并据此求解控制点的坐标旋转矩阵；
所述坐标向量位置插补单元包括控制点各轴向偏移量求解模块、控制点位置插补模块，用于计算控制点相对旋转中心的轴向偏移量并生成控制点的位置向量。
本发明的双机器人协同旋转的路径规划方法和装置，有基于四元数与旋转矩阵的路径圆弧插补方法及其装置，该路径规划方法能计算出路径圆弧轨迹的插补控制点，插补路径平滑，符合关节角限制，圆度误差低。可实现双机器人协调搬运工件在空间中的多角度对中心旋转，具有普遍适用性。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明的双机器人系统的平台示意图；
图2是本发明的双机器人协同平台的简化结构示意图；
图3是本发明的路径圆弧插补流程图；
图4是本发明的路径圆弧轴向偏移量图；
图5是本发明的路径圆弧插补的控制点示意图。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂，下面将结合附图和具体的实施例对本发明的技术方案进行详细说明。需要指出的是，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供了一种双机器人协同旋转的路径规划方法，应用于一双机器人协同系统，该系统包括二机器人，二机器人均包含用以共同夹持代加工工件的末端执行器，所述方法用于对机器人末端六轴进行圆弧运动的控制点坐标插补，以实现双机器人协调夹持工件在空间中的多角度对中心旋转。具体的，本发明提供的方法包括：
二机器人相对世界坐标z轴对立放置，依据夹持工件的尺寸设定路径圆弧半径，设置路径偏转角，执行协同旋转工况。
依据机器人平台布置的相对关系及机器人标准d-h参数，获取二机器人末端基坐标的齐次变换矩阵。
分别对二机器人进行坐标旋转插补和基于空间几何的控制点坐标向量插补，生成控制点关于机器人基坐标的齐次变换矩阵。
依据逆动力学，求解控制点齐次变换矩阵对应的关节坐标角度变量。
具体的，在本方法中，将末端基坐标齐次变换矩阵拆分为三维旋转坐标与位置向量两部分。对三维旋转坐标采用基于四元数的旋转插补，对位置向量采用基于空间几何的坐标插补，并整合为控制点关于对应机器人基坐标的齐次变换矩阵。对齐次变换矩阵进行逆动力学求解，输出机器人在控制点处的各轴关节角度值。
更进一步的，在三位旋转坐标的插补过程中考虑圆弧轴向偏移量的影响，依据圆弧轴向偏移量的限制求解控制点的最大间隔角度值。
参见图1至图5，是本发明的方法应用的一个优选实施例。
图1为本发明的协同系统平台示意图，主从机器人(图中示意为主机器人1和从机器人2)分别为负载20kg与5kg的工业机器人。两机器人在空间上参照世界坐标系z轴对立布置，机器人末端装载末端执行器(本实施例中具体为气动吸盘)，同时夹持工件执行工况。
图2示出了双机器人协同平台的简化结构示意图。平面简化结构主要描述双机器人在世界坐标系中的相对位置关系及d-h参数属性。存在x轴间距l与y轴间距b。机器人1底座高度为h1，机器人2底座高度为h2；左右两侧机器人的关节连杆长度分别为ai和a’i，偏移量分别为di和d’i。
图3示出了本发明的路径圆弧插补流程图，由既定路径偏转角与工件尺寸决定的路径圆弧半径，计算初始控制点的齐次变换矩阵。
图4为路径圆弧的最大轴向偏移量示意图。当采用movej直线运动指令进行圆弧路径规划时，存在路径圆弧轴向偏差。实验平台末端采用柔性弹性气爪，弹性极限为0.01m。路径圆弧的最大轴向偏移量设置为0.005m。控制点等分路径圆弧，最大轴向偏移量产生在两个相邻控制点的中心线上。
图5为路径圆弧插补的控制点示意图。由初始控制点的齐次变换矩阵提取三维旋转矩阵
(1)将t3转化为四元数形式的q0＝q0+q1i+q2j+q3k，其中i、j、k为虚数单位，q0、q1、q2、q3是具体变量值，设置控制点的四元数表达式为qi＝[wi,(xi,yi,zi)]t，wi是实部变量值，xi,yi,zi是虚部变量值，依据qi＝q′*q0*q′-1及四元数乘法的计算公式，代入控制点的最大圆弧最大角度，得到各旋转后控制点的四元数，转化为任意控制点处的旋转矩阵。
(2)控制点相对坐标系p的坐标偏移量同样可由各坐标轴方向上的偏移向量px，py，pz表示,将控制点的坐标向量表示为
优选的，初始控制齐次变换矩阵的求解方法为：旋转路径相对xpzp平面偏转角度为α，路径圆弧半径为r，路径起始点坐标p0先由yp顺时针旋转90°，然后相对z轴顺时针旋转(α+90°)，再沿z轴轴向平移-rm，(0≤m<360)。p0相对p点坐标系的齐次变换矩阵矩阵表示为：
优选的，圆弧控制点的最大角度值满足：
优选的，初始控制点q0的四元数表示一般形式为：
优选的，四元数绕轴旋转角度为n，旋转轴的相关四元数表示为：
优选的，四元数qi＝[wi,(xi,yi,zi)]t姿态旋转的一般形式表示为：
四元数转换为旋转矩阵的公式为：
优选的，控制点相对点p坐标系坐标向量的各元素满足关系：
优选的，两机器人控制点关于机器人基座标的齐次变换矩阵可表示为：
l为两机器人x轴方向相距，b为y轴方向相距，h1为左侧机器人的底座高度为，h2为右侧机器人的底座高度。
本发明相应提供了一种双机器人协同旋转的路径规划装置，包括机器人坐标标定模块、控制点插补模块以及逆动力学求解模块；
所述机器人坐标标定模块，用于标定单位机器人的基坐标与末端轴坐标属性，并在机器人协同系统中建立相对坐标关系；
控制点插补模块，用于依据机器人末端齐次变换矩阵，针对路径圆弧的插补需求，执行对二机器人的圆弧路径插补；
所述控制点插补模块，包括初始控制点单元、坐标旋转插补单元、坐标向量位置插补单元。
所述初始控制点求解器包括路径圆弧建模模块和初始控制点坐标系的齐次变换矩阵矩阵求解模块。用于对路径圆弧进行建模并求解初始控制点的齐次变换矩阵。
所述坐标旋转插补求单元包括圆弧轴向偏差求解模块、旋转矩阵绕轴旋转的四元数插补模块。用于计算满足圆弧轴向偏差要求的最大控制点间隔角，并据此求解控制点的坐标旋转矩阵。
所述坐标向量位置插补单元包括，控制点各轴向偏移量求解模块，控制点位置插补模块。用于计算控制点相对旋转中心的轴向偏移量并生成控制点的位置向量。
逆动力学求解模块，用于合并各控制点的齐次变换矩阵，求解对应六轴的关节角度变化量。
本发明的双机器人协同旋转的路径规划方法和装置，有无差别适用于多种机器人的四元数与旋转矩阵的路径圆弧插补方法，该方法可计算出路径圆弧轨迹的插补控制点,插补路径较平滑，符合关节角限制，圆度误差低。实现了双机器人协同搬运工件在空间中多角度对中心旋转，具有普遍适用性。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。
技术特征：
1.一种双机器人协同旋转的路径规划方法，应用于一双机器人协同系统，所述系统包括二机器人，所述二机器人均包含用以共同夹持待加工工件的末端执行器，所述方法用于对所述机器人末端第六轴进行空间中多角度绕中心点协同旋转运动的路径规划，其特征在于，所述方法包括：
在单个机器人独立建模的基础上，建立机器人平台的相互位置关系，构造具有相对坐标变换的运动学系统；
依据机器人平台布置策略，设置世界坐标系，标定两机器人的基坐标系与末端坐标系；
以两机器人各基坐标系为中心坐标，依据标准d-h参数为基本运动学参数，求取机器人末端坐标的齐次变换矩阵基本形式；
在所述机器人系统协同旋转工况上建立路径圆弧模型，设对称中心坐标系为路径基作为原点；
依据已知路径偏转角和路径圆弧半径，计算路径初始控制点的齐次变换矩阵；
在所述路径初始控制点的齐次变换矩阵中，提取三维旋转矩阵进行基于四元数的坐标旋转插补；
在所述路径初始控制点的齐次变换矩阵中，提取初始坐标向量进行基于空间几何的控制点坐标向量插补；
基于逆动力学的齐次变换矩阵转换，获取最终控制点的关节控制角度。
2.根据权利要求1所述的双机器人协同旋转的路径规划方法，其特征在于，在三位旋转坐标的插补过程中考虑圆弧轴向偏移量的影响，依据圆弧轴向偏移量的限制求解控制点的最大间隔角度值。
3.根据权利要求1所述的双机器人协同旋转的路径规划方法，其特征在于，由初始控制点的齐次变换矩阵提取三维旋转矩阵
(1)将t3转化为四元数形式的q0＝q0+q1i+q2j+q3k，其中i、j、k为虚数单位，q0、q1、q2、q3是具体变量值，设置控制点的四元数表达式为qi＝[wi,(xi,yi,zi)]t，wi是实部变量值，xi,yi,zi是虚部变量值，依据qi＝q′*q0*q′-1及四元数乘法的计算公式，代入控制点的最大圆弧最大角度，得到各旋转后控制点的四元数，转化为任意控制点处的旋转矩阵；
(2)控制点相对坐标系p的坐标偏移量同样可由各坐标轴方向上的偏移向量px，py，pz表示,将控制点的坐标向量表示为
4.根据权利要求3所述的双机器人协同旋转的路径规划方法，其特征在于，初始控制齐次变换矩阵的求解方法为：旋转路径相对xpzp平面偏转角度为α，路径圆弧半径为r，路径起始点坐标p0先由yp顺时针旋转90°，然后相对z轴顺时针旋转(α+90°)，再沿z轴轴向平移-rm，(0≤m<360)；p0相对p点坐标系的齐次变换矩阵矩阵表示为：
5.根据权利要求4所述的双机器人协同旋转的路径规划方法，其特征在于，圆弧控制点的最大角度值满足：
6.根据权利要求5所述的双机器人协同旋转的路径规划方法，其特征在于，初始控制点q0的四元数表示一般形式为：
四元数绕轴旋转角度为n，旋转轴的相关四元数表示为：
四元数qi＝[wi,(xi,yi,zi)]t姿态旋转的一般形式表示为：
四元数转换为旋转矩阵的公式为：
7.根据权利要求6所述的双机器人协同旋转的路径规划方法，其特征在于，控制点相对点p坐标系坐标向量的各元素满足关系：
8.根据权利要求7所述的双机器人协同旋转的路径规划方法，其特征在于，两机器人控制点关于机器人基座标的齐次变换矩阵表示为：
l为两机器人x轴方向相距，b为y轴方向相距，h1为左侧机器人的底座高度为，h2为右侧机器人的底座高度。
9.一种双机器人协同旋转的路径规划装置，其特征在于，包括机器人坐标标定模块、控制点插补模块以及逆动力学求解模块；
所述机器人坐标标定模块，用于标定单位机器人的基坐标与末端轴坐标属性，并在机器人协同系统中建立相对坐标关系；
所述控制点插补模块，用于依据机器人末端齐次变换矩阵，针对路径圆弧的插补需求，执行对二机器人的圆弧路径插补；
逆动力学求解模块，用于合并各控制点的齐次变换矩阵，求解对应六轴的关节角度变化量。
10.根据权利要求9所述的双机器人协同旋转的路径规划装置，其特征在于，所述控制点插补模块包括初始控制点单元、坐标旋转插补单元以及坐标向量位置插补单元；
所述初始控制点单元包括路径圆弧建模模块和初始控制点坐标系的齐次变换矩阵矩阵求解模块，用于对路径圆弧进行建模并求解初始控制点的齐次变换矩阵；
所述坐标旋转插补求单元包括圆弧轴向偏差求解模块、旋转矩阵绕轴旋转的四元数插补模块，用于计算满足圆弧轴向偏差要求的最大控制点间隔角，并据此求解控制点的坐标旋转矩阵；
所述坐标向量位置插补单元包括控制点各轴向偏移量求解模块、控制点位置插补模块，用于计算控制点相对旋转中心的轴向偏移量并生成控制点的位置向量。
技术总结
本发明涉及一种双机器人协同旋转的路径规划方法和装置，所述方法用于对所述机器人末端六轴相对于所述工件的圆弧路径插补方法；包括二机器人相对世界坐标z轴对立放置，依据夹持工件的尺寸设定路径圆弧半径，设置路径偏转角，执行协同旋转工况；依据机器人平台布置的相对关系及机器人标准D‑H参数，获取二机器人末端基坐标的齐次变换矩阵；分别对二机器人进行坐标旋转插补和基于空间几何的控制点坐标向量插补，生成控制点关于机器人基坐标的齐次变换矩阵；依据逆动力学，求解控制点齐次变换矩阵对应的关节坐标角度变量。本发明能够精确控制双机器人末端执行器参照待加工工件尺寸执行各偏转角度下的圆弧路径，轨迹圆度误差小，具有普遍适用性。
技术研发人员：张弓;包翔宇;候至丞;杨文林;王建;徐征;冯伟;王卫军;韩彰秀
受保护的技术使用者：广州中国科学院先进技术研究所
技术研发日：.10.12
技术公布日：.02.07
