一种双机器人协作装配传爆管和引信本体的轨迹规划方法

1.本发明属于军工装备精细装配技术领域，具体涉及一种双机器人协作装配传爆管与引信的轨迹规划方法，适用于通过双机器人协作进行导弹传爆管和引信本体装配的机器人装配轨迹规划。


背景技术：

2.导弹内的传爆管与引信本体属于易燃易爆的危险物品，其装配方式受到诸多限制，现在引信本体和传爆管的装配工序依然依靠操作人员手工装配，劳动强度大，效率低，且危险系数较高，所以导致产品的良品率不高。因此，实现传爆管与引信本体自动装配是目前装配自动化装配行业的研究重点。
3.双机器人系统在执行装配任务时，需要两台机器人相互协调配合，因此轨迹是复杂多变的，这使得双机器人在示教方面具有很大的难度。传统的工业机器人使用示教器指定一系列轨迹点或以拖动机器人末端的方式进行示教，这需要对机器人每个关节动作细节都仔细的规定，通常用于对于单机器人的示教。若采用此方法应用于双机器人协作，则在示教时不仅要考虑每台机器人自身的动作，还必须考虑到双臂的配合时机、在运动过程中双臂所夹持的物体是否会发生碰撞，以及运动到死点等问题。因此，将上述动作级的示教方法应用于双机器人协调装配无论在效率方面还是可行性方面都不理想。


技术实现要素：

4.为了解决现有技术存在的上述问题，本发明提供一种双机器人协作装配传爆管和引信本体的轨迹规划方法，针对传爆管与引信本体的双机器人协作装配，该方法能解决双机器人协作装配时轨迹规划问题。
5.本发明的目的是通过以下技术方案实现的：
6.一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其应用于双机器人协作装配系统，所述双机器人协作装配系统包括机器人模块以及机器视觉系统模块；所述机器人模块包括两台六轴工业机器人，每个六轴工业机器人末端安装有一个夹爪，分别用于抓取并夹持传爆管和引信本体；所述机器视觉模块包括两个单目相机，两个单目相机分别固定在两个六轴工业机器人的夹爪上；
7.所述轨迹规划方法包括：
8.s1.当六轴工业机器人末端执行器在协调装配区外时，采用对称控制策略，对两个六轴工业机器人分别进行轨迹规划：
9.步骤一、采用eye_in_hand手眼标定布局形式，将单目相机安装在六轴工业机器人末端，利用标定板进行标定；
10.步骤二、标定完成之后，通过示教器将六轴工业机器人移至待装配工件上方，将此位置设置为路点1；
11.步骤三、控制单目相机进行图像采集，经过图像预处理，识别并确定待装配工件的
位姿，并确立抓取点；
12.步骤四、通过视觉系统引导六轴工业机器人到抓取点进行工件抓取，抓取完成后，六轴工业机器人通过示教法移动至步骤二所设定的路点1；
13.步骤五、通过示教器控制两个六轴工业机器人分别将传爆管与引信本体移动到协作装配起点；
14.s2.当六轴工业机器人末端执行器在协调装配区内时，采用主从对称策略，进行双六轴工业机器人协作装配的轨迹规划：
15.步骤六、两台六轴工业机器人分别将传爆管与引信本体移动到使两者轴心对齐的协调装配起始位置；
16.步骤七、主机器人和从机器人分别以v0和v1的速度夹持传爆管与引信本体运动到两者接触位置；
17.步骤八、主机器人夹持传爆管以的速度继续运动，从机器人夹持引信本体原地旋转，直至将引信本体拧紧。
18.进一步地，所述步骤一中采用多点标定法进行六轴工业机器人与单目相机视觉传感器的标定：
19.六轴工业机器人带动单目相机移动到指定路点之后，开始标定，将九宫格标定板放在待识别区，将探针安装在六轴工业机器人末端，示教机器人末端的标定顶尖到达标定板中预先设定好的空间固定点，标定获得机器人工具坐标系在机器人基坐标系下的转换坐标并记录该组空间固定点在机器人基坐标系下的坐标q；在视觉传感器中获取空间固定点坐标p，根据公式求解传感器坐标系与机器人工具坐标系的转换矩阵即完成标定。
20.进一步地，所述步骤三中，采用中值滤波对单目像机采集的图像进行滤波处理，滤除背景中孤立的噪声点和离群点，保留待装配组件的图像边缘信息；二维中值滤波表达式为：
21.g(x,y)＝med[f(x-i,y-i)]
[0022]
式中，(i,j)∈m,m是所取领域中像素的坐标；f(x,y),g(x,y)分别为滤波处理前与滤波处理后的图像。
[0023]
进一步地，所述步骤五中，通过示教器引导两个六轴工业机器人分别将传爆管与引信本体移动到协作装配起点的过程中，如果已知区域存在奇异区域，则六轴工业机器人轨迹动态曲线在进行轨迹优化之前必须先进行奇异值规避。
[0024]
优选地，所述轨迹动态曲线在进行轨迹优化之前的奇异值规避过程为：
[0025]
采用循环阻尼系数法来平滑关节速度曲线，机械臂某关节速度点与关节之间计算关系有：
[0026][0027]
判断与之间差值是否超过阈值δt间关节速度常态最大差值的数量级在10-3
；如果差值超过则使阻尼系数λ循环作用k次，直到差
值小于否则继续排查下一对离散速度，循环修改后的关节速度为：
[0028][0029]
其中：
[0030][0031]
将关节速度曲线进行奇异值规避之后，奇异区域的关节速度曲线变为继而对应的离散关节角度值也发生改变，通过对关节速度积分就得对应时刻的关节角度：
[0032][0033]
最终用相同时刻的新关节角度值θi(t)'替代θi(t)，将奇异值规避后的关节角度序列融入优化算法参与轨迹规划过程。
[0034]
进一步地，所述步骤六中，假设(x0,y0,z0)是主机器人末端夹爪的坐标系，记为{t}；(x1,y1,z1)是从机器人末端夹爪的坐标系，记为{t1}；v0、v1分别为主、从机器人末端夹爪抓取物体运行时的速度，v0、v1等值反向；l为传爆管和引信本体相对的一端之间的距离；d为两个末端夹爪坐标系原点之间的距离，初始值为d0；主机器人的基坐标系记为{b}；从机器人的基坐标系记为{b1}；为坐标系{b}到坐标系{t}的变换矩阵，为坐标系{t}到坐标系{t1}的变换矩阵，以此类推；坐标系{t}与{t1}之间的变换矩阵为：
[0035][0036]
进一步地，所述步骤七中，坐标系{b}到{t1}的转换矩阵为：
[0037][0038]
将等式左右两边左乘得到从机器人基座标系到从坐标系到从机器人末端手抓坐标之间的转换关系为：
[0039][0040]
其中，为主从机器人基坐标系之间的变换矩阵，在协调装配的过程中不会改变，在给定主机器人末端夹爪坐标时，利用的逆运动学计算各关节的旋转度数，然后由运动学方程求得；
[0041]
[0042]
其中，d＝d
0-(v0+v1)t，即每一时刻两末端夹爪坐标系原点的距离。
[0043]
进一步地，所述步骤八中，根据位姿约束关系从机器人第6个关节夹持引信本体旋转角度δθ6为：
[0044][0045]
其中，dis是引信本体旋转一周的距离，即螺距。
[0046]
本发明具有以下优点：
[0047]
本发明针对传爆管与引信本体的装配，提供了一种双机器人协作装配传爆管和引信本体的轨迹规划方法，采用主从控制与对称控制相结合的混合协调控制策略，当双机器人协作装配系统的末端执行器处在协调装配区外时，(即双机器人分别对传爆管与引信本体进行识别与抓取，并进入装配区)采用对称控制策略，两个机器人独立运动，互不干涉，机器人之间没有形成耦合关系，这样做可以减少机器人与计算机的通讯时间，缩短传爆管与引信本体装配任务的时间。
[0048]
当双机器人协作装配系统的末端执行器进入协作装配区时，为了装配动作准确进行，采用主从控制策略，机器人之间通过末端建立一定的耦合约束关系使系统成为冗余机器人系统。
[0049]
依据协作机器人末端位姿间的相对运动形式对多机器人系统的协作运动进行分类，针对每种相对运动形式推到机器人末端位姿间的位姿关系，依据推导结果确定双机器人协作装配时的轨迹示教步骤，示教信息，将双机器人系统的协作装配运动轨迹问题转化为单个机器人空间轨迹规划问题，从而完成多机器人协作装配系统的轨迹示教与规划任务。
[0050]
由于机器人协作完成传爆管与引信本体装配过程中，需要主、机器人分别加持传爆管与引信本体在协作工作空间内进行类似螺丝孔拧紧的动作，从机器人相对主机器人有一定的叠加运动，配合主机器人完成装配过程，本发明以运动学为基础，并基于笛卡尔坐标系转换关系对协调装配过程中主、从机器人的运动学约束关系进行分析和推导。
附图说明
[0051]
图1为本发明所述一种双机器人协作装配传爆管和引信本体的轨迹规划方法的流程图；
[0052]
图2为本发明所述轨迹规划原理框图。
具体实施方式
[0053]
为了使本技术领域的人员更好的理解本发明的技术方案，下面结合附图对发明作进一步说明。
[0054]
实施例
[0055]
本实施例为一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其应用于双机器人协作装配系统进行传爆管和引信本体装配。
[0056]
所述双机器人协作装配系统包括机器人模块以及机器视觉系统模块；所述机器人模块包括两台ur10e六轴工业机器人、专用防爆夹爪以及机器人控制箱，所述专用机器人夹
爪分别安装在两台ur10e六轴工业机器人末端的转动机械手上，分别用于抓取并夹持传爆管和引信本体；所述六轴工业机器人及专用防爆夹爪分别与机器人控制箱连接；所述机器视觉模块包括两个单目二维相机、光源、工控机一体机，工控一体机内置有机器视觉软件系统，所述单目二维单目相机为海康威视单目二维相机，两个单目二维相机分别通过支架固定在两个专用防爆夹爪上，光源均匀分布在单目二维相机拍照区的周围，单目二维相机和工控一体机分别与机器人控制箱连接，所述机器视觉软件利用海康威视自配软件及算法将采集的待装配的传爆管与引信本体的图像像素坐标转换为全局坐标，针对传爆管与引信本体的不同位姿，经机器视觉软件系统分析运算后，分别得出待装配的传爆管与引信本体的位置坐标，引导六轴工业机器人进行工件抓取并夹持移动到待装配区。
[0057]
本实施例采用以下技术方案对传爆管与引信本体的双机器人协作装配进行轨迹规划：
[0058]
针对传爆管与引信本体的装配，采用主从控制与对称控制相结合的混合协调控制策略，当双机器人协作装配系统的末端执行器处在协调装配区外时，(即双机器人分别对传爆管与引信本体进行识别与抓取，并进入装配区)采用控制对称策略，两个机器人独立运动，互不干涉，机器人之间没有形成耦合关系，这样做可以减少机器人与计算机的通讯时间，缩短传爆管与引信本体装配任务的时间。当双机器人协作装配系统的末端执行器进入协作装配区时，为了装配动作准确进行，采用主从控制策略，机器人之间通过末端建立一定的耦合约束关系使系统成为冗余机器人系统，各机器人将双机器人协作装配系统。
[0059]
如图1所示，本实施例一种双机器人协作装配传爆管和引信本体的轨迹规划方法，包括以下步骤：
[0060]
当六轴工业机器人末端执行器在协调装配区外时，采用对称控制策略，对两个六轴工业机器人分别进行轨迹规划：
[0061]
步骤一、采用eye_in_hand手眼标定布局形式，将单目二维相机安装在六轴工业机器人末端，利用标定板进行标定；
[0062]
本实施例中，采用多点标定法进行机器人与单目二维相机视觉传感器的标定：
[0063]
机器人带动相机移动到路点1之后，开始标定，将九宫格标定板放在待识别区，将探针安装在六轴机器人末端，示教机器人末端的标定顶尖到达标定板中预先设定好的空间固定点，标定获得机器人工具坐标系在机器人基坐标系下的转换坐标并记录该组空间固定点在机器人基坐标系下的坐标q；在视觉传感器中获取所述空间固定点坐标p，根据公式求解传感器坐标系与机器人工具坐标系的转换矩阵即完成标定。
[0064]
步骤二、标定完成之后，通过示教器将六轴工业机器人移至待装配工件(传爆管及引信本体)上方，将此位置设置为路点1。
[0065]
步骤三、控制单目二维相机进行图像采集，经过一系列的图像预处理，识别与确定待装配组件的位姿，并确立抓取点。
[0066]
本实施例中，采用中值滤波对采集的图像进行滤波处理，有效滤除背景中孤立的噪声点和离群点，较好的保留待装配组件的图像边缘信息，标准的二维中值滤波表达式为：
[0067]
g(x,y)＝med[f(x-i,y-i)]
[0068]
式中，(i,j)∈m,m是所取领域中像素的坐标，f(x,y),g(x,y)分别为滤波处理前与
滤波处理后的图像。
[0069]
步骤四、相机识别图像并设置完抓取点后，视觉系统引导六轴工业机器人到抓取点，六轴机器人配合防爆夹爪将待装配组件(防爆管、引信本体)抓起，并通过机器人示教分别将组件移动至步骤二所设置的路点1。
[0070]
步骤五、通过示教器引导两个六轴工业机器人分别将传爆管与引信本体移动到协作装配起点。
[0071]
通过示教器将待装配工件(引信本体与传爆管)移动至装配起始点，在此过程中，如果已知区域存在奇异区域，在轨迹优化的过程中，如果已知轨迹曲线存在奇异区域，势必会使原本正常的轨迹动态曲线发生局部突变，使得基于动态阈值的优化算法误辨识，影响最优结果的获取。因此轨迹动态曲线在进行轨迹优化之前必须先进行奇异值规避。ur的机器人的运动是将6个关节角度曲线输入到运动控制器中进行电机驱动控制的。输入关节角度曲线是基于最小间隔时间
△
t的离散时间关节角度曲线。在轨迹规划过程中，由于笛卡尔空间路径己知，总体运行时间与每时每刻的关节角度是确定的，因此控制所需的离散关节角度曲线可以得到，具体框图如图2所示。
[0072]
将同一时刻的前个和后三个关节角度分别代入j
11
和j
22
(j
11
称为前端奇异，j
22
称为腕部奇异)并计算其行列式，如果有一组行列式为0，则说明对应关节角度导致了前端或腕部奇异。如果计算结果在0附近，这组结果的关节速度或者微分运动速度可能非常大，反解往往不可行，针对这种情况，需要采取合理的手段将奇异值区域规避过去，使机器人在末端误差在允许的范围内可以正常运行。排查整个轨迹中j
11
和j
22
奇异的关节角度组θ，利用δt计算此区域关节速度曲线会出现剧烈波动。为了回避奇异区域，提出了采用一种循环阻尼系数法来平滑关节速度曲线，机械臂某关节速度点与关节之间计算关系有：
[0073][0074]
判断与之间差值是否超过阈值δt间关节速度常态最大差值的数量级在10-3
，在奇异值出现时，其导致的δt内关节速度差值远大于常态差值，因此将的数量级设定为10-1
。如果差值超过则使阻尼系数λ循环作用k次，直到差值小于否则继续排查下一对离散速度，循环修改后的关节速度为：
[0075][0076]
其中：
[0077][0078]
将关节速度曲线进行奇异值规避之后，奇异区域的关节速度曲线变为继而对应的离散关节角度值也发生改变，可以通过对关节速度积分就得对应时刻的关节角度
[0079][0080]
最终用相同时刻的新关节角度值θi(t)'替代θi(t)，将奇异值规避后的关节角度序列融入优化算法参与轨迹规划过程。
[0081]
当六轴工业机器人末端执行器在协调装配区内时，采用主从对称策略，进行双六轴工业机器人协作装配的轨迹规划：
[0082]
主从对称策略的原理为：
[0083]
依据协作机器人末端位姿间的相对运动形式对多机器人系统的协作运动进行分类，针对每种相对运动形式推到机器人末端位姿间的位姿关系，依据推导结果确定双机器人协作装配时的轨迹示教步骤，示教信息，将双机器人系统的协作装配运动轨迹问题转化为单个机器人空间轨迹规划问题，从而完成多机器人协作装配系统的轨迹示教与规划任务。
[0084]
根据协作机器人末端的相对运动，将多机器人系统的协作运动分为两类：
[0085]
1.耦合运动：具有协作关系的机器人在某一时刻同时开始相同形式的运动，运动过程中保持末端位姿之间的相对位姿关系不变。上文所述耦合运动典型代表是双机器人搬运系统，由于从机器人跟随主机器人运动，因此从机器人不需要单独的轨迹示教指令，整个系统的示教和规划可以在主机器人的基坐标系下完成。
[0086]
2.叠加运动：具有协作关系的机器人在某一时刻同时开始运动，其中主机器人的末端运动轨迹独立，从机器人相对主机器人工具手坐标系产生运动，从机器人的运动轨迹是参考主机器人末端的相对运动与主机器人末端运动轨迹的叠加。上文所述的叠加运动多用于处理机器人运动过程中需要相互配合的情况，从机器人的路径是在主机器人工具手下进行规划的，运动过程中考虑的事从机器人末端相对于主机器人末端的运动轨迹。
[0087]
3.由于机器人协作完成传爆管与引信本体装配过程中，需要主、机器人分别加持传爆管与引信本体在协作工作空间内进行类似螺丝孔拧紧的动作，从机器人相对主机器人有一定的叠加运动，配合主机器人完成装配过程，本发明以运动学为基础，并基于笛卡尔坐标系转换关系对协调装配过程中主、从机器人的运动学约束关系进行分析和推导。
[0088]
4.步骤六、两台六轴工业机器人分别抓取引信本体与传爆管，将工件移动到使其轴心对齐的协调装配起始位置。
[0089]
5.假设(x0,y0,z0)是主机器人末端夹爪的坐标系，记为{t}；(x1,y1,z1)是从机器人末端夹爪的坐标系，记为{t1}；v0、v1分别为主、从机器人末端夹爪抓取物体运行时的速度，v0、v1等值反向；l为传爆管和引信本体相对的一端之间的距离；d为两个末端夹爪坐标系原点之间的距离，初始值为d0；主机器人的基坐标系记为{b}；从机器人的基坐标系记为{b1}。在文中,为坐标系{b}到坐标系{t}的变换矩阵，为坐标系{t}到坐标系{t1}的变换矩阵，以此类推。由图2可以看出，主机器人末端手抓坐标系{t}到从机器人坐标系{t1}的变换关系，只需要将坐标系{t}沿y轴移动距离d0，然后绕x轴逆时针旋转90
°
就可以得到坐标系{t}与{t1}之间的变换矩阵为：
[0090][0091]
步骤七、主从机器人分别以v0和v1的速度夹持传爆管与引信本体运动到两者接触位置。
[0092]
坐标系{b}到{t1}的转换矩阵可以表示为：
[0093][0094]
将等式左右两边左乘得到从机器人基座标系到从坐标系到从机器人末端手抓坐标之间的转换关系为：
[0095][0096]
其中，为主从机器人基坐标系之间的变换矩阵，在协调装配的过程中不会改变，可以在给定主机器人末端手抓坐标时，利用的逆运动学计算各关节的旋转度数，然后由运动学方程求得；
[0097][0098]
其中，d＝d
0-(v0+v1)t，即每一时刻两末端手爪坐标系原点的距离。
[0099]
步骤八、主机器人夹持传爆管以的速度继续运动，从机器人夹持引信本体原地旋转。由工业机器人各关节的特点可知，从机器人只需夹持引信本体将其第6个关节旋转相应的角度就行，其他的关节保持原来的状态不变。根据位姿约束关系从机器人第6个关节夹持引信本体旋转角度δθ6为：
[0100][0101]
其中，dis是引信本体旋转一周的距离，即螺距。主机器人加紧持续运动，从机器人抓住引信本体旋转直至将引信本体拧紧为止。
[0102]
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其应用于双机器人协作装配系统，所述双机器人协作装配系统包括机器人模块以及机器视觉系统模块；所述机器人模块包括两台六轴工业机器人，每个六轴工业机器人末端安装有一个夹爪，分别用于抓取并夹持传爆管和引信本体；所述机器视觉模块包括两个单目相机，两个单目相机分别固定在两个六轴工业机器人的夹爪上；其特征在于，所述轨迹规划方法包括：s1.当六轴工业机器人末端执行器在协调装配区外时，采用对称控制策略，对两个六轴工业机器人分别进行轨迹规划：步骤一、采用eye_in_hand手眼标定布局形式，将单目相机安装在六轴工业机器人末端，利用标定板进行标定；步骤二、标定完成之后，通过示教器将六轴工业机器人移至待装配工件上方，将此位置设置为路点1；步骤三、控制单目相机进行图像采集，经过图像预处理，识别并确定待装配工件的位姿，并确立抓取点；步骤四、通过视觉系统引导六轴工业机器人到抓取点进行工件抓取，抓取完成后，六轴工业机器人通过示教法移动至步骤二所设定的路点1；步骤五、通过示教器控制两个六轴工业机器人分别将传爆管与引信本体移动到协作装配起点；s2.当六轴工业机器人末端执行器在协调装配区内时，采用主从对称策略，进行双六轴工业机器人协作装配的轨迹规划：步骤六、两台六轴工业机器人分别将传爆管与引信本体移动到使两者轴心对齐的协调装配起始位置；步骤七、主机器人和从机器人分别以v0和v1的速度夹持传爆管与引信本体运动到两者接触位置；步骤八、主机器人夹持传爆管以的速度继续运动，从机器人夹持引信本体原地旋转，直至将引信本体拧紧。2.如权利要求1所述的一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其特征在于，所述步骤一中采用多点标定法进行六轴工业机器人与单目相机视觉传感器的标定：六轴工业机器人带动单目相机移动到指定路点之后，开始标定，将九宫格标定板放在待识别区，将探针安装在六轴工业机器人末端，示教机器人末端的标定顶尖到达标定板中预先设定好的空间固定点，标定获得机器人工具坐标系在机器人基坐标系下的转换坐标并记录该组空间固定点在机器人基坐标系下的坐标q；在视觉传感器中获取空间固定点坐标p，根据公式求解传感器坐标系与机器人工具坐标系的转换矩阵即完成标定。3.如权利要求1所述的一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其特征在于，所述步骤三中，采用中值滤波对单目像机采集的图像进行滤波处理，滤除背景中孤立的噪声点和离群点，保留待装配组件的图像边缘信息；二维中值滤波表达式为：
g(x,y)＝med[f(x-i,y-i)]式中，(i,j)∈m,m是所取领域中像素的坐标；f(x,y),g(x,y)分别为滤波处理前与滤波处理后的图像。4.如权利要求1所述的一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其特征在于，所述步骤五中，通过示教器引导两个六轴工业机器人分别将传爆管与引信本体移动到协作装配起点的过程中，如果已知区域存在奇异区域，则六轴工业机器人轨迹动态曲线在进行轨迹优化之前必须先进行奇异值规避。5.如权利要求4所述的一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其特征在于，所述轨迹动态曲线在进行轨迹优化之前的奇异值规避过程为：采用循环阻尼系数法来平滑关节速度曲线，机械臂某关节速度点与关节之间计算关系有：判断与之间差值是否超过阈值δt间关节速度常态最大差值的数量级在10-3
；如果差值超过则使阻尼系数λ循环作用k次，直到差值小于否则继续排查下一对离散速度，循环修改后的关节速度为：其中：将关节速度曲线进行奇异值规避之后，奇异区域的关节速度曲线变为继而对应的离散关节角度值也发生改变，通过对关节速度积分就得对应时刻的关节角度：最终用相同时刻的新关节角度值θ
i
(t)'替代θ
i
(t)，将奇异值规避后的关节角度序列融入优化算法参与轨迹规划过程。6.如权利要求1所述的一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其特征在于，所述步骤六中，假设(x0,y0,z0)是主机器人末端夹爪的坐标系，记为{t}；(x1,y1,z1)是从机器人末端夹爪的坐标系，记为{t1}；v0、v1分别为主、从机器人末端夹爪抓取物体运行时的速度，v0、v1等值反向；l为传爆管和引信本体相对的一端之间的距离；d为两个末端夹爪坐标系原点之间的距离，初始值为d0；主机器人的基坐标系记为{b}；从机器人的基坐标系记为{b1}；为坐标系{b}到坐标系{t}的变换矩阵，为坐标系{t}到坐标系{t1}的变换矩阵，以此类推；坐标系{t}与{t1}之间的变换矩阵为：
7.如权利要求6所述的一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其特征在于，所述步骤七中，坐标系{b}到{t1}的转换矩阵为：将等式左右两边左乘得到从机器人基座标系到从坐标系到从机器人末端手抓坐标之间的转换关系为：其中，为主从机器人基坐标系之间的变换矩阵，在协调装配的过程中不会改变，在给定主机器人末端夹爪坐标时，利用的逆运动学计算各关节的旋转度数，然后由运动学方程求得；其中，d＝d
0-(v0+v1)t，即每一时刻两末端夹爪坐标系原点的距离。8.如权利要求1所述的一种双机器人协作装配传爆管和引信本体的轨迹规划方法，其特征在于，所述步骤八中，根据位姿约束关系从机器人第6个关节夹持引信本体旋转角度δθ6为：其中，dis是引信本体旋转一周的距离，即螺距。

技术总结
本发明公开了一种双机器人协作装配传爆管和引信本体的轨迹规划方法，采用主从控制与对称控制相结合的混合协调控制策略，当双机器人协作装配系统的末端执行器处在协调装配区外时，采用对称控制策略，两个机器人独立运动，互不干涉，机器人之间没有形成耦合关系；当双机器人协作装配系统的末端执行器进入协作装配区时，为了装配动作准确进行，采用主从控制策略，机器人之间通过末端建立一定的耦合约束关系使系统成为冗余机器人系统，从而完成双机器人协作装配系统的轨迹示教与规划任务。器人协作装配系统的轨迹示教与规划任务。器人协作装配系统的轨迹示教与规划任务。


技术研发人员：马国庆 曹国华 李洋富 贾冰 唐晨
受保护的技术使用者：长春理工大学
技术研发日：2023.07.25
技术公布日：2023/9/13