一种智能抓钢机机械臂路径规划方法及规划系统技术方案

本发明专利技术公开了一种智能抓钢机机械臂路径规划方法和规划系统，其中路径规划方法包括：建立械臂二维工作场景；生成随机点Q

【技术实现步骤摘要】
一种智能抓钢机机械臂路径规划方法及规划系统


[0001]本专利技术属于机械臂运动控制领域，具体涉及一种智能抓钢机机械臂路径规划方法及规划系统。

技术介绍

[0002]抓钢机的机械臂运动规划问题可以转化为二维场景中的路径规划问题，即在存在障碍物的给定区域内将物体从起点移动至目的位置。目前广泛使用RRT*算法来进行路径规划。该算法中通过在状态空间随机采样来生成新节点，并不断优化起点到新节点的路径，从而得到一条从起点到目的位置的路径。随机采样具有盲目性，无法保证新节点较优，从而导致优化时间较长，减低了效率。

技术实现思路

[0003]专利技术目的：针对现有技术中存在的问题，本专利技术提供一种智能抓钢机机械臂路径规划方法，该方法解决了RRT*算法规划时间长、效率低的问题，能够根据具体的工作场景快速规划出抓钢机机械臂的路径。
[0004]技术方案：本专利技术一方面公开了一种智能抓钢机机械臂路径规划方法，包括步骤：
[0005]S1、根据抓钢需求，获取机械臂二维工作场景，对所述二维工作场景进行栅格化处理，并设置每个栅格为可通行或障碍物；在可通行栅格喷洒等量信息素，即可通行栅格的信息素初始值相等，设为l0；获取机械臂的起点位置Q
start
和目的位置Q
goal
；如果从Q
start
到Q
goal
的直线路径没有碰触障碍物，则规划路径为从Q
start
到Q
goal
的直线段；否则，将Q
start
作为RRT算法中搜索树的根节点，根据后续步骤确定规划路径；M只蚂蚁将起点Q
start
作为当前位置Q
current
；
[0006]S2、M只蚂蚁从当前位置Q
current
开始进行匀速运动，运动方向为从当前位置向周围栅格中信息素最大的方向运动，如果当前位置周围栅格信息素最大的栅格有多个，从中随机选择一个；设第m只蚂蚁运动到点Q
rand,m
，检查Q
current
与点Q
rand,m
的连线是否与障碍物发生碰撞，如果发生碰撞，则将点Q
rand,m
丢弃；m＝1,2,
…
,M；在剩下的Q
rand,m
中选择距离Q
current
最近的节点作为随机点Q
rand
；
[0007]S3、以随机点Q
rand
为起点，蚂蚁在信息素分布的可通行区域遍历Q
rand
周围所有的节点，找到距离Q
rand
最近的节点Q
min
；
[0008]S4、计算障碍物到节点Q
min
距离的最小值L；如果L≥ρ，则蚂蚁运用定向扩展法运动到新节点Q
new
，若L＜ρ，则以随机扩展法运动到新节点Q
new
；ρ为蚂蚁的运动步长；
[0009]所述定向扩展法具体为：
[0010]以Q
min
为圆心，L为半径得到一个安全的圆形运动区，若L≥ρ，目的位置Q
goal
给Q
min
一个吸引力F，此时新节点Q
new
的位置为：
[0011][0012]式中，α为吸引系数，α∈(0,1]；
[0013]所述随机扩展法具体如下：
[0014]以近邻点Q
min
作为起点，蚂蚁向随机点Q
rand
方向前进固定步长ρ，扩展生成一个新的节点Q
new
；
[0015]S5、判断新生成的Q
new
和近邻点Q
min
是否与障碍物有碰撞，若没有碰撞则将新节点Q
new
添加到树上，其父节点为Q
min
，则发芽成功，并删除所有栅格的不可扩展标记；若碰撞了就剔除该新节点，生长作废，发芽失败，将Q
min
对应的栅格标记为不可扩展，重新生成近邻点Q
min
，跳转至步骤S4重新生成新节点；
[0016]所述重新生成近邻点的步骤为：
[0017]以随机点Q
rand
为起点，蚂蚁在信息素分布的可通行区域遍历Q
rand
周围所有的节点，找到距离Q
rand
最近、且没有不可扩展标记的节点作为新的近邻点Q
min
；
[0018]S6、优化Q
current
到Q
new
的路径；
[0019]S7、更新Q
current
到Q
new
路径上的信息素；
[0020]S8、更新路径表；将Q
current
到Q
new
的路径加入路径表；
[0021]S9、更新Q
new
到Q
goal
范围的信息素；
[0022]S10、当Q
new
位于目的位置Q
goal
的邻域或者为Q
goal
本身时，或迭代次数t达到预设的最大迭代次数N时，停止迭代；否则，将所有可通行栅格的信息素进行蒸发，栅格g蒸发后的信息素l
g
′
(t)为：l
g
′
(t)＝l
g
(t)(1
‑
σ)
t
；
[0023]其中σ为蒸发系数，l
g
(t)为栅格g蒸发前的信息素；
[0024]令t加一，将蚂蚁运动到的当前位置作为Q
current
，跳转至步骤S2进行下一次迭代；
[0025]S11、迭代结束，根据路径表得到初始规划路径；
[0026]S12、对初始规划路径采用三角不等式优化，得到优化后的路径，作为最终规划路径。
[0027]具体地，所述步骤S6具体为：
[0028]S6.1、以节点Q
current
为圆心、以第一预设长度R为半径构成圆形区域，搜索树上位于所述圆形区域内的节点组成集合Vnear；以Vnear中的每个节点为圆形，第二预设长度dnear为半径，构成邻域集合VprtNear
[0029]S6.2、计算节点Q
current
到Q
new
的路径代价C
min
：
[0030]C
min
＝cost(Q
current
,Q
min
)+cost(Q
min
,Q
new
)
[0031]其中cost(Q
current
,Q
min
)为Q...

【技术保护点】

【技术特征摘要】
1.一种智能抓钢机机械臂路径规划方法，其特征在于，包括步骤：S1、根据抓钢需求，获取机械臂二维工作场景，对所述二维工作场景进行栅格化处理，并设置每个栅格为可通行或障碍物；在可通行栅格喷洒等量信息素，即可通行栅格的信息素初始值相等，设为l0；获取机械臂的起点位置Q
start
和目的位置Q
goal
；如果从Q
start
到Q
goal
的直线路径没有碰触障碍物，则规划路径为从Q
start
到Q
goal
的直线段；否则，将Q
start
作为RRT算法中搜索树的根节点，根据后续步骤确定规划路径；M只蚂蚁将起点Q
start
作为当前位置Q
current
；S2、M只蚂蚁从当前位置Q
current
开始进行匀速运动，运动方向为从当前位置向周围栅格中信息素最大的方向运动，如果当前位置周围栅格信息素最大的栅格有多个，从中随机选择一个；设第m只蚂蚁运动到点Q
rand,m
，检查Q
current
与点Q
rand,m
的连线是否与障碍物发生碰撞，如果发生碰撞，则将Q
rand,m
丢弃；m＝1,2,
…
,M；在剩下的Q
rand,m
中选择距离Q
current
最近的节点作为随机点Q
rand
；S3、以随机点Q
rand
为起点，蚂蚁在信息素分布的可通行区域遍历Q
rand
周围所有的节点，找到距离Q
rand
最近的节点Q
min
；S4、计算障碍物到节点Q
min
距离的最小值L；如果L≥ρ，则蚂蚁运用定向扩展法运动到新节点Q
new
，若L＜ρ，则以随机扩展法运动到新节点Q
new
；ρ为蚂蚁的运动步长；所述定向扩展法具体为：以Q
min
为圆心，L为半径得到一个安全的圆形运动区，若L≥ρ，目的位置Q
goal
给Q
min
一个吸引力F，此时新节点Q
new
的位置为：式中，α为吸引系数，α∈(0,1]；所述随机扩展法具体如下：以近邻点Q
min
作为起点，蚂蚁向随机点Q
rand
方向前进固定步长ρ，扩展生成一个新的节点Q
new
；S5、判断新生成的Q
new
和近邻点Q
min
是否与障碍物有碰撞，若没有碰撞则将新节点Q
new
添加到树上，其父节点为Q
min
，则发芽成功，并删除所有栅格的不可扩展标记；若碰撞了就剔除该新节点，生长作废，发芽失败，将Q
min
对应的栅格标记为不可扩展，重新生成近邻点Q
min
，跳转至步骤S4重新生成新节点；所述重新生成近邻点的步骤为：以随机点Q
rand
为起点，蚂蚁在信息素分布的可通行区域遍历Q
rand
周围所有的节点，找到距离Q
rand
最近、且没有不可扩展标记的节点作为新的近邻点Q
min
；S6、优化Q
current
到Q
new
的路径；S7、更新Q
current
到Q
new
路径上的信息素；S8、更新路径表；将Q
current
到Q
new
的路径加入路径表；S9、更新Q
new
到Q
goal
范围的信息素；S10、当Q
new
位于目的位置Q
goal
的邻域或者为Q
goal
本身时，或迭代次数t达到预设的最大迭代次数N时，停止迭代；否则，将所有可通行栅格的信息素进行蒸发，栅格g蒸发后的信息
素l
′
g
(t)为：l
′
g
(t)＝l
g
(t)(1
‑
σ)
t
；其中σ为蒸发系数，l
g
(t)为栅格g蒸发前的信息素；令t加一，将蚂蚁运动到的当前位置作为Q
current
，跳转至步骤S2进行下一次迭代；S11、迭代结束，根据路径表得到初始规划路径；S12、对初始规划路径采用三角不等式优化，得到优化后的路径，作为最终规划路径。2.根据权利要求1所述的智能抓钢机机械臂路径规划方法，其特征在于，所述步骤S6具体为：S6.1、以节点Q
current
为圆心、以第一预设长度R为半径构成圆形区域，搜索树上位于所述圆形区域内的节点组成集合Vnear；以Vnear中的每个节点为圆形，第二预设长度dnear为半径，构成邻域集合VprtNearS6.2、计算节点Q
current
到Q
new
的路径代价C
min
：C
min
＝cost(Q
current
,Q
min
)+cost(Q
min
,Q
new
)其中cost(Q
current
,Q
min
)为Q
current
到Q
min
的直线距离，cost(Q
min
,Q
new
)为Q
min
到Q
new
的直线距离；S6.3、遍历Vnear∪VprtNear中的节点，如果将节点Q
new
的父节点替换为Vnear∪VprtNear中的节点Xnear后，如果缩短从Q
current
到Q
new
的路径长度，则将搜索树上Q
new
的父节点替换为Xnear。3.根据权利要求1所述的智能抓钢机机械臂路径规划方法，其特征在于，所述步骤S7具体为：对于从Q
current
到Q
new
路径上的每一个节点q，更新后的信息素为：l
q
(t+1)＝(1
‑
β)
·
l
q
(t)+Δl
q
(t)+Δl
q
*(t)其中t表示当前迭代次数，β为信息素挥发因子，l
q
(t)为节点q上一次迭代后的信息素，Δl
q
(t)为M只蚂蚁引起的信息素总增加量：加量：Q为信息素总量，L
k
表示第k只蚂蚁在上一次迭代中走过的路径总长度；Δl
q
*(t)为当前情况下最优路径的信息素浓度：Δl
ij
*(t)＝λ
·
Δl
ij
(t)，λ表示信息素浓度变化系数。4.根据权利要求1所述的智能抓钢机机械臂路径规划方法，其特征在于，所述步骤S9具体为：S9.1、计算Q
new
与Q
goal
的连线与X轴的夹角θ，根据θ的值修正导向信息素参数μ：其中μ0为预设的修正导向信息素参数初始值；S9.2、对θ进行矫正，矫正后的夹角θ
′
为：
其中x
Qgoal
，y
Qgoal
分别为目的位置Q
goal
在二维场景中的横纵坐标值；x
Qnew
，y
Qnew
分别为节点Q
new
在二维场景中的横纵坐标值；S9.3、根据矫正后的夹角θ
′
得到矫正新节点Q
′
new
，更新Q
′
new
到Q
goal
连线上可通行节点的信息素：设节点p为Q
′
new
...

【专利技术属性】
技术研发人员：马国军，段云龙，胡和荣，甄友，王志明，张博文，
申请(专利权)人：江苏大隆凯科技有限公司，
类型：发明
国别省市：