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

权利要求书

1.基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，如下：

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

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

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

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

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

2.如权利要求1所述的基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，步骤1中，将主/从协调控制作为双机械臂的控制策略。

3.如权利要求1所述的基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，步骤2中，测量机械臂的尺寸数据，依据获得的尺寸数据使用软件建立机械臂三维模型；计算尺寸数据得到机械臂D-H参数表，依据标准D-H参数法建立双机械臂系统的运动学模型和URDF文件。

4.如权利要求1所述的基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，步骤3中所述的碰撞检测模型可通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。

5.如权利要求1所述的基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，步骤4中采用直线插值函数拟合起点到第一中间点，以及第二中间点到终点的距离，采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。

6.一种基于改进人工势场法的双机械臂避碰轨迹规划系统，其特征在于，包括：

模块一：被配置为制定双机械臂的控制策略，设定两台机械臂的优先级；

模块二：被配置为获取机械臂的尺寸数据，依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型；

模块三：被配置为依据三维模型和运动学模型建立双机械臂的碰撞检测模型；

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

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

7.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，模块一中将主/从协调控制作为双机械臂的控制策略。

8.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，所述的模块二被配置为测量机械臂的尺寸数据，依据获得的尺寸数据使用软件建立机械臂三维模型；计算尺寸数据得到机械臂D-H参数表，依据标准D-H参数法建立双机械臂系统的运动学模型和URDF文件。

9.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，所述的碰撞检测模型被配置为通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。

10.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法，其特征在于，所述的模块四本配置为采用直线插值函数拟合起点到第一中间点，以及第二中间点到终点的距离，采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。