(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210415041.1 (22)申请日 2022.04.20 (71)申请人 安徽大学 地址 230601 安徽省合肥市经济技 术开发 区九龙路1 11号 (72)发明人 杨东　董理　 (74)专利代理 机构 济南圣达知识产权代理有限 公司 372 21 专利代理师 赵敏玲 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 基于改进人工势场法的双机械臂避碰轨迹 规划方法及系统 (57)摘要 本发明公开了一种基于改进人工势场法的 双机械臂避碰轨迹规划方法及系统， 本发明对人 工势场法进行改进： 在改进人工势场法中， 将势 场力与机械臂关节速度直接相关。 这一改动不仅 减少了算法的计算量， 且解决了机械臂在目标的 振荡问题。 在人工势场法中的引力场函数中设置 一阈值距离， 当机械臂到目标点之间的距离大于 此阈值距离后， 引力不再增加， 而是等于一 常数。 这一改动避免了机械臂在距离目标点较远时受 到过大的引力。 权利要求书2页 说明书7页 附图4页 CN 114643581 A 2022.06.21 CN 114643581 A 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所述的基于改进人工势场法的双机械臂避碰轨迹规划方法， 其特征在权　利　要　求　书 1/2 页 2 CN 114643581 A 2于， 所述的模块二被配置为测量机械臂的尺寸数据， 依据获得的尺寸数据使用软件建立机 械臂三维模型； 计算尺寸数据得到机械臂D ‑H参数表， 依据标准D ‑H参数法建立双机械臂系 统的运动学模型和URDF文件。 9.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法， 其特征在 于， 所述的碰撞检测模型被配置为通过离线计算从机械臂到主机械臂的距离来判断两台机 械臂是否有碰撞风险。 10.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法， 其特征在 于， 所述的模块四本配置为采用直线插值函数拟合起点到第一中间点， 以及第二中间点到 终点的距离， 采用三次多 项式插值 函数拟合第一中间点到第二中间点的轨 迹。权　利　要　求　书 2/2 页 3 CN 114643581 A 3