CN114879666A

CN114879666A - 一种基于rrt算法的水面无人艇路径规划方法和装置

Info

Publication number: CN114879666A
Application number: CN202210431753.2A
Authority: CN
Inventors: 谢远龙; 吴志渊; 毛瑞琪; 胡一杨; 胡智文; 艾琪; 吴昊; 蒋立泉; 王书亭
Original assignee: Huazhong University of Science and Technology
Current assignee: Huazhong University of Science and Technology
Priority date: 2022-04-22
Filing date: 2022-04-22
Publication date: 2022-08-09

Abstract

本发明属于无人艇路径规划领域，具体涉及一种基于RRT算法的水面无人艇路径规划方法和装置，包括：采用RRT算法选择从起始点到目标点的各路径节点，实现无人艇路径规划，其中在选择每个路径节点时，在基于当前路径节点的用于对下一路径节点进行随机采样的采样区域随机取点，并从满足转角值约束的若干候选节点中基于代价函数择优，作为下一路径节点；代价函数为从当前路径节点到候选节点的航行能耗、两个节点间的路径长度以及两节点处无人艇船头朝向之间的转角值三者的线性加权；航行能耗和船头朝向均通过无人艇在实际环境因素作用下的运动学模型计算得到。本发明考虑实际环境因素，使无人艇在指定目标点间依据能耗较低原则实现路径规划与避障。

Description

一种基于RRT算法的水面无人艇路径规划方法和装置

技术领域

本发明属于无人艇路径规划领域，更具体地，涉及一种基于RRT算法的水面无人艇路径规划方法和装置。

背景技术

无人水面艇(Unmanned Surface Vessel，USV)是现代多种水域应用下的重要执行工具，在港口导航、救援、环境控制、军事等方面有重要应用价值。对无人水面艇的路径规划是其完成指定任务的必要前提，优化路径规划的算法，能够使得无人水面艇尽可能快速、安全、长时间的运行。

快速扩展随机树(Rapidly-exploring Random Trees，RRT)算法是近十几年得到广泛发展与应用的基于采样的运动规划算法，具有生成速度快，适应性强等诸多优点。根据RRT的采样思想，连接一系列从无障碍的空间中随机采样的点，适合解决多自由度无人水面艇在复杂环境下和动态环境中的路径规划问题，从而建立一条从初始状态到目标状态的路径。面向USV的工作场景，主要有以下三个问题：

(1)为保障概率完备性，RRT算法在大范围空间中的路径搜索具有较强的随机性，空间过度拟合、重复搜索等方式都会造成收敛速度过慢。

(2)空间路径的搜索过程可以通过对了路径的重复搜索可以得到相对最短的路径，然而该路径并未考虑运行的能量模型，在实际的风力与水流影响下难以提高无人水面艇的运行时间。

(3)由于节点生长的随机性，RRT算法规划的路径难以满足无人水面艇的运动模型的需求。

因此，亟需一种考虑能耗与运动约束模型的RRT无人艇路径规划与导航方法。

发明内容

针对现有技术的缺陷和改进需求，本发明提供了一种基于RRT算法的水面无人艇路径规划方法和装置，其目的在于考虑水面实际环境因素，使无人艇在指定目标点之间依据能耗较低原则实现路径规划和航行。

为实现上述目的，按照本发明的一个方面，提供了一种基于RRT算法的水面无人艇路径规划方法，包括：

采用RRT算法选择从起始点到目标点的各路径节点，实现无人艇路径规划，其中在选择每个路径节点时，在基于当前路径节点的用于对下一路径节点进行随机采样的采样区域随机取点，并从满足转角值约束的若干候选节点中基于代价函数择优，作为下一路径节点；

所述代价函数为从当前路径节点到候选节点的航行能耗、两个节点间的路径长度以及两节点处无人艇船头朝向之间的转角值三者的线性加权；所述航行能耗和所述船头朝向均通过无人艇在实际环境因素作用下的运动学模型计算得到。

进一步，将当前路径节点ξ_init在全局坐标系{e}的状态信息描述为：

将候选节点ξ_new在全局坐标系{e}的状态信息描述为：

x、y和β分别表示横纵坐标值以及船头朝向与横轴的夹角；所述运动学模型为：

式中，

和α分别表示当前实际环境中位于全局坐标系{e}的水流速度矢量及其方向；

为水面无人艇在静水中的速度；v_b表示期望的合速度，方向沿坐标系{b}的x轴正方向，该x轴正方向为当前路径节点ξ_init与候选节点ξ_new的连线方向；λ为当前路径节点ξ_init至候选节点ξ_new的几何角度。

进一步，所述转角值的确定方式为：

式中，

为船头朝向在候选节点相对当前路径节点的转角值；

所述转角值约束为：

式中ε_β为水面无人艇的最大转角，为已知量。。

进一步，从当前路径节点到候选节点的航行能耗

式中，f_w,s表示无人艇受到的水流阻力f_w在

方向上的投影值，

为v_r与x_v方向的夹角，

为水流相对于无人艇的速度，

分别为v_r在以无人艇在静水中的速度

方向为正方向所构建的坐标系{v}中横纵坐标轴上的投影速度，d为ξ_init到ξ_new的欧氏距离，C_wind为风阻系数，ρ_a为空气密度，v_wind为风速，S_w为无人艇的迎风面积，ψ为风速方向与水平方向的夹角，大小为[0,2π]，ρ₀为水的密度，C_aq与C_f分别为附体阻力系数与流体摩擦阻力系数，S_s为无人艇在v_r方向的投影面积，且无人艇受到的流体阻力方向沿v_r方向。

进一步，所述采样区域由引力势场和斥力势场在节点ξ_init处的合力所产生一随机采样区域Random_sampling；

其中，所述引力势场U_att(ξ_init)表示为：

所述斥力势场U_rep(ξ_init)表示为：

所述采样区域Random_sampling表示为：

式中，k_att与k_rep分别为引力增益常数与斥力增益常数，d₀为引力距离的一个阈值，d(ξ_init,ξ_goal)表示当前节点ξ_init距离目标点ξ_goal的欧氏距离，d_obs为障碍物obs的影响半径，d(ξ_init,obs)表示当前节点ξ_init距离障碍物形心的最近距离；

与

分别是在生成的采样区域中当前路径节点ξ_init在全局坐标系{e}的x与y坐标；K_F为采样区域的边长系数，决定了采样区域的大小；θ_x为当前路径节点ξ_init处的势场力与全局坐标系{e}的x轴方向的夹角，θ_max为最大的期望区域张角的一半。

进一步，所述代价函数表示为：

式中，n为在采样区域随机采的采样点数量，k_length和k_angle为路经长度与转角值的权重，P_k-1,i为从上一个节点ξ_k-1到当前采样点ξ_k,i的能耗，且P_k-1,i可以取负值，k解释为RRT树的第k次生长，进而在随机取的n个采样点中选取代价函数值最小的随机点作为RRT树第k次生长的新节点ξ_k。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序被处理器运行时控制所述存储介质所在设备执行如上所述的一种基于RRT算法的水面无人艇路径规划方法。

本发明还提供一种基于RRT算法的水面无人艇路径规划设备，包括：处理器、收发机，以及如上所述的计算机可读存储介质，其中，

所述收发机，用于在所述处理器的控制下收发数据；

所述处理器执行所述计算机可读存储介质上的计算机程序时实现如上所述一种基于RRT算法的水面无人艇路径规划方法的步骤。

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

(1)本发明在无人艇运动学模型的基础上，得到了两节点之间的距离、转角、速度等因素，基于这些因素得到了从能耗角度考虑的节点间代价函数(加权函数)，在通过RRT采样取点的过程中便将代价函数作为最优取点法则，进而可以使得规划出的整条路径能耗最低或相对最低。

(2)本发明建立的无人艇的运动学模型考虑到了无人艇运动过程中的欠约束控制特点，在无人艇的静水速度基础上求解相对水流速度以及相应的水流阻力，同时考虑到了风力的影响，最终求解得到在相应实际环境(水流场、风力场)影响下的无人艇朝向，通过父子节点之间的这一朝向重新考虑无人艇的运动约束(转角等)，避免了在理论转角约束与实际环境因素作用下的航线差异。

(3)本发明采用了改进的APFS(人工势场函数)来进行行进中避障的局部规划。人工势场函数是通过人为构建以障碍物和目标点为中心的斥力场和引力场，进而通过斥力与引力的合力决定无人艇局部规划的运动方向。当无人艇离障碍物较近时，斥力明显大于引力，引导无人艇避障。当无人艇远离障碍物时，目标点的引力起主导作用，引导无人艇继续向下一节点行进，实现局部规划。对常规的人工势场函数(Artificial Potential Filed,APF)做了一定的优化改进，实现了局部条件下的加速收敛，保障在路径可行条件下的功耗相对最小。

附图说明

图1为本发明实施例提供的能耗模型场景示意图；

图2为本发明实施例提供的路径规划流程图。

具体实施方式

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

实施例一

一种基于RRT算法的水面无人艇路径规划方法，包括：

也就是，本实施例方法，需要首先根据无人水面艇的实际工作情况，建立了考虑水流影响的无人艇运动模型，并基于此运动模型得到在指定流速场影响下无人艇的航行朝向。并根据预测得到的无人艇运动方式，考虑在此方式下风力对无人艇的影响，进而得到了考虑水流与风力的能耗模型。其次，根据实际情况下无人水面艇欠约束控制的运动学特点，建立无人艇的运动学模型，给出转角约束条件。最后，使用改进的人工势场函数方法，建立可行的探测区域，在区域中随机取一定数量点，结合考虑上述能耗模型和约束的代价函数，取较优点作为子节点。判断路径规划是否成功，即新节点距离终点的距离是否小于指定阈值，成功则结束规划，输出路径，反之则重复上述取点过程。

优选的，运动学模型的构建方法如下：

如图1所示，在全局坐标系{e}中，用三自由度的坐标形式描述无人水面艇的状态信息，其中ξ_init与ξ_new为当前节点与下一个可能的节点：

ξ_init:[x_init,y_init,β_init]^T

ξ_new:[x_new,y_new,β_new]^T

其中β_init与β_new均为待求量。

与α分别表示当前环境中的水流速度矢量及其方向，

为水面无人艇在静水中的速度。λ为ξ_init节点与ξ_new节点连线在全局坐标系中与x轴的角度，则在此方向上建立坐标系{b}，则：

式中，v_b表示期望的合速度，方向沿{b}的x轴方向。则有：

其中：

即在此水流作用下，水面无人艇应朝向β方向行进。

优选的，能耗的确定方式：

在以

方向为x正方向的{v}系中根据如下关系计算水流对水面无人艇的阻力：

其中

为水流相对于无人水面艇的速度，ρ₀为水的密度，C_aq与C_f分别为附体阻力系数与流体摩擦阻力系数，分别可取为10％与20％，S_s为无人水面艇在v_r方向的投影面积，且无人水面艇受到的流体阻力方向沿v_r方向，f_w表示无人艇受到的水流阻力大小。对于相对速度v_r，有：

其中

为v_r与x_v方向的夹角，

分别为v_r在{v}系中的投影速度。

则有水流阻力在

方向上的投影值f_w,s：

则有从ξ_init到ξ_new的功耗代价函数：

其中d为节点ξ_init到ξ_new的欧氏距离，C_wind为风阻系数，取为30％，ρ_a为空气密度，v_wind为风速，S_w为无人艇的迎风面积，ψ为风速方向与水平方向的夹角，大小为[0,2π]。

可记从节点ξ_init出发至节点ξ_new的路径中，无人水面艇的期望角度为β_init。同理可用β_init-1表示从节点ξ_init-1至节点ξ_init的路径中船头的角度。则有转角值：

其中

为节点ξ_init至节点ξ_new的几何角度；

可得角度约束条件：

其中ε_β为无人水面艇的最大转角。

建立人工势场函数，设随机节点ξ_init的势场函数U(ξ_init)为目标点的引力势场U_att(ξ_init)和障碍物的斥力势场U_rep(ξ_init)的矢量和。其中，改进的势场函数表达式分别为：

其中k_att与k_rep分别为引力增益常数与斥力增益常数，d₀为引力距离的一个阈值，d(ξ_init,ξ_goal)表示当前节点ξ_init距离目标点ξ_goal的欧氏距离，d_obs为障碍物obs的影响半径，d(ξ_init,obs)表示当前节点ξ_init距离障碍物形心的最近距离。本实施例的斥力势场U_rep(ξ_init)是在常规的二次反比例斥力函数的形式上乘上新的因子d(ξ_init,obs)，使得在距离目标点较近时，斥力接近于0，可以有效解决目标点不可达的问题。

由此可得在节点ξ_init处的合力表示为两势场梯度的矢量和：

进而由合力F产生一随机采样区域Random_sampling,则为当前随机节点下具有偏向目标点ξ_goal给定裕度的采样区域，满足：

式中，

与

此采样区域形状为扇形区域，可以有效解决常规人工势场函数出现局部斥力远大于引力的情况，加快了收敛速度。

进一步地，在从起点ξ₀向目标点ξ_goal的规划中，根据上述人工势场方法，生成基于节点ξ_k-1的用于对下一节点ξ_k进行随机采样的区域，在区域中随机取n个采样点ξ_k,1～ξ_k,n，则建立考虑能耗、路径长度与转角值的代价函数，函数结合步骤1中得到的功耗代价函数，对三者采用线性加权的方式作为代价值：

其中k_length和k_angle为路经长度与转角值的权重，实践中可取参考值60％,80％。P_k-1,i为步骤1中建立的从上一个节点ξ_k-1到当前采样点ξ_k,i的能耗，且P_k-1,i可以取负值，k解释为RRT树的第k次生长。进而在随机取的n个采样点中选取代价函数值最小的随机点作为RRT树第k次生长的新节点ξ_k。

也就是，本实施例一方面利用无人艇运动学模型建立了路径规划的转角约束模型，同时采用基于改进的人工势场函数的RRT思想，对常规的人工势场函数(ArtificialPotential Filed,APF)做了一定的优化改进，实现了局部条件下的加速收敛，保障在路径可行条件下的功耗相对最小；另一方面，建立了无人水面艇的功耗模型并作为路径规划加权权重，从而使得无人艇的理想功耗最小或次最小，符合实际应用场景的需求。

进一步优选地，在求节点ξ₁的规划过程中得到了在全局坐标系{e}内ξ₁相对ξ_goal的朝向β₀的值，在ξ₁之后的节点生长过程中，需要优先对所取的n个随机点做角度约束判断：

|Δβ_k-1，i|＝|β_k-1,i-β_k-2|≤ε_β,k≥2；

剔除不满足转角约束的点，再进行能耗代价择优。

如图2所示，通过判断新的采样点到目标点ξ_goal的距离是否小于阈值d_target，若是，则直接连接当前采样点与目标点ξ_goal，输出完整的路径节点序列。其中，阈值d_target默认情况下可取0.5m。

本实施例方法建立了无人艇在水流冲击影响下的速度方程，实现了对一定水流影响下的无人艇运动轨迹的预测；进一步的，得到了无人艇在水流与风力综合影响下的指定节点间的考虑能耗的代价函数，实现了基于能耗最低的路径点择优采样；进一步地，建立了基于人工势场法的采样区域，减低了RRT采样的随机性，使得算法的收敛速度大大提高；最后，根据无人水面艇的运动特点，构建运动约束模型，使得算法生成的规划路径更符合无人水面艇的运动特点，更具有使用意义。算法整体达到了使无人艇能够在指定目标点之间依据能耗较低原则实现路径规划与避障的效果。

实施例二

一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序被处理器运行时控制所述存储介质所在设备执行如上所述的一种基于RRT算法的水面无人艇路径规划方法。

相关技术方案同实施例一，在此不再赘述。

实施例三

一种基于RRT算法的水面无人艇路径规划设备，包括：处理器、收发机，以及如实施例二所述的计算机可读存储介质，其中，

所述收发机，用于在所述处理器的控制下收发数据；

所述处理器执行所述计算机可读存储介质上的计算机程序时实现如实施例一所述一种基于RRT算法的水面无人艇路径规划方法的步骤。

相关技术方案同实施例一，在此不再赘述。

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