CN110196602A - 目标导向集中优化的快速水下机器人三维路径规划方法 - Google Patents

Abstract

本发明公开一种目标导向集中优化的快速水下机器人三维路径规划方法，在RRT^*路径规划方法中引进目标导向的高斯采样策略，减少了水下障碍物分布稀少时采样点的随机性；并结合随机扰动进行采样，可以使随机树合理地远离障碍区域，避免陷入局部最小值，快速地将搜索导向目标；并采用集中优化搜索策略对本发明获得的初始路径进行路径优化处理，提高优化收敛速率和路径的质量，实现渐进最优。本发明改进方法与传统RRT^*方法相比，规划出来的初始路径更优，路径优化的速度更快，大大减少了时间和内存的消耗。

Description

目标导向集中优化的快速水下机器人三维路径规划方法

技术领域

本发明属于移动机器人路径规划技术领域，涉及一种水下机器人三维路径规划方法。

背景技术

机器人水下导航是导航技术的重要应用和新发展，路径规划是导航研究的一个重要环节和课题，是指机器人在当前环境中按照一定标准搜索出一条从起始状态点到目标状态点，并且能够绕开障碍物的最优或次优路径。水下机器人的三维路径规划必须针对水下地形障碍和漂浮障碍通过有效的方法寻找路径。目前全局路径规划方法主要有A^*、蚁群算法、RRT等。A^*依赖启发式函数，不能很好地处理高维和大范围等问题。蚁群算法又较容易陷入局部最小值，造成无法规划出可行的路径。RRT更适合于高维空间和非线性动态环境，但是不能保证渐近最优。

快速搜索随机树星(RRT*)将RRT扩展到寻找最优解，与RRT相比具有概率完备性和渐进最优性等优点。然而针对水下起伏的地势障碍和散落的漂浮障碍，RRT*方法通过在整个规划域迭代采样优化，因而存在搜索节点时随机性大与效率低的问题，且在大规模或高维度空间中还存在收敛速度慢、对内存要求高等问题。

发明内容

发明目的：本发明目的在于提出一种目标导向集中优化的快速水下机器人三维路径规划方法，以克服现有技术在水下环境中存在的缺陷，在降低搜索的随机性的同时有效避免陷入局部最优，提高了搜索效率和收敛速率。

技术方案：为了实现上述目的，本发明采用的技术方案是一种目标导向集中优化的快速水下机器人三维路径规划方法，该方法包括如下步骤：

(1)创建包含顶点和边的三维路径搜索树，初始时刻搜索树中只包含起始点；

(2)根据目标点坐标，采用目标导向高斯采样策略在规划域中进行高斯采样得到采样点；

(3)在已有搜索树上寻找距离高斯采样点最近的节点x_nearest，并根据步长得到新的扩展节点x_new；

(4)如果x_nearest和x_new间存在障碍物则重复步骤(2)和(3)重新进行高斯采样，重新采样达到一定次数后x_nearest和x_new间仍存在障碍物则进行随机采样直到x_nearest和x_new间不存在障碍物；如果两点间不存在障碍物，就将扩展节点x_new连接到x_nearest并对x_new附近的节点进行路径的调整；

(5)重复步骤(2)至(4)，直到找到初始路径λ；

(6)计算路径节点距离初始点与目标点间直线的最大半径r，以r为半径，起始点和目标点分别为上下底面圆心作圆柱，形成圆柱子集；

(7)在圆柱子集中进行随机采样，得到采样点后更新路径和圆柱子集，继续采样优化路径直到达到指定迭代次数。

进一步地，所述步骤(2)中采用目标导向策略在目标点附近进行高斯采样，将高斯分布概率密度函数中的平均值设为目标点坐标，得到高斯采样点。

所述高斯分布概率密度函数为：

其中，x为三维规划域中的采样点，X_free为规划域中无障碍的点集，d表示x的维数,在水下机器人三维规划场景d＝3，σ表示d×d的协方差矩阵，设置μ为目标点坐标，通过设置σ可以决定分布的范围大小。

进一步地，所述步骤(4)中对新的扩展节点x_new和距离高斯点最近的节点x_nearest之间进行障碍物检测，定义一个标记i记录高斯采样的次数,如果两点之间存在障碍物则返回重新进行高斯采样，同时i值加1，只有两点间不存在障碍物，继续往下进行成功扩展一个节点才会将i值置0；如果i达到一定次数则表示陷入局部最小值，进行随机采样一次，得到随机采样点。

进一步地，所述步骤(6)中得到圆柱子集的具体步骤包括：

(6.1)由起始点x_start和目标点x_end确定一条直线，确定当前路径λ＝{x_start,x₁,x₂,x₃,......x_n,x_end}，路径节点包括x₁,x₂,x₃,......x_n和起始点、目标点；

(6.2)计算每个路径节点到直线距离的长度其中，k＝2,3,…,n,分子为两个向量的叉乘取模；

(6.3)计算路径节点距离初始点与目标点间直线的最大距离r_max＝max(d_k)，以此为半径，起始点和目标点分别为上下底面圆心做圆柱，形成圆柱子集。

进一步地，所述步骤(7)中在圆柱子集中进行随机采样优化路径的具体步骤包括：

(7.1)由公式得到随机采样点，σ∈[0,1]，

p＝δx_start+(1-δ)x_end为起始点与目标点线段上任意一点，δ∈[0,1]，方向向量s＝x_s-p表示经过点p与起始点和目标点直线垂直的方向向量，x_s满足(x_end-x_start)(x_s-p)^T＝0；

(7.2)得到新采样点x_sample后进行树和路径的调整,此时若存在障碍物，则返回步骤(7.1)在当前圆柱子集重新取点；若路径有更新则更新圆柱子集；

(7.3)重复步骤(7.1)和(7.2)直到达到指定迭代次数，输出最终树和路径。

有益效果：本发明针对RRT*方法在水下环境中存在的不足做了以下改进，使用目标导向高斯分布采样快速得到初始可行解，减少了采样点的随机性，提高了搜索效率，并加入随机扰动有效防止陷入局部最优；得到初始解后，采用集中优化搜索策略进行路径优化处理，提高优化收敛速率和路径的质量，实现渐进最优。

附图说明

图1是本发明实施例的方法流程图。

图2是高斯采样结合随机扰动示意图。

图3是圆柱子集集中搜索过程示意图。

图4是RRT*方法和本发明方法在缺少障碍物环境下路径规划的结果图。其中图(a)为RRT*方法的规划结果，图(b)为本发明的规划结果。

图5是RRT*方法和本发明方法在漂浮障碍环境、地形障碍物环境和混合型障碍环境下路径规划的结果图。其中图(a)(b)(c)为RRT*在三种环境下规划的初始路径，(d)(e)(f)为本发明在三种环境下规划出的初始路径。

图6是RRT*方法和本发明方法在混合型障碍环境下路径优化的结果图。(a)(b)(c)为RRT*方法规划的路径，其中(a)为初始路径，运行0.164s，路径长度为158、(b)为运行7.56s得到的扩展树和路径、(c)为运行7.56s后得到的路径，路径长度为152；(d)(e)(f)为本发明规划的路径，其中(d)为初始路径，运行0.077s，路径长度为139、(e)为运行7.56s后得到的扩展树和路径、(c)为运行7.56s后得到的路径，路径长度为133。

图7是RRT*方法和本发明方法在不同迭代次数后的路径长度对比图。

图8是RRT*方法和本发明方法达到相同程度的路径长度所需要的时间对比图。

具体实施方式

为了清楚的突出本发明的目的和优点，下面将结合本发明实例中的附图对本发明作进一步的描述，在本发明实施例公开的一种目标导向集中优化的快速水下机器人三维路径规划方法，实现过程主要包括如下步骤：

(1)导入环境信息，设置起始点和终点位置，创建一个包含顶点V和边E的搜索树Tree＝(V,E)，初始时刻，V只包含起始点x_start，E为空集。

(2)根据目标点坐标，采用目标导向高斯采样策略在规划域中取高斯采样点x_gs并赋值给x_sample。

(3)在树上找到离采样点最近的点x_nearest，根据公式

得到新的节点x_new，其中l为步长。

(4)如果x_nearest和x_new间存在障碍物则回到步骤(2)重新进行高斯采样，重新采样达到一定次数可以判断为陷入局部最优，这时候进行一次随机采样得到采样点x_rand，赋值给x_sample，回到步骤(3)，如果两点间不存在障碍物，就将扩展点x_new连接到x_nearest。对x_new附近的节点进行路径的调整，具体包括：

(4.1)根据公式X_near＝{x_near∈V:||x_new-x_near||≤r}获取树上与x_new邻近的点集X_near，其中邻域半径随着采样次数增加而减小，d是空间的维数，在本发明水下机器人三维路径规划实验中d＝3，γ是一个常数，n表示采样次数，遍历邻近点集X_near中的每一个点x_near，找到到x_new代价最小即距离最短且无障碍物的点x_min，调整扩展点x_new连接到x_min。

(4.2)再次遍历X_near中的点x_near，如果经过x_new到达x_near的代价小于原先的代价，就更新x_near的父节点，将x_near连接到x_new并删除x_near与原父节点之间的边；

(5)重复(2)到(4)，当扩展节点进入目标区域X_end，X_end＝{x∈X_free|||x-x_end||₂≤l}，其中l为步长，将x_end的父节点设置为该节点，并从x_end开始寻找父节点直到找到x_start，并生成初始路径λ。

(6)计算路径节点距离初始点与目标点间直线的最大半径r，以此作圆柱子集。

(7)在圆柱子集中进行随机采样，得到采样点x_sample后更新路径和圆柱子集,继续采样优化路径直到达到指定迭代次数。

所述步骤(2)中采用目标导向策略在目标点附近进行高斯采样得到高斯采样点x_gs，多维空间中高斯分布概率密度函数为：

其中x为三维规划域中的采样点，X_free为规划域中无障碍的点集，d表示x的维数，在本发明中d＝3，σ表示d×d的协方差矩阵，设置μ为目标点坐标，通过设置σ可以决定分布的范围大小。

所述步骤(4)中对x_nearest和x_new两点之间进行障碍物检测，并定义一个标记i记录存在障碍物时高斯采样的次数,如果存在障碍物则返回步骤(2)重新进行高斯采样，同时i值加1，只有两点间不存在障碍物，继续往下进行成功扩展一个节点才会将i值置0；如果i达到一定次数则表示陷入局部最小值，进行随机采样一次，得到随机采样点x_rand，返回步骤(3)。高斯采样结合随机扰动示意图如图2所示。

所述步骤(6)中假设已经得到了一条可行的初始路径，得到圆柱子集的具体步骤为：

(6.1)确定当前路径λ＝{x_start,x₁,x₂,x₃,......x_n,x_end}，路径节点包括x₁,x₂,x₃,......x_n和起始点、目标点。

(6.2)每个路径节点到直线距离的长度其中，k＝2,3,…,n,分子为两个向量的叉乘取模。

(6.3)计算路径节点距离初始点与目标点间直线的最大距离r_max＝max(d_k)，以此为半径，起始点和目标点分别为上下底面圆心做圆柱，形成圆柱子集。

所述步骤(7)中在圆柱子集中随机采样优化路径如图3所示，具体包括以下步骤：

(7.1)由公式得到随机采样点，σ∈[0,1]，

p＝δx_start+(1-δ)x_end为起始点与目标点线段上任意一点，δ∈[0,1]，方向向量s＝x_s-p表示经过点p与起始点和目标点直线垂直的方向向量，x_s满足(x_end-x_start)(x_s-p)^T＝0。

(7.2)得到新采样点x_sample后按照步骤(3)和步骤(4)的方法进行路径调整,此时若存在障碍物，则返回步骤(7.1)。若路径有更新则根据步骤(6)的方式更新圆柱子集。

(7.3)重复步骤(7.1)和步骤(7.2)直到达到指定迭代次数，输出最终树和路径。

本发明改进后的RRT^*路径规划方法过程如下所示：

为了验证本发明在三维水下环境下路径规划的效果，在MATLAB平台上进行了仿真实验。当水下机器人在水中工作时，主要存在两种障碍物：一种是漂浮障碍物，如水草，珊瑚,游动缓慢的大型鱼类等，另一种是海底地形障碍物，如海底的山坡、峡谷。所以在实验中用不同的环境来模拟这两种情况。为了方便地实现仿真实验，假设水下机器人为质点，设置步长为10。

首先在无障碍环境下使用RRT*方法和本发明改进RRT*方法对机器人路径进行规划，实验结果如图4所示，本发明改进RRT*方法针对水下无障碍物情况下，通过很少的采样就能够快速得到一条从起始点到目标点近似直线的路径。

为验证本发明在不同环境下的适应性，在球型障碍环境、地形障碍物环境和混合型障碍环境下分别使用RRT^*算法和本发明改进RRT^*算法对机器人路径进行规划，实验结果如图5所示，第一行为RRT*方法的结果，第二行为本发明改进RRT*方法结果，可以看出引进目标导向高斯采样策略后，采样的随机性大大减小，路径更为理想。

为验证本发明改进RRT*方法优化路径的有效性和可行性，在混合型障碍环境下分别使用RRT*方法和本发明改进RRT*方法进行路径优化，实验结果如图6所示，第一行为RRT*方法优化的过程，第二行为本发明改进RRT*方法优化的过程，可以看出，引进集中优化搜索策略后，采样更加集中，且只需相对很少的采样就能得到更趋向最优的路径。

多次运行标准RRT*方法与本发明改进方法得到初始路径时，记录每次运行结果，得到平均运行时间、迭代次数、平均路径节点和平均路径长度得到的实验数据对比如表1所示，在不同障碍物环境下，改进RRT*方法与原始RRT*方法相比，运行时间更短，迭代次数更少，路径长度更短。

表1实验数据对比表

增加迭代次数来优化初始路径时，RRT*方法和本发明改进RRT*方法在不同迭代次数后得到的路径长度如图7所示，达到相同程度的路径长度所需要的时间如图8所示，可以看到，本发明改进RRT*方法收敛速度更快。

Claims (6)

1.一种目标导向集中优化的快速水下机器人三维路径规划方法，其特征在于：包括如下步骤：

(1)创建包含顶点和边的三维路径搜索树，初始时刻搜索树中只包含起始点；

(2)根据目标点坐标，采用目标导向高斯采样策略在规划域中进行高斯采样得到采样点；

(3)在已有搜索树上寻找距离高斯采样点最近的节点x_nearest，并根据步长得到新的扩展节点x_new；

(4)如果x_nearest和x_new间存在障碍物则重复步骤(2)和(3)重新进行高斯采样，重新采样达到一定次数后x_nearest和x_new间仍存在障碍物则进行随机采样直到x_nearest和x_new间不存在障碍物；如果两点间不存在障碍物，就将扩展节点x_new连接到x_nearest并对x_new附近的节点进行路径的调整；

(5)重复步骤(2)至(4)，直到找到初始路径λ；

(6)计算路径节点距离初始点与目标点间直线的最大半径r，以r为半径，起始点和目标点分别为上下底面圆心作圆柱，形成圆柱子集；

(7)在圆柱子集中进行随机采样，得到采样点后更新路径和圆柱子集，继续采样优化路径直到达到指定迭代次数。

2.根据权利要求1所述的一种目标导向集中优化的快速水下机器人三维路径规划方法，其特征在于：所述步骤(2)中采用目标导向策略在目标点附近进行高斯采样，将高斯分布概率密度函数中的平均值设为目标点坐标，得到高斯采样点。

3.根据权利要求2所述的一种目标导向集中优化的快速水下机器人三维路径规划方法，其特征在于：所述高斯分布概率密度函数为：

其中，x为三维规划域中的采样点，X_free为规划域中无障碍的点集，d表示x的维数,在水下机器人三维规划场景d＝3，σ表示d×d的协方差矩阵，设置μ为目标点坐标，通过设置σ可以决定分布的范围大小。

4.根据权利要求1所述的一种目标导向集中优化的快速水下机器人三维路径规划方法，其特征在于：所述步骤(4)中对新的扩展节点x_new和距离高斯点最近的节点x_nearest之间进行障碍物检测，定义一个标记i记录高斯采样的次数,如果两点之间存在障碍物则返回重新进行高斯采样，同时i值加1，只有两点间不存在障碍物，继续往下进行成功扩展一个节点才会将i值置0；如果i达到一定次数则表示陷入局部最小值，进行随机采样一次，得到随机采样点。

5.根据权利要求1所述的一种目标导向集中优化的快速水下机器人三维路径规划方法，其特征在于：所述步骤(6)中得到圆柱子集的具体步骤包括：

(6.1)由起始点x_start和目标点x_end确定一条直线，确定当前路径λ＝{x_start,x₁,x₂,x₃,......x_n,x_end}，路径节点包括x₁,x₂,x₃,......x_n和起始点、目标点；

(6.2)计算每个路径节点到直线距离的长度其中，k＝2,3,…,n,分子为两个向量的叉乘取模；

(6.3)计算路径节点距离初始点与目标点间直线的最大距离r_max＝max(d_k)，以此为半径，起始点和目标点分别为上下底面圆心做圆柱，形成圆柱子集。

6.根据权利要求1所述的一种目标导向集中优化的快速水下机器人三维路径规划方法，其特征在于：所述步骤(7)中在圆柱子集中进行随机采样优化路径的具体步骤包括：

(7.1)由公式得到随机采样点，σ∈[0,1]，p＝δx_start+(1-δ)x_end为起始点与目标点线段上任意一点，δ∈[0,1]，方向向量s＝x_s-p表示经过点p与起始点和目标点直线垂直的方向向量，x_s满足(x_end-x_start)(x_s-p)^T＝0；

(7.2)得到新采样点x_sample后进行树和路径的调整,此时若存在障碍物，则返回步骤(7.1)在当前圆柱子集重新取点；若路径有更新则更新圆柱子集；

(7.3)重复步骤(7.1)和(7.2)直到达到指定迭代次数，输出最终树和路径。