CN116952239A - 一种基于改进a*与dwa融合的无人艇路径规划方法 - Google Patents

Abstract

本发明提供一种基于改进A*与DWA融合的无人艇路径规划方法，包括以下步骤：对参数及栅格地图进行初始化，设置路径规划的起点和目标点；基于改进的A*算法在初始化的地图中进行全局路径规划，形成初步全局路径；利用二次插点法对初步全局路径中的节点进行处理，生成全局优化路径；获取全局优化路径中的关键转折点，并将所述关键转折点作为局部目标点，基于改进的DWA算法进行局部路径规划，获取最优运动轨迹。本发明建立逐步扩展的搜索区域来限制搜索范围，减少了计算量，通过引入模糊逻辑控制策略，根据障碍物的分布情况和无人艇行驶位置动态调整启发函数的权重参数，降低了路径规划时间。通过使用二次插点法有效减少了路径的转折点。

Description

一种基于改进A*与DWA融合的无人艇路径规划方法

技术领域

本发明涉及无人艇路径规划技术领域，具体涉及一种基于改进A*与DWA融合的无人艇路径规划方法。

背景技术

水面无人艇是一种小型的智能水面运载工具，其优势在于体积小、成本低、灵活性高，因此在我国的许多领域中都有广泛的应用，如环境监测，科研勘察，水下测绘，搜索救援以及安全巡逻等等。其中自主导航是水面无人艇的核心技术之一，在实际海洋环境中航行时会遇到复杂的地形和障碍物、如岛屿、暗礁、灯塔、浮标以及其他船只等情况，有时无法获得完整的环境信息。因此路径规划方法大致可分为两种，在已知完整环境信息时，可以根据现有的地图信息进行全局的路径规划，以实现在保证安全的前提下耗费最少的时间和能量到达目的地。目前已经有很多成熟的算法可应用于水面无人艇的全局路径规划，例如Dijkstra算法、PSO算法、蚁群算法和遗传算法。当无人艇处于复杂环境条件下无法获取完整信息时，便需要利用局部路径规划算法，使其能够在环境发生变化的情况下自主完成避障和进行新路径的规划，这就需要对局部动态路径规划方法进行深入研究，以寻找适用于水面无人艇的路径规划算法。目前常用的算法包括人工势场法、动态窗口法、遗传算法以及模糊逻辑算法等。

基于动态窗口法，Tang等提出了一种OAABHW的避障算法，该算法将窗口划分为艏向窗口和速度窗口，并单独构建2个评价指标分别对最优的避障速度和避障方向进行筛选，该方式得到的轨迹比较平滑，符合真实无人艇的运行，有效解决了围绕障碍物绕行的问题。程杰等针对A*算法规划路径与障碍物相邻部分距离过近导致的安全问题，提出在A*算法搜索过程中引入安全距离约束和无人艇的转向角约束，提高规划路径的安全性。传统A*算法是一种经典的路径规划算法，但其缺点在于需要搜索大量的节点，且算法评价函数的权值是固定的，从而导致运算效率低下；此外，规划出来的路线会有很多拐点、平滑性较差。

发明内容

鉴于现有技术存在的不足，本发明提供一种基于改进A*与DWA融合的无人艇路径规划方法。本发明首先对参数及栅格地图进行初始化，确定无人艇的起始点和目标点。随后根据改进的A*算法进行全局路径规划，获得一组连续的路径节点。提取其中的关键节点，作为局部路径规划的目标点。最后利用改进的DWA算法完成局部路径规划，生成最终无人船行驶路径。

本发明采用的技术手段如下：

一种基于改进A*与DWA融合的无人艇路径规划方法，包括以下步骤：

S1、对参数及栅格地图进行初始化，设置路径规划的起点和目标点；

S2、基于改进的A*算法在初始化的地图中进行全局路径规划，形成初步全局路径；

S3、利用二次插点法对初步全局路径中的节点进行处理，生成全局优化路径；

S4、获取全局优化路径中的关键转折点，并将所述关键转折点作为局部目标点，基于改进的DWA算法进行局部路径规划，获取最优运动轨迹。

进一步地，基于改进的A*算法在初始化的地图中进行全局路径规划，形成初步全局路径，包括：

S201、获取无人船的起点位置、目标点位置以及障碍物信息，基于所述获取无人船的起点位置、目标点位置以及障碍物信息计算障碍物密集度以及无人船的位置比重；

S202、由起点开始，利用改进A*算法的评价函数选择周围邻域中的最优节点，之后将最优节点作为父节点继续搜索后续子节点，直至达到目标位置，所述改进A*算法的评价函数的权值参数通过模糊控制器动态调整；

S203、回溯每次搜索提取的最有节点，形成初步全局路径。

进一步地，利用二次插点法对初步全局路径中的节点进行处理，生成全局优化路径，包括：

S301、将起点与终点放置到坐标集F中，并以起点为当前点，依次提取出当前点S0、子节点S1和子节点S1的子节点S2的坐标(x0,y0)、(x1，y1)、(x2、y2)；

S302、计算当前点与子节点、子节点的子节点坐标之间的向量并判断两向量的夹角θ；

S303、当夹角θ为0时，删除冗余节点S1，当夹角θ不为0时，则计算出节点S0与节点S2连线S0S2的直线方程y＝kx+b,其中

S304、对连线S₀S₂进行条件判断：若连线S0S2不经过障碍物区域，则忽略当前节点S0并返回S303计算后续节点；若连线S0S2经过障碍物区域，则节点S1为关键转折点存储至坐标集F中，并将节点S1设置为新的当前点S0继续优化，返回S301，直到抵达目标点结束；

S305、将坐标集F中的路径点按照从起点到终点的顺序依次连接，得到一次插点法优化的路径规划结果；

S306、提取一次插点法所得到的节点，并计算得到相邻两个节点的中心点，并对其进行如S302至S304中的优化，且在S304中增加一个判断，所取的连线与最近障碍物的距离应大于安全距离D；将二次插点法得到的节点按照从起点到终点的顺序依次连接，得到规划后的全局路径。

进一步地，获取全局优化路径中的关键转折点，并将所述关键转折点作为局部目标点，基于改进的DWA算法进行局部路径规划，获取最优运动轨迹，包括：

S401、基于无人艇运动学模型、速度约束，无人艇在速度空间集合中进行采样，模拟出一定时间间隔内的无数条可行行驶路线；

S402、以所述关键转折点作为DWA优化算法的局部目标点，利用改进的DWA算法的评价函数评估轨迹优劣，从而获取最优轨迹；

S403、设置一个位于无人艇前方的扇形区域，无人艇的艏向在此区域中心，扇形区域随无人艇航行运动，扇形窗口以传统视觉传感器探测的最大距离为半径，无人艇仅对进入窗口内的障碍物进行评估，以此分析前方海域的环境复杂程度；

S404、引入碰撞锥检测机制对进入扇形区域内的动态障碍物进行碰撞危险度的评估；

S405、将最优轨迹对应的速度空间保留下来，并进行速度平滑处理；

S406、无人艇沿着处理后的最优轨迹移动，到达局部目标点后判断是否到达终点，如果是终点则结束运行，如果不是终点，则返回S401。

较现有技术相比，本发明具有以下优点：

1、本发明建立逐步扩展的搜索区域来限制搜索范围，减少了计算量，降低了路径规划时间；同时，引入了模糊逻辑控制策略根据障碍物的分布情况和无人艇行驶位置来动态调整启发函数的权重参数，当障碍物稀疏时增加启发式函数的权重，减小搜索空间，提高路径规划的速度和质量，有效减少路径的拐点和转折点。当障碍物密集时，减小启发函数的权重，增大搜索空间，避免算法陷入局部最优；最后利用插点法剔除冗余点，保留关键转折点，缩短了路径长度，提升路径平滑度。

2、本发明对原始DWA算法进行改进，首先对轨迹采样评价函数进行优化，帮助其贴近A*算法所规划出的全局路径，其次通过动态调整评价函数dist的权重参数来适应不同海况下的障碍物分布特点，提高局部路径规划的效率与避障效果；引入碰撞锥检测机制对进入扇形区域内的动态障碍物进行碰撞危险度的评估，对可能存在碰撞风险的障碍物提前进行避让动作。

3、本发明提供了一种全局最优和实时避障能力兼备的高效路径规划算法，克服了传统A星算法和DWA算法各自的局限。该算法利用改进A星算法提取的局部关键目标点实现局部方向指引，并融合DWA优化算法进行局部路径规划，从而同时实现全局最优与动态避障。与单独使用A星算法和DWA算法相比，本发明提高了路径规划效率、避障灵敏性和安全性，同时提高了水面无人艇在海域中的工作效率和安全可靠性，具有显著的实际应用价值。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一种基于改进A*与DWA融合的无人艇路径规划方法流程图。

图2为实施例中拓展搜索区域示意图。

图3为实施例中插点法优化路径原理示意图。

图4为实施例中二次插点法优化路径原理示意图。

图5为实施例中全局与局部路径规划交集示意图。

图6为实施例中碰撞锥检测模型示意图。

图7为实施例中模糊逻辑变量的隶属度函数示意图，其中a为无人艇位置比重，b为障碍物密集度，c为输出量α，d为输出量β。

图8为实施例中优化评价函数的A*算法仿真效果图。

图9为实施例中本发明改进的A*算法仿真效果图。

图10为实施例中本发明融合算法的仿真结果图。

图11为实施例中本发明融合算法仿真的实时线速度图。

图12为实施例中本发明融合算法仿真的实时角速度图。

图13为实施例中本发明融合算法在对遇情况的避碰效果示意图。

具体实施方式

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

如图1所示，本发明提供了一种基于改进A*与DWA融合的无人艇路径规划方法，总体流程说明如下：首先对参数及栅格地图进行初始化，确定无人艇的起始点和目标点。随后根据改进的A*算法进行全局路径规划，获得一组连续的路径节点。提取其中的关键节点，作为局部路径规划的目标点。最后利用改进的DWA算法完成局部路径规划，生成最终无人船行驶路径。具体来说，包括以下步骤：

S1、对参数及栅格地图进行初始化，设置路径规划的起点和目标点。

S2、基于改进的A*算法进行全局路径规划方法，实现步骤如下：

S201、首先，设定Open list和Close list，分别用于存储未访问和已访问的节点信息。其次，设置路径规划起点、目标点，并将起点加入Open list作为父节点，同时Closelist为空；最后建立拓展的搜索区域Ci，初始搜索区域Ci与初始点和目标点有关，它是从初始点到目标点的直线构成的有界区域，如图2所示。为了减少计算量将Ci空间下边界表示为c_l上边界表示为c_h，Ci空间线性展开规则如下所示：c_l'＝c_l-c"c'_h＝c_h+c"，其中c”大小取决于搜索区域边界障碍物的数量。

S202、根据起点与目标点之间的障碍物信息，计算该环境下的障碍物密集度，公式如下：

ins＝ω·n+τ·dis+υ·s

其中ins表示障碍物密集度，n表示拓展搜索区域内障碍物个数，dis表示拓展搜索区域内障碍物间的最小距离，s表示拓展搜索区域内障碍物占据的栅格面积，ωτυ分别代表各部分的占比；计算无人船位置比重：

其中d_n表示无人艇当前位置到目标点的欧氏距离，d_all表示起始点到目标点的欧氏距离。

S203、开始节点搜索，当访问过起点后将其从Open list中删除并加入到Closelist中，并利用改进A*算法的评价函数选择周围邻域中的最优节点作为下一父节点，继续搜索后续节点；每次搜索八个相邻节点，并利用改进A*算法的评价函数选取其中的最优节点；改进A*算法的评价函数为：F(n)＝αG(n)+βH(n)。通过模糊逻辑控制策略动态调整G(n)与F(n)的权值参数α、β。其中模糊控制器选用Mamdani型，采用双入双出结构，输入变量为密集度ins与无人艇位置比重η，输出变量为权重系数α与β。

S204、模糊逻辑控制策略如下表所示：当密集度为较小，且无人艇位置比重较小时，采用较小的权值参数α和中等的权值参数β；无人艇位置比重中等时，采用中等的权值参数α和较大的权值参数β；无人艇位置比重较大时，采用较大的权值参数α和较大的权值参数β；

当密集度为较大，且无人艇位置比重较小时，采用较小的权值参数α和较小的权值参数β；无人艇位置比重中等时，采用中等的安全性权值参数和较小的权值参数β；无人艇距离障碍物的最短距离较大时，采用较大的权值参数α和中等的权值参数β；

表1模糊逻辑控制输入子集

表2模糊逻辑控制输出子集

表3模糊规则

S205、在到达目标点之前，需要继续搜索起点的相邻节点，并判断搜索到的新节点是否在Open list中：如果新节点不在Open list中，则将该节点加入Open list中，采用改进A*算法的评价函数计算该节点的代价值F(n)，同时将该节点设置为当前搜索点的父节点，加入Close list中；如果新节点在Open list中，采用改进A*算法的评价函数计算该节点的代价值F(n)，并与前一节点的代价值F(n)进行对比，选择代价值F(n)最小的最优节点作为下一次搜索点的父节点，并将代价值F(n)最小的节点加入Close list中，然后继续朝目标方向搜索节点，将搜索到的节点加入到Open list中，更新节点信息；

S206、循环执行S203和S204以扩展子节点，直至达到目标位置；当搜索到目标点时，将目标点添加到Close list中，循环结束；

S207、回溯提取Close list中的节点，形成初步全局路径；

S3、利用二次插点法优化路径节点，具体策略如图3-4所示，实现步骤如下：

S301、提取改进A*算法所得初步全局路径，将起点与终点放置到坐标集F中，并以起点为当前点，依次提取出当前点S0、子节点S1和子节点S1的子节点S2的坐标(x0,y0)、(x1，y1)、(x2、y2)；

S302、计算当前点与子节点、子节点的子节点坐标之间的向量并判断两向量的夹角θ：

S303、当夹角θ为0时，说明当前点、子节点及子节点的子节点在同一条直线上，则删除冗余节点S1；当夹角θ不为0时，则计算出节点S0与节点S2连线S0S2的直线方程y＝kx+b,其中

S304、对连线S0S2进行条件判断：若连线S0S2不经过障碍物区域，则忽略该节点继续往下按照S303计算后续节点；若连线S0S2经过障碍物区域，则节点S1为关键转折点存储至坐标集F中，并将节点S1设置为新的当前点S0继续优化，重复步骤S301-S304，直到抵达目标点结束。

S305、将坐标集F中的路径点按照从起点到终点的顺序依次连接，得到一次插点法优化的路径规划结果。

S306、提取一次插点法所得到的节点，并计算得到相邻两个节点的中心点。并对其进行S302-S304的优化，且在S304中增加一个判断，所取的连线与最近障碍物的距离应大于安全距离D。

S307、将二次插点法得到的节点按照从起点到终点的顺序依次连接，得到规划后的全局路径。

S4、基于改进的DWA算法进行局部路径规划，实现步骤如下：

S401、基于无人艇运动学模型、速度约束，无人艇在速度空间集合V(v，ω)中进行采样，模拟出一定时间间隔内的无数条可行行驶路线。由于无人艇动力学限制，给定无人艇最大线速度V_max、最大角速度ω_max、当前行驶线速度V_cur、当前行驶角速度ω_cur和时间间隔d_t，速度空间V_s(v,ω)和V_d(v,ω)应满足：

V_s(v,ω)＝{0≤v≤v_max,-ω_max≤ω≤ω_max}

V_d(v,ω)＝{v_cur-vd_t≤v≤v_cur+vd_t,ω_cur-ωd_t≤ω≤ω_cur+ωd_t}

V＝V_S∩V_d

S402、以第三部分中保留的关键转折点作为DWA优化算法的局部目标点，指引局部方向如图4所示，利用改进的DWA算法的评价函数评估轨迹优劣，从步骤1中生成的无穷多条可行运动轨迹中选择最优轨迹。其中，DWA优化算法的评价函数如下所示：

G(v,ω)＝σ(αheading(v,ω)+n·β·dist(v,ω)+γ·velocity(v,ω))

式中heading(v,ω)表示采样轨迹末端方向与局部目标方向的角度差，dist(v,ω)表示采样轨迹末端点与障碍物间的最小距离，velocity(v,w)表示当前行驶速度；α、β、γ分别为评价函数各子函数相应的权重系数；σ代表归一化过程，将三个参数归一化处理到[0,1]之间，避免一项值过大，n表示当前行驶区域内的环境复杂程度。

S403、为了提高避障效率，设置一个位于无人艇前方的扇形区域，无人艇的艏向在此区域中心，扇形区域随无人艇航行运动，如图4所示；扇形窗口以传统视觉传感器探测的最大距离为半径，无人艇仅对进入窗口内的障碍物进行评估，以此分析前方海域的环境复杂程度。环境复杂程度n计算公式如下：n＝μ·S+λ·a^inst其中μ和λ表示各部分占比参数，inst表示扇形区域内的障碍物与无人艇的最小距离，S表示扇形区域内障碍物所占栅格面积，α取值为0.04.

S404、引入碰撞锥检测机制对进入扇形区域内的动态障碍物进行碰撞危险度的评估，如图5所示.R_A和R_B分别代表动态障碍物和无人艇的碰撞半径，V_A和V_B分别代表动态障碍物和无人艇的行驶速度，V_B-V_A代表相对速度，浅灰色锥形区域代表未来某一时刻会与无人艇发生碰撞的速度集合，所以当动态障碍物与无人艇的相对速度处于锥形区域内，无人艇就要针对障碍物采取避让动作。

S405、将最优轨迹对应的速度空间保留下来，并进行速度平滑处理；

S406、无人艇沿着处理后的最优轨迹移动，到达局部目标点后判断是否到达终点，如果是终点则结束运行，如果不是终点，则重复执行步骤S401-S405。

为证明本发明所提出算法的可行性，利用MATLAB仿真软件进行测试。仿真环境为模拟复杂海洋环境的栅格地图，地图中的白色栅格为可行域、黑色栅格为障碍物、左上角☆为起始点、右下角☆为目标点。针对无人艇的工作环境，设计25*25的栅格地图，障碍物的分布率为0.17，以此进行仿真对比实验。图5表示优化评价函数的A*算法的仿真结果，图6表示本发明改进的A*算法仿真结果对比可以看出，两种算法均成功规划出一条完整可行的全局路径，但是所规划出来的路径性能优劣不同，具体实验参数如下表所示。

表4两种A*算法的路径规划实验参数对比

通过表1可知，由于改进后的A星算法不仅通过插点法剔除了冗余节点，缩短了路径长度，还采用模糊逻辑控制策略改进评价函数和限制搜索区域的方法提升了路径搜索效率，本发明改进A星算法的路径长度、转折点个数和关键节点数相较于其他A*算法均有不同程度的减少。此外由于转折点的大幅减少，本发明改进A星算法的路径拐点转弯角度也有所减小，说明路径的平滑度有进一步提升。

为验证融合算法的有效性，重新构建了一个21*21的栅格地图，障碍物分布率为0.126，此外还设置了未知障碍物和动态障碍物。未知障碍物为灰色栅格，动态障碍物为黄色栅格，并横穿规划的路线轨迹。本次实验设置冲突半径D＝0.65m,保持无人艇与障碍物间的安全距离。仿真中无人艇的运动学参数如表5所示

表5无人艇运动学参数表

图10表示本文改进A*融合DWA算法在21*21栅格地图上的仿真结果，虚线表示全局引导路径，*为局部目标点，实线为最终规划路径。从图中可以看出在融合算法下，无人艇从起始点到达目标点的路径，有效避开了设置的未知障碍物，以及横穿路径轨迹的动态障碍物且路径十分平滑，符合无人艇对行驶路线的需求。

图13表示无人艇在对遇情况下的仿真结果，可以看出无人艇在运用本发明算法进行避障时可以预知碰撞的发生，提前进行避让的动作，这样不仅提高了无人艇行驶路线的安全性，还减小了避障过程中无人艇的转弯角度。所以本发明提出的一种基于改进A*与DWA融合的无人艇路径规划算法实现了无人艇导航的全局路径最优以及实时避障功能，具有一定的有效性和可行性。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (4)

1.一种基于改进A*与DWA融合的无人艇路径规划方法，其特征在于，包括以下步骤：

S1、对参数及栅格地图进行初始化，设置路径规划的起点和目标点；

S2、基于改进的A*算法在初始化的地图中进行全局路径规划，形成初步全局路径；

S3、利用二次插点法对初步全局路径中的节点进行处理，生成全局优化路径；

S4、获取全局优化路径中的关键转折点，并将所述关键转折点作为局部目标点，基于改进的DWA算法进行局部路径规划，获取最优运动轨迹。

2.根据权利要求1所述的一种基于改进A*与DWA融合的无人艇路径规划方法，其特征在于，基于改进的A*算法在初始化的地图中进行全局路径规划，形成初步全局路径，包括：

S201、获取无人船的起点位置、目标点位置以及障碍物信息，基于所述获取无人船的起点位置、目标点位置以及障碍物信息计算障碍物密集度以及无人船的位置比重；

S202、由起点开始，利用改进A*算法的评价函数选择周围邻域中的最优节点，之后将最优节点作为父节点继续搜索后续子节点，直至达到目标位置，所述改进A*算法的评价函数的权值参数通过模糊控制器动态调整；

S203、回溯每次搜索提取的最优节点，形成初步全局路径。

3.根据权利要求2所述的一种基于改进A*与DWA融合的无人艇路径规划方法，其特征在于，利用二次插点法对初步全局路径中的节点进行处理，生成全局优化路径，包括：

S301、将起点与终点放置到坐标集F中，并以起点为当前点，依次提取出当前点S0、子节点S1和子节点S1的子节点S2的坐标(x0,y0)、(x1，y1)、(x2、y2)；

S302、计算当前点与子节点、子节点的子节点坐标之间的向量并判断两向量的夹角θ；

S303、当夹角θ为0时，删除冗余节点S1，当夹角θ不为0时，则计算出节点S0与节点S2连线S0S2的直线方程y＝kx+b,其中

S304、对连线S₀S₂进行条件判断：若连线S0S2不经过障碍物区域，则忽略当前节点S0并返回S303计算后续节点；若连线S0S2经过障碍物区域，则节点S1为关键转折点存储至坐标集F中，并将节点S1设置为新的当前点S0继续优化，返回S301，直到抵达目标点结束；

S305、将坐标集F中的路径点按照从起点到终点的顺序依次连接，得到一次插点法优化的路径规划结果；

S306、提取一次插点法所得到的节点，并计算得到相邻两个节点的中心点，并对其进行如S302至S304中的优化，且在S304中增加一个判断，所取的连线与最近障碍物的距离应大于安全距离D；将二次插点法得到的节点按照从起点到终点的顺序依次连接，得到规划后的全局路径。

4.根据权利要求3所述的一种基于改进A*与DWA融合的无人艇路径规划方法，其特征在于，获取全局优化路径中的关键转折点，并将所述关键转折点作为局部目标点，基于改进的DWA算法进行局部路径规划，获取最优运动轨迹，包括：

S401、基于无人艇运动学模型、速度约束，无人艇在速度空间集合中进行采样，模拟出一定时间间隔内的无数条可行行驶路线；

S402、以所述关键转折点作为DWA优化算法的局部目标点，利用改进的DWA算法的评价函数评估轨迹优劣，从而获取最优轨迹；

S403、设置一个位于无人艇前方的扇形区域，无人艇的艏向在此区域中心，扇形区域随无人艇航行运动，扇形窗口以传统视觉传感器探测的最大距离为半径，无人艇仅对进入窗口内的障碍物进行评估，以此分析前方海域的环境复杂程度；

S404、引入碰撞锥检测机制对进入扇形区域内的动态障碍物进行碰撞危险度的评估；

S405、将最优轨迹对应的速度空间保留下来，并进行速度平滑处理；

S406、无人艇沿着处理后的最优轨迹移动，到达局部目标点后判断是否到达终点，如果是终点则结束运行，如果不是终点，则返回S401。