CN115454107A - 一种近海巡检型auv的路径规划方法 - Google Patents

Abstract

一种近海巡检型AUV的路径规划方法，涉及自主水下航行器AUV的路径规划领域。本发明是为了解决现有的近海巡检AUV的路径规划方法还存在计算开销大、极易陷入局部最小值而导致的无法在存在复杂动态且较大障碍物的近海环境完成路径规划的问题。本发明包括：步骤一、初始化地图空间V，随机树空间T，拓展步长l，AUV运动学参数，自适应子目标点选取阈值k，设置起始点x_rand，终点x_goal；步骤二、利用步骤一初始化后的参数值采用RRT算法获得规划好的全局路径；步骤三、以AUV运动学参数为约束采用DWA算法对步骤二获得的规划好的全局路径进行处理，使AUV巡检路径在符合运动学约束的同时实现动态避障。本发明用于对巡检AUV进行路径规划。

Description

一种近海巡检型AUV的路径规划方法

技术领域

本发明涉及自主水下航行器AUV的路径规划领域，特别涉及一种近海巡检型AUV的路径规划方法。

背景技术

自主水下航行器(Autonomous Underwater Vehicle，AUV)在水下搜救、地形勘探、探测资源等领域有着重要的地位，而自主路径规划是AUV的核心技术之一，其决定着AUV执行任务的效率和安全性。近海环境下由于存在大量礁石、人工建筑物以及航行的渔船导致AUV的航行情况变得极其复杂，相应的对路径规划的快速性要求也更高。

目前快速拓展随机树算法(Rapid-exploration Random Tree，RRT)被广泛用于解决路径规划问题，但是RRT算法存在拓展盲目性、路径冗余度高、路径不平滑等问题，主要的改进方向有增加算法的导向性和提高的路径可行性。增加算法导向性的改进有刘成菊在《基于改进RRT算法的RoboCup机器人动态路径规划》中提出的在拓展阶段以一定概率选择目标点，采样的其他点则增加引力分量使其偏向目标点，同时使用路径缓存策略以及动态扩展策略解决动态避碰问题。在提高路径可行性方面的改进有尹高扬在《基于改进RRT算法的无人机航迹规划》中提出的节点拓展阶段添加运动学约束、航迹距离约以达到增加路径的可行性以及缩短距离的作用。上述提出的算法存在计算开销大、不适用于近海环境存在复杂动态障碍物的路径规划问题。动态窗口法(Dynamic Window Approach，DWA)是一种在线规划的算法，是根据AUV当前时刻的状态、外部环境以及AUV自身性能，将下一时刻AUV的性能离散出一系列符合AUV运动学约束的轨迹。之后根据评价函数对各个轨迹的得分选择出众多轨迹中得分最高一条轨迹的运动状态作为AUV下一时刻的运动参数。评价函数由艏向角得分、安全距离得分以及速度得分三部分构成。其中艏向角得分为越朝向目标点得分越高，安全距离得分为以当前状态速度航行直至遇到障碍物，航行距离越远得分越高，速度得分为航行速度越快得分越高。但是DWA算法仅能对窗口大小的范围内的运送参数做评估，所以在一些狭小空间内做规划路径，极易陷入局部最小值，导致在遇到较大的障碍物时无法完成路径规划。

发明内容

本发明目的是为了解决现有的近海巡检AUV的路径规划方法还存在计算开销大、极易陷入局部最小值而导致的无法在存在复杂动态且较大障碍物的近海环境完成路径规划的问题，而提出了一种近海巡检型AUV的路径规划方法。

一种近海巡检型AUV的路径规划方法具体过程为：

步骤一、初始化地图空间V，随机树空间T，拓展步长l，AUV运动学参数，自适应子目标点选取阈值k，设置起始点x_rand，终点x_goal；

步骤二、利用步骤一初始化后的参数值采用RRT算法获得规划好的全局路径；

所述RRT算法为改进的RRT算法，具体包括：

首先，生成随机数，根据随机数确定采样点为终点还是随机点，从而生成拓展路径；

然后，对新生成的拓展路径进行碰撞检测，不产生碰撞的拓展路径构成从起点到终点的树；

最后，从终点开始依次寻找节点的父节点直至找到起始点，获得一条全局路径，再去除全局路径上的冗余节点，获得规划好的全局路径；

步骤三、以AUV运动学参数为约束，采用DWA算法对步骤二获得的规划好的全局路径进行处理，获得AUV巡检路径实现动态避障。

本发明的有益效果为：

本发明提出了一种改进的RRT算法，其中包括标记空间拓展策略，即给每个节点设置目标采样以及随机采样两个标记位：首先生成一个随机数，当随机数小于阈值时选择目标点作为采样节点，如果拓展失败此节点的目标采样标记位被标记为失败，当随机数大于阈值时在采样空间进行随机采样，如果拓展失败此节点的随机采样标记位加一；之后在使用目标点作为采样点进行拓展时，父节点的选取则去除目标采样标记位被标记为失败的节点，当时使用随机节点进行拓展时，父节点的选取则去除随机采样标记位到达阈值的节点。改进的RRT算法减少了无效路径的拓展，加快收敛速度；之后执行路径膨胀策略，在碰撞检测的过程中对路径的检测添加安全距离的约束，增加路径和障碍物之间的距离，提高路径的安全性，减少了AUV发生碰撞的概率。本发明在完成规划全局路径后，从起点回溯至终点将全局路径的冗余节点去除，提高路径的平滑性，使本发明提出的RRT算法与DWA可以更好的结合，从而进行AUV路径规划。

本发明将改进RRT算法与DWA算法融合算法，规划出一条符合运动学约束的路径，同时实现了对未知动态障碍物的避障功能，弥补仅使用改进RRT算法规划的路径不符合AUV的运动学约束问题和改进RRT算法无法对未知动态障碍物避障的问题，以及DWA算法不能利用地图全局信息进行路径规划导致的极易陷入局部极小值的缺陷。本发明提出的自适应子目标点选取策略，提高了DWA算法绕过障碍物时的平滑度，防止陷入局部极小值问题的发生；本发明提出重规划策略在路径规划过程中出现较大未知障碍物导致没有可行路径时，以当前位置为起点调用改进RRT算法重新规划出一条全局可行安全路径，之后以新规划的路径为基础重新执行DWA算法，使路径规划快速摆脱极小值，从而完成在存在复杂动态且较大障碍物的近海环境的路径规划。

附图说明

图1为本发明总流程图；

图2为路径膨胀策略示意图；

图3为去除冗余节点示意图；

图4为自适应子目标点选取示意图；

图5为重规划策略结果图。

具体实施方式

具体实施方式一：如图1所示，本实施方式一种近海巡检型AUV的路径规划方法具体过程为：

步骤一、初始化地图空间V,随机树空间T，拓展步长l，AUV运动学参数，自适应子目标点选取阈值k，设置起始点x_rand，终点x_goal；

步骤二、采用RRT算法利用步骤一初始化后的参数值获得规划好的全局路径：

步骤二一、如图4所示，生成随机数，将生成的随机数与第一阈值比较，根据比较结果确定采样点为随机采样点还是终点，若采样点为终点，则执行(i)；若采样点为随机采样点则执行(ii)；

若随机数小于第一阈值则以终点为采样点，若随机数大于第一阈值则以随机采样点为采样点；

(i)、若采样点为终点则以距离终点最近的节点朝向终点的矢量方向为拓展方向，按照拓展步长进行l拓展获得一个新的节点，此时判断新产生的路径是否产生碰撞，若没有产生碰撞则拓展成功，将该节点加入到随机树空间；若新产生的路径发生碰撞则拓展失败；

然后重新生成随机数将生成的随机数与第一阈值比较，根据比较结果确定采样点为随机采样点还是终点；若以终点为采样点，则以当前随机树空间中与终点距离最近的节点到终点的方向为拓展方向进行拓展；

其中，每个节点包括：一个目标采样标记位和一个随机采样标记位；

所述目标采样标记位和随机采样标记位初值为0；

在判断新产生的路径是否产生碰撞采用路径膨胀策略，如图2所示具体为：

首先，在路径拓展的过程中，以节点的父节点为起点，以父节点到采样点的矢量方向为拓展方向，拓展l的距离获得拓展路径；

然后，在拓展路径的两侧延伸出预设安全距离的宽度，获得障碍物与延伸后的拓展路径之间的距离a，若a小于预设安全距离则拓展失败，则新产生的路径产生碰撞；若a大于等于预设安全距离则拓展成功，则新产生的路径没有产生碰撞；

(ii)、若采样点为随机采样点，则以起点到随机点采样点的方向为拓展方向，按照拓展步长l进行拓展获得一个新的节点，此时判断新产生的路径是否产生碰撞，若没有产生碰撞则拓展成功，将该节点加入到随机树空间；若新产生的路径发生碰撞则拓展失败；

然后重新生成随机数将生成的随机数与第一阈值比较，根据比较结果确定采样点为随机采样点还是终点；若以起点到随机采样点的方向为拓展方向，则以当前随机树空间中与随机采样点距离最近的节点到随机采样点的方向为拓展方向进行拓展；

当以当前随机树空间中与终点最近的节点的连接方向为拓展方向进行拓展时，在随机树空间寻找父节点时忽略目标采样标记位为1的节点；

所述目标采样标记位为1的节点为目标采样失败的节点，即以终点作为采样点拓展失败时被选为父节点的节点；同时在使用终点作为采样点时此节点将不会被选为父节点。

当以当前随机树空间中与随机采样点最近的节点的连接方向为拓展方向进行拓展时，在随机树空间中寻找父节点时忽略随机采样标记位标记次数到达阈值的节点；

所述随机采样标记位标记次数到达阈值的节点通过以下方式获得：当以随机点作为拓展点拓展失败时，将节点n的随机采样标记位数字加一，当节点n的随机采样标记位的值大于预设第二阈值时，再以随机点作为采样点时此节点将不会被选为父节点；

所述节点n为以随机点作为拓展点拓展失败时，被选为父节点的节点。

步骤二二、重复执行步骤二一直至获得起点到终点的树；

步骤二三、在随机树空间中从终点开始依次寻找节点的父节点直至找到起始点，获得一条全局路径，然后去除全局路径上的冗余节点，获得去除冗余节点后的全局路径即规划好的全局路径：

如图3所示，连接起始点和下一个子节点执行进行碰撞检测，如果没有发生碰撞则检测下一个子目标和起始点连线是否发生碰撞，逐点检测直至连接至发生碰撞的子目标点，去除此子目标点和起始中间的冗余节点，获得去除冗余节点后的全局路径即规划好的全局路径。

步骤三、以AUV运动学参数为约束采用DWA算法对步骤二获得的规划好的全局路径进行处理，获得AUV巡检路径：

步骤三一、对步骤二一获得的去除冗余节点后的全局路径上的节点进行延伸，获得延伸后的全局路径，具体为：

S1、计算AUV当前位置和子目标点沿着全局路径的距离和AUV当前位置和子目标点欧式距离的比值b；

其中，d_{path_dis}为当前起点沿着全局路径到达子目标点的距离，d_{euc_dis}为当前起点到达子目标点的欧氏距离；

S2、当b小于阈值k时在全局路径上延伸子目标点，直至等于阈值k；当拓展至终点时，即使比值仍小于阈值，也将使用终点作为子目标点；S3、在到达子目标点之后，以当前位置为起点重新执行S1使用自适应子目标点选取策略选取新的子目标点，进行下一次DWA的动态轨迹规划；

步骤三二、如图5所示，按照延伸后的全局路径进行巡检，若陷入局部极小值，则以当前AUV位置为起点，重新执行步骤一到三获得重新规划后的巡检路径：

当遇到较大的障碍物时可能会出现无法绕过此障碍物的情况，进而使DWA算法陷入局部极小值，当DWA算法陷入局部极小值之后使用重规划策略，即以当前位置为新的起点重新执行步骤一使用改进RRT算法重现规划出一条全局路径，之后再启用DWA算法以重新规划的全局路径为基础重新进行路径规划。

本实施方式中，本发明的改进RRT算法使用概率选择目标点作为采样节点，减少算法的无效拓展，加快算法的收敛速度；使用的标记空间拓展策略，防止随机树空间拓展成功率较低的节点被选为父节点，以避免无效拓展；使用的路径膨胀策略，在碰撞检测的过程中对路径的检测添加安全距离的约束，增加路径和障碍物之间的距离，提高路径的安全性，减少AUV发生碰撞的概率。对使用不同拓展方式失败的父节点进行标记处理，在选择目标点作为拓展点失败时其父节点如果再次被选为拓展点也会拓展失败，因此在下次拓展时将其剔除，将会避免无效拓展；在使用随机点作为拓展点时，某一节点在被选为父节点时拓展失败次数越多，代表其成功拓展的概率越低，标记在到达一定阈值之后其拓展的成功的概率将会降低至一个极小值，在使用随机节点作为拓展点时将其剔除将会避免一定的无效拓展。最后规划的全局路径将其冗余节点去除，提高路径的平滑性，便于后续DWA算法对全局路径的使用。

Claims (9)

1.一种近海巡检型AUV的路径规划方法，其特征在于所述方法具体过程为：

步骤一、初始化地图空间V，随机树空间T，拓展步长l，AUV运动学参数，自适应子目标点选取阈值k，设置起始点x_rand，终点x_goal；

步骤二、利用步骤一初始化后的参数值采用RRT算法获得规划好的全局路径；

所述RRT算法为改进的RRT算法，具体包括：

首先，生成随机数，根据随机数确定采样点为终点还是随机点，从而生成拓展路径；

然后，对新生成的拓展路径进行碰撞检测，不产生碰撞的拓展路径构成从起点到终点的树；

最后，从终点开始依次寻找节点的父节点直至找到起始点，获得一条全局路径，再去除全局路径上的冗余节点，获得规划好的全局路径；

步骤三、以AUV运动学参数为约束，采用DWA算法对步骤二获得的规划好的全局路径进行处理，获得AUV巡检路径实现动态避障。

2.根据权利要求1所述的一种近海巡检型AUV的路径规划方法，特征在于：所述步骤二中的利用步骤一初始化后的参数值采用RRT算法获得规划好的全局路径，包括以下步骤：

步骤二一、生成随机数，将生成的随机数与第一阈值比较，根据比较结果确定采样点为随机采样点还是终点，若采样点为终点，则执行(i)；若采样点为随机采样点则执行(ii)；

(i)、以距离终点最近的节点朝向终点的矢量方向为拓展方向，按照拓展步长进行l拓展获得一个新的节点，判断新产生的路径是否产生碰撞，若没有产生碰撞则拓展成功，将该节点加入到随机树空间；若新产生的路径发生碰撞则拓展失败；

其中，每个节点包括：一个目标采样标记位和一个随机采样标记位；

所述目标采样标记位和随机采样标记位初值为0；

然后重新生成随机数将生成的随机数与第一阈值比较，根据比较结果确定采样点为随机采样点还是终点；若以终点为采样点，则以当前随机树空间中与终点距离最近的节点到终点的方向为拓展方向进行拓展；

(ii)以起点到随机采样点的方向为拓展方向，按照拓展步长l进行拓展获得一个新的节点，判断新产生的路径是否产生碰撞，若没有产生碰撞则拓展成功，将该节点加入到随机树空间；若新产生的路径发生碰撞则拓展失败；

然后重新生成随机数将生成的随机数与第一阈值比较，根据比较结果确定采样点为随机采样点还是终点；若以起点到随机采样点的方向为拓展方向，则以当前随机树空间中与随机采样点距离最近的节点到随机采样点的方向为拓展方向进行拓展；

步骤二二、重复执行步骤二一直至获得起点到终点的树；

步骤二三、在随机树空间中从终点开始依次寻找节点的父节点直至找到起始点，获得一条全局路径，然后去除全局路径上的冗余节点，获得去除冗余节点后的全局路径即规划好的全局路径。

3.根据权利要求2所述的一种近海巡检型AUV的路径规划方法，特征在于：所述步骤二一中的将生成的随机数与第一阈值比较，根据比较结果确定采样点为随机采样点还是终点，具体为：

若随机数小于第一阈值则以终点为采样点，若随机数大于第一阈值则以随机采样点为采样点。

4.根据权利要求3所述的一种近海巡检型AUV的路径规划方法，特征在于：所述步骤二一中的判断新产生的路径是否产生碰撞采用路径膨胀策略，具体为：

首先，在路径拓展过程中，以节点的父节点为起点，以父节点到采样点的矢量方向为拓展方向，拓展l的距离获得拓展路径；

然后，在拓展路径的两侧延伸出预设安全距离的宽度，获得障碍物与延伸后的拓展路径之间的距离a，若a小于预设安全距离则拓展失败，则新产生的路径产生碰撞；若a大于等于预设安全距离则拓展成功，则新产生的路径没有产生碰撞。

5.根据权利要求4所述的一种近海巡检型AUV的路径规划方法，特征在于：所述步骤二一中的以当前随机树空间中与终点距离最近的节点到终点的方向为拓展方向进行拓展时，在随机树空间寻找父节点时忽略目标采样标记位为1的节点；

所述目标采样标记位为1的节点为目标采样失败的节点，即以终点作为采样点拓展失败时被选为父节点的节点；同时在使用终点作为采样点时此节点将不会被选为父节点。

6.根据权利要求5所述的一种近海巡检型AUV的路径规划方法，特征在于：所述步骤二一中的若以起点到随机采样点的方向为拓展方向，则以当前随机树空间中与随机采样点距离最近的节点到随机采样点的方向为拓展方向进行拓展时，在随机树空间中寻找父节点时忽略随机采样标记位标记次数到达阈值的节点；

所述随机采样标记位标记次数到达阈值的节点通过以下方式获得：当以随机点作为拓展点拓展失败时，将节点n的随机采样标记位数字加一，当节点n的随机采样标记位的值大于预设第二阈值时，再以随机点作为采样点时此节点将不会被选为父节点；

所述节点n为以随机点作为拓展点拓展失败时，被选为父节点的节点。

7.根据权利要求6所述的一种近海巡检型AUV的路径规划方法，特征在于：所述步骤二三中的去除全局路径上的冗余节点，获得去除冗余节点后的全局路径即规划好的全局路径，具体为：

连接起始点和下一个节点执行碰撞检测，如果没有发生碰撞则检测下一个节点与起始的连线是否发生碰撞，逐点检测直至连接至发生碰撞的节点，去除此节点和起始点中间的冗余节点，获得去除冗余节点后的全局路径即规划好的全局路径。

8.根据权利要求7所述的一种近海巡检型AUV的路径规划方法，特征在于：所述步骤三中的以AUV运动学参数为约束采用DWA算法对步骤二获得的规划好的全局路径进行处理，获得AUV巡检路径实现动态避障，包括以下步骤：

步骤三一、对步骤二三获得的去除冗余节点后的全局路径上的节点进行延伸，获得延伸后的全局路径；

步骤三二、按照延伸后的全局路径进行巡检，若陷入局部极小值，则以当前AUV位置为起点，重新执行步骤一到三获得重新规划后的AUV巡检路径。

9.根据权利要求8所述的一种近海巡检型AUV的路径规划方法，特征在于：所述步骤三一中的对步骤二三获得的去除冗余节点后的全局路径上的节点进行延伸，获得延伸后的全局路径，包括以下步骤：

S1、设置初始子目标点，计算d_{path_dis}与d_{path_dis}的比值b：

其中，d_{path_dis}为当前起点沿着全局路径到达子目标点的距离，d_{euc_dis}为当前起点到达子目标点的欧氏距离；

S2、当b小于阈值k时在全局路径上延伸子目标点，直至b等于阈值k获得子目标点；当拓展至终点时，即使比值仍小于阈值，也将使用终点作为子目标点；

S3、在到达子目标点之后，以当前位置为起点重新执行S1，进行下次动态轨迹规划。

Images