(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210973726.8 (22)申请日 2022.08.15 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 何德峰　梁爽　邢科新　 (74)专利代理 机构 杭州赛科专利代理事务所 (普通合伙) 33230 专利代理师 宋飞燕 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于改进人工势场法的机械臂避障路 径规划方法 (57)摘要 本发明涉及一种基于改进人工势场法的机 械臂避障路径 规划方法， 对机械臂及障碍物进行 建模， 得到机械臂的各个关节在空间中的位置和 姿态； 碰撞检测后利用改进人工势场法构造目标 函数， 基于机械臂的关节空间进行优化， 当机械 臂的运动陷入局部最小值则以变步长搜索构造 中间点， 否则利用多项式插值的方法平滑轨迹， 得到机械臂各个 关节的完整的运行轨迹。 本发明 采用改进人工势场法进行机械臂的避障路径规 划， 可以消除复杂障碍物环境下的抖动问题， 同 时结合变步长搜索方法， 可以使得机械臂在运动 的过程中能跳出局部最小值， 再结合五次多项式 插值方法对机械臂的运行轨迹进行优化， 使 得机 械臂运行的更加平稳、 光滑， 机械臂在运动的同 时能够避开障碍物。 权利要求书3页 说明书9页 附图4页 CN 115416016 A 2022.12.02 CN 115416016 A 1.一种基于改进人工势场法的机械臂避障路径规划方法， 其特征在于： 所述方法包括 以下步骤： 步骤1： 分别对机械臂及障碍物进行建模， 并通过正运动学计算得到机械臂的各个关节 在空间中的位置和姿态； 步骤2： 对机械臂和障碍物进行碰撞检测， 遍历机械臂相邻关节 组合， 若存在碰撞， 则舍 去当前对应的一组关节组合， 否则， 进行 下一步； 步骤3： 基于机械臂的关节空间优化， 利用改进人工势场法构造目标函数， 使机械臂沿 着目标函数减少最快的方向运动直到能够到 达目标点附近； 步骤4： 判断机械臂的运动是否陷入局部最小值， 若是， 则以变步长搜索构造 中间点， 进 行下一步， 否则， 直接进行 下一步； 步骤5： 利用多 项式插值的方法平 滑轨迹， 得到机械臂各个关节的完整的运行轨 迹。 2.根据权利要求1所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 所述 步骤1中， 对机 械臂建模 包括以下步骤： 步骤1.1： 在机械臂的每个关节处建立坐标系， 根据相邻坐标系之间的旋转变换关系得 到机械臂的每个关节在空间中的具体位置， 进而确定机械臂的当前状态， 同时， 对机械臂的 外形采用圆柱形包络法进行简化， 令膨胀半径是r0； 步骤1.2： 根据机械臂的各个连杆之间参数信息， 进行DH法建模， 得到相邻关节坐标系 之间的位姿变换关系； 步骤1.3： 以正运动学得到机械臂各个关节坐标系相对于机械臂底座的基坐标系在三 维空间中的位姿； 步骤1.4： 根据机 械臂各个关节的位姿就得到 机械臂各个关节在空间中的三维坐标。 3.根据权利要求2所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 所述 位姿变换关系为， 其中， S＝sinθi、 C＝cosθi为一个关节与下一个关节的公共法线和上一个关节的公共 法 线的夹角， 定义为一个关节与下一个关节的公共法线和它与上一个关节的公共法线绕这个 关节轴的转角， di为一个关节的公共法线和上一个公共法线沿着关节轴的距离， ai为两个关 节的轴之间公共法线长度， αi为一个关节的轴相对于另一个关节的轴绕它们的公垂线旋转 的角度。 4.根据权利要求1所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 所述步骤1 中， 采用包络盒模 型对障碍物进 行建模,对障碍物采用包围球模 型处理， 令包围球半径是r1。 5.根据权利要求1所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 碰撞检测中， 计算机械臂各个连杆到障碍物中心的距离d， 若d≥r0+r1+dsafe则不会权　利　要　求　书 1/3 页 2 CN 115416016 A 2发生碰撞， 否则会发生碰撞， 其中， dsafe为安全距离， r0为机械臂半径， r1为障碍物的外径。 6.根据权利要求1所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 所述步骤3中， 改进的人工势场法构造的目标函数为 以 Nlink为连杆个数， Nobs为障碍物个数， n和m分别为连杆和障碍物的索引， θ为机械臂的各个 关节角度， Uat t为引力函数， Urep为斥力函数。 7.根据权利要求6所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 所述引 力函数为， 其中， ξ为引力系数， q为机械臂的一组关节角， (x0,y0,z0)为机械臂的末端坐标， (xg,yg,zg)为目标 点。 8.根据权利要求6所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 所述斥力函数为， 其中， δ 为斥力系数， p0为障碍物的斥力范围， p(q)为连 杆到障碍物的距离 。 9.根据权利要求1所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 所述 步骤4中， 采用变步长 搜索构造中间点包括以下步骤： 步骤4.1： 基于 机械臂各个关节的初始角定义 一个步长 λ； 步骤4.2： 以机械臂在相邻关节空间搜索， 得到一系列关节角度组合， Cn＝{ λ‑θ1+λ, λ‑θ2 +λ, λ‑θ3+λ, λ‑θ4+λ, λ‑θ5+λ, λ‑θ6+λ }， 其中θ1～θ6为机械臂各个关节角度， Cn表示机械臂相邻 关节空间的所有关节组合； 步骤4.3： 基于关节角度组合， 删除会发生碰撞的组合； 对剩下的关节角度组合计算合 势能， 选取合势能最小的一组关节组合作为 最优关节组合； 步骤4.4： 以最优关节组合为最优解， 重复步骤4.2和步骤4.3， 生成相邻关节组合， 直至 机械臂末端与目标点的距离小于预设值， 如d< 0.1m， 进行 下一步； 步骤4.5： 在机械臂的工作空间中得到若干离散的路径点， 选择离障碍物较近的点为中 间点， 同时得到中间点对应的一组关节角度； 步骤4.6： 采用小步长进行搜索， 以中间点为目标点， 机械臂先从初始点运动 到中间点， 再从中间点 运动到目标点， 直到 机械臂末端与目标点的距离小于阈值， 结束。 10.根据权利要求9所述的一种基于改进人工势场法的机械臂避障路径规划方法， 其特 征在于： 对于小步长 搜索产生的离 散的路径点， 获得对应的关节角度组合， 构造插值 函数， θ(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5，权　利　要　求　书 2/3 页 3 CN 115416016 A 3