CN116414123A - 基于改进快速行进法的水面无人艇路径规划方法 - Google Patents

Abstract

基于改进快速行进法的水面无人艇路径规划方法，属于水面无人艇技术领域。解决了全局路径规划局面下，对未知障碍物实时避碰效果差的问题。本发明首先获取全局地图信息，利用快速行进法构建以目标终点为源点的初始全局环境导航势场，通过全局环境导航势场的梯度方向，获取全局地图中任意位置的初始期望航行艏向；无人艇航行过程中环境感知层实时对障碍物进行探测，若是探测到障碍物，构建基于速度的无人艇碰撞模型；并判断是否存在碰撞风险，如不存在，则按照当前位置的初始期望艏向方向继续导航前进，否则，基于碰撞模型与初始期望艏向利用速度障碍法计算当前位置的避障期望艏向，进行避障航行。本发明适用于无人艇路径规划。

Description

基于改进快速行进法的水面无人艇路径规划方法

技术领域

本发明属于水面无人艇技术领域。

背景技术

水面无人艇(Unmanned Surface Vehicles,USV)作为近年来新型的海上装备，凭借其高速、智能、无人的特点，具有广泛的应用前景和重要的研究价值。安全高效的路径规划技术是USV自主完成各种任务的必要条件，在很大程度上决定了无人艇的智能化水平。

根据对环境信息的掌握程度不同，USV的路径规划可以分为全局路径规划和局部路径规划。

全局路径规划能够在已知全局环境信息的基础上，主要对静态障碍物进行规避。全局路径规划涉及的算法目前主要包括Dijkstra算法、蚁群算法、快速行进法等。然而以上规划方法都存在着缺陷，例如快速行进法虽然规避了局部最优的问题，但是在网格细分之后，计算量增大。并且全局路径规划方法对突然出现的未知障碍物躲避能力较弱，不能根据动态环境实时制定策略。

局部路径规划算法在船载传感器等设备捕获到无人艇周围未知环境信息或者当雷达等传感装置接收到动态障碍物信息后，能够在短时间、小范围内执行的规避措施。目前主要包括人工势场法、速度障碍法等。人工势场法在应用过程中，可能出现当目标位置区域附近有障碍物时目标不可达以及局部极小值问题；速度障碍法则忽略了无人艇的动力约束，只求解出当前局面的安全角度并没有考虑是否能够安全实现艏向的改变。并且，上述算法只适用于短时间内、突发的随机局面，而不适用于全局静态环境路径的寻优。

发明内容

本发明的目的是为了解决全局路径规划局面下，对未知障碍物实时避碰效果差的问题，提出了一种基于改进快速行进法的水面无人艇路径规划方法。

本发明所述的基于改进快速行进法的水面无人艇路径规划方法，该方法包括：

步骤一、获取待规划路径行驶海域的全局地图信息，利用快速行进法(FastMarching Method，FMM)构建以目标终点为源点的初始全局环境导航势场，通过全局环境导航势场的梯度方向，获取全局地图中任意位置的初始期望航行艏向；

步骤二、无人艇在按照所述初始期望航行艏向航行过程中环境感知层实时对障碍物进行探测，当探测到障碍物，根据障碍物当前位置，构建基于速度的无人艇碰撞模型；

步骤三、根据无人艇当前位置的初始期望艏向与碰撞模型判断是否存在碰撞风险，若不存在，则按照当前位置的初始期望艏向继续导航前进，直至到达目标位置，完成无人艇路径规划；否则，执行步骤四；

步骤四、基于碰撞模型与无人艇当前位置的初始期望艏向，利用速度障碍法计算无人艇当前位置的避障期望艏向，进行避障航行；避障航行结束后返回执行步骤二，直至到达目标位置，完成无人艇路径规划。

进一步地，本发明中，步骤一中，获取全局地图中任意位置的初始期望航行艏向的具体方法为：

步骤一一、将全局地图转化为网格地图，从全局地图和岛屿海岸线数据中提取静态障碍物的位置、大小和覆盖范围；

步骤一二、以目标终点为起始源点，利用快速行进法算法结合静态障碍物的位置、大小和覆盖范围构建静态障碍物与无人艇之间的安全区域，提取安全区域内每个网格节点的速度信息，保存在向量矩阵V中，计算无人艇到达每个网格节点的时间，保存在矩阵T中；

步骤一三、对所述向量矩阵V和矩阵T进行归一化处理，构建出全局导航势场M_PF，获取全局地图中任意位置的初始期望航行艏向。

进一步地，本发明中，步骤一二中所述的无人艇到达每个网格节点的时间的计算方法为：

建立一维的程函方程：

T(z)是源点到达第z个节点的时间，V(z)是达到第z个节点时的行进速度；

已知二维各相同性介质中的程函方程为：

其中，t为源点出发抵达节点(x,y)的时间；s(x,y)为慢度，即速度的倒数；Ω为环境模型空间；

快速行进法通过求解逆风差分格式的程函方程得到数值解：

T₁＝min(T_(x-Δx,y),T_(x+Δx,y))

T₂＝min(T_(x,y-Δy),T_(x,y+Δy))

其中，空间域内任一点(x,y)，与(x,y)相邻的点表示为(x+Δx,y)，(x-Δx,y)，(x,y+Δy)，(x,y-Δy)，T₁是x方向上，到达左右两个相邻节点所需用时的较小值；T₂是y方向上，到达上下两个相邻节点所需用时的较小值。

其中，Δx,Δy是网格在x和y方向上的间距，T(x,y)的解为：

进一步地，本发明中，步骤二中，无人艇航行过程中环境感知层实时对障碍物进行探测，若是探测到障碍物，构建基于速度的无人艇碰撞模型的具体方法为：

确定无人艇和目标船在各自运动状态不变的情况下无人艇和目标船的实时相对方向λ(U,v)：

λ(U,v)＝{U+vt|t≥0}

其中，t表示时间，U是无人艇的当前位置，无人艇以速度V_u和艏向角α航行，O是目标障碍物，R是障碍物圆形安全区域的半径，目标障碍物以速度V_o和艏向角α航行，v表示V_u和V_o的合速度，方向为

γ表示UO和V之间的角度，定义从无人艇开始沿着相对速度V的射线方向航行，集合RCA为相对碰撞区域，如果无人艇在RCA设定范围内以相对速度航行，则存在与目标船相撞的风险；相对碰撞域集合的表达式：

RCA是基于相对速度定义的空间，如果无人艇遇到多艘船，则存在多个基准点，无法统一描述，利用船舶领域的概念，对速度障碍法适用的碰撞模型进行修改，获得新的相对碰撞域集合:

RCA＝{v_u|α∈[θ_aft,θ_fore]}

如果无人艇的速度矢量V_u落在设定范围内，则两艘船有碰撞的危险，无人艇在航行过程中需要实时调整速度矢量V_u偏离RCA，避免碰撞，否则，无人艇保持其原始运动状态继续航行，其中，θ_fore是障碍物前部的安全方向角，θ_aft是障碍物后部的安全方向角。。当无人艇的安全回避范围内没有危险障碍物时，无人艇的V_u是从无人艇当前位置到目标位置的方向，这将导致处理复杂环境的能力较弱。因此，可以通过FMM算法来优化无人艇的初始速度矢量。

本发明中，采用以无人艇的绝对速度作为参考空间，并转换其他目标障碍物的速度，统一对于目标障碍物的描述，同时引入船舶领域的概念，当目标障碍物以低速行驶时，船的面积接近圆形，当目标障碍物以更高的速度行驶时，船舶领域更接近于半椭圆。

在引入船舶领域之后，对速度障碍法适用的碰撞模型进行修改，得到如图4所示的示意图。O是目标船舶，SA_fore是船舶安全域的船首部分，后SA_aft是船舶安全域的船尾部分。

本发明利用快速行进法对全局环境进行遍历，生成全局环境引导势场。导航势场是通过从实际导航地图和岛屿海岸线数据中提取信息生成的，因此，CFMM-VO(ConditionedFast Marching Method-Velocity Obstacle，约束状态下的快速行进速度障碍法)算法只用关注周围船只的信息，不必构建静态全局环境。可以帮助环境感知系统过滤掉沿海边界信息，并减少在通信系统中传播的信息量。再以全局环境导航势场梯度方向作为后续采用速度障碍法躲避障碍物的初始期望艏向。

在采取避碰动作时，会根据特定的速度生成不同的船舶碰撞模型，使得无人艇可以更安全和有效地进行避碰。避碰过程中的最大艏向变化，有约束层限制，以此得到更加平滑和安全的路线。此外，回避距离也会根据航行速度的增大而调整，可以帮助无人艇(USV)更早地采取回避动作。待避碰结束，按照当前位置的初始期望艏向调整姿态，重新导航向目标点前进。

本发明所述方法能适应于无人艇海上航行环境，根据航行任务生成最优无碰撞路径，并在动态环境中，无人艇能够基于COLREGS(International Regulations forPreventing Collisions at Sea国际海上避碰规则)对规划路径进行调整。通过在实际海图环境中进行模拟试验证明本发明可以适应复杂的海上环境。并且基于“天行一号”无人艇实艇操纵性实验数据对路径规划进行转角约束，使行驶路径更加光顺、避碰路线更加安全。

附图说明

图1是使用快速扫描算法的典型情况图；

图2是基于速度障碍法的碰撞模型图；

图3是基于速度的船舶领域形状图；

图4是新构建的碰撞模型图；

图5a是国际海上航行规则中正面相遇示意图；

图5b是正面相遇的避碰规则引导图；

图5c是国际海上航行规则中追越相遇示意图；

图5d是追越相遇的避碰规则引导图；

图5e是国际海上航行规则中交叉相遇示意图；

图5f是交叉相遇的避碰规则引导图；

图6是基于改进的快速行进法提出的CFMM-VO算法的流程图；

图7是使用CFMM-VO算法的典型情况图；

图8a是使用FMM算法障碍物未进入避碰范围的路径规划结果图；

图8b是使用FMM算法障碍物进入避碰范围的路径规划结果图；

图8c是使用FMM-VO算法的路径规划结果图；

图9a是存在两个障碍物的环境仿真图；

图9b是使用FMM生成的全局环境导航势场图；

图10a是使用FMM-VO算法模拟30个周期时的结果示意图；

图10b是使用FMM-VO算法模拟50个周期时的结果示意图；

图10c是使用FMM-VO算法模拟70个周期时的结果示意图；

图10d是使用FMM-VO算法模拟100个周期时的结果示意图；

图11是简单动态环境中FMM和FMM-VO算法的路径规划比较图；

图12是实际海洋环境和初始化条件示意图；

图13a是基于FMM-VO算法计算至14个步长的结果示例图；

图13b是基于FMM-VO算法计算至119个步长的结果示例图；

图13c是基于FMM-VO算法计算至147个步长的结果示例图；

图13d是基于FMM-VO算法计算至159个步长的结果示例图；

图14是实际动态海洋环境下FMM和FMM-VO算法的路径规划对比图；

图15是多动态障碍物的测试区域图；

图16是速度30节时FMM-VO与CFMM-VO算法的路径规划对比图；

图17a是速度30节时FMM-VO与CFMM-VO算法的艏向角度对比图；

图17b是速度30节时FMM-VO与CFMM-VO算法的艏向速率对比图；

图17c是速度30节时FMM-VO与CFMM-VO算法的安全距离对比图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

具体实施方式一：下面结合图1至图6说明本实施方式，本实施方式所述，基于改进快速行进法的水面无人艇路径规划方法，该方法包括：

步骤一、获取全局地图信息，利用快速行进法构建以目标终点为源点的初始全局环境导航势场，通过全局环境导航势场的梯度方向，获取全局地图中任意位置的初始期望航行艏向；

步骤二、无人艇在按照所述初始期望航行艏向航行过程中环境感知层实时对障碍物进行探测，当探测到障碍物，根据障碍物当前位置，构建基于速度的无人艇碰撞模型；

步骤三、根据无人艇当前位置的初始期望艏向与碰撞模型判断是否存在碰撞风险，如不存在，则按照当前位置的初始期望艏向方向继续导航前进，否则，基于碰撞模型与初始期望艏向利用速度障碍法计算当前位置的避障期望艏向，进行避障航行；避障航行结束后返回执行步骤二，直至到达目标位置，完成无人艇路径规划。

进一步地，本实施方式中，步骤一中，获取全局地图中任意位置的初始期望航行艏向的具体方法为：

步骤一一、将全局地图转化为网格地图，从全局地图和岛屿海岸线数据中提取静态障碍物的位置、大小和覆盖范围；

步骤一二、以目标终点为起始源点，利用快速行进法算法结合静态障碍物的位置、大小和覆盖范围构建静态障碍物与无人艇之间的安全区域，提取安全区域内每个网格节点的速度信息，保存在向量矩阵V中，计算无人艇到达每个网格节点的时间，保存在矩阵T中；

步骤一三、对所述向量矩阵V和矩阵T进行归一化处理，构建出全局导航势场M_PF，获取全局地图中任意位置的初始期望航行艏向。

进一步地，本实施方式中，步骤一二中所述的无人艇到达每个网格节点的时间的计算方法为：建立一维的程函方程：

T(z)是源点到达第z个节点的时间，V(z)是达到第z个节点时的行进速度；

已知二维各相同性介质中的程函方程为：

其中，t为源点出发抵达节点(x,y)的时间；s(x,y)为慢度，即速度的倒数；Ω为环境模型空间；快速行进法通过求解逆风差分格式的程函方程得到数值解：

T₁＝min(T_(x-Δx,y),T_(x+Δx,y))

T₂＝min(T_(x,y-Δy),T_(x,y+Δy))

其中，空间域内任一点(x,y)，与(x,y)相邻的点表示为(x+Δx,y)，(x-Δx,y)，(x,y+Δy)，(x,y-Δy)，T₁是x方向上，到达左右两个相邻节点所需用时的较小值；T₂是y方向上，到达上下两个相邻节点所需用时的较小值；其中，Δx,Δy是网格在x和y方向上的间距，T(x,y)的解为：

首先在网格中，将所有节点分为三类。第一类是未知点，即到达时间未定的点，在第一步中，除起点外所有节点均为远点；第二类是已知点，确定到达时间的点，并且其值不会在迭代中变化；第三类是试验点，其到达时间是被计算得到，并且在过程中可能会发生变化。

因此，在第一步时，只有起点为已知点，且T为0。与起点相邻的四个点均为试验点，下一个时间步，在试验点集中选取到达时间成本最小的点作为新的已知点，其相邻点也被标记为试验点。重复该过程更新网格图，最终结果展示为起点的到达时间最小为0；而其他点的到达时间随着到起点的距离成比例增加，形成势场，势场值为位于起点的最小势能的节点到达时间。

环境模型空间内陆地、岛屿等不可航行区域的传播速度为0，到达时间趋于无穷大。此数值可以作为检测当前点是否安全的指标，无人艇应在到达时间可行的区域上航行以保证航行安全。

进一步地，本实施方式中，步骤二中，无人艇航行过程中环境感知层实时对障碍物进行探测，若是探测到障碍物，构建基于速度的无人艇碰撞模型的具体方法为：

确定无人艇和目标船在各自运动状态不变的情况下无人艇和目标船的实时相对方向λ(U,v)：

λ(U,v)＝{U+vt|t≥0}

其中，t表示时间，U是无人艇的当前位置，无人艇以速度V_u和艏向角α航行，O是目标障碍物，R是障碍物圆形安全区域的半径，目标障碍物以速度V_o和艏向角α航行，v表示V_u和V_o的合速度，方向为

γ表示UO和V之间的角度，定义从无人艇开始沿着相对速度V的射线方向航行，集合RCA为相对碰撞区域，如果无人艇在RCA设定范围内以相对速度航行，则存在与目标船相撞的风险；相对碰撞域集合的表达式：

RCA是基于相对速度定义的空间，如果无人艇遇到多艘船，则存在多个基准点，无法统一描述，利用船舶领域的概念，对速度障碍法适用的碰撞模型进行修改，获得新的相对碰撞域集合:

RCA＝{v_u|α∈[θ_aft,θ_fore]}

如果无人艇的速度矢量V_u落在设定范围内，则两艘船有碰撞的危险，无人艇在航行过程中需要实时调整速度矢量V_u偏离RCA，避免碰撞，否则，无人艇保持其原始运动状态继续航行，其中，θ_fore是障碍物前部的安全方向角，θ_aft是障碍物后部的安全方向角。结合图2至图4对本实施方式进行说明，图中，U是USV的当前位置，USV以速度V_u和艏向角α航行。O是目标船舶，R是船舶圆形安全区域的半径，目标船舶以速度V_o和艏向角α航行。V表示V_u和V_o的合速度，其方向为

γ表示UO和V之间的角度。定义从USV开始沿着相对速度V的射线，当无人艇的安全回避范围内没有危险障碍物时，无人艇的V_u是从无人艇当前位置到目标位置的方向，这将导致处理复杂环境的能力较弱。因此，可以通过FMM算法来优化无人艇的初始速度矢量。当USV的安全回避范围内没有危险障碍物时，USV的V_u是从USV当前位置到目标位置的方向，这将导致处理复杂环境的能力较弱。因此，可以通过FMM算法来优化USV的初始速度矢量。

本实施方案中，利用FMM算法读取无人艇速度信息，保存在向量矩阵V中，计算到达各节点的时间，保存在矩阵T中，进行归一化处理，构建出全局导航势场M_PF用于为速度障碍法提供初始期望艏向。本实施方式中未公开的技术特征与具体实施方式一相同。其中快速行进法(Fast Marching Method，FMM)。

本实施方式中，结合图1进行说明，将地图划分为400×400像素的网格进行计算，利用FMM构建静态障碍物与无人艇之间的安全区域，得到全局导航势场，利用梯度下降法在全局导航势场中规划出无人艇的航行路径，并且计算出初始偏航角。

在引入船舶领域之后，对速度障碍法适用的碰撞模型进行修改，得到如图4所示的示意图。O是目标船舶，SA_fore是船舶安全域的船首部分，后SA_aft是船舶安全域的船尾部分。

本发明中，遭遇不同会遇情况的动态障碍物时，根据国际海上避碰规则要求需要遵从不同的避碰策略进行规避。结合图5a至图5f进行说明。左侧图表示当前位置会遇情况的类型，依次为正面相遇、后方追越以及交叉相遇。右侧图则是左侧图对应的避碰规则且满足海上航行规则(COLREGS)。当会遇船只航向反向位于无人艇艏向角度±15度范围内时，此时两船会遇场景为正面相遇局面。无人艇需要采取向右转向让路的避碰措施；当无人艇航向位于动态船只航线±22.5度扇形区间内，此时可判定两船会遇场景为追越相遇局面。当无人艇位于移动船只的左下方时，需左转从前方船只的左舷航行，而当其位于移动船只左上方时则需要右转从会遇船只右舷航行；当无人艇的会遇局面既不属于追越相遇，也不属于正面相遇局面时，可以判定无人艇当前处于交叉相遇局面。为了确保航行的安全性，无论是左交叉还是右交叉场景无人船必须从动态障碍物的后方航行。

结合图1至图17说明本实施方式，在环境地图中设计两个动态障碍物。应用FMM-VO算法，无人艇在初始阶段沿着导航势场梯度方向航行，当动态船舶进入避让范围时，USV需要做出避让动作。通过RCA＝{v_u|α∈[θ_aft,θ_fore]}计算期望艏向，直到避碰动作结束。然后，基于步骤一中的全局导航势场，计算无人艇当前位置的初始期望艏向，沿着初始期望艏向朝目标位置航行。本实施方式中未公开的技术特征与具体实施方式四相同。

本实施方式中，结合图7进行说明。无人艇先使用FMM算法，扫描环境地图，生成全局环境导航势场，按照导航势场梯度方向作为初始艏向角从起点位置出发。行驶过程中，当障碍物进入安全范围RCA时，基于碰撞模型重新计算期望艏向。当避碰结束，基于全局导航势场，计算无人艇当前位置的初始期望艏向，沿着初始期望艏向朝目标位置航行。过程中规划路径平滑，且与岸边以及小岛等静态障碍物保持了一定安全距离，以最近的路线完成避碰并到达规划点，满足经济效应要求。实现了复杂环境下的点对点任务规划全局路径规划和局部路径规划的切换与配合。

本发明使用的FMM-VO方法比FMM方法的计算量更小，实时性更优。能在出现随机动态障碍物的情况下及时做出反应进行规避。现通过在大连海域中，设置简单环境进行验证直观比较两种方法的区别。本实施方式中未公开的技术特征与具体实施方式五相同。

本实施方式中，结合图12至图14进行说明。可以发现，FMM-VO算法在对动态障碍物进行规避时总是更加光滑。图12是对实验的海洋环境进行条件初始化，设定了起点和终点，以及障碍物。环境分辨率设置为800×400像素。在图中，黑色空间表示陆地和岛屿等静态障碍物，白色空间表示USV的可行空间。USV以3像素/周期的速度从(526，355)航行到(63，71)，感知范围为160像素，防撞范围为80像素。模拟了三条虚拟船Obs1、Obs2和Obs3，速度分别为1、2和3像素/周期。图13a至图13d是使用FMM-VO算法过程中一些步长时刻的结果展示。如图所示，该算法在动态环境中生成了可接受的安全路径，并在规避操作完成后快速更新了预期的全局路径，表明该算法具有优异的路径重规划能力。可以发现，感知层的范围大于安全避碰范围，这给避碰动作的实施留出了准备空间，整个动作更加安全。在图14中可以发现，FMM-VO算法相比于FMM算法，艏向角度改变较小，路线更加平滑和安全。

FMM-VO算法虽相比于FMM算法，规划的路径更加平滑，解决了路径为折线的问题。但是未考虑实艇操纵性能的影响，仍然可能会出现大角度调整艏向的情况。因此结合实艇操纵性约束无人艇航向角对FMM-VO算法进一步改进，设计了约束层用以限制最大转角，基于此提出了CFMM-VO算法。并且以哈尔滨工程大学水下机器人重点实验室研制的“天行一号”无人艇实艇操纵性实验数据为基础，定量反映CFMM-VO算法相比FMM-VO算法的改进。

本实验方案中，结合图15至图17c进行说明。图15是进行更加复杂的多障碍物测试的海域环境图。规划空间为4.0公里×2.0公里，环境中增加了9艘虚拟船，设置无人艇的速度为30节。图16是CFMM-VO算法和FMM-VO算法的路径规划对比图，USV执行的任务需从坐标(555，1470)到(3310，1790)，USV的防撞距离和感知距离分别为400米和500米。图17a和图17b显示了USV行驶过程中的艏向角和航行速度。基于FMM-VO的USV的艏向角在部分路径中振荡。与FMM-VO相比，CFMM-VO在船首转弯期间表现出更小的艏向角度变化和更平滑的路径改变，更好地满足USV的轨迹跟踪要求。USV与船舶之间的最小距离如图17c所示。最小距离值500表示感知范围内没有船舶，结果表明转角约束不会影响路径的安全性。总体而言，CFMM-VO的性能更好。这在无人艇高速航行时对航行安全性尤为重要，且更加有利于实艇执行任务时的机动实现，使算法的安全性和实用性进一步提高。

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (4)

1.基于改进快速行进法的水面无人艇路径规划方法，其特征在于，该方法包括：

步骤一、获取待规划路径行驶海域的全局地图信息，利用快速行进法构建以目标终点为源点的初始全局环境导航势场，通过全局环境导航势场的梯度方向，获取全局地图中任意位置的初始期望航行艏向；

步骤二、无人艇在按照所述初始期望航行艏向航行过程中环境感知层实时对障碍物进行探测，当探测到障碍物，根据障碍物当前位置，构建基于速度的无人艇碰撞模型；

步骤三、根据无人艇当前位置的初始期望艏向与碰撞模型判断是否存在碰撞风险，若不存在，则按照当前位置的初始期望艏向继续导航前进，直至到达目标位置，完成无人艇路径规划；否则，执行步骤四；

步骤四、基于碰撞模型与无人艇当前位置的初始期望艏向，利用速度障碍法计算无人艇当前位置的避障期望艏向，进行避障航行；避障航行结束后返回执行步骤二，直至到达目标位置，完成无人艇路径规划。

2.根据权利要求1所述的基于改进快速行进法的水面无人艇路径规划方法，其特征在于，步骤一中，获取全局地图中任意位置的初始期望航行艏向的具体方法为：

步骤一一、将全局地图转化为网格地图，从网格地图和岛屿海岸线数据中提取静态障碍物的位置、大小和覆盖范围；

步骤一二、以目标终点为起始源点，利用快速行进法算法结合静态障碍物的位置、大小和覆盖范围构建静态障碍物与无人艇之间的安全区域，提取安全区域内每个网格节点的速度信息，保存在向量矩阵V中，计算无人艇到达每个网格节点的时间，保存在矩阵T中；

步骤一三、对所述向量矩阵V和矩阵T进行归一化处理，构建出全局导航势场M_PF，获取全局地图中任意位置的初始期望航行艏向。

3.根据权利要求1所述的基于改进快速行进法的水面无人艇路径规划方法，其特征在于，步骤一二中所述的无人艇到达每个网格节点的时间的计算方法为：

建立一维的程函方程：

T(z)是源点到达第z个节点的时间，V(z)是达到第z个节点时的行进速度；

已知二维各相同性介质中的程函方程为：

其中，t为源点出发抵达节点(x,y)的时间；s(x,y)为到达网格地图中点(x,y)的慢度，即速度的倒数；Ω为环境模型空间；

快速行进法通过求解程函方程：

T₁＝min(T_(x-Δx,y),T_(x+Δx,y))

T₂＝min(T_(x,y-Δy),T_(x,y+Δy))

其中，空间域内任一点(x,y)，与(x,y)相邻的点表示为(x+Δx,y)，(x-Δx,y)，(x,y+Δy)，(x,y-Δy)，T₁表示点(x,y)到达(x+Δx,y)和(x-Δx,y)所用的时间中较小的值；T₂表示点(x,y)到达相邻节点(x,y+Δy)，(x,y-Δy)所用的时间中较小的值；其中，Δx,Δy分别表示网格在x和y方向上的间距，到达网格节点(x,y)的时间T(x,y)为：

其中，V_(x,y)表示到达网格节点(x，y)时无人艇的速度。

4.根据权利要求1所述的基于改进快速行进法的水面无人艇路径规划方法，其特征在于，步骤二中，无人艇航行过程中环境感知层实时对障碍物进行探测，若是探测到障碍物，构建基于速度的无人艇碰撞模型的具体方法为：

确定无人艇和目标船在各自运动状态不变的情况下无人艇和目标船的实时相对方向λ(U,v)：

λ(U,v)＝{U+vt|t≥0}

其中，t表示时间，U是无人艇的当前位置，无人艇以速度V_u和艏向角α航行，O是目标移动障碍物，目标障碍物以速度V_o和艏向角α航行,v表示V_u和V_o的合速度，为从无人艇当前位置开始沿着相对速度v的射线方向航行，集合RCA为相对碰撞区域，如果无人艇在RCA范围内以相对速度航行，则存在与目标船相撞的风险；相对碰撞域集合的表达式：

其中，SA表示船舶安全域，RCA是基于相对速度定义的空间，如果无人艇遇到多个移动障碍物，则存在多个基准点，利用船舶领域的概念，对速度障碍法适用的碰撞模型进行修改，获得新的相对碰撞域集合:

RCA＝{V_u|α∈[θ_aft,θ_fore]}

如果无人艇的速度矢量V_u落在设定范围内，则两艘船有碰撞的危险，无人艇在航行过程中需要实时调整速度矢量V_u偏离RCA，避免碰撞，否则，无人艇保持其原始运动状态继续航行，其中，θ_fore是障碍物前部的安全方向角，θ_aft是障碍物后部的安全方向角。

Images