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

摘要

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

主权项

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