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

说明书

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

技术领域

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

背景技术

机器人水下导航是导航技术的重要应用和新发展，路径规划是导航研究的一个重要环节和课题，是指机器人在当前环境中按照一定标准搜索出一条从起始状态点到目标状态点，并且能够绕开障碍物的最优或次优路径。水下机器人的三维路径规划必须针对水下地形障碍和漂浮障碍通过有效的方法寻找路径。目前全局路径规划方法主要有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)中采用目标导向策略在目标点附近进行高斯采样，将高斯分布概率密度函数中的平均值设为目标点坐标，得到高斯采样点。

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