[发明专利]基于改进型RRT*算法的水下机器人三维全局路径规划方法

权利要求书

1.基于改进型RRT*算法的水下机器人三维全局路径规划方法，其特征在于，包括如下步骤：

S1：对得到的全局水下环境信息进行预处理；

S2：采用离散正态分布的采样策略，进行随机树采样，把目标点的位置设置为正态分布的均值，采样概率为正态分布的概率密度函数；

S3：遍历随机树上的节点，采用基本RRT算法获取到新节点q_new；

S4：采用基于三角不等式的几何修剪算法进行节点筛选，完成路径规划；

所述步骤S2中的采样函数f(x,y)的数学表达式如下，算法中采用其的离散形式来实现采样点服从正态分布的空间采样：

其中，σ1，σ2分别为x，y方向上的随机变量分布的标准差，μ1，μ2分别为x，y方向上的随机变量分布的均值；

所述步骤S4具体为：在q_new附近以定义的范围内在搜索树找出其附近节点的集合q_near及q_near父节点q_parent的集合，去除q_near中的节点，保留q_parent中的节点；直接把q_parent中的节点与q_new连接起来，如果新节点q_new与目标点的欧式距离小于目标阈值，则完成路径规划，否则回到步骤S2。

2.根据权利要求1所述的基于改进型RRT*算法的水下机器人三维全局路径规划方法，其特征在于，所述步骤S1中对全局水下环境信息进行栅格化的预处理，具体的处理方法为：用不同颜色的rgb信息来区分障碍物和无障碍物区域。

3.根据权利要求1所述的基于改进型RRT*算法的水下机器人三维全局路径规划方法，其特征在于，所述步骤S3具体为：遍历随机树上的节点，找出与随机采样点q_sample欧式距离最小的点q_nearest，从节点q_nearest向采样点q_sample方向扩展固定步长得到新节点q_new。