[发明专利]针对RRT算法改进的路径优化方法

摘要

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

主权项

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