(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210819956.9 (22)申请日 2022.07.13 (71)申请人 江苏科技大学 地址 212003 江苏省镇江市梦溪路2号 申请人 江苏大隆凯科技有限公司 (72)发明人 马国军　段云龙　胡和荣　甄友　 王志明　张博文　 (74)专利代理 机构 南京苏高专利商标事务所 (普通合伙) 32204 专利代理师 常虹 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种智能抓钢机机械臂路径规划方法及规 划系统 (57)摘要 本发明公开了一种智能抓钢机机械臂路径 规划方法和规划系统， 其中路径规划方法包括： 建立械臂二维工作场景； 生成随机点Qrand； 寻找 随机点的最近节点Qmin； 生成新节点Qnew； 优化 Qcurrent到Qnew的路径； 更新Qcurrent到Qnew路径上的 信息素； 更新路径表； 将Qcurrent到Qnew的路径加入 路径表； 更新Qnew到Qgoal范围的信息素； 当达到迭 代停止条件时停止迭代； 否则， 将所有可通行栅 格的信息素进行蒸发， 更新参数进行下一次迭 代； 迭代结束， 根据路径表得到初始规划路径； 对 初始规划路径采用三角不等式优化， 得到优化后 的路径， 作为最终规划路径。 该方法解决了RRT* 算法规划时间长、 效率低的问题， 能够根据具体 的工作场景 快速规划出抓钢机 机械臂的路径。 权利要求书5页 说明书8页 附图5页 CN 115091460 A 2022.09.23 CN 115091460 A 1.一种智能抓钢机 机械臂路径规划方法， 其特 征在于， 包括 步骤： S1、 根据抓钢需求， 获取机械臂二维工作场景， 对所述二维工作场景进行栅格化处理， 并设置每个栅格为可通行或障碍物； 在可通行栅格 喷洒等量信息素， 即可通行栅格的信息 素初始值相等， 设为l0； 获取机械臂的起点位置Qstart和目的位置Qgoal； 如果从Qstart到Qgoal的 直线路径没有碰触障碍物， 则规划路径为从Qstart到Qgoal的直线段； 否则， 将Qstart作为RRT算 法中搜索树的根节点， 根据后续步骤确定规划路径； M只蚂蚁将起点Qstart作为当前位置 Qcurrent； S2、 M只蚂蚁从当前位置Qcurrent开始进行匀速运动， 运动 方向为从当前位置向周围栅格 中信息素最大 的方向运动， 如果当前位置周围栅格信息素最大 的栅格有多个， 从中随机选 择一个； 设第m只蚂蚁运动到点Qrand,m， 检查Qcurrent与点Qrand,m的连线是否与障碍物发生碰 撞， 如果发生碰撞， 则将 Qrand,m丢弃； m＝1,2, …,M； 在剩下的Qrand,m中选择距离Qcurrent最近的 节点作为随机点 Qrand； S3、 以随机点Qrand为起点， 蚂蚁在信息素分布的可通行区域遍历Qrand周围所有的节点， 找到距离Qrand最近的节点 Qmin； S4、 计算障碍物到节点Qmin距离的最小值L； 如果L≥ρ， 则 蚂蚁运用定向扩展法运动 到新 节点Qnew， 若L＜ρ， 则以随机扩展法运动到新节点 Qnew； ρ 为蚂蚁的运动步长； 所述定向扩展法具体为： 以Qmin为圆心， L为 半径得到一个安全的圆形运动区， 若L≥ρ， 目的位置Qgoal给Qmin一个吸 引力F， 此时新节点 Qnew的位置为： 式中， α 为吸引系数， α ∈(0,1]； 所述随机扩展法具体如下： 以近邻点Qmin作为起点， 蚂蚁向随机点Qrand方向前进固定步长ρ， 扩展 生成一个新的节点 Qnew； S5、 判断新生成的Qnew和近邻点Qmin是否与障碍物有碰撞， 若没有碰撞则将新节点Qnew添 加到树上， 其父节 点为Qmin， 则发芽成功， 并删除所有栅格的不可扩展 标记； 若碰撞了就剔除 该新节点， 生长作废， 发芽失败， 将Qmin对应的栅格标记 为不可扩展， 重新生 成近邻点Qmin， 跳 转至步骤S4重新 生成新节点； 所述重新 生成近邻点的步骤为： 以随机点Qrand为起点， 蚂蚁在信息素分布的可通行区域遍历Qrand周围所有的节点， 找到 距离Qrand最近、 且没有不可扩展标记的节点作为 新的近邻点 Qmin； S6、 优化Qcurrent到Qnew的路径； S7、 更新Qcurrent到Qnew路径上的信息素； S8、 更新路径 表； 将Qcurrent到Qnew的路径加入路径 表； S9、 更新Qnew到Qgoal范围的信息素； S10、 当Qnew位于目的位置Qgoal的邻域或者为Qgoal本身时， 或迭代次数t达到预设 的最大 迭代次数N时， 停止迭代； 否则， 将所有可通行栅格的信息素进行蒸发， 栅格g蒸发后的信息权　利　要　求　书 1/5 页 2 CN 115091460 A 2素l′g(t)为： l′g(t)＝lg(t)(1‑σ )t； 其中σ 为蒸发系数， lg(t)为栅格g蒸发前的信息素； 令t加一， 将蚂蚁运动到的当前位置作为 Qcurrent， 跳转至步骤S2进行 下一次迭代； S11、 迭代结束， 根据路径 表得到初始规划路径； S12、 对初始规划路径采用三角不 等式优化， 得到优化后的路径， 作为 最终规划路径。 2.根据权利要求1所述的智能抓钢机机械臂路径规划方法， 其特征在于， 所述步骤S6具 体为： S6.1、 以节点Qcurrent为圆心、 以第一预设长度R为 半径构成圆形区域， 搜索树上位于 所述 圆形区域内的节 点组成集合Vnear； 以V near中的每个节 点为圆形， 第二预设长度dnear 为半 径， 构成邻域 集合VprtNear S6.2、 计算节点 Qcurrent到Qnew的路径代价Cmin： Cmin＝cost(Qcurrent,Qmin)+cost(Qmin,Qnew) 其中cost(Qcurrent,Qmin)为Qcurrent到Qmin的直线距离， cost(Qmin,Qnew)为Qmin到Qnew的直线 距离； S6.3、 遍历Vnear∪VprtNear中的节点， 如果将节点Qnew的父节点替换为Vnear∪ VprtNear中的节点Xnear后， 如果缩 短从Qcurrent到Qnew的路径长度， 则 将搜索树上Qnew的父节 点替换为Xnear。 3.根据权利要求1所述的智能抓钢机机械臂路径规划方法， 其特征在于， 所述步骤S7具 体为： 对于从Qcurrent到Qnew路径上的每一个节点q， 更新后的信息素为： lq(t+1)＝(1 ‑β )·lq(t)+Δlq(t)+Δlq*(t) 其中t表示当前迭代次数， β 为信息素挥发因子， lq(t)为节点q上一次迭代后的信息素， Δlq(t)为M只蚂蚁引起的信息素总增 加量： Q为信息素总量， Lk表示第k只蚂蚁在上一次迭代 中走过的路径总长度； Δlq*(t)为当前情况下最优路径的信息素浓度： Δlij*(t)＝λ·Δlij(t)， λ表示信息素 浓度变化系数。 4.根据权利要求1所述的智能抓钢机机械臂路径规划方法， 其特征在于， 所述步骤S9具 体为： S9.1、 计算 Qnew与Qgoal的连线与X轴的夹角 θ， 根据 θ 的值 修正导向信息素参数 μ： 其中 μ0为预设的修 正导向信息素参数初始值； S9.2、 对 θ进行矫 正， 矫正后的夹角 θ ′为：权　利　要　求　书 2/5 页 3 CN 115091460 A 3