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

说明书

本发明公开了一种基于改进人工势场法的双机械臂避碰轨迹规划方法及系统，本发明对人工势场法进行改进：在改进人工势场法中，将势场力与机械臂关节速度直接相关。这一改动不仅减少了算法的计算量，且解决了机械臂在目标的振荡问题。在人工势场法中的引力场函数中设置一阈值距离，当机械臂到目标点之间的距离大于此阈值距离后，引力不再增加，而是等于一常数。这一改动避免了机械臂在距离目标点较远时受到过大的引力。

技术领域

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

背景技术

双机械臂系统具有比单机械臂更高的灵活性，具有更高的工作效率。当前双机械臂系统已应用于多种工业场合，如搬运、装车、装配、焊接等。但双机械臂系统往往共享同一工作空间，因此需要对双机械臂进行协调控制，以避免机械臂之间发生碰撞。双机械臂系统的协调控制策略主要有主/从协调控制、混合位置/力控制、动态协调控制等。其中主/从协调控制最适合与双机械臂系统的避碰轨迹规划结合。其原理为将两台机械臂分别设定为主机械臂和从机械臂，从机械臂通过延时动作、变更速度、限制工作范围等方式避免与主机械臂发生碰撞。也有学者提出对双机械臂采用时间调度、B样条插值局部改变机械臂轨迹、采用启发式规则生成机械臂轨迹等方法。但已有的很多方法会影响机械臂的工作效率，有些计算复杂，求解时间长，不利于双机械臂的协调运动与避碰轨迹规划。

传统人工势场法(TAPF)的原理是在双机械臂的运动空间中建立虚拟人工势场，虚拟人工势场由引力场和斥力场组成。引力场设置在目标点处，斥力场设置在障碍物处。机械臂在斥力场的作用下避开障碍物，并在引力场的作用下到达其轨迹的目标点。但该方法存在一些问题，传统人工势场法中的势场力与机械臂的关节加速度相关，这会导致机械臂在到达目标点时加速度为0，但仍有速度，从而造成机械臂在目标点附近振荡。而且传统人工势场法中的引力大小与机械臂到引力场的距离成正比，当机械臂距离目标点较远时，势场引力会很大，这可能导致两台机械臂碰撞。

发明内容

针对现有技术存在的不足，本发明的目的是提供一种基于改进人工势场法的双机械臂避碰轨迹规划方法，此方法计算简便，求解速度快，且通过离线规划提前生成双机械臂系统的运动轨迹，安全性更高。

为了实现上述目的，本发明是通过如下的技术方案来实现：

第一方面，本发明提出了一种基于改进人工势场法的双机械臂避碰轨迹规划方法，如下：

步骤1：制定双机械臂的控制策略，设定两台机械臂的优先级；

步骤2：获取机械臂的尺寸数据，依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型；

步骤3：依据三维模型和运动学模型建立双机械臂的碰撞检测模型；

步骤4：确定双机械臂工作路径的所有途经点，包括起始点、第一中间点、第二中间点、目标点，完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹；

步骤5：使用碰撞模型离线检测初始轨迹，当检测到两台机械臂之间有碰撞风险时，改进人工势场法开始作用并修改从机械臂的初始轨迹，改进人工势场法在双机械臂系统的工作空间内生成虚拟引力场和斥力场，引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离，斥力场设置在主机械臂的各个关节处；通过改进人工势场法得到修改后的轨迹。

作为进一步的技术方案，将主/从协调控制作为双机械臂的控制策略。

作为进一步的技术方案，步骤2中，测量机械臂的尺寸数据，依据获得的尺寸数据使用软件建立机械臂三维模型；计算尺寸数据得到机械臂D-H参数表，依据标准D-H参数法建立双机械臂系统的运动学模型和URDF文件。

作为进一步的技术方案，步骤3中所述的碰撞检测模型可通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。