基于改进人工势场法的双机械臂避碰轨迹规划方法及系统

1.本发明涉及轨迹规划技术领域，涉及一种使用改进人工势场法的双机械臂避碰轨迹规划方法及系统。


背景技术：

2.双机械臂系统具有比单机械臂更高的灵活性，具有更高的工作效率。当前双机械臂系统已应用于多种工业场合，如搬运、装车、装配、焊接等。但双机械臂系统往往共享同一工作空间，因此需要对双机械臂进行协调控制，以避免机械臂之间发生碰撞。双机械臂系统的协调控制策略主要有主/从协调控制、混合位置/力控制、动态协调控制等。其中主/从协调控制最适合与双机械臂系统的避碰轨迹规划结合。其原理为将两台机械臂分别设定为主机械臂和从机械臂，从机械臂通过延时动作、变更速度、限制工作范围等方式避免与主机械臂发生碰撞。也有学者提出对双机械臂采用时间调度、b样条插值局部改变机械臂轨迹、采用启发式规则生成机械臂轨迹等方法。但已有的很多方法会影响机械臂的工作效率，有些计算复杂，求解时间长，不利于双机械臂的协调运动与避碰轨迹规划。
3.传统人工势场法(tapf)的原理是在双机械臂的运动空间中建立虚拟人工势场，虚拟人工势场由引力场和斥力场组成。引力场设置在目标点处，斥力场设置在障碍物处。机械臂在斥力场的作用下避开障碍物，并在引力场的作用下到达其轨迹的目标点。但该方法存在一些问题，传统人工势场法中的势场力与机械臂的关节加速度相关，这会导致机械臂在到达目标点时加速度为0，但仍有速度，从而造成机械臂在目标点附近振荡。而且传统人工势场法中的引力大小与机械臂到引力场的距离成正比，当机械臂距离目标点较远时，势场引力会很大，这可能导致两台机械臂碰撞。


技术实现要素：

4.针对现有技术存在的不足，本发明的目的是提供一种基于改进人工势场法的双机械臂避碰轨迹规划方法，此方法计算简便，求解速度快，且通过离线规划提前生成双机械臂系统的运动轨迹，安全性更高。
5.为了实现上述目的，本发明是通过如下的技术方案来实现：
6.第一方面，本发明提出了一种基于改进人工势场法的双机械臂避碰轨迹规划方法，如下：
7.步骤1：制定双机械臂的控制策略，设定两台机械臂的优先级；
8.步骤2：获取机械臂的尺寸数据，依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型；
9.步骤3：依据三维模型和运动学模型建立双机械臂的碰撞检测模型；
10.步骤4：确定双机械臂工作路径的所有途经点，包括起始点、第一中间点、第二中间点、目标点，完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹；
11.步骤5：使用碰撞模型离线检测初始轨迹，当检测到两台机械臂之间有碰撞风险
时，改进人工势场法开始作用并修改从机械臂的初始轨迹，改进人工势场法在双机械臂系统的工作空间内生成虚拟引力场和斥力场，引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离，斥力场设置在主机械臂的各个关节处；通过改进人工势场法得到修改后的轨迹。
12.作为进一步的技术方案，将主/从协调控制作为双机械臂的控制策略。
13.作为进一步的技术方案，步骤2中，测量机械臂的尺寸数据，依据获得的尺寸数据使用软件建立机械臂三维模型；计算尺寸数据得到机械臂d-h参数表，依据标准d-h参数法建立双机械臂系统的运动学模型和urdf文件。
14.作为进一步的技术方案，步骤3中所述的碰撞检测模型可通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。
15.作为进一步的技术方案，步骤4中采用直线插值函数拟合起点到第一中间点，以及第二中间点到终点的距离，采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。
16.第二方面，本发明还提出了一种基于改进人工势场法的双机械臂避碰轨迹规划系统，包括：
17.模块一：被配置为制定双机械臂的控制策略，设定两台机械臂的优先级；
18.模块二：被配置为获取机械臂的尺寸数据，依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型；
19.模块三：被配置为依据三维模型和运动学模型建立双机械臂的碰撞检测模型；
20.模块四：被配置为确定双机械臂工作路径的所有途经点，包括起始点、第一中间点、第二中间点、目标点，完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹；
21.模块五：被配置为使用碰撞模型离线检测初始轨迹，当检测到两台机械臂之间有碰撞风险时，改进人工势场法开始作用并修改从机械臂的初始轨迹，改进人工势场法在双机械臂系统的工作空间内生成虚拟引力场和斥力场，引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离，斥力场设置在主机械臂的各个关节处；通过改进人工势场法得到修改后的轨迹。
22.作为进一步的技术方案，所述的模块一中将主/从协调控制作为双机械臂的控制策略。
23.作为进一步的技术方案，所述的模块二被配置为测量机械臂的尺寸数据，依据获得的尺寸数据使用软件建立机械臂三维模型；计算尺寸数据得到机械臂d-h参数表，依据标准d-h参数法建立双机械臂系统的运动学模型和urdf文件。
24.作为进一步的技术方案，所述的碰撞检测模型被配置为通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。
25.作为进一步的技术方案，所述的模块四本配置为采用直线插值函数拟合起点到第一中间点，以及第二中间点到终点的距离，采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。
26.上述本发明的实施例的有益效果如下：
27.本发明提出了一种基于改进人工势场法的双机械臂避碰轨迹规划方法，该方法避碰效果好，结构简单，易于实现。首先，该方法提出了一种碰撞检测模型，检测到碰撞后局部
修改机械臂的轨迹以实现避碰，相对于全局修改轨迹的避碰方法，降低了对双机械臂工作效率的影响。其次，该方法对人工势场法进行改进，解决了人工势场法中引力过大和末端振荡的问题，并降低了计算量。最后，对该方法进行仿真测试，结果表明该方法收敛和求解速度快，在该方法的作用下，两台机械臂在运动过程中始终保持安全距离。
28.本发明在改进人工势场法中，将势场力与机械臂关节速度直接相关。这一改动不仅减少了算法的计算量，且解决了机械臂在目标的振荡问题；在人工势场法中的引力场函数中设置一阈值距离，当机械臂到引力场之间的距离大于此阈值距离后，引力不再增加，而是等于一常数。
附图说明
29.构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
30.图1为本实施例的实施对象-龙门架双机械臂装车系统示意图；
31.图2为本实施例的机械臂的三维模型示意图；
32.图3为本实施例的机械臂的d-h参数表；
33.图4、图5、图6为本实施例的碰撞检测模型中几何体的位置关系；
34.图7为本实施例的双机械臂系统避碰轨迹；
35.图8为本实施例的机械臂r1与机械臂r2末端执行器之间的距离。
36.图中：11主机该械臂、12从机械臂、13机械抓手、14传送带、15待装车辆、16龙门架、21机械臂底座、22机械臂旋转轴，23机械臂旋转轴，24机械臂旋转轴，25机械臂旋转轴，26机械臂旋转轴，27机械臂旋转轴。
具体实施方式
37.应该指出，以下详细说明都是例示性的，旨在对本发明提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
38.需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非本发明另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合；
39.正如背景技术所介绍的，现有技术中存在的不足，为了解决如上的技术问题，本发明提出了一种基于改进人工势场法的双机械臂避碰轨迹规划方法及系统。
40.实施例1
41.请参阅图1，本实施例选取图1所示的龙门架双机械臂装车系统作为本发明方法的实施对象，龙门架双机械臂装车系统的关键装置包括主机该械臂11、从机械臂12、机械抓手13、传送带14、待装车辆15和龙门架16；本发明提供的基于改进人工势场法的双机械臂避碰轨迹规划方法，包括以下步骤：
42.包括以下步骤：
43.步骤1：根据主/从协调控制原理将两台机械臂分别设定为主机械臂r1和从机械臂
r2，设定主机械臂的优先级高于从机械臂。检测到双机械臂之间有碰撞风险时，不改变高优先级的主机械臂的初始轨迹，通过改变低优先级的从机械臂的轨迹来避免两台机械臂之间的碰撞。
44.步骤2：本实例中选用的机械臂型号为kuka kr 270r3100 ultra k。测量机械臂的尺寸数据，包括大臂、小臂、旋转轴等的形状尺寸。依据测量得到的尺寸数据在solidworks软件中建立机械臂的三维模型，机械臂的三维模型请参阅图2；依据机械臂的尺寸计算机械臂的d-h参数，d-h参数表请参阅图3。图3中αi是连杆扭角,pi是连杆长度,di是连杆扭矩,θi是关节变量,i＝1,2,
…
,6。依据d-h参数表可建立机械臂的运动学模型和urdf文件。
45.步骤3：为了方便地分析碰撞问题，将双机械臂三维模型简化为几何模型。将主机械臂r2的末端执行器视为半径为ra的球形区域，将主机械臂r1的关节视为半径为rb的球体，将主机械臂r1的连杆视为长度为lb的圆柱体，圆柱体的半径为rc。主机械臂r2末端执行器与主机械臂r1关节和连杆之间的距离分别为[d1,
…
,di]和[d1,
…
,dj],主机械臂r1的末端执行器和主机械臂r2的末端执行器之间的距离为dk。碰撞检测模型中几何体之间的位置关系请参阅图4、图5、图6，图中r1j、r1l、r1e分别代表机械臂r1的关节区域、连杆区域、末端执行器区域，r2e代表机械臂r2的末端执行器区域。依据运动学模型计算两台机械臂在轨迹中同时刻的距离，当满足公式(1)中的下列条件之一时，视为两个机械臂发生碰撞。
[0046][0047]
式中d
s1
,d
s2
,d
s3
分别是机械臂r2末端执行器到机械臂r1关节、连杆、末端执行器的最小安全距离。本实例中d
s1
,d
s2
,d
s3
的值分别为0.8m,0.8m,1.28m。
[0048]
步骤4：确定双机械臂工作路径的所有途经点，包括起始点、第一中间点、第二中间点、目标点。其中起始点在传送带末端的物料抓取处，第一中间点物料抓去处上方，第二中间点在待装车辆的物料码放处上方，目标点在待装车辆的物料码放处。采用直线插值函数拟合起点到第一中间点，以及第二中间点到终点的距离。采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。
[0049]
三次多项式的插值函数见公式(2)。
[0050][0051]
式中θ(t),分别代表关节角度，关节角速度，关节角加速度，a0,a1,a2,a3分别代表插值函数的系数。完成所有途经点间的插值拟合即可得到两台机械臂完整的初始轨迹。
[0052]
步骤5：使用碰撞模型离线检测初始轨迹，当检测到两台机械臂之间有碰撞风险时，改进人工势场法开始作用并修改从机械臂的初始轨迹。在双机械臂系统的关节空间内设置虚拟力场，该虚拟力场只对机械臂r2起作用以改变其轨迹。改进人工势场法的引力场设置在机械臂r2目标点处，其引力场函数如公式(3)所示：
[0053][0054]
式中，k
att
是引力系数，ρ(q,qg)是一向量，其值是机械臂当前位置q到目标点qg的欧氏距离，其方向是从机械臂指向目标点位置，dq是机械臂到目标点之间的距离，rq是引力场的临界作用距离，uk是一个常数。引力场的势场力函数如公式(4)所示：
[0055][0056]
式中fk是一个常数。斥力场设置在机械臂r1的6个关节处。按照传统人工势场法，机械臂的受到的势场力为k
att
ρ(q,qg)。由于刚检测到碰撞风险时，机械臂r2距离目标点较远，因此会受到一个较大的势场力，这要求机械臂r2在瞬间产生较大的加速度，可能会对机械臂关节造成损伤。但改进人工势场法在引力场函数内设置了阈值距离rq，当机械臂r2与目标点距离大于此阈值距离时，机械臂受到的势场力为常数fk，保证了机械臂r2的安全运行。
[0057]
改进人工势场法的斥力场函数如公式(5)所示：
[0058][0059]
式中k
rep
是斥力系数，ρ(q,qo)是一向量，其值是机械臂当前位置q到障碍物qo的欧氏距离，其方向是从机械臂指向障碍物位置，di是机械臂r2到机械臂r1关节之间的距离，rs是斥力场的临界作用距离。斥力场的势场力函数如公式(6)所示：
[0060][0061]
全局势场由双机械臂系统工作空间内所有虚拟引力场和斥力场合成，其势场函数如公式(7)所示：
[0062][0063]
其中u代表全局势场，n是机械臂的连杆数量。全局势场力的函数如公式(8)所示：
[0064][0065]
全局势场力与机械臂的关节速度相关，机械臂的关节速度可由公式(9-11)得到：
[0066]
m＝j
ft
(θ1,θ2,
…
,θ6)f(9
[0067]
[0068][0069]
式中m是关节力矩，jf是雅可比矩阵，是机械臂的关节速度，bj是机械臂关节的转动惯量。如果按照传统人工势场法，机械臂由全局势场力得到的是关节加速度这会导致机械臂到达目标点时，关节加速度为0，但关节速度不为0，机械臂由于这种惯性速度继续运动，而不停在目标点。而改进人工势场法将势场力与关节速度直接相关，机械臂运动到目标点时，关节速度也减为0，机械臂停止在目标点，解决了这种振荡现象。
[0070]
经由前五个步骤，就获得了双机械臂的避碰轨迹。具体的，可以借助matlab中的roboticssystem toolbox工具将机械臂urdf文件可视化。已知机械臂末端执行器的轨迹，通过求机械臂运动学逆解获得机械臂关节空间的轨迹，依据机械臂关节空间的轨迹规划机械臂的运动。将机械臂的避碰过程通过仿真实现：
[0071]
两台机械臂在y轴相距2.25m，他们的底座中心坐标分别为[0,0,0]和[0,2.25,0]。机械臂r1从初始点[1.5,-1.2,-0.5]运动到目标点[2,0.5,-1]，机械臂r2从初始点[1.5,3.45,-0.5]运动到目标点[2,1,-1]。两台机械臂在t＝2.9s时检测到碰撞风险，改进人工势场法开始作用，机械臂r2在虚拟斥力场的作用下避开r1，r1完成码放任务后返回初始点，r2在虚拟引力场的作用下到达目标点。机械臂r1和r2的避碰轨迹请参阅图7。机械臂r1和r2末端执行之间的距离请参阅图8。由图8可见，改进人工势场法先于传统人工势场法收敛，且在避碰过程中机械臂r2与机械臂r1末端执行器的距离始终大于最小安全距离d
s3
。改进人工势场法作用下的机械臂r2平稳运动到目标点，未发生在目标点振荡的现象。
[0072]
实施例2
[0073]
本实施例提供了一种基于改进人工势场法的双机械臂避碰轨迹规划系统，该系统包括：
[0074]
模块一：被配置为制定双机械臂的控制策略，设定两台机械臂的优先级；
[0075]
模块二：被配置为获取机械臂的尺寸数据，依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型；
[0076]
模块三：被配置为依据三维模型和运动学模型建立双机械臂的碰撞检测模型；
[0077]
模块四：被配置为确定双机械臂工作路径的所有途经点，包括起始点、第一中间点、第二中间点、目标点，完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹；
[0078]
模块五：被配置为使用碰撞模型离线检测初始轨迹，当检测到两台机械臂之间有碰撞风险时，改进人工势场法开始作用并修改从机械臂的初始轨迹，改进人工势场法在双机械臂系统的工作空间内生成虚拟引力场和斥力场，引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离，斥力场设置在主机械臂的各个关节处；通过改进人工势场法得到修改后的轨迹。
[0079]
上述的每个模块的具体步骤可以参阅实施例1，模块一对应实施例1中的步骤1，模块二对应实施例1中的步骤2，模块三对应实施例1中的步骤3，模块四对应实施例1中的步骤4，模块五对应实施例1中的步骤5，在此不进行赘述了。
[0080]
最后还需要说明的是，诸如第一和第二之类的关系术语仅仅用来将一个实体或者操作与另一实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。
[0081]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。