CN112904869B - 基于多树rrt的无人艇加权迭代路径规划方法及设备 - Google Patents

Abstract

本发明属于无人艇领域，公开了一种基于多树RRT的无人艇加权迭代路径规划方法及设备。该方法包括：1、从起始点到目标点构建第一棵RRT随机树；2、判断是否找到从起始点到达目标点的路径，是则设为第一条路径，否则重规划第一棵随机树；3、保留和记录上一棵随机树的节点NodeList作为引导节点构建新的RRT随机树；4、判断是否规划成功，成功则继续步骤5；不成功则输出上一次规划路径作为最终优化的路径；5、计算规划路径的加权代价，若高于预设的期望代价，则返回步骤3；若不高于预设的期望代价或者达到预设最大迭代次数，则直接输出此时路径为最终优化的路径。本发明能够快速获得最优解或次优解，且随机性和收敛性易于控制。

Description

基于多树RRT的无人艇加权迭代路径规划方法及设备

技术领域

本发明属于无人艇运动规划领域，更具体地，涉及一种基于多树RRT的无人艇加权迭代路径规划方法及设备。

背景技术

无人艇的路径规划是实现其自主运动的重要保障，对此也有很多成熟的算法，其中，快速扩展随机树(Rapidly-exploring Random Trees，RRT)算法，是近十几年得到广泛发展与应用的基于采样的运动规划算法，该方法具有适应性强、概率完备、生成速度快等诸多优点。根据RRT的采样思想，连接一系列从无障碍的空间中随机采样的点，适合解决多自由度无人艇在复杂环境下和动态环境中的路径规划问题，从而建立一条从初始状态到目标状态的路径。但是对于单纯的RRT或者RRT*算法，总是存在收敛速率未知，无效拓展区域等问题，在实际场景中，往往需要根据需求找到一条满足运动条件约束的路径。

为了加速RRT的收敛，不断有对于RRT的改进算法，如基于概率的RRT，即在随机树生长的过程中，根据概率来选择生长方向是目标点还是空间随机点；连接型RRT，算法从一开始同时从初始状态点和目标状态点生长两棵随机树，每次迭代过程中，以其中一棵随机树进行拓展尝试连接另一棵随机树的最近节点来扩展新节点；RRT*改进了父节点的选择方式，重新连接代价最小的节点来保证渐进最优。总体来说，基于改进RRT算法的思路，并且采样数量足够多情况下总可以找到可行解，RRT*算法可以得到接近最优的次优解。但是，基于传统RRT思想的路径规划，还存在一些亟待解决的问题：

(1)RRT需要对整个空间进行随机探索，随机探索的范围大，计算量增多，改进的RRT如基于概率的目标偏好拓展性随概率而变，不易把握随机性和收敛性之间的关系；

(2)RRT*算法可以得到接近最优的次优解，由于也是全局搜索拓展，其优化程度是以计算量和时间为代价的，运行效率较低。

因此，亟需一种能够快速获得最优解或次优解，且随机性和收敛性易于控制的RRT路径规划方法。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于多树RRT的无人艇加权迭代路径规划方法及设备，其目的在于，通过按次序生成RRT树节点列表，利用上一棵随机数的节点列表为下一棵随机树提供启发式搜索，进行多树迭代得到渐进优化的路径轨迹，由此实现加速收敛，又可以不必要进行全局搜索而得到渐进最优解或次优解。

为实现上述目的，按照本发明的一个方面，提供了一种基于多树RRT的无人艇加权迭代路径规划方法，其特征在于，包括以下步骤：

步骤1：基于RRT算法从起始点开始朝向目标点生成随机节点从而构建第一棵RRT随机树；

步骤2：判断是否找到从起始点到达目标点的路径，若是，则输出该路径作为第一条路径，进入步骤3；否则，需要进行重规划第一棵随机树，即重复执行步骤1，直至找到从起始点到达目标点的路径作为第一条路径并进入步骤3；若重复预设的K_m次步骤1仍然未找到从起始点到达目标点的路径，则判定不存在从起始点到目标点的通路；

步骤3：保留和记录上一棵随机树的节点NodeList，以NodeList作为引导节点构建新的RRT随机树；

步骤4：判断是否规划成功，成功则继续步骤5；若不成功，输出上一次规划路径作为最终优化的路径；

步骤5：计算规划路径的加权代价，若高于预设的期望代价，则返回步骤3；若不高于预设的期望代价或者达到预设最大迭代次数K_n，则直接输出此时路径为最终优化的路径。

进一步地，步骤1中还包括在人工势场APFs作用下构建可行探测区域，引导随机点生成函数，具体包括：

利用势场函数U来建立人工势场，设某随机节点ξ的势场U(ξ)为目标点的引力势场U_att(ξ)和障碍物的斥力势场U_rep(ξ)之和：

其中，改进的势场函数表达式为：

其中，K_att为引力增益常数，K_rep为斥力系数，d为引力距离阈值，ρ(ξ,ξ_goal)表示ξ离目标点ξ_goal的最近距离，ρ₀为障碍物obstacle的影响半径，ρ(ξ,obstacle)表示ξ离障碍物obstacle的最近距离；在ξ处的梯度势场向量F(ξ)为引力势场梯度

和斥力势场梯度

矢量和：

因此，通过Rand_sample采样函数生成一个随机数ξ_rand来表示ξ_rand∈Region_sample，则Region_sample即为APF在势场向量F(ξ)作用下的具有方向偏向目标点的给定裕度的可行探测区域，表示为：

其中，

ξ_rand.x为随机点ξ_rand的x坐标，ξ_rand.y为随机点ξ_rand的y坐标，ξ_nearest.x为当前要拓展点ξ_nearest的x坐标，ξ_nearest.y为当前要拓展点ξ_nearest的y坐标；K_p为区域Region_sample的边长比例系数,φ为水平方向与F(ξ)的夹角，Angle为区域Region_sample的最大夹角。

进一步地，步骤1中还包括节点过滤步骤，具体为：

将不可寻的轨迹点列为不可行区域，若在ξ_nearest和新节点ξ_new之间检测到障碍物，则将ξ_new为圆心、filter_radius为半径的区域过滤，记为过滤区域RegionFilter，即：

其中，ξ_rand.x、ξ_rand.y分别为节点ξ的x、y坐标，ξ_new.x、ξ_new.y分别为新节点ξ_new的x、y坐标。

进一步地，在步骤2中，判断是否找到从起始点到达目标点的路径的方法如下：

输出第一次RRT规划的路径节点列表NodeList，通过判断节点列表最后一个节点与目标点的误差是否超过预设阈值d表示是否找到路径，若大于阈值d则视为未找到路径，需要返回步骤1进行重规划。

进一步地，步骤3中，以上一棵随机树的节点列表为引导重新构建RRT随机树，同时考虑角度代价和长度代价选择父节点重新连线，具体包括：

在上一棵随机树的NodeList中随机选取节点作为新随机树的随机点，再寻找和随机点距离最近的节点ξ_nearest，通过控制量偏移u*拓展新节点ξ_new并对ξ_new进行碰撞检测；碰撞检测通过后，以ξ_new为圆心考虑角度代价和长度代价，寻找上一棵随机树的节点与ξ_new代价最小的节点以及ξ_new的邻域内的节点作为备选父节点ξ_preparent，重新连线进而优化路径轨迹；

经过邻域内点的代价Cost为距离代价length(ξ_init,ξ_preparent,ξ_new)和角度代价SumAngle(ξ_init,ξ_preparent,ξ_new)的加权，记为：

Cost＝length(ξ_init,ξ_preparent,ξ_new)+SumAngle(ξ_init,ξ_preparent,ξ_new)

其中，init为起始点下标，preparent为备选父节点下标，ξ_preparent.x、ξ_preparent.y分别为ξ_preparent的x、y坐标。

进一步地，步骤5中的加权代价ValueFunction(α,β,γ,λ)如下：

ValueFunction(α,β,γ,λ)＝α*NodeList.Num+β*NodeList.COST+γ*NodeList.MaxAngle+λ*NodeList.SumAngle

其中，

NodeList.Num表示节点列表NodeList中的节点总数量，

NodeList.COST表示节点列表NodeList中的节点所连接成的路径长度代价，

NodeList.MaxAngle表示节点列表NodeList中的节点最大角度，

NodeList.SumAngle表示节点列表NodeList中的节点角度之和，

α,β,γ,λ分别是NodeList.Num、NodeList.COST、NodeList.MaxAngle、NodeList.SumAngle对应的权值，通过调整这些权值以得到不同偏好的路径。

为了实现上述目的，按照本发明的另一个方面，提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现如前任一项所述的无人艇加权迭代路径规划方法。

为了实现上述目的，按照本发明的另一个方面，提供了一种基于多树RRT的无人艇加权迭代路径规划设备，包括如前所述的计算机可读存储介质以及处理器，处理器用于调用和处理计算机可读存储介质中存储的计算机程序。

总体而言，本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、本发明基于RRT多树迭代思想，发明了一种既可以加速收敛，又可以不必要进行全局搜索而得到渐进最优解的多树迭代规划路径算法。主要的算法思路为按次序生成RRT树节点列表，为下一棵随机树提供启发式搜索，通过多树迭代得到渐进优化的路径轨迹。由于不是同时维护这些树的生成，而是按顺序依次生成RRT树，通过多树迭代得到渐进优化的路径轨迹，可以提高收敛速度。

2、对于RRT算法而言，相比全局规划，这种基于改进APFs的方法生成区域，第一棵随机树优先对区域内的点进行随机探索，以人工势场法计算得到的势场作为RRT的指引点，克服了RRT原有的随机性强的问题，减小不必要的拓展，随机探索的范围减小，计算量减小，使得其规划效率和避碰等方面都得到了很大的提高。此后的第二棵树开始，负责的主要是优化轨迹路径，按照迭代的思想后一棵随机树以前一棵随机树的路径节点作为随机点生成的备选集合，基于RRT*的思想直到抵达目标点。通过代价评估函数终止条件得到满足模型运动约束的最优解或者次优解。

3、第一棵随机树通过结合人工势场方法限定树的生长方向，既可以朝着目标点方向，又可以避让障碍物，此过程中通过节点过滤去除无效节点邻域，从而提高拓展效率，最终生成一条基本的路径轨迹，可以更为快速、高效地加速收敛并提升准确性。

4、在重新构建RRT随机树过程中，同时考虑角度代价和长度代价选择父节点重新连线，可以更为快速、准确地产生最优解。

5、代价评估函数的权值可变，能够根据不同偏好进行权值调整从而获得不同偏好类型的路径规划结果，灵活度高，适用场景广泛。

附图说明

图1是基于多树RRT的无人艇加权迭代路径规划方法流程图；

图2是APFs的引导下的随机点区域；

图3是基于APFs的引导生成第一棵随机树的过程；

图4是考虑角度代价和长度代价的加权迭代优化过程。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明优选的一种基于多树RRT的无人艇加权迭代路径规划方法，如附图1的流程图所示，包括如下步骤：

步骤1：构建第一棵随机树，基于RRT思想从起始点开始生成节点，在改进人工势场APFs作用下构建可行的探测区域，生成随机点ξ_rand，寻找距离随机点最近的节点ξ_nearest，由ξ_rand方向根据控制量偏移u作为新节点备选，再进行碰撞检测判断是否可为新节点，若有碰撞，进行节点过滤，如此循环，直到到达目标点，完成第一棵随机树的生成。

步骤2：输出路径，判断是否找到路径，若没有找到合适的路径，需要进行重规划，重复自定义次数K_m次步骤1，若仍然未找到，算法判断不存在从起始点到目标点的通路。

步骤3：保留和记录上一棵随机树的节点NodeList，以NodeList作为引导节点构建新的RRT树。具体为，以NodeList中的节点作为新树产生的随机点，寻找最近的节点ξ_nearest，通过控制量偏移u*，碰撞检测后确定ξ_new，以ξ_new为圆心考虑角度代价和长度代价寻找邻域内与之代价最小的节点以及上一棵随机树的节点，从而作为所选父节点，重新连线进而优化路径轨迹。

步骤4：判断是否规划成功，成功则继续步骤5，若不成功，输出上一次规划路径。

步骤5：算法终止条件。根据加权代价评估函数，若高于期望代价，则重复步骤3，若满足期望代价值，则直接输出此时路径为最终路径，另外同时判断迭代次数n若大于设定次数K_n，认为此时路径也是优化后的最终路径轨迹。

优选地，步骤1中通过与RRT相结合，可以利用RRT的随机性来解决APF容易陷入局部最优的缺点，即利用随机点离开局部最小值，同时，APF还可以将收敛速度提高。传统人工势场法引力部分与无人车和目标点位置距离成正比，当无人车距离目标点较远时，引力部分过大，斥力相对于引力而言就会变得非常小，这就会导致无人车与障碍物相撞。因此，有必要对引力场加以改进，利用势场函数U来建立人工势场，某点ξ的势场U(ξ)为目标点的引力势场U_att(ξ)和障碍物的斥力势场U_rep(ξ)之和：

其中函数的表达式为：

其中，K_att为引力增益常数，K_rep是斥力增益系数，d为引力距离阈值，则对应的势场梯度向量表示为：

其中，ρ₀为障碍物的影响半径，ρ(ξ,obstacle)表示ξ离障碍物的最近距离，f_attractive(ξ)指向目标点方向，f_repulsion(ξ)指向背离障碍物等效形心方向，实际过程所有障碍物内部点都作为障碍物边缘点处理，f_repulsion(ξ)表示所有障碍物的斥力叠加之后的斥力向量，所以，在ξ处的梯度势场向量F(ξ)为引力向量f_attractive(ξ)和斥力向量f_repulsion(ξ)矢量和：

的方向可以根据目标点ξ_goal和ξ表示：

背离障碍物等效形心的方向可表示为：

其中，θ表示引力势场梯度向量与水平方向的夹角，

表示斥力势场梯度向量与水平方向的夹角，如附图2所示。由合成向量三角形的余弦定理和正弦定理，可得F(ξ)的大小和方向φ：

因此，通过Rand_sample采样函数生成一个随机数表示ξ_rand∈Region_sample。Region_sample即为APF在势场向量F(ξ)作用下的具有方向偏向目标点的给定裕度的区域，在附图3中所示，F(ξ)指向最近节点ξ_nearest，形成带有顶角2Angle三角形区域Region_sample1，节点1向节点2往随机点方向拓展一个步长。随机采样区域Region_sample定义为：

其中，ξ_rand.x为随机点的x方向，ξ_rand.y为随机点的y方向，ξ_nearest.x为当前要拓展点的x方向，ξ_nearest.y为当前要拓展点的y方向；K_p为区域为边长比例系数，max，min分别表示取最大和最小。

进一步，步骤1中，还包括节点过滤的步骤，具体为：

为减少碰撞检测次数，考虑节点过滤，即把不可寻的轨迹点列为不可行区域，若判断的ξ_nearest,ξ_new之间检测到障碍物，以ξ_new为圆心的邻域为可能碰撞区域，如图2中节点2-3过程中，认为以3为圆心，半径为filter_radius的RegionFilter区域不存在满足条件的新节点，即：

进一步的，在步骤2中，还包括输出路径和重规划的过程：

输出第一次RRT规划的路径节点列表NodeList，通过判断节点列表最后一个节点与目标点的误差是否超过阈值d表示是否找到路径，若大于阈值则需要进行重规划，设置最大重规划次数K_m次步骤1，若仍然未找到，算法判断不存在从起始点到目标点的通路。

进一步的，步骤3中，以上一棵随机树的节点列表为引导重新构建RRT随机树，考虑角度代价和长度代价选择父节点重新连线，具体包括：

在上一棵随机树的NodeList中随机选取节点作为新树的随机点，再寻找和随机点距离最近的ξ_nearest，通过控制量偏移u*拓展新节点ξ_new，碰撞检测通过后，以ξ_new为圆心考虑角度代价和长度代价寻找邻域内与之代价最小的节点以及上一棵随机树的节点，从而作为所选父节点，重新连线进而优化路径轨迹。

经过邻域内点的代价Cost为距离代价和角度代价的加权，距离代价包含经过邻域内备选父节点ξ_preparent与拓展节点ξ_new的距离以及起始点到ξ_preparent的距离：

角度代价包含经过邻域内备选父节点ξ_preparent与拓展节点ξ_new的角度以及起始点到ξ_preparent的角度之和：

如附图4中所示，以拓展节点5为圆心的邻域内有备选父节点3，6，9，10，最近节点为4，通过代价比较，ξ_init-1-2-6-5为代价最小的路径。

进一步的，步骤5中，所述的加权代价评估函数，具体包括：

节点列表中节点总数量NodeList.Num，路径长度代价NodeList.COST，节点最大角度NodeList.MaxAngle以及节点角度之和NodeList.SumAngle，代价评估函数为这些指标的加权，调整权值α,β,γ,λ可以得到不同偏好的路径：

ValueFunction(α,β,γ,λ)＝α*NodeList.Num+β*NodeList.COST+γ*NodeList.MaxAngle+λ*NodeList.SumAngle

下面以一个简单的算法流程示意来对本发明的步骤1-3及步骤5进行介绍，以使得本发明的方法更易于理解，所述一种基于多树RRT的无人艇加权迭代路径规划方法，其步骤1-3及步骤5的算法流程表示如下：

步骤1中，First_RRT(ξ_init,K,Δt,Angle,K_p,filter_radius,ξ_goal)：生成第一棵基于人工势场APFs引导的随机树，返回第一次规划的节点列表NodeList。

输入：ξ_init和ξ_goal：分别表示规划起始点和目标点位置；

K：RRT探索次数；

Δt：节点偏移量控制下发时间间隔；

Angle和K_p：人工势场APFs区域角度和边长系数；

filter_radius：节点过滤区域半径；

算法中的符号和函数：

NodeList：节点列表，为一系列节点node的集合；

RegionFilter：节点过滤区域；

APFs(ξ_i,obstacle,ξ_goal,Angle,R)：基于人工势场APFs的引导采样函数，返回采样区域；

Rand_sample(ξ_i,Region_sample)：在区域内随机采样；

ξ_nearest←Nearest_Neighbor(ξ_rand,NodeList)：寻找离随机点欧氏距离最近的节点；

Form_Node(ξ_nearest,u,Δt)：最近节点往随机点方向偏移控制量u，生成新节点；

Judge(ξ_new)：新节点碰撞检测，若发生碰撞则进行节点过滤并生成过滤区域，否则将节点添加到NodeList中；

步骤2中，Replanning(NodeList,K_m)：判断第一棵RRT随机树是否规划成功并进行重规划算法，返回是否成功。

输入：NodeList：第一棵随机树输出的节点列表；

K_m：重规划次数；

算法中的符号和函数：

NodeList.last：列表中最后一个节点；

d：到达目标点的允许误差；

步骤3中，Modified_RRT(ξ_init,inter_max,NodeList,radius,ξ_goal)：根据前一棵随机树的NodeList引导下一棵随机树的生成，返回更新后的NodeList。

输入：ξ_init和ξ_goal：分别表示规划起始点和目标点位置；

inter_max：最大RRT搜索次数；

NodeList：前一棵随机树的节点列表；

radius：重选父节点的邻域半径；

算法中的符号和函数：

Tree：初始化随机树；

Steer_Input(ξ_rand,ξ_nearest)和Steer(ξ_near,ξ_rand,Δt,u*)：产生生长控制量并生成新节点；

Neighbor(Tree.V∪NodeList,ξ_new,radius)：产生以新节点为圆心，radius为半径的邻域，

其中Tree.V∪NodeList为现列表和添加原节点列表的所有区域内节点合集作为预选父节点；

collision_free(ξ_near,ξ_new)：碰撞检测函数；

第8和第13分别表示计算经过最近节点和备选父节点路径的代价：a，b分别为长度代价和角度代价，length：长度代价，SumAngle：从起始点经过最近节点或者备选父节点的路径角度之和；

best_edge(E,ξ_parent,ξ_new)：将代价最小的边连接，重新布线；

Tree.add_vertex(ξ_parent,ξ_new)：更新优化后的节点，将父节点和新节点添加到现随机树列表中；

步骤5中，StopFunction(NodeList,K_n,Cost_SetValue)：算法终止条件，根据最终加权评价函数判断路径是否满足条件，返回迭代次数。

输入：NodeList：上一次迭代的路径节点列表；

K_n：设置的最大优化迭代次数；

NodeList：前一棵随机树的节点列表；

Cost_SetValue：设置的可接受代价最大值；

算法中的符号和函数：

Tree：初始化随机树；

ValueFunction(α,β,γ,λ)：代价评估函数，输入的是权值对应NodeList.Num(节点数量)，NodeList.COST(路径长度代价)，NodeList.MaxAngle(路径节点最大角度)，NodeList.SumAngle(节点角度之和)；

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于多树RRT的无人艇加权迭代路径规划方法，其特征在于，包括以下步骤：

步骤1：基于RRT算法从起始点开始朝向目标点生成随机节点从而构建第一棵RRT随机树；

步骤2：判断是否找到从起始点到达目标点的路径，若是，则输出该路径作为第一条路径，进入步骤3；否则，需要进行重规划第一棵随机树，即重复执行步骤1，直至找到从起始点到达目标点的路径作为第一条路径并进入步骤3；若重复预设的K_m次步骤1仍然未找到从起始点到达目标点的路径，则判定不存在从起始点到目标点的通路；

步骤3：保留和记录上一棵随机树的节点NodeList，以NodeList作为引导节点构建新的RRT随机树；步骤3中，以上一棵随机树的节点列表为引导重新构建RRT随机树，同时考虑角度代价和长度代价选择父节点重新连线，具体包括：

在上一棵随机树的NodeList中随机选取节点作为新随机树的随机点，再寻找和随机点距离最近的节点ξ_nearest，通过控制量偏移u*拓展新节点ξ_new并对ξ_new进行碰撞检测；碰撞检测通过后，以ξ_new为圆心考虑角度代价和长度代价，寻找上一棵随机树的节点与ξ_new代价最小的节点以及ξ_new的邻域内的节点作为备选父节点ξ_preparent，重新连线进而优化路径轨迹；

经过邻域内点的代价Cost为距离代价length(ξ_init,ξ_preparent,ξ_new)和角度代价SumAngle(ξ_init,ξ_preparent,ξ_new)的加权，记为：

Cost＝length(ξ_init,ξ_preparent,ξ_new)+SumAngle(ξ_init,ξ_preparent,ξ_new)

其中，init为起始点下标，preparent为备选父节点下标，ξ_preparent.x、ξ_preparent.y分别为ξ_preparent的x、y坐标；

步骤4：判断是否规划成功，成功则继续步骤5；若不成功，输出上一次规划路径作为最终优化的路径；

步骤5：计算规划路径的加权代价，若高于预设的期望代价，则返回步骤3；若不高于预设的期望代价或者达到预设最大迭代次数K_n，则直接输出此时路径为最终优化的路径。

2.如权利要求1所述的一种基于多树RRT的无人艇加权迭代路径规划方法，其特征在于，步骤1中还包括在人工势场APFs作用下构建可行探测区域，引导随机点生成函数，具体包括：

利用势场函数U来建立人工势场，设某随机节点ξ的势场U(ξ)为目标点的引力势场U_att(ξ)和障碍物的斥力势场U_rep(ξ)之和：

其中，改进的势场函数表达式为：

其中，K_att为引力增益常数，K_rep为斥力系数，d为引力距离阈值，ρ(ξ,ξ_goal)表示ξ离目标点ξ_goal的最近距离，ρ₀为障碍物obstacle的影响半径，ρ(ξ,obstacle)表示ξ离障碍物obstacle的最近距离；在ξ处的梯度势场向量F(ξ)为引力势场梯度

和斥力势场梯度

矢量和：

因此，通过Rand_sample采样函数生成一个随机数ξ_rand来表示ξ_rand∈Region_sample，则Region_sample即为APF在势场向量F(ξ)作用下的具有方向偏向目标点的给定裕度的可行探测区域，表示为：

其中，

ξ_rand.x为随机点ξ_rand的x坐标，ξ_rand.y为随机点ξ_rand的y坐标，ξ_nearest.x为当前要拓展点ξ_nearest的x坐标，ξ_nearest.y为当前要拓展点ξ_nearest的y坐标；K_p为区域Region_sample的边长比例系数， φ为水平方向与F(ξ)的夹角，Angle为区域Region_sample的最大夹角。

3.如权利要求2所述的一种基于多树RRT的无人艇加权迭代路径规划方法，其特征在于，步骤1中还包括节点过滤步骤，具体为：

将不可寻的轨迹点列为不可行区域，若在ξ_nearest和新节点ξ_new之间检测到障碍物，则将ξ_new为圆心、filter_radius为半径的区域过滤，记为过滤区域RegionFilter，即：

其中，ξ.x、ξ.y分别为节点ξ的x、y坐标，ξ_new.x、ξ_new.y分别为新节点ξ_new的x、y坐标。

4.如权利要求1～3任一项所述的一种基于多树RRT的无人艇加权迭代路径规划方法，其特征在于，在步骤2中，判断是否找到从起始点到达目标点的路径的方法如下：

输出第一次RRT规划的路径节点列表NodeList，通过判断节点列表最后一个节点与目标点的误差是否超过预设阈值d表示是否找到路径，若大于阈值d则视为未找到路径，需要返回步骤1进行重规划。

5.如权利要求1～3任一项所述的一种基于多树RRT的无人艇加权迭代路径规划方法，其特征在于，步骤5中的加权代价ValueFunction(α,β,γ,λ)如下：

ValueFunction(α,β,γ,λ)＝α*NodeList.Num+β*NodeList.COST+γ*NodeList.MaxAngle+λ*NodeList.SumAngle

其中，

NodeList.Num表示节点列表NodeList中的节点总数量，

NodeList.COST表示节点列表NodeList中的节点所连接成的路径长度代价，

NodeList.MaxAngle表示节点列表NodeList中的节点最大角度，

NodeList.SumAngle表示节点列表NodeList中的节点角度之和，

α,β,γ,λ分别是NodeList.Num、NodeList.COST、NodeList.MaxAngle、NodeList.SumAngle对应的权值，通过调整这些权值以得到不同偏好的路径。

6.一种计算机可读存储介质，其特征在于，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现如权利要求1～5任一项所述的无人艇加权迭代路径规划方法。

7.一种基于多树RRT的无人艇加权迭代路径规划设备，其特征在于，包括如权利要求6所述的计算机可读存储介质以及处理器，处理器用于调用和处理计算机可读存储介质中存储的计算机程序。

Images