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

权利要求书

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为半径，起始点和目标点分别为上下底面圆心作圆柱，形成圆柱子集；得到圆柱子集的具体步骤包括：

(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)直到达到指定迭代次数，输出最终树和路径。

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达到一定次数则表示陷入局部最小值，进行随机采样一次，得到随机采样点。