一种双臂机器人协调控制方法

1.本发明属于机器人技术领域，具体涉及一种双臂机器人协调控制方法。


背景技术：

2.机器人已经在生产和生活中的各个行业和领域得到了广泛的应用，单臂机器人目前在生产制造领域已经得到了较为普遍的应用，能够很好地胜任搬运、码垛、焊接、装配、喷涂等方面的工作，尤其是在搬运和焊接领域具有较大的比重。相比于单臂机器人，双臂机器人并不仅仅是将两个机械臂组合在一起，它是通过协调约束与协调控制使得双臂配合完成相应的任务和工作。
3.双臂机器人的协调操作可以分为松协调与紧协调两类。松协调是指双臂机器人在同一工作空间中分别执行各自无关的作业任务。紧协调是指双臂机器人在同一工作空间中执行相同的工作任务，各机械臂均与操作对象直接接触构成闭链机构并通过末端执行器产生力的作用，在保持机器人运动灵活性的同时可以增加机器人的刚度和强度。为实现双臂的协调，常见的控制方式有主从控制、分散控制和集中控制。主从控制模式中，将双臂分别定义为主臂和从臂。主臂通常使用位置控制，并且跟随特定的运动轨迹；从臂则使用力控制以维持双臂与物体间的运动学约束关系，并跟随主臂的运动。但由于力跟踪的滞后性，紧约束场景下很实现真正的协调。集中控制模式中，双臂机器人为一个整体，使用一台控制器同时控制两个机械臂。共用的控制器既要解决规划、控制、协调问题，又要处理来自各种外部传感器的信息等，所以系统的计算量较大。分散控制模式下，根据左右臂的负载能力，将抓取物体所需的力分配给左、右臂，进而根据左臂与物体运动情况单独设计左臂控制器，根据右臂与物体运动情况单独设计右臂控制器。分散控制借助负载分配将双臂协同控制问题分解为单个机械臂的控制设计，但如何负载运动情况实时分配双臂的抓取力是难点。此外，现存双臂协同控制方法存在对模型依赖程度高、位置控制的精度不高、力控制中只能解决单一的外力、内力或接触力的控制等问题。


技术实现要素：

4.针对现有技术的不足，本发明提供一种双臂机器人协调控制方法，该方法不仅可以控制较少运动副运动实现机器人的精准作业，还可以解决双臂机器人紧协调控制中负载力分配不均、从臂滞后等问题。
5.本发明解决所述技术问题的技术方案是，设计一种双臂机器人协调控制方法，其特征在于，该方法适用于双臂紧协调作业，该方法包括以下步骤：
6.步骤1：双臂紧协调作业要求双臂机器人具有严格的运动约束关系，被抓取物体与机器人的双臂构成了一个完整的闭链结构，形成一个并联机构；定义：{o
t
}为被抓物体质心坐标系，{o
t1
}、{o
t2
}分别为机械臂r1、r2的工具坐标系，{o}为双臂机器人基座坐标系，{o1}、{o2}分别为机械臂r1、r2的基坐标系；
7.步骤2：依据螺旋理论对双臂机器人进行运动学建模：
[0008][0009][0010]
其中，为机械臂r1中各运动副的运动旋量，n为机械臂r1中运动副的数目；为机械臂r2中各运动副的运动旋量，m为机械臂r2中运动副的数目；[l
i m
i n
i p
i q
i ri]为运动副的pl
ü
cker坐标表达形式，当运动副为移动副时，li、mi、ni均为0，pi、qi、ri代表运动副的轴线方向向量在x、y、z三轴的分量；当运动副为转动副时，li、mi、ni代表运动副的轴线方向向量在x、y、z三轴的分量，pi、qi、ri代表运动副的轴线方向向量对原点的线矩在x、y、z三轴的分量；表示机械臂r1或者机械臂r2中的任意一个运动副；
[0011]
步骤3：运用输入选取方法，得到双臂机器人所有合理的输入组合；所述的输入选取方法为：当所有的主动副被锁定后，机构失去所有的自由度，不能运动，驱动输入就是合理的，否则不合理；因此，在对双臂进行输入选择时，选择各分支中的某个运动副，并假设将其刚化，然后分析此时双臂机器人的约束螺旋系，如果此时约束螺旋系最大线性无关数为6，则双臂机器人失去所有自由度，输入选择合理；如果最大线性无关数小于6，则双臂机器人并不能失去所有自由度，还存在不可控自由度，输入选择不合理；在双臂机器人中任意选取若干个运动副为一个组合，所有满足输入选取方法并且在机械臂r1、r2上选取的运动副数量相同的组合均为合理的输入组合；
[0012]
步骤4：根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：
[0013][0014][0015]
其中为坐标系{o
t
}在坐标系{o}中的位姿，为坐标系{o1}在坐标系{o}中位姿，为坐标系{o
t1
}在坐标系{o1}中的位姿，为坐标系{o
t
}在坐标系{o
t1
}中的位姿，为坐标系{o2}在坐标系{o}中的位姿，为坐标系{o
t2
}在坐标系{o2}中的位姿，为坐标系{o
t
}在坐标系{o
t2
}中的位姿；
[0016]
式(3)、式(4)中的和与机械臂r1、r2的安装方式有关，均为已知；
在已知被抓物体期望位姿时，通过约束关系得到机械臂r1、r2末端相对于基座的位姿为：
[0017][0018][0019]
在已知机械臂r1、r2的具体构型后，进而得到机械臂r1、r2的正向运动学与逆向运动学方程；
[0020]
步骤5：以承载力性能为评价指标，在步骤3得到的双臂机器人所有合理的输入组合中选择最优输入组合；评价指标为：
[0021][0022]
其中，n是在被抓物体工作轨迹上均匀选取样本总数，s2是样本方差，μ是样本均值，q是一个输入合理组合的总方差，代表该输入组合所有角加速的方差之和，a是样本值，代表一个输入运动副的角加速度，通过式(6)计算得到：
[0023][0024]
其中tm是从第m+1个样本点到第m个样本点的时间，ω(i,j)和a(i,j)分别是第j个机械臂的第i个运动副在这段时间内的角速度和角加速度，pxm、pym、pzm分别是预期到达的第m个样本点的位置，θ(i,j)是第j个机械臂的第i个运动副角度，v是被转物体的假设速度，v＝0.1m/s；
[0025]
每个输入组合的总方差q越小，则该输入方案速度的波动越小，双臂机器人的运动性能越好，q值最小的组合被选为最优输入组合；
[0026]
步骤6：将步骤5得到的最优输入组合中的运动副作为输入副，双臂机器人中其余运动副为被动副，调节输入副的旋转角度实现双臂机器人的双臂紧协调作业控制。
[0027]
与现有技术相比，本发明专利的有益效果是：本发明设计的一种双臂机器人协调控制方法，根据双臂紧协调作业时的形成闭链的并联结构这一特点，提出通过优选输入副，退化其余运动副为被动副的方法，可以有效的降低双臂协调控制难度并且具有很好的通用性。通过采用本发明的控制方法，不仅可以控制较少运动副运动实现机器人的精准作业，还可以提高驱使被动副运动力的有效利用程度和机构的传动质量，通过优选得到的最优输入组合使双臂机器人运动的速度波动较小，机器人的运动更加稳定。
附图说明
[0028]
图1为本发明一种双臂机器人协调控制方法一种实施例的双臂机器人的坐标系设
置示意图。
[0029]
图2为本发明一种双臂机器人协调控制方法一种实施例的步骤流程示意图。
[0030]
图3为本发明一种双臂机器人协调控制方法一种实施例中的2-rppps双臂机器人结构示意图。
[0031]
图4为本发明一种双臂机器人协调控制方法一种实施例中的2-rppps双臂机器人选取最优输入组合时的部分运动副(关节)角加速度图；其中，图4中的(a)为机械臂r1的第1运动副、第5运动副、第6运动副、第7运动副的角加速度图，图4中的(b)为机械臂r1的第2运动副、第3运动副、第4运动副的角加速度图，图4中的(c)为机械臂r2的第1运动副、第5运动副、第6运动副、第7运动副的角加速度图，图4中的(d)为机械臂r2的第2运动副、第3运动副、第4运动副的角加速度图。
具体实施方式
[0032]
以下将结合说明书附图和具体实施例对本发明做进一步详细说明。
[0033]
本发明提供一种双臂机器人协调控制方法，该方法适用于双臂紧协调作业(即双臂机器人的双臂共同抓取一个物体的场景)，该方法包括以下步骤：
[0034]
步骤1：双臂紧协调作业要求双臂机器人具有严格的运动约束关系，被抓取物体与机器人的双臂构成了一个完整的闭链结构，形成一个并联机构。定义：{o
t
}为被抓物体质心坐标系，{o
t1
}、{o
t2
}分别为机械臂r1、r2的工具坐标系，{o}为双臂机器人基座坐标系，{o1}、{o2}分别为机械臂r1、r2的基坐标系，如图1所示。
[0035]
步骤2：依据螺旋理论对双臂机器人进行运动学建模：
[0036][0037][0038]
其中为机械臂r1中各运动副的运动旋量，n为机械臂r1中运动副的数目；为机械臂r2中各运动副的运动旋量，m为机械臂r2中运动副的数目。[l
i m
i n
i p
i q
i ri]为运动副的pl
ü
cker坐标表达形式，当运动副为移动副时，li、mi、ni均为0，pi、qi、ri代表运动副的轴线方向向量在x、y、z三轴的分量；当运动副为转动副时，li、mi、ni代表运动副的轴线方向向量在x、y、z三轴的分量，pi、qi、ri代表运动副
的轴线方向向量对原点的线矩在x、y、z三轴的分量。表示机械臂r1或者机械臂r2中的任意一个运动副。
[0039]
步骤3：运用输入选取方法，得到双臂机器人所有合理的输入组合。所述的输入选取方法为：当所有的主动副被锁定后，机构失去所有的自由度，不能运动，驱动输入就是合理的，否则不合理。因此，在对双臂进行输入选择时，选择各分支中的某个运动副，并假设将其刚化，然后分析此时双臂机器人的约束螺旋系，如果此时约束螺旋系最大线性无关数为6，则双臂机器人失去所有自由度，输入选择合理；如果最大线性无关数小于6，则双臂机器人并不能失去所有自由度，还存在不可控自由度，输入选择不合理。在双臂机器人中任意选取若干个运动副为一个组合，所有满足输入选取方法并且在机械臂r1、r2上选取的运动副数量相同的组合均为合理的输入组合。
[0040]
步骤4：根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：
[0041][0042][0043]
其中为坐标系{o
t
}在坐标系{o}中的位姿，为坐标系{o1}在坐标系{o}中位姿，为坐标系{o
t1
}在坐标系{o1}中的位姿，为坐标系{o
t
}在坐标系{o
t1
}中的位姿，为坐标系{o2}在坐标系{o}中的位姿，为坐标系{o
t2
}在坐标系{o2}中的位姿，为坐标系{o
t
}在坐标系{o
t2
}中的位姿。
[0044]
式(3)、式(4)中的和与机械臂r1、r2的安装方式有关，均为已知；在已知被抓物体期望位姿时，通过约束关系得到机械臂r1、r2末端相对于基座的位姿为：
[0045][0046][0047]
在已知机械臂r1、r2的具体构型后，进而得到机械臂r1、r2的正向运动学与逆向运动学方程。
[0048]
步骤5：以承载力性能为评价指标，在步骤3得到的双臂机器人所有合理的输入组合中选择最优输入组合。评价指标为：
[0049][0050]
其中，n是在被抓物体工作轨迹上均匀选取样本总数，s2是样本方差，μ是样本均值，q是一个输入合理组合的总方差，代表该输入组合所有角加速的方差之和，a是样本值，代表一个输入运动副的角加速度，通过式(6)计算得到：
[0051][0052]
其中tm是从第m+1个样本点到第m个样本点的时间，ω(i,j)和a(i,j)分别是第j个机械臂的第i个运动副在这段时间内的角速度和角加速度，pxm、pym、pzm分别是预期到达的第m个样本点的位置，θ(i,j)是第j个机械臂的第i个运动副角度，v是被转物体的假设速度，v＝0.1m/s。
[0053]
每个输入组合的总方差q越小，则该输入方案速度的波动越小，双臂机器人的运动性能越好，q值最小的组合被选为最优输入组合。
[0054]
步骤6：将步骤5得到的最优输入组合中的运动副作为输入副，双臂机器人中其余运动副为被动副，调节输入副的旋转角度实现双臂机器人的双臂紧协调作业控制。
[0055]
下面选用一种2-rppps双臂机器人作为实施例详细描述本发明方法，其他的并联机器人同样可以按上述方法进行应用。
[0056]
本实施例提供一种双臂机器人协调控制方法，该方法适用于双臂紧协调作业，该方法包括以下步骤：
[0057]
步骤1：双臂紧协调作业要求双臂机器人具有严格的运动约束关系，被抓取物体与机器人的双臂构成了一个完整的闭链结构，形成一个并联机构。定义：{o
t
}为被抓物体质心坐标系，{o
t1
}、{o
t2
}分别为机械臂r1、r2的工具坐标系，{o}为双臂机器人基座坐标系，{o1}、{o2}分别为机械臂r1、r2的基坐标系；
[0058]
步骤2：依据螺旋理论对2-rppps双臂机器进行运动学建模：
[0059][0060]
[0061]
其中为机械臂r1中各运动副的运动旋量，为机械臂r2中各运动副的运动旋量。2-rppps双臂机器人中，两机械臂的运动副的数量均为17个；式(7)中为移动副，前三列均为0，后三列为其轴线方向向量在x、y、z三轴的分量，为转动副，前三列为其轴线方向向量在x、y、z三轴的分量，后三列为其轴线方向向量对原点的线矩在x、y、z三轴的分量，a1、b1、c1分别为机械臂r1末端位置矢量在y、z、x三轴的分量；式(8)中，式(8)中，为移动副，前三列均为0，后三列为其轴线方向向量在x、y、z三轴的分量，为转动副，前三列为其轴线方向向量在x、y、z三轴的分量，后三列为其轴线方向向量对原点的线矩在x、y、z三轴的分量，a2、b2、c2分别为机械臂r2末端位置矢量在y、z、x三轴的分量。
[0062]
步骤3：运用输入选取方法，得到双臂机器人所有合理的输入组合。所述的输入选取方法为：当所有的主动副被锁定后，机构失去所有的自由度，不能运动，驱动输入就是合理的，否则不合理。因此，在对双臂进行输入选择时，可以选择各分支中的某个运动副，并假设将其刚化，然后分析此时双臂机器人的约束螺旋系.如果机构的约束螺旋系最大线性无关数为6，则机构失去所有自由度，输入选择合理；如果最大线性无关数小于6，则机构并不能失去所有自由度，还存在不可控自由度，输入选择不合理。
[0063]
在2-rppps双臂机器人中任意选取8个运动副为一个组合，所有满足输入选取方法并且在机械臂r1、r2上选取的运动副数量相同的组合均为合理的输入组合。则得到2-rppps双臂机器人所有合理的输入组合58个，如表1所示。
[0064]
表1合理的输入组
[0065]
[0066][0067]
步骤4：根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：
[0068][0069][0070]
其中为坐标系{o
t
}在坐标系{o}中的位姿，为坐标系{o1}在坐标系{o}中位姿，为坐标系{o
t1
}在坐标系{o1}中的位姿，为坐标系{o
t
}在坐标系{o
t1
}中的位姿，为坐标系{o2}在坐标系{o}中的位姿，为坐标系{o
t2
}在坐标系{o2}中的位姿，为坐标系{o
t
}在坐标系{o
t2
}中的位姿。
[0071]
式(9)、式(10)中的和与机械臂r1、r2的安装方式有关，均为已知；在已知被抓物体期望位姿时，可通过约束关系得到机械臂r1、r2末端相对于基座的位姿为：
[0072][0073][0074]
已知机械臂r1、r2的具体构型后，进而得到机械臂r1、r2的正向运动学与逆向运动学方程。
[0075]
步骤5：以承载力性能为评价指标，在步骤3得到的双臂机器人所有合理的输入组合中选择最优输入组合。评价指标为：
[0076][0077]
其中，n是在被抓物体工作轨迹上均匀选取样本总数，s2是样本方差，μ是样本均值，q是一个输入合理组合的总方差，代表该输入组合所有角加速的方差之和，a是样本值，代表一个输入运动副的角加速度，通过式(12)计算得到：
[0078][0079]
其中tm是从第m+1个样本点到第m个样本点的时间，ω(i,j)和a(i,j)是第j个机械臂的第i个运动副在这段时间内的角速度和角加速度，pxm、pym、pzm分别是预期到达的第m个样本点的位置，θ(i,j)是第j个机械臂的第i个运动副角度，v是被转物体的假设速度，v＝0.1m/s。
[0080]
每个输入组合的总方差q越小，则该输入方案速度的波动越小，双臂机器人的运动性能越好，q值最小的组合被选为最佳组合。设置一条在z＝100的平面内，以(0，400)为圆心，以(3000，400)为起点(单位：mm)的圆弧轨迹为被抓物体运动轨迹，保持某固定姿态作逆时针旋转30度的运动，将该运动轨迹均匀分解为100个样本点，计算每个合理输入组合的a值和q值，输入组合的总方差q越小，则该输入组合速度的波动越小，双臂机器人的运动性能越好，2-rppps双臂机器人所有合理的输入组合表1所示，在所有合理的输入组合中选取q值最小的组合为最优输入组合，如下表2所示。
[0081]
表2最优输入组合
[0082][0083]
步骤6：将步骤5得到的最优输入组合中的运动副作为输入副，双臂机器人中其余运动副为被动副，调节输入副的旋转角度实现双臂机器人的双臂紧协调作业控制。
[0084]
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。
[0085]
本发明未述及之处适用于现有技术。

技术特征：
1.一种双臂机器人协调控制方法，其特征在于，该方法适用于双臂紧协调作业，该方法包括以下步骤：步骤1：双臂紧协调作业要求双臂机器人具有严格的运动约束关系，被抓取物体与机器人的双臂构成了一个完整的闭链结构，形成一个并联机构；定义：{o
t
}为被抓物体质心坐标系，{o
t1
}、{o
t2
}分别为机械臂r1、r2的工具坐标系，{o}为双臂机器人基座坐标系，{o1}、{o2}分别为机械臂r1、r2的基坐标系；步骤2：依据螺旋理论对双臂机器人进行运动学建模：依据螺旋理论对双臂机器人进行运动学建模：其中，$
11
、$
12
、$
13
……
$
1n
为机械臂r1中各运动副的运动旋量，n为机械臂r1中运动副的数目；$
21
、$
22
、$
23
……
$
2m
为机械臂r2中各运动副的运动旋量，m为机械臂r2中运动副的数目；[l
i m
i n
i p
i q
i r
i
]为运动副$
i
的pl
ü
cker坐标表达形式，当运动副$
i
为移动副时，l
i
、m
i
、n
i
均为0，p
i
、q
i
、r
i
代表运动副$
i
的轴线方向向量在x、y、z三轴的分量；当运动副$
i
为转动副时，l
i
、m
i
、n
i
代表运动副$
i
的轴线方向向量在x、y、z三轴的分量，p
i
、q
i
、r
i
代表运动副$
i
的轴线方向向量对原点的线矩在x、y、z三轴的分量；$
i
表示机械臂r1或者机械臂r2中的任意一个运动副；步骤3：运用输入选取方法，得到双臂机器人所有合理的输入组合；所述的输入选取方法为：当所有的主动副被锁定后，机构失去所有的自由度，不能运动，驱动输入就是合理的，否则不合理；因此，在对双臂进行输入选择时，选择各分支中的某个运动副，并假设将其刚化，然后分析此时双臂机器人的约束螺旋系，如果此时约束螺旋系最大线性无关数为6，则双臂机器人失去所有自由度，输入选择合理；如果最大线性无关数小于6，则双臂机器人并不能失去所有自由度，还存在不可控自由度，输入选择不合理；在双臂机器人中任意选取若干个运动副为一个组合，所有满足输入选取方法并且在机械臂r1、r2上选取的运动副数量相同的组合均为合理的输入组合；步骤4：根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：
其中为坐标系{o
t
}在坐标系{o}中的位姿，为坐标系{o1}在坐标系{o}中位姿，为坐标系{o
t1
}在坐标系{o1}中的位姿，为坐标系{o
t
}在坐标系{o
t1
}中的位姿，为坐标系{o2}在坐标系{o}中的位姿，为坐标系{o
t2
}在坐标系{o2}中的位姿，为坐标系{o
t
}在坐标系{o
t2
}中的位姿；式(3)、式(4)中的和与机械臂r1、r2的安装方式有关，均为已知；在已知被抓物体期望位姿时，通过约束关系得到机械臂r1、r2末端相对于基座的位姿为：为：在已知机械臂r1、r2的具体构型后，进而得到机械臂r1、r2的正向运动学与逆向运动学方程；步骤5：以承载力性能为评价指标，在步骤3得到的双臂机器人所有合理的输入组合中选择最优输入组合；评价指标为：其中，n是在被抓物体工作轨迹上均匀选取样本总数，s2是样本方差，μ是样本均值，q是一个输入合理组合的总方差，代表该输入组合所有角加速的方差之和，a是样本值，代表一个输入运动副的角加速度，通过式(6)计算得到：其中t
m
是从第m+1个样本点到第m个样本点的时间，ω(i,j)和a(i,j)分别是第j个机械臂的第i个运动副在这段时间内的角速度和角加速度，px
m
、py
m
、pz
m
分别是预期到达的第m个样本点的位置，θ(i,j)是第j个机械臂的第i个运动副角度，v是被转物体的假设速度，v＝0.1m/s；每个输入组合的总方差q越小，则该输入方案速度的波动越小，双臂机器人的运动性能越好，q值最小的组合被选为最优输入组合；步骤6：将步骤5得到的最优输入组合中的运动副作为输入副，双臂机器人中其余运动副为被动副，调节输入副的旋转角度实现双臂机器人的双臂紧协调作业控制。2.根据权利要求1所述的一种双臂机器人协调控制方法，其特征在于，所述双臂机器人为2-rppps双臂机器人。3.根据权利要求1所述的一种双臂机器人协调控制方法，其特征在于，步骤3中，合理的
输入组合包含8个运动副。4.根据权利要求1所述的一种双臂机器人协调控制方法，其特征在于，步骤5中，n＝100。

技术总结
本发明公开一种双臂机器人协调控制方法，该方法根据双臂紧协调作业时的形成闭链的并联结构这一特点，提出通过优选输入副，退化其余运动副为被动副的方法，可以有效的降低双臂协调控制难度并且具有很好的通用性。通过采用本发明的控制方法，不仅可以控制较少运动副运动实现机器人的精准作业，还可以提高驱使被动副运动力的有效利用程度和机构的传动质量，通过优选得到的最优输入组合使双臂机器人运动的速度波动较小，机器人的运动更加稳定。机器人的运动更加稳定。机器人的运动更加稳定。


技术研发人员：杨冬 毕燕旭 李铁军 韩明 熊斌 廉旺旺 杨朔瀚 车佳津 王昊
受保护的技术使用者：河北工业大学
技术研发日：2023.05.19
技术公布日：2023/9/14