1.一种目标导向集中优化的快速水下机器人三维路径规划方法,其特征在于:包括如下步骤:(1)创建包含顶点和边的三维路径搜索树,初始时刻搜索树中只包含起始点;
(2)根据目标点坐标,采用目标导向高斯采样策略在规划域中进行高斯采样得到采样点;
(3)在已有搜索树上寻找距离高斯采样点最近的节点xnearest,并根据步长得到新的扩展节点xnew;
(4)如果xnearest和xnew间存在障碍物则重复步骤(2)和(3)重新进行高斯采样,重新采样达到一定次数后xnearest和xnew间仍存在障碍物则进行随机采样直到xnearest和xnew间不存在障碍物;如果两点间不存在障碍物,就将扩展节点xnew连接到xnearest并对xnew附近的节点进行路径的调整;
(5)重复步骤(2)至(4),直到找到初始路径λ;
(6)计算路径节点距离初始点与目标点间直线的最大半径r,以r为半径,起始点和目标点分别为上下底面圆心作圆柱,形成圆柱子集;得到圆柱子集的具体步骤包括:(6.1)由起始点xstart和目标点xend确定一条直线,确定当前路径λ={xstart,x1,x2,x3,......xn,xend},路径节点包括x1,x2,x3,......xn和起始点、目标点;
(6.2)计算每个路径节点到直线距离的长度 其中,k=2,
3,…,n,分子为两个向量的叉乘取模;
(6.3)计算路径节点距离初始点与目标点间直线的最大距离rmax=max(dk),以此为半径,起始点和目标点分别为上下底面圆心做圆柱,形成圆柱子集;
(7)在圆柱子集中进行随机采样,得到采样点后更新路径和圆柱子集,继续采样优化路径直到达到指定迭代次数;在圆柱子集中进行随机采样优化路径的具体步骤包括:(7.1)由公式 得到随机采样点,σ∈[0,1],p=δxstart+(1-δ)xend为起始点与目标点线段上任意一点,δ∈[0,1],方向向量s=xs-p表示经过点p与起始点和目标点直线垂直的方向向量,xs满足(xend-xstart)(xs-p)T=0;
(7.2)得到新采样点xsample后进行树和路径的调整,此时若存在障碍物,则返回步骤(7.1)在当前圆柱子集重新取点;若路径有更新则更新圆柱子集;
(7.3)重复步骤(7.1)和(7.2)直到达到指定迭代次数,输出最终树和路径。
2.根据权利要求1所述的一种目标导向集中优化的快速水下机器人三维路径规划方法,其特征在于:所述步骤(2)中采用目标导向策略在目标点附近进行高斯采样,将高斯分布概率密度函数中的平均值设为目标点坐标,得到高斯采样点。
3.根据权利要求2所述的一种目标导向集中优化的快速水下机器人三维路径规划方法,其特征在于:所述高斯分布概率密度函数为:其中,x为三维规划域中的采样点,Xfree为规划域中无障碍的点集,d表示x的维数,在水下机器人三维规划场景d=3,σ表示d×d的协方差矩阵,设置μ为目标点坐标,通过设置σ可以决定分布的范围大小。
4.根据权利要求1所述的一种目标导向集中优化的快速水下机器人三维路径规划方法,其特征在于:所述步骤(4)中对新的扩展节点xnew和距离高斯点最近的节点xnearest之间进行障碍物检测,定义一个标记i记录高斯采样的次数,如果两点之间存在障碍物则返回重新进行高斯采样,同时i值加1,只有两点间不存在障碍物,继续往下进行成功扩展一个节点才会将i值置0;如果i达到一定次数则表示陷入局部最小值,进行随机采样一次,得到随机采样点。