针对RRT算法改进的路径优化方法技术

本发明专利技术提供了一种针对RRT算法改进的路径优化方法。该方法在RRT算法中引入无效路点的过滤方法和自适应扩展方法，有效的快速填充局部最小值并防止过度搜素配置空间，并且通过重建联合空间中的边界点，不断改进可达空间信息，避免重复扩展无效的路点，这样可以提高搜索效率，缩短时间。它可以使路径规划算法更快的跳出局部最小区域并快去的接近目标区域。

An improved path optimization method for RRT algorithm

【技术实现步骤摘要】
针对RRT算法改进的路径优化方法
本专利技术属于工业机器人的路径规划领域，特别是针对RRT算法引入了无效路点的过滤方法和自适应扩展方法的路径优化方法。
技术介绍
随着现代化制造业的发展，工业机器人已广泛应用于很多领域，例如集成电路，汽车食品和其他自动化生产线。然而，由于应用广泛，工业机器人的编程技术面临了许多新的挑战；对于很多复杂的任务，传统的在线编程和离线编程方法需要花大量的时间和精力，并且不能确保得到满意的效果；因此，工业机器人的自主路劲规划已经成为当前的迫切需要。多年以来，机器人路径规划在机器人研究中获得了很多的关注，其相关的算法也是很全面的。相关的规划方法有概率路线图方法、人工势场方法，快速探索随机树(RRT)等。与轨迹优化相比，路径规划是机器人运动的几何描述，应为它被定义为几何路径的生成，而没有提及任何特定的时间规律。在大多数情况下，路径优化先于轨迹优化。RRT算法的开创性形式随机的将树大面积的扩展到未探测区域，并最终达到目标状态。其具有概率完备性，易于实现并且适用于解决高维问题。然而，RRT算法会慢慢地对配置空间进行统一的搜索。基于RRT算法的一些改进算法可以在一定程度上提高搜索效率，但它们很容易陷入局部最小值。因此，当机器人操作手臂面对相对狭窄的操作环境时，原始的RRT算法不能很好地解决路劲规划问题。大多数基于RRT算法的研究都集中在算法的本身，它们在不同程度上提高了算法的效率。但是，它们并没有特别紧密地集成特定的应用环境和工艺特性。例如，对于复杂环境中机器人操纵器的路径规划。中国专利技术专利文献(CN108444489A)于2018年8月24日公开的一种改进RRT算法的路径规划方法中，随机树在扩展过程中当遇到障碍物进行随机扩展；当没有遇到障碍物时，引入目标引力策略修正随机树的扩展方向；引入双向扩展方法，分别从起始点和目标点进行扩展。该方法存在以下不足：(1)在随机树的扩展过程中没有考虑到多次重复到同一个路点的问题，大大降低了路径扩展的效率；(2)在随机树的扩展过程中没有考虑到路点在将来的扩展过程中是否会与障碍物发送碰撞，若发生碰撞就会增加路径扩展时间；中国专利技术专利文献(CN109211242A)于2019年1月15日公开的一种融合RRT与蚁群算法的三维空间多目标路径规划方法中，将各目标点之间的直线距离作为初始路径代价，将三维空间中多目标路径规划问题转变为已知路径的旅行商问题，然后用蚁群算法对该旅行商问题进行优化求解。该方法存在不足的地方为：没有考虑扩展过程中的效率问题，对无效路点大量进行扩展降低路径规划的效率。因此，一种针对RRT算法改进的路径优化方法具有重要的研究意义和应用价值。
技术实现思路
本专利技术针对RRT算法所存在的不足，提出了引入无效路点的过滤方法和自适应扩展方法来对其进行优化，其目的是提高RRT算法的效率。本专利技术目的是这样实现的，本专利技术提供了一种针对RRT算法改进的路径优化方法，在RRT算法中引入了无效路点的过滤方法和自适应扩展方法来提高路径搜索效率，具体步骤如下：步骤1，参数设定初始化机器人扩展步长p，设置起始路点Xinit、目标路点Xgoal和阈值τ；步骤2，设随机树T扩展到目标路点Xgoal需要的扩展次数为n,即通过n次扩展，将随机树T扩展到目标路点Xgoal，并得到n个新路点，将n个新路点中的任意一个记为新路点Xnewi，i＝1，2，..，n；以起始路点Xinit作为第一次扩展的父路点，对随机树T进行n次扩展；记n次扩展中的任一次扩展为扩展i，扩展i中的父路点为当前父路点Xpi，i＝1，2，..，，则一次扩展的步骤如下：步骤2.1，从当前父路点Xpi开始扩展，在自由空间Cfree中随机任选一个探索路点Xrandi，i＝1，2，..，n，然后在随机树T中找到一个与探索路点Xrandi之间欧式距离最小的路点并记为最小路点Xneari，i＝1，2，..，n，所述自由空间Cfree为机器人避障环境中无障碍物的安全空间；步骤2.2，在步骤2.1得到的探索路点Xrandi和最小路点Xneari之间作连线并记为连线A，并判断连线A上是否有障碍物：当连线A上有障碍物时，返回步骤2.1，重新选择探索路点Xrandi；当连线A上没有遇到障碍物时，执行步骤2.3；步骤2.3，在连线A上任意位置随机选择一个路点记为新路点Xnewi；步骤2.4，记新路点Xnewi与当前父路点Xpi之间的欧式距离为D(Xnewi，Xpi)、新路点Xnewi和最小路点Xneari之间的欧式距离为D(Xnewi，Xneari)，进行如下判断：若D(Xnewi，Xpi)＞D(Xnewi，Xneari)，新路点Xnewi是无效路点，返回步骤2.3，重新选择新路点Xnewi；若D(Xnewi，Xpi)≤D(Xnewi，Xneari)，新路点Xnewi是有效路点，执行步骤2.5；步骤2.5，根据新路点Xnewi的状态值η和新路点Xnewi的碰撞指数κ之间的关系，进行以下判断：若η＞κ，则新路点Xnewi的未来扩展将会发生冲突，返回2.3，重新选择新路点Xnewi；若η值≤κ，则新路点Xnew1的未来扩展不会发生冲突,执行步骤2.6；步骤2.6，将步骤2.5得到的新路点Xnewi与目标路点Xgoal之间的欧式距离记为欧式距离D(Xnewi，Xgoal)，将欧式距离D(Xnewi，Xgoal)与设定的阈值τ进行比较，判断随机树T是否已经扩展到目标路点Xgoal，即判断是否达到扩展目标：(1)若D(Xnewi，Xgoal)＞τ，则未达到扩展目标，更新随机树T得到第i+1次扩展的父路点Xp(i+1)，Xp(i+1)＝Xnewi；返回步骤2.1，并将第i+1次的父路点Xp(i+1)代入当前父路点Xpi进行下一次随机树T的扩展；(2)若D(Xnewi，Xgoal)≤τ,已达到扩展目标，随机树T扩展结束，记录新路点Xnewi，并进入步骤3；步骤3，记录通过步骤2得到的n个新路点,并以起始路点Xinit为起点、目标路点Xgoal为终点，将n个新路点与起始路点Xinit、目标路点Xgoal顺序连接得到路径队列(Xinit，Xnew1，Xnew2....Xnewn，Xgoal)；步骤4，对路径队列(Xinit，Xnew1，Xnew2，...Xnewn，Xgoal)进行平滑处理；步骤4.1，令起始路点Xinit为起点，依次尝试连接路径队列(Xinit，Xnew1，Xnew2，...，Xnewn，Xgoal)中的每一个路点，若在连接过程中发现路径队列(Xinit，Xnew1，Xnew2，...，Xnewn，Xgoal)中的一个路点与该路点前相邻的两个路点不在同一条直线上,则记该路点为不可到达路点Xnewj，用一条直线连接起始路点与路点Xnew(j-1)得到一条平滑路径(Xinit，Xnew1，Xnew2，...，Xnew(j-1))；j＝1,2...m,m为正整数，m≤n；步骤4.2，首先从路径队列(Xinit，Xnew1，Xnew2，...，Xnewn，Xgoal)中删除步骤4.1得到的平滑路径(Xinit，Xnew1，Xnew2，...，Xnew(j-1))，，得到一条剩余路径队列(Xnew(j-1)，Xnewj，...，Xnewn，Xg本文档来自技高网...

【技术保护点】
1.一种针对RRT算法改进的路径优化方法，其特征在于，在RRT算法中引入了无效路点的过滤方法和自适应扩展方法来提高路径搜索效率，具体步骤如下：步骤1，参数设定初始化机器人扩展步长p，设置起始路点Xinit、目标路点Xgoal和阈值τ；步骤2，设随机树T扩展到目标路点Xgoal需要的扩展次数为n，即通过n次扩展，将随机树T扩展到目标路点Xgoal，并得到n个新路点，将n个新路点中的任意一个记为新路点Xnewi，i＝1，2，..，n；以起始路点Xinit作为第一次扩展的父路点，对随机树T进行n次扩展；记n次扩展中的任一次扩展为扩展i，扩展i中的父路点为当前父路点Xpi，i＝1，2，..，n，则一次扩展的步骤如下：步骤2.1，从当前父路点Xpi开始扩展，在自由空间Cfree中随机任选一个探索路点Xrandi，i＝1，2，..，n，然后在随机树T中找到一个与探索路点Xrandi之间欧式距离最小的路点并记为最小路点Xneari，i＝1，2，..，n，所述自由空间Cfree为机器人避障环境中无障碍物的安全空间；步骤2.2，在步骤2.1得到的探索路点Xrandi和最小路点Xneari之间作连线并记为连线A，并判断连线A上是否有障碍物：当连线A上有障碍物时，返回步骤2.1，重新选择探索路点Xrandi；当连线A上没有遇到障碍物时，执行步骤2.3；步骤2.3，在连线A上任意位置随机选择一个路点记为新路点Xnewi；步骤2.4，记新路点Xnewi与当前父路点Xpi之间的欧式距离为D(Xnewi，Xpi)、新路点Xnewi和最小路点Xneari之间的欧式距离为D(Xnewi，Xneari)，进行如下判断：若D(Xnewi，Xpi)＞D(Xnewi，Xneari)，新路点Xnewi是无效路点，返回步骤2.3，重新选择新路点Xnewi；若D(Xnewi，Xpi)≤D(Xnewi，Xneari)，新路点Xnewi是有效路点，执行步骤2.5；步骤2.5，根据新路点Xnewi的状态值η和新路点Xnewi的碰撞指数κ之间的关系，进行以下判断：若η＞κ，则新路点Xnewi的未来扩展将会发生冲突，返回2.3，重新选择新路点Xnewi；若η值≤κ，则新路点Xnew1的未来扩展不会发生冲突，执行步骤2.6；步骤2.6，将步骤2.5得到的新路点Xnewi与目标路点Xgoal之间的欧式距离记为欧式距离D(Xnewi，Xgoal)，将欧式距离D(Xnewi，Xgoal)与设定的阈值τ进行比较，判断随机树T是否已经扩展到目标路点Xgoal，即判断是否达到扩展目标：(1)若D(Xnewi，Xgoal)＞τ，则未达到扩展目标，更新随机树T得到第i+1次扩展的父路点Xp(i+1)，Xp(i+1)＝Xnewi；返回步骤2.1，并将第i+1次的父路点Xp(i+1)代入当前父路点Xpi进行下一次随机树T的扩展；(2)若D(Xnewi，Xgoal)≤τ，已达到扩展目标，随机树T扩展结束，记录新路点Xnewi，并进入步骤3；步骤3，记录通过步骤2得到的n个新路点，并以起始路点Xinit为起点、目标路点Xgoal为终点，将n个新路点与起始路点Xinit、目标路点Xgoal顺序连接得到路径队列(Xinit，Xnew1，Xnew2....Xnewn，Xgoal)；步骤4，对路径队列(Xinit，Xnew1，Xnew2，...Xnewn，Xgoal)进行平滑处理；步骤4.1，令起始路点Xinit为起点，依次尝试连接路径队列(Xinit，Xnew1，Xnew2，...，Xnewn，Xgoal)中的每一个路点，若在连接过程中发现路径队列(Xinit，Xnew1，Xnew2，...，Xnewn，Xgoal)中的一个路点与该路点前相邻的两个路点不在同一条直线上，则记该路点为不可到达路点Xnewj，用一条直线连接起始路点Xinit与路点Xnew(j‑1)得到一条平滑路径(Xinit，Xnew1，Xnew2，...，Xnew(j‑1))；j＝1，2...m，m为正整数，m≤n；步骤4.2，首先从路径队列(Xinit，Xnew1，Xnew2，...，Xnewn，Xgoal)中删除步骤4.1得到的平滑路径(Xinit，Xnew1，Xnew2，...，Xnew(j‑1))，得到一条剩余路径队列(Xnew(j‑1)，Xnewj，...，Xnewn，Xgoal)，然后以路点Xnew(j‑1)为起点，按步骤4.1的方法依次尝试连接剩余路径队列(Xnew(j‑1)，Xnewj，...，Xnewn，Xgoal)中的每一个路点，又得到一条平滑路径；依次类推，当起点为目标路点xgoal时，共得到若干条平滑路径，即路径队列(Xinit，Xnew1，Xnew2，...Xnewn，Xgoal)被平滑为若干相连直线，路径优化结束。...

【技术特征摘要】
1.一种针对RRT算法改进的路径优化方法，其特征在于，在RRT算法中引入了无效路点的过滤方法和自适应扩展方法来提高路径搜索效率，具体步骤如下：步骤1，参数设定初始化机器人扩展步长p，设置起始路点Xinit、目标路点Xgoal和阈值τ；步骤2，设随机树T扩展到目标路点Xgoal需要的扩展次数为n，即通过n次扩展，将随机树T扩展到目标路点Xgoal，并得到n个新路点，将n个新路点中的任意一个记为新路点Xnewi，i＝1，2，..，n；以起始路点Xinit作为第一次扩展的父路点，对随机树T进行n次扩展；记n次扩展中的任一次扩展为扩展i，扩展i中的父路点为当前父路点Xpi，i＝1，2，..，n，则一次扩展的步骤如下：步骤2.1，从当前父路点Xpi开始扩展，在自由空间Cfree中随机任选一个探索路点Xrandi，i＝1，2，..，n，然后在随机树T中找到一个与探索路点Xrandi之间欧式距离最小的路点并记为最小路点Xneari，i＝1，2，..，n，所述自由空间Cfree为机器人避障环境中无障碍物的安全空间；步骤2.2，在步骤2.1得到的探索路点Xrandi和最小路点Xneari之间作连线并记为连线A，并判断连线A上是否有障碍物：当连线A上有障碍物时，返回步骤2.1，重新选择探索路点Xrandi；当连线A上没有遇到障碍物时，执行步骤2.3；步骤2.3，在连线A上任意位置随机选择一个路点记为新路点Xnewi；步骤2.4，记新路点Xnewi与当前父路点Xpi之间的欧式距离为D(Xnewi，Xpi)、新路点Xnewi和最小路点Xneari之间的欧式距离为D(Xnewi，Xneari)，进行如下判断：若D(Xnewi，Xpi)＞D(Xnewi，Xneari)，新路点Xnewi是无效路点，返回步骤2.3，重新选择新路点Xnewi；若D(Xnewi，Xpi)≤D(Xnewi，Xneari)，新路点Xnewi是有效路点，执行步骤2.5；步骤2.5，根据新路点Xnewi的状态值η和新路点Xnewi的碰撞指数κ之间的关系，进行以下判断：若η＞κ，则新路点Xnewi的未来扩展将会发生冲突，返回2.3，重新选择新路点Xnewi；若η值≤κ，则新路点Xnew1的未来扩展不会发生冲突，执行步骤2.6；步骤2.6，将步骤2.5得到的新路点Xnewi与目标路点Xgoal之间的欧式距离记为欧式距离D(Xnewi，Xgoal)，将欧式距离D(Xnewi，Xgoal)与设定的阈值τ进行比较，判断随机树T是否已经扩展到目标路点Xgoal，即判断是否达到扩展目标：(1)若D(Xnewi...

【专利技术属性】
技术研发人员：朱敏，吴志伟，储昭碧，董学平，
申请(专利权)人：合肥工业大学，
类型：发明
国别省市：安徽,34