本发明提供了一种针对RRT算法改进的路径优化方法。该方法在RRT算法中引入无效路点的过滤方法和自适应扩展方法，有效的快速填充局部最小值并防止过度搜素配置空间，并且通过重建联合空间中的边界点，不断改进可达空间信息，避免重复扩展无效的路点，这样可以提高搜索效率，缩短时间。它可以使路径规划算法更快的跳出局部最小区域并快去的接近目标区域。
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；
步骤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)被平滑为若干相连直线，路径优化结束。
2.根据权利要求1所述的一种针对RRT算法改进的路径优化方法，其特征在于，所述阈值τ＝P/3。
3.根据权利要求1所述的一种针对RRT算法改进的路径优化方法，其特征在于，所述欧式距离为两个路点之间的直线距离，计算公式如下：其中D(p1，p2)为任意路点p1和任意路点p2的欧式距离，(x1，y1)为任意路点p1的平面坐标；(x2，y2)任意路点p2的平面坐标。
针对RRT算法改进的路径优化方法
技术领域
[0001] 本发明属于工业机器人的路径规划领域，特别是针对RRT算法引入了无效路点的过滤方法和自适应扩展方法的路径优化方法。
背景技术
[0002] 随着现代化制造业的发展，工业机器人已广泛应用于很多领域，例如集成电路，汽车食品和其他自动化生产线。然而，由于应用广泛，工业机器人的编程技术面临了许多新的挑战；对于很多复杂的任务，传统的在线编程和离线编程方法需要花大量的时间和精力，并且不能确保得到满意的效果；因此，工业机器人的自主路劲规划已经成为当前的迫切需要。
[0003] 多年以来，机器人路径规划在机器人研究中获得了很多的关注，其相关的算法也是很全面的。相关的规划方法有概率路线图方法、人工势场方法，快速探索随机树(RRT)等。与轨迹优化相比，路径规划是机器人运动的几何描述，应为它被定义为几何路径的生成，而没有提及任何特定的时间规律。在大多数情况下，路径优化先于轨迹优化。RRT算法的开创性形式随机的将树大面积的扩展到未探测区域，并最终达到目标状态。其具有概率完备性，易于实现并且适用于解决高维问题。然而，RRT算法会慢慢地对配置空间进行统一的搜索。
基于RRT算法的一些改进算法可以在一定程度上提高搜索效率，但它们很容易陷入局部最小值。因此，当机器人操作手臂面对相对狭窄的操作环境时，原始的RRT算法不能很好地解决路劲规划问题。大多数基于RRT算法的研究都集中在算法的本身，它们在不同程度上提高了算法的效率。但是，它们并没有特别紧密地集成特定的应用环境和工艺特性。例如，对于复杂环境中机器人操纵器的路径规划。
[0004] 中国发明专利文献(CN 108444489A)于2018年8月24日公开的一种改进RRT算法的路径规划方法中，随机树在扩展过程中当遇到障碍物进行随机扩展；当没有遇到障碍物时，引入目标引力策略修正随机树的扩展方向；引入双向扩展方法，分别从起始点和目标点进行扩展。该方法存在以下不足：
[0005] (1)在随机树的扩展过程中没有考虑到多次重复到同一个路点的问题，大大降低了路径扩展的效率；
[0006] (2)在随机树的扩展过程中没有考虑到路点在将来的扩展过程中是否会与障碍物发送碰撞，若发生碰撞就会增加路径扩展时间；
[0007] 中国发明专利文献(CN 109211242A)于2019年1月15日公开的一种融合RRT与蚁群算法的三维空间多目标路径规划方法中，将各目标点之间的直线距离作为初始路径代价，将三维空间中多目标路径规划问题转变为已知路径的旅行商问题，然后用蚁群算法对该旅行商问题进行优化求解。该方法存在不足的地方为：没有考虑扩展过程中的效率问题，对无效路点大量进行扩展降低路径规划的效率。
[0008] 因此，一种针对RRT算法改进的路径优化方法具有重要的研究意义和应用价值。
发明内容
[0009] 本发明针对RRT算法所存在的不足，提出了引入无效路点的过滤方法和自适应扩展方法来对其进行优化，其目的是提高RRT算法的效率。
[0010] 本发明目的是这样实现的，本发明提供了一种针对RRT算法改进的路径优化方法，在RRT算法中引入了无效路点的过滤方法和自适应扩展方法来提高路径搜索效率，具体步骤如下：
[0011] 步骤1，参数设定
[0012] 初始化机器人扩展步长p，设置起始路点Xinit、目标路点Xgoal和阈值τ；
[0013] 步骤2，设随机树T扩展到目标路点Xgoal需要的扩展次数为n,即通过n次扩展，将随机树T扩展到目标路点Xgoal，并得到n个新路点，将n个新路点中的任意一个记为新路点Xnewi，i＝1，2，..，n；以起始路点Xinit作为第一次扩展的父路点，对随机树T进行n次扩展；记n次扩展中的任一次扩展为扩展i，扩展i中的父路点为当前父路点Xpi，i＝1，2，..，，则一次扩展的步骤如下：
[0014] 步骤2.1，从当前父路点Xpi开始扩展，在自由空间Cfree中随机任选一个探索路点Xrandi，i＝1，2，..，n，然后在随机树T中找到一个与探索路点Xrandi之间欧式距离最小的路点并记为最小路点Xneari，i＝1，2，..，n，所述自由空间Cfree为机器人避障环境中无障碍物的安全空间；
[0015] 步骤2.2，在步骤2.1得到的探索路点Xrandi和最小路点Xneari之间作连线并记为连线A，并判断连线A上是否有障碍物：
[0016] 当连线A上有障碍物时，返回步骤2.1，重新选择探索路点Xrandi；
[0017] 当连线A上没有遇到障碍物时，执行步骤2.3；
[0018] 步骤2.3，在连线A上任意位置随机选择一个路点记为新路点Xnewi；
[0019] 步骤2.4，记新路点Xnewi与当前父路点Xpi之间的欧式距离为D(Xnewi，Xpi)、新路点Xnewi和最小路点Xneari之间的欧式距离为D(Xnewi，Xneari)，进行如下判断：
[0020] 若D(Xnewi，Xpi)＞D(Xnewi，Xneari)，新路点Xnewi是无效路点，返回步骤2.3，重新选择新路点Xnewi；
[0021] 若D(Xnewi，Xpi)≤D(Xnewi，Xneari)，新路点Xnewi是有效路点，执行步骤2.5；
[0022] 步骤2.5，根据新路点Xnewi的状态值η和新路点Xnewi的碰撞指数κ之间的关系，进行以下判断：
[0023] 若η＞κ，则新路点Xnewi的未来扩展将会发生冲突，返回2.3，重新选择新路点Xnewi；
[0024] 若η值≤κ，则新路点Xnew1的未来扩展不会发生冲突,执行步骤2.6；
[0025] 步骤2.6，将步骤2.5得到的新路点Xnewi与目标路点Xgoal之间的欧式距离记为欧式距离D(Xnewi，Xgoal)，将欧式距离D(Xnewi，Xgoal)与设定的阈值τ进行比较，判断随机树T是否已经扩展到目标路点Xgoal，即判断是否达到扩展目标：
[0026] (1)若D(Xnewi，Xgoal)＞τ，则未达到扩展目标，更新随机树T得到第i+1次扩展的父路点Xp(i+1)，Xp(i+1)＝Xnewi；
[0027] 返回步骤2.1，并将第i+1次的父路点Xp(i+1)代入当前父路点Xpi进行下一次随机树T的扩展；
[0028] (2)若D(Xnewi，Xgoal)≤τ,已达到扩展目标，随机树T扩展结束，记录新路点Xnewi，并进入步骤3；
[0029] 步骤3，记录通过步骤2得到的n个新路点,并以起始路点Xinit为起点、目标路点Xgoal为终点，将n个新路点与起始路点Xinit、目标路点Xgoal顺序连接得到路径队列(Xinit，Xnew1，Xnew2....Xnewn，Xgoal)；
[0030] 步骤4，对路径队列(Xinit，Xnew1，Xnew2，...Xnewn，Xgoal)进行平滑处理；
[0031] 步骤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；
[0032] 步骤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)被平滑为若干相连直线，路径优化结束。
[0033] 优选地，所述阈值τ＝P/3。
[0034] 优选地，所述欧式距离为两个路点之间的直线距离，计算公式如下：
[0035]
[0036] 其中D(p1，p2)为任意路点p1和任意路点p2的欧式距离，(x1，y1)为任意路点ｐ1的平面坐标；(x2，y2)任意路点ｐ２的平面坐标。
[0037] 与现有技术相比，本发明的有益效果为：
[0038] 本发明可以有效的快速填充局部最小值并防止过度搜素配置空间，并且通过重建联合空间中的边界点，不断改进可达空间信息，避免重复扩展无效的路点，这样可以提高搜索效率，缩短时间。它可以使路径规划算法更快的跳出局部最小区域并快去的接近目标区域。
附图说明
[0039] 图1是本发明针对RRT算法改进的路径优化方法的流程图。
[0040] 图2是本发明的路径扩展中无效路点的示意图。
具体实施方式
[0041] 下面将结合附图对本发明的技术方案进行清楚、完整的描述。
[0042] 图1是本发明针对RRT算法改进的路径优化方法的流程图。从图1可见，本发明提供了一种针对RRT算法改进的路径优化方法，在RRT算法中引入了无效路点的过滤方法和自适应扩展方法来提高路径搜索效率，具体步骤如下：
[0043] 步骤1，参数设定
[0044] 初始化机器人扩展步长p，设置起始路点Xinit、目标路点Xgoal和阈值τ。所述阈值τ＝P/3。
[0045] 步骤2，设随机树T扩展到目标路点Xgoal需要的扩展次数为n,即通过n次扩展，将随机树T扩展到目标路点Xgoal，并得到n个新路点，将n个新路点中的任意一个记为新路点Xnewi，i＝1，2，..，n。
[0046] 以起始路点Xinit作为第一次扩展的父路点，对随机树T进行n次扩展；记n次扩展中的任一次扩展为扩展i，扩展i中的父路点为当前父路点Xpi，i＝1，2，..，n，则一次扩展的步骤如下：
[0047] 步骤2.1，从当前父路点Xpi开始扩展，在自由空间Cfree中随机任选一个探索路点Xrandi，i=1，2，..，n，然后在随机树T中找到一个与探索路点Xrandi之间欧式距离最小的路点并记为最小路点Xneari，i=1，2，..，n，所述自由空间Cfree为机器人避障环境中无障碍物的安全空间。
[0048] 步骤2.2，在步骤2.1得到的探索路点Xrandi和最小路点Xneari之间作连线并记为连线A，并判断连线A上是否有障碍物：
[0049] 当连线A上有障碍物时，返回步骤2.1，重新选择探索路点Xrandi；
[0050] 当连线A上没有遇到障碍物时，执行步骤2.3。
[0051] 步骤2.3，在连线A上任意位置随机选择一个路点记为新路点Xnewi。
[0052] 步骤2.4，判断新路点Xnewi是否满足无效路点的过滤条件。具体的，记新路点Xnewi与当前父路点Xpi之间的欧式距离为D(Xnewi，Xpi)、新路点Xnewi和最小路点Xneari之间的欧式距离D(Xnewi，Xneari)，进行如下判断：
[0053] 若D(Xnewi，Xpi)＞D(Xnewi，Xneari)，新路点Xnewi是无效路点，返回步骤2.3，重新选择新路点Xnewi；
[0054] 若D(Xnewi，Xpi)≤D(Xnewi，Xneari)，新路点Xnewi是有效路点，执行步骤2.5。
[0055] 步骤2.5，判断新路点Xnewi是否满足自适应扩展的条件。具体的，根据新路点Xnewi的状态值η和新路点Xnewi的碰撞指数κ之间的关系，进行以下判断：
[0056] 若η＞κ，则新路点Xnewi的未来扩展将会发生冲突，返回2.3，重新选择新路点Xnewi；
[0057] 若η值≤κ，则新路点Xnew1的未来扩展不会发生冲突,执行步骤2.6。
[0058] 所述碰撞指数是导致机器人操作手臂与连接到路点的所有边缘发生碰撞的指数。新路点Xnew1的κ值越小，新路点Xnew1成功扩展的可能性越大。在本发明中，令κ＝0.8。
[0059] 所述路点的状态值是衡量路点是否可以进行自由扩展的参数。在随机树T的扩展过程中，若所有路点的状态值η的初始值都是0，即表示所有路点可以自由扩展。假设当前有r条边连接到路点q，若经碰撞检测q的子路点与障碍物发生碰撞，q路点的η值增加 若
经碰撞检测下一个子路点没有与障碍物发生碰撞时，q路点的η值减 在本发明中，设新
路点Xnewi的状态值η的初始值为0，即新路点Xnewi可以自由扩展。
[0060] 步骤2.6，将步骤2.5得到的新路点Xnewi与目标路点Xgoal之间的欧式距离记为欧式距离D(Xnewi，Xgoal)，将欧式距离D(Xnewi，Xgoal)与设定的阈值τ进行比较，判断随机树T是否已经扩展到目标路点Xgoal，即判断是否达到扩展目标：
[0061] (1)若D(Xnewi，Xgoal)＞τ，则未达到扩展目标，更新随机树T得到第i+1次扩展的父路点Xp(i+1)，Xp(i+1)＝Xnewi；
[0062] 返回步骤2.1，并将第i+1次的父路点Xp(i+1)代入当前父路点Xpi进行下一次随机树T的扩展。
[0063] (2)若D(Xnewi，Xgoal)≤τ,已达到扩展目标，随机树T扩展结束，记录新路点Xnewi，并进入步骤3。
[0064] 步骤3，记录通过步骤2得到的n个新路点,并以起始路点Xinit为起点、目标路点Xgoal为终点，将n个新路点与起始路点Xinit、目标路点Xgoal顺序连接得到路径队列(Xinit，Xnew1，Xnew2，...，Xnewn，Xgoal)。
[0065] 步骤4，对路径队列(Xinit，Xnew1，Xnew2，...，Xnewn，Xgoal)进行平滑处理。
[0066] 步骤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。
[0067] 步骤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)被平滑为若干相连直线，路径优化结束。
[0068] 在以上步骤中，所述的欧式距离为两个路点之间的直线距离，计算公式如下：
[0069]
[0070] 其中D(p1，p2)为任意路点ｐ1和任意路点p2的欧式距离，(x1，y1)为任意路点p1的平面坐标；(x2，y2)任意路点p2的平面坐标。
[0071] 图2是本发明的路径扩展中无效路点的示意图。由图2可见，从当前父路点Xpi出发，通过最小路点Xneari，找到新路点Xnewi的途径。因为D(Xnewi，Xpi)＞D(Xnewi，Xneari)，所以该新路点为无效路点。另外通过本发明，已经将路径队列平滑成若干相连直线。
