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

摘要

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

说明书

技术领域

本发明属于无人艇运动规划领域，更具体地，涉及一种基于多树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

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

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

步骤5：计算规划路径的加权代价，若高于预设的期望代价，则返回步骤3；若不高于预设的期望代价或者达到预设最大迭代次数K

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

利用势场函数U来建立人工势场，设某随机节点ξ的势场U(ξ)为目标点的引力势场U

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

其中，K

因此，通过Rand_sample采样函数生成一个随机数ξ

其中，

ξ

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

将不可寻的轨迹点列为不可行区域，若在ξ

其中，ξ

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

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

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

在上一棵随机树的NodeList中随机选取节点作为新随机树的随机点，再寻找和随机点距离最近的节点ξ

经过邻域内点的代价Cost为距离代价length(ξ

Cost＝length(ξ

其中，init为起始点下标，preparent为备选父节点下标，ξ

进一步地，步骤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作用下构建可行的探测区域，生成随机点ξ

步骤2：输出路径，判断是否找到路径，若没有找到合适的路径，需要进行重规划，重复自定义次数K

步骤3：保留和记录上一棵随机树的节点NodeList，以NodeList作为引导节点构建新的RRT树。具体为，以NodeList中的节点作为新树产生的随机点，寻找最近的节点ξ

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

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

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

其中函数的表达式为：

其中，K

其中，ρ

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

因此，通过Rand_sample采样函数生成一个随机数表示ξ

其中，ξ

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

为减少碰撞检测次数，考虑节点过滤，即把不可寻的轨迹点列为不可行区域，若判断的ξ

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

输出第一次RRT规划的路径节点列表NodeList，通过判断节点列表最后一个节点与目标点的误差是否超过阈值d表示是否找到路径，若大于阈值则需要进行重规划，设置最大重规划次数K

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

在上一棵随机树的NodeList中随机选取节点作为新树的随机点，再寻找和随机点距离最近的ξ

经过邻域内点的代价Cost为距离代价和角度代价的加权，距离代价包含经过邻域内备选父节点ξ

角度代价包含经过邻域内备选父节点ξ

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

进一步的，步骤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(ξ

输入：ξ

K：RRT探索次数；

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

Angle和K

filter_radius：节点过滤区域半径；

算法中的符号和函数：

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

RegionFilter：节点过滤区域；

APFs(ξ

Rand_sample(ξ

ξ

Form_Node(ξ

Judge(ξ

步骤2中，Replanning(NodeList,K

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

K

算法中的符号和函数：

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

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

步骤3中，Modified_RRT(ξ

输入：ξ

inter

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

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

算法中的符号和函数：

Tree：初始化随机树；

Steer_Input(ξ

Neighbor(Tree.V∪NodeList,ξ

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

collision_free(ξ

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

best_edge(E,ξ

Tree.add_vertex(ξ

步骤5中，StopFunction(NodeList,K

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

K

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

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

算法中的符号和函数：

Tree：初始化随机树；

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

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