基于势函数的机器人路径规划方法技术

本发明专利技术公开了一种基于势函数的机器人路径规划方法，其特征在于:首先采用基于势函数的随机采样方式；其次基于概率，采用与随机采样点欧式距离最近和代价函数值最小的方式选择最近邻节点，并在新节点拓展上采用两次拓展的方法加快算法的搜索效率，第一次拓展采用目标偏向的拓展策略、第二次拓展采用矩形区域随机采样的策略；然后对新节点做重新选择父节点操作和对路径做重新布线操作，得到一条从起始点到目标点的无碰撞的路径，利用删除冗余节点和最大曲率约束对生成的路径进行去除冗余节点和最大曲率约束操作，提高路径的平滑度。本发明专利技术具有能够有效地减少了搜索时间和路径的长度，同时平均采样节点数也有了明显的减少的特点。特点。特点。

【技术实现步骤摘要】
基于势函数的机器人路径规划方法


[0001]本专利技术涉及机器人运动规划领域，具体涉及一种基于势函数的机器人路径规划方法。

技术介绍

[0002]机械臂作为工业机器人的运动稳定性是机器人研究的关键工作，因此找到一条高效且无碰撞的路径对机器人的运动就显得尤为重要了。现有技术中，运动规划RRT算法在状态空间中随机采样，不需要对状态空间进行预处理，其随机性就证明了RRT算法本身具有较强的搜索能力，且算法是概率完备的，许多学者对RRT算法进行了深入的研究和应用。Jr等人提出了建立双向快速探索随机树(RRT
‑
connect)算法，两树分别根植于开始状态和目标状态，两树分别向周围空间进行探索，加快了搜索效率。KaramanS等人引入了新的算法RRT*算法，该算法是渐近最优的，算法引入重新选择父节点和重新布线操作，使解的代价收敛至最优，降低了搜索的成本，且提高了算法的效率。KimD等人提出了一种基于RRT*的采样启发式算法，局部地利用不同同伦类中的解使收敛速度更快，进而达到优化算法的目的。Nasir J等人提出了RRT*
‑
Smart，采用了RRT*的路径优化和智能采样方法，达到了加快收敛速度的目的。Adiyatov O等人提出了RRT*FN算法，当树中的节点数增加到预定设置时，则随机删除一个低成本的弱节点。Gammell J D等人提出了新算法Informed
‑
rrt*，在椭球体中进行采样，该方法提高了收敛速度和路径的质量。Qureshi A H等人在RRT*的基础上将人工势场法的思想加入到RRT*算法中得到P
‑
RRT*算法，该算法使收敛速度大大提高且减少了内存的占用。Jeong I B等人提出了Q
‑
RRT*算法，其在RRT*的基础上又考虑了新节点的父节点的父节点，扩大了父顶点的范围，还将此方法用到了重新布线的操作中，进一步提高了算法的搜索效率和路径的质量。Li Y等人提出了一种将P_RRT*和Q_RRT*算法相结合的PQ
‑
RRT*算法，充分结合两个算法的优势，进一步提高了算法的收敛速率和效率。总之，目前面对复杂的障碍物环境改进的RRT算法仍然存在效率低、收敛至最优解的速度慢、占用计算内存较大等问题；对于机器人的运动路径，存在较多转折点，未对规划出来的路径作出很好的平滑处理，严重影响了机器人的使用寿命。

技术实现思路

[0003]本专利技术的目的在于克服上述缺点而提出了一种能够有效地减少了搜索时间和路径的长度，同时平均采样节点数也有了明显的减少的基于势函数的机器人路径规划方法。
[0004]本专利技术的一种基于势函数的机器人路径规划方法，包括以下步骤：
[0005]步骤1：初始化随机树中的各个参数，包括插入起始点、目标点、障碍物、步长、目标偏向步长、障碍物；
[0006]步骤2：利用P_RRT*算法中势函数随机采样点选择方法RGD(q
rand
)进行采样，获得随机采样点q
prand
；
[0007]步骤3：获取服从均匀分布的随机数概率rand，进行最近邻节点q
nearest
选取：如果
随机数概率rand小于最近邻节点概率p
nearest
＝0.5，则最近邻节点是与随机采样点q
prand
欧式距离最近的树中的节点；如果随机概率rand大于最近邻节点概率p
nearest
＝0.5，则采用公式(1)计算代价函数C(q)，选择树中代价函数C(q)最小的点作为最近邻节点；
[0008]所述公式(1)为：
[0009][0010]其中||q
end
‑
q||为目标点q
end
与当前节点q之间的欧式距离，w
d
为距离比例系数，w
c
为混合比例系数，k表示当前节点附近圆内障碍物的个数，最近邻节点则选择树中最小的代价函数C(q)；
[0011]步骤4：利用得到的最近邻节点q
nearest
进行拓展，第一步拓展利用目标偏向策略，通过公式(2)得到新节点q
1new
，并判断新节点q
1new
是否与障碍物碰撞，如果碰撞了则返回步骤2；否则，执行步骤5；
[0012]所述公式(2)为：
[0013][0014]其中kp表示引力系数，ρ为随机树的生长步长，q
prand
表示随机采样点，q
nearest
表示最近邻节点，||q
end
‑
q
nearest
||表示目标点q
end
到最近邻节点q
nearest
的欧氏距离，||q
prand
‑
q
nearest
||表示随机采样点q
prand
到最近邻节点q
nearest
的欧式距离；
[0015]步骤5：在拓展的新节点q
1new
上，通过公式(3)进行第二步拓展，即采用矩形区域内随机选取点作为第二次拓展的得到新节点q
2new
，并判断新节点q
2new
与障碍物是否碰撞或者新节点q
2new
与新节点q
1new
之间的连线与障碍物是否碰撞，如果碰撞则执行步骤5重新进行新节点q
2new
的随机采样；否则，执行步骤6；
[0016]所述公式(3)为：
[0017][q
2new
(x)，q
2new
(y)，q
2new
(z)]＝[q
lnew
(x)，q
1new
(y)，q
1new
(z)]+r*[rand(q
1new
(x)
→
q
prand
(x))，rand(q
1new
(y)
→
q
prand
(y))，rand(q
1new
(z)
→
q
prand
(z))][0018]其中q
2new
(*)、q
1new
(*)和q
prand
(*)分别表示q
2new
、q
1new
、q
prand
的坐标值；r表示拓展比例系数，根据实验分析取r＝0.1；rand(*)为选取的随机数值；并判断q
2new
与q
1new
之间的连线是否与障碍物碰撞，如果碰撞则放弃q
2new
的拓展，重新进行q
2new
的采样，若没有碰撞则将q
2new
加入到随机树中，本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于势函数的机器人路径规划方法，其特征在于：该方法包括以下步骤：步骤1：初始化随机树中的各个参数，包括插入起始点、目标点、障碍物、步长、目标偏向步长、障碍物；步骤2：利用双向快速探索随机树算法P_RRT*中势函数随机采样点选择方法RGD(q
rand
)进行采样，获得随机采样点q
prand
；步骤3：获取服从均匀分布的随机数概率rand，进行最近邻节点q
nearest
选取：如果随机数概率rand小于最近邻节点概率p
nearest
，则最近邻节点是与随机采样点q
prand
欧式距离最近的树中的节点；如果随机概率rand大于最近邻节点概率p
nearest
，则采用公式(1)计算代价函数C(q)，选择树中代价函数C(q)最小的点作为最近邻节点；所述公式(1)为：其中||q
end
‑
q||为目标点q
end
与当前节点q之间的欧式距离，w
d
为距离比例系数，w
c
为混合比例系数，k表示当以前节点为中心、以半径r作的圆内障碍物的个数，最近邻节点则选择树中最小的代价函数C(q)；步骤4：利用得到的最近邻节点q
nearest
进行拓展，第一步拓展利用目标偏向策略，通过公式(2)得到新节点q
1new
，并判断新节点q
1new
是否与障碍物碰撞，如果碰撞了则返回步骤2；否则，执行步骤5；所述公式(2)为：其中k
p
表示引力系数，ρ表示随机树的生长步长，q
prand
表示随机采样点，q
nearest
表示最近邻节点，||q
end
‑
q
nearest
||表示目标点q
end
到最近邻节点q
nearest
的欧氏距离，||q
prand
‑
q
nearest
||表示随机采样点q
prand
到最近邻节点q
nearest
的欧式距离；步骤5：在拓展的新节点q
1new
上，通过公式(3)进行第二步拓展，即采用由q
1new
到q
prand
构成一个矩形区域内随机选取点作为第二次拓展的得到新节点q
2new
，并判断新节点q
2new
与障碍物是否碰撞或者新节点q
2new
与新节点q
1new
之间的连线与障碍物是否碰撞，如果碰撞则执行步骤5重新进行新节点q
2new
的随机采样；否则，执行步骤6；所述公式(3)为：[q
2new
(x)，q
2new
(y)，q
2new
(z)]＝[q
1new
(x)，q
1new
(y)，q
1new
(z)]+r*[rand(q
1new
(x)
→
q
prand
(x))，rand(q
1new
(y)
→
q
prand
(y))，rand(q
1new
(z)
→
q
prand
(z))]其中q
2new
(*)、q
1new
(*)和q
prand
(*)分别表示q
2new
、q
1new
、q
prand
的坐标值；r表示拓展比例系数，取r＝0.1；rand(*)为选取的随机数值；并判断q
2new
与q
1new
之间的连线是否与障碍物碰撞，如果碰撞则放弃q
2new
的拓展，重新进行q
2new
的采样，若没有碰撞则将q
2new
加入到随机树中，并将父节点赋予q
1new
；步骤6：以新节点q
2new
为中心、半径r1作邻近圆R1，为新节点q
2new
在邻近圆R1内进行重新选择父节点的操作，找到欧氏距离代价值最小，且无碰撞的新节点q
2new
的父节点；步骤7：以新节点q
2new
为中心、以半径r2作邻近圆R2，以新节点q
2new
作为父节点在邻近圆R2里进行重新布线操作，找到邻近圆R2里的点以...

【专利技术属性】
技术研发人员：袁庆霓，衣君辉，孙睿彤，白欢，杨观赐，吴杨东，蓝伟文，
申请(专利权)人：贵州大学，
类型：发明
国别省市：