CN109582027B - 一种基于改进粒子群优化算法的usv集群避碰规划方法 - Google Patents

Abstract

本发明属于无人水面艇避障领域，具体涉及一种基于改进粒子群优化算法的USV集群避碰规划方法。本发明主要包括以下步骤：根据导航雷达和光电传感器的参数特性建立USV综合视域模型；构建坐标系；构建环境模型；设计用于USV集群避碰规划的滚动优化策略和改进粒子群优化算法；将综合传感器探测到的信息及目标点的信息输入到改进粒子群优化算法中，获得USV下一时刻的航行和航速的调整指令。本发明不仅克服了标准粒子群优化算法易于陷入局部最优的缺点，而且结合了USV当前环境信息，提高了USV避碰规划的实时性，在适应度函数中加入了USV的转角优化，在获得最优路径的同时也提高了路径的平滑性。

Description

一种基于改进粒子群优化算法的USV集群避碰规划方法

技术领域

本发明属于无人水面艇避障领域，具体涉及一种基于改进粒子群优化算法的USV集群避碰规划方法。

背景技术

近几年来，越来越多的国家注重海上力量的发展，无人水面艇以其体积小、灵活性高和作战能力强的优势成为了研究的热点。无人水面艇不仅可以自主航行和自主规划，还能完成目标跟踪和港湾巡逻等任务，所以无论在军事领域还是民用领域，无人水面艇都发挥着越来越重要的作用。然而，面对未知的工作环境和多样化的任务使命时，单一USV显得势单力薄并不能高效的完成任务，而由多艘USV联合起来构成的集群***，鲁棒性和机动性强、作业效率高且作业范围也比较广，USV集群***的这些优势受到了许多国家的青睐，在未来的一段时间内，无人水面艇的智能化、体系化、标准化将是各国发展的重点，无人水面艇集群在与其它武器的综合协同作战中，也将扮演更加重要的角色，而无人水面艇集群***对未来海上战场的整体面貌也将会产生革命性的影响。海洋环境复杂多变，无论是岛屿、礁石还是一些其他的动态物体都可能会对无人艇集群得航行构成一定的威胁，而集群中的各USV之间也必须保持一定的安全距离，这就要求无人水面艇集群在完成各项任务过程中必须具备一定的自主避碰能力，无人水面艇集群自主避碰的能力是保障其高效、安全地完成各项使命任务的关键技术之一。

人工势场法、可视图法、滚动窗口法等传统方法以及遗传算法、蚁群算法、粒子群算法、神经网络算法等智能算法是目前常用的避碰规划算法。大部分避碰规划算法都不能在短时间内为USV规划出一条无避碰最优路径，传统方法如人工势场法，虽然具有简单的原理、较小的计算量、较快运算速度，但容易找不到全局最优解，甚至在避碰过程中出现震荡现象，群智能算法如遗传算法虽然可以避免陷入局部极值点，但是原理复杂，计算量大，且规划时间较长，因此，为USV设计一种简单、快速、实时在线的避碰规划算法是至关重要的。采用基于滚动优化策略和粒子群优化算法相结合的方法可以实现USV集群的自主避碰，该算法利用传感器实时感知获得的局部环境信息来进行运算，使用粒子群优化算法快速为集群中的每个USV在综合视域内规划出一条无避碰最优路径。

发明内容

本发明的目的在于提供一种基于改进粒子群优化算法的USV集群避碰规划方法。

一种基于改进粒子群优化算法的USV集群避碰规划方法，主要包括以下步骤：

步骤1：根据导航雷达和光电传感器的参数特性建立USV综合视域模型；

步骤2：构建坐标系；

步骤3：构建环境模型；

步骤4：设计用于USV集群避碰规划的滚动优化策略和改进粒子群优化算法；

步骤5：将综合传感器探测到的信息及目标点的信息输入到改进粒子群优化算法中，获得USV下一时刻的航行和航速的调整指令。

步骤2中建立三个坐标系：全局环境坐标系X_GO_GY_G、基于USV的局部坐标系X_UO_UY_U和基于传感器综合感知信息的局部坐标系X_RO_RY_R；

全局坐标系X_GO_GY_G采用北东坐标系，左下角为坐标原点，X轴指向正北方向，Y轴指向正东方向；

局部坐标系分为船体坐标系和传感器坐标系，船体坐标系的原点位于USV的重心处，导航雷达和光电传感器安装在距离船体中心a处，其X轴和Y轴平行于USV局部坐标系的X轴和Y轴。

步骤3中构建环境模型具体包括以下步骤：

(3.1)数据采集：导航雷达和光电传感器的感知范围为360°，假设每隔1°的角度上传感器获取且只能获取一个距离信息，按逆时针方向角度从0°变化至360°，即传感器每扫描一次便获得361组感知数据点，这些数据点包含两个参数，第一个参数是点与传感器之间的角度，第二个参数点到传感器的距离；

(3.2)将感知数据点提取为线段特征：首先，通过区域划分的方式将点集些划分为多个区域，然后再将每个区域内数据点集进行线性拟合，进而提取为线段特征；

(3.3)静态障碍物膨化：对静态障碍物的膨化只是对其部分进行膨化，膨化方式主要分为以下三种情况：单线段的膨化、相邻线段的膨化和曲线的膨化；

(3.4)动态障碍物的轨迹预测：首先将动态障碍物膨化为一个圆形，然后根据自回归模型预测出动态障碍物在全局坐标系下的位置和方向。

步骤4具体包括：USV在运动过程中，每次以综合视域内的子目标点规划局部路径，然后使用改进粒子群优化算法优化每个综合视域内的窗口，粒子群优化算法的改进包括惯性权重和学习因子的设计、适应度函数的选择。

USV在运动过程中，每次以综合视域内的子目标点规划局部路径，运动过程中将执行以下步骤：

(4.1)USV通过自身携带的传感器获取综合视域内的环境信息；

(4.2)从起点到子目标点利用粒子群算法规划出一条无避碰最优路经；

(4.3)USV将沿着规划出的路径从起点运动到子目标点；

(4.4)以此子目标为起点，返回步骤1，直到USV行驶到全局的终点。

采取基于正切函数的惯性调整策略改进设计的惯性权重ω的值，公式为：

式中，α＝π/4，m为控制因子，k为当前迭代次数，k_max为最大迭代次数；

采取基于线性调整策略改进设计学习因子c₁和c₂的值，公式为：

式中k为当前迭代次数，k_max为最大迭代次数，c_1s＝2.5，c_2s＝1.0，c_1e＝1.0，c_2e＝2.5；

适应度函数的选择：

已知USV的起始点为(x₀,y₀)，目标点为(x_n,y_n)，子路径点为(x_i,y_i)，其中i＝0,1,2…n-1,n，则适应度函数定义为：

根据USV的每次转角定义的适应度函数为：

结合上述公式，得出USV在避碰规划过程中总的适应度函数：

f＝λ₁f₁+λ₂f₂。

粒子群优化算法的具体包括以下步骤：

Step 1：初始化粒子群算法中的参数：ω_s、ω_e、c_1s、c_1e、c_2s、c_2e，种群数量PNUM，最大迭代次数k_max，令k＝0；

Step 2：初始化粒子种群，即粒子的速度和位置，将粒子个体的历史最优值设为pbest，群体最优值设置为gbest；

Step 3：在当代的进化中，计算每个粒子的适应度函数值f；

Step 4：更新粒子的pbest和gbest；

Step 5：更新粒子的速度和位置；

Step 6：若满足终止条件，输出gbest并结束，否则转Step 5。

粒子的更新为以下公式：

其中，c₁、c₂为学习因子，通常情况下c₁+c₂≤4，r₁、r₂为[0,1]之间的随机数，ω为惯性权重，p_id为个体最优值，g_id为全局最优值，下标“i”、“d”分别表示第i个粒子和第i个粒子维数。

本发明的有益效果在于：

本发明通过在集群中每个USV的综合视域内产生同心圆进行建模，利用导航雷达和光电传感器实时测得的综合环境信息与改进后粒子群优化算法相结合以滚动的方式来规划每个USV综合视域内的无避碰路径，不仅克服了标准粒子群优化算法易于陷入局部最优的缺点，而且结合了USV当前环境信息，提高了USV避碰规划的实时性，在适应度函数中加入了USV的转角优化，在获得最优路径的同时也提高了路径的平滑性。

附图说明

图1是本发明的综合视域模拟图；

图2是本发明的坐标示意图；

图3(a)是单线段的膨化图；图3(b)是相邻线段的膨化图；

图4(a)是凸曲线的膨化图；图4(b)是凹曲线的膨化图；

图5(a)是障碍物膨化后的可视切线图；图5(b)是当USV行驶一段距离后障碍物膨化后的可视切线图；

图6是本发明的算法流程图；

图7是本发明的仿真效果图。

具体实施方式

下面结合附图对本发明进一步详细说明。

根据导航雷达和光电传感器的参数特性建立USV综合视域模型；

导航雷达和光电传感器的参数特性如表1所示：

表1 USV常用传感器性能参数表

构建全局坐标系和局部坐标系

如图2所示，USV的局部坐标系X_UO_UY_U的原点位于USV的重心处，以当前USV的前进方向为X轴正方向，垂直于X轴且指向USV右舷侧的方向为Y轴正方向。导航雷达和光电传感器安装在距离船体中心a处，其X轴和Y轴平行于USV局部坐标系的X轴和Y轴，Q_i为传感器感知到的障碍，它与传感器的距离为d，角度为θ，USV在全局环境坐标系的坐标为(x_u,y_u)，α_u是USV的艏向与全局环境坐标轴X_G(北向)的夹角。

将传感器数据的坐标(x_i,y_i)转化为全局坐标系中的坐标的(x_g,y_g)的公式为：

障碍物的膨化

1.单线段的膨化。如图3(a)所示假定某线段的两个端点分别为a₁、a₂，USV与障碍物的安全航行距离为ds，则将a₁延长ds长度到a₁₁，a₂延长ds长度到a₂₂,过a₁₁做a₁₁a₂₂的垂线，在垂线上取两点b₁和b₂，b₁和b₂与a₁₁的距离均为ds，同理，过a₂₂做a₁₁a₂₂的垂线，b₃与b₄便是垂线上距离a₂₂均为ds的两点，连接b₁b₂b₄b₃便是该线段膨化后的多边形。

2.相邻线段的膨化。如图3(b)所示，对于相邻线段组成的折线，首先，按照单线段的处理方法将每条线段膨化为一个四边形，然后，合并相邻的四边形，对于两个四边形相交的部分，则将它们的交点e₄作为合并后新多边形顶点，舍弃部分其它的交点和原有的部分顶点，如果一个多边形的顶点出现在另一个多边形内，那么这个顶点便要舍弃，最后，两个四边形合称为一个多边形，将相邻的两个线段膨化后，通过合并便形成了一个含有六个顶点e₁e₃e₂e₄e₅e₆的多边形。

3.凸曲线的膨化。凸曲线的膨化如图4(a)所示，首先，连接凸曲线的两个端点h₁、h₂，分别延长ds长度到h₁₁和h₂₂，过h₁₁做h₁₁h₂₂的垂线，取垂线上一点h₁₂，h₁₂到h₁₁的距离为ds，同理，过h₂₂做h₁₁h₂₂的垂线，取垂线上一点h₂₁，h₂₁到h₂₂的距离为ds，曲线连接h₁₂和h₂₁，h₁₂h₂₁就为曲线h₁₁h₂₂膨化后的曲线。

4.凹曲线的膨化。凹曲线的膨化如图4(b)所示，首先，连接凸曲线的两个端点g₁、g₂，过g₁做g₁g₂的垂线，取垂线上一点g₁₁，g₁₁到g₁的距离为ds，同理，过g₂做g₁g₂的垂线，取垂线上一点g₂₂，g₂₂到g₂的距离为ds，曲线连接g₁₁和g₂₂，g₁₁g₂₂就为曲线g₁g₂膨化后的曲线。

如图5(a)所示，在USV的综合视域内导航雷达和光电传感器只能检测到障碍物1、障碍物2和障碍物3的一部分，即实线表示的部分，而且障碍物3有一部分被障碍物1遮挡，障碍物1的可视部分经过膨化后的边界点为l₁和l₂，障碍物2的可视部分经过膨化后的边界点为l₃和l₄，障碍物3的可视部分边界经过膨化后的边界点为l₅和l₆，连接Ol₁、Ol₂、Ol₃、Ol₄、Ol₅和Ol₆，其中，l₁、l₂、l₃和l₄称为膨化可视切点，l₅和l₆称为膨化伪可视切点，Ol₁、Ol₂、Ol₃和Ol₄称为膨化可视切线，Ol₅和Ol₆称为膨化伪可视切线。当USV行驶一段距离后，如图5(b)所示，视点位于O₁处时，l₇和l₈便是视点O₁的膨化可视切点，O₁l₇和O₁l₈是其膨化可视切线。

障碍物位置预测的自回归模型

根据上述分析可知，USV通过传感器可以获得t时刻动态障碍物的速度和位置信息，而动态障碍物下一时刻的位置和方向则可以通过自回归模型预测出，假设第k个动态障碍物在t时刻的位置为L_k(t)＝(x_ob,y_ob)，障碍物的位置序列用{L_k(t),t＝1,2…}表示，则可以用公式表示第k个动态障碍物的位置的n阶自回归模型：

其中，e(t)为预测误差，系数α_i为2×2的矩阵。

通过将取样的时间缩短可以使动态障碍物的加速度变化缓慢，因此可以采用一阶自回归模型对动态障碍物的加速度{a_k(t),t＝1,2…}进行建模：

a_k(t)＝β_k,ta_k(t-1)+w(t)

其中，w(t)为预测误差，β_k,t为加速度模型系数。

假设t时刻动态障碍物的速度为v_k(t)，则其加速度和位置的关系如下所示：

a_k(t)＝v_k(t)-v_k(t-1)

＝|L_k(t)-L_k(t-1)|-|L_k(t-1)-L_k(t-2)|

＝L_k(t)-2L_k(t-1)+L_k(t-2)

通过上述分析可预测出第k个动态障碍物在t+1时刻的位置为：

L_k(t)＝(2+β_k,t)L_k(t-1)+(1+2β_k,t)L_k(t-2)+β_k,tL_k(t-3)+w(t)

L_k(t+1)＝L_k(t)+v_k(t)+β_ka_k(t)

USV在运动过程中，每次以综合视域内的子目标点规划局部路径，运动过程中将执行以下步骤：

(4.1)USV通过自身携带的传感器获取综合视域内的环境信息；

(4.2)从起点到子目标点利用粒子群算法规划出一条无避碰最优路经；

(4.3)USV将沿着规划出的路径从起点运动到子目标点；

粒子群优化算法的自我更新：

(4.4)以此子目标为起点，返回步骤1，直到USV行驶到全局的终点。

在每次迭代中，粒子通过两个“极值”来评价和更新自己：第一个就是粒子本身在历史搜索过程中找到的无避碰最优子路径点，称之为个体极值，另一个是整个种群找到的无避碰最优子路径点，称为全局极值。粒子的自我更新如公式：

其中，c₁、c₂为学习因子，通常情况下c₁+c₂≤4，r₁、r₂为[0,1]之间的随机数，ω为惯性权重，p_id为个体最优值，g_id为全局最优值，下标“i”、“d”分别表示第i个粒子和第i个粒子维数，其中粒子的维数d的值为2。

粒子群优化算法的参数设计

ω的值越大，粒子的全局搜索能力就越强，同时也加快了算法的收敛速度，但是问题的最优解不易获得，ω的值变小，局部搜索能力变强，优化问题的最优解也容易获得，但是算法的收敛速度将变慢，甚至陷入停滞状态。在粒子群优化算法迭代前期，为了尽可能的发现更大的解空间，粒子需要有较强的全局搜索能力，而到后期的时候，粒子需要加强局部搜索能力，尽可能的搜寻最优解。因此，随着迭代次数的增加，惯性权重ω应不断减小。采取基于正切函数的惯性调整策略改进设计ω的值，如公式：

式中，α＝π/4，ω_s＝0.9，ω_e＝0.4，m为控制因子，其值为0.5，k为当前迭代次数，k_max为最大迭代次数，ω_k随着迭代次数的增加从0.9线性递减到0.4。

在算法初期，粒子的全局搜索能力要强，以便尽可能的探测全局空间，到了算法后期，粒子局部搜索将是寻找全局最优解的关键，因此要加强粒子的局部搜索能力，因此，在迭代过程中c₁应逐渐减小，c₂应逐渐增大。采取基于线性调整策略改进设计学习因子的调节，如公式：

式中k为当前迭代次数，k_max为最大迭代次数，c_1s＝2.5，c_2s＝1.0，c_1e＝1.0，c_2e＝2.5，c₁随着迭代次数的增加从2.5非线性递减到1.0，c₂从1.0非线性增加到2.5。

种群规模PNUM的大小为80。

已知USV的起始点为(x₀,y₀)，目标点为(x_n,y_n)，子路径点为(x_i,y_i)，其中i＝0,1,2…n-1,n，则适应度函数定义为：

根据USV的每次转角定义的适应度函数为：

结合以上分析，得出USV在避碰规划过程中总的适应度函数：

f＝λ₁f₁+λ₂f₂

其中λ₁＝0.8，λ₂＝20。

根据以上分析可得出粒子群优化算法的具体步骤如下：

Step 1：初始化粒子群算法中的参数：ω_s、ω_e、c_1s、c_1e、c_2s、c_2e，种群数量PNUM，最大迭代次数k_max，令k＝0；

Step 2：初始化粒子种群，即粒子的速度和位置，将粒子个体的历史最优值设为pbest，群体最优值设置为gbest；

Step 3：在当代的进化中，计算每个粒子的适应度函数值f；

Step 4：更新粒子的pbest和gbest；

Step 5：更新粒子的速度和位置；

Step 6：若满足终止条件，输出gbest并结束，否则转Step 5。

本次发明以Qt为开发平台搭建仿真实验环境，并根据导航雷达与光电传感器的参数特性模拟USV综合探测周围环境信息，在仿真实验中，模拟环境的大小为130×110，单位为千米，导航雷达与光电传感器只能探测到障碍物的边界信息，其中导航雷达的探测半径为10千米，光电传感器的探测半径为15千米。本次仿真实验共有3艘USV，其初始速度大小均为40节，为了方便分析，3艘USV的起始点和终点是确定的，环境中的静态障碍物包括岛屿，如圆型障碍物和不规则障碍，人工放置的多边形障碍，如三角形、四边形和五边形，动态障碍物为一个小型的等边三角形，粒子群优化算法的最大迭代次数为200次，种群规模为80，算法的计算时间为292ms。3艘USV在整个环境下的避碰规划过程如图7所示。

Claims (1)

1.一种基于改进粒子群优化算法的USV集群避碰规划方法，其特征在于，主要包括以下步骤：

步骤1：根据导航雷达和光电传感器的参数特性建立USV综合视域模型；

步骤1.1：构建坐标系；

建立三个坐标系：全局环境坐标系X_GO_GY_G、基于USV的局部坐标系X_UO_UY_U和基于传感器综合感知信息的局部坐标系X_RO_RY_R；

全局坐标系X_GO_GY_G采用北东坐标系，左下角为坐标原点，X轴指向正北方向，Y轴指向正东方向；

基于USV的局部坐标系X_UO_UY_U的原点位于USV的重心处，以当前USV的前进方向为X轴正方向，垂直于X轴且指向USV右舷侧的方向为Y轴正方向，导航雷达和光电传感器安装在距离船体中心a处，基于传感器综合感知信息的局部坐标系X_RO_RY_R的X轴和Y轴平行于USV局部坐标系的X轴和Y轴，Q_i为传感器感知到的障碍，它与传感器的距离为d，角度为θ；USV在全局环境坐标系的坐标为(x_u,y_u)，α_u是USV的艏向与全局环境坐标轴X_G的夹角；

将传感器数据的坐标(x_i,y_i)转化为全局坐标系中的坐标的(x_g,y_g)的公式为：

步骤1.2：构建环境模型；

步骤1.2.1：数据采集；

导航雷达和光电传感器的感知范围为360°，每隔1°的角度上传感器只能获取一个距离信息，按逆时针方向角度从0°变化至360°，即传感器每扫描一次便获得361组感知数据点，这些感知数据点包含两个参数，第一个参数为感知数据点与传感器之间的角度，第二个参数为感知数据点到传感器的距离；

步骤1.2.2：将感知数据点提取为线段特征；通过区域划分的方式将感知数据点划分为多个区域，然后再将每个区域内感知数据点集进行线性拟合，进而提取为线段特征；

步骤1.2.3：静态障碍物膨化，膨化方式包括单线段的膨化、相邻线段的膨化、凸曲线的膨化、凹曲线的膨化；

单线段的膨化：某线段的两个端点分别为a₁、a₂，USV与障碍物的安全航行距离为ds，则将a₁延长ds长度到a₁₁，a₂延长ds长度到a₂₂,过a₁₁做a₁₁a₂₂的垂线，在垂线上取两点b₁和b₂，b₁和b₂与a₁₁的距离均为ds；同理，过a₂₂做a₁₁a₂₂的垂线，b₃与b₄便是垂线上距离a₂₂均为ds的两点，连接b₁b₂b₄b₃便是该线段膨化后的多边形；

相邻线段的膨化：对于相邻线段组成的折线，首先按照单线段的处理方法将每条线段膨化为一个四边形，然后，合并相邻的四边形，对于两个四边形相交的部分，则将它们的交点e₄作为合并后新多边形顶点，舍弃部分其它的交点和原有的部分顶点；如果一个多边形的顶点出现在另一个多边形内，那么这个顶点便要舍弃，最后，两个四边形合称为一个多边形，将相邻的两个线段膨化后，通过合并便形成了一个含有六个顶点e₁e₃e₂e₄e₅e₆的多边形；

凸曲线的膨化：连接凸曲线的两个端点h₁、h₂，分别延长ds长度到h₁₁和h₂₂，过h₁₁做h₁₁h₂₂的垂线，取垂线上一点h₁₂，h₁₂到h₁₁的距离为ds；同理，过h₂₂做h₁₁ h₂₂的垂线，取垂线上一点h₂₁，h₂₁到h₂₂的距离为ds，曲线连接h₁₂和h₂₁，h₁₂ h₂₁就为曲线h₁₁h₂₂膨化后的曲线；

凹曲线的膨化，连接凸曲线的两个端点g₁、g₂，过g₁做g₁g₂的垂线，取垂线上一点g₁₁，g₁₁到g₁的距离为ds；同理，过g₂做g₁g₂的垂线，取垂线上一点g₂₂，g₂₂到g₂的距离为ds，曲线连接g₁₁和g₂₂，g₁₁g₂₂就为曲线g₁g₂膨化后的曲线；

步骤1.2.4：动态障碍物的轨迹预测；将动态障碍物膨化为一个圆形，根据自回归模型预测出动态障碍物在全局坐标系下的位置和方向；

假设第k个动态障碍物在t时刻的位置为L_k(t)＝(x_ob,y_ob)，障碍物的位置序列用{L_k(t),t＝1,2…}表示，则第k个动态障碍物的位置的n阶自回归模型为：

其中，e(t)为预测误差，系数α_i为2×2的矩阵；

采用一阶自回归模型对动态障碍物的加速度{a_k(t),t＝1,2…}进行建模：

a_k(t)＝β_k,ta_k(t-1)+w(t)

其中，w(t)为预测误差，β_k,t为加速度模型系数；

假设t时刻动态障碍物的速度为v_k(t)，则其加速度和位置的关系如下所示：

a_k(t)＝v_k(t)-v_k(t-1)

＝|L_k(t)-L_k(t-1)|-|L_k(t-1)-L_k(t-2)|

＝L_k(t)-2L_k(t-1)+L_k(t-2)

通过分析可预测出第k个动态障碍物在t+1时刻的位置为：

L_k(t)＝(2+β_k,t)L_k(t-1)+(1+2β_k,t)L_k(t-2)+β_k,tL_k(t-3)+w(t)

L_k(t+1)＝L_k(t)+v_k(t)+β_k,ta_k(t)；

步骤2：以综合视域内的子目标点规划局部路径，从起点到子目标点利用粒子群算法规划出一条无避碰最优路经；

步骤2.1：初始化粒子群算法中的参数：ω_s、ω_e、c_1s、c_1e、c_2s、c_2e，种群数量PNUM，最大迭代次数k_max，令当前迭代次数k＝0；已知USV的起始点为(x₀,y₀)，目标点为(x_n,y_n)，子路径点为(x_i,y_i)，i＝0,1,2…n-1,n；

步骤2.2：初始化粒子种群，即粒子的速度和位置，将粒子个体的历史最优值设为pbest，群体最优值设置为gbest；

步骤2.3：在当代的进化中，计算每个粒子的适应度函数值f；

f＝λ₁f₁+λ₂f₂

其中，λ₁、λ₂为设定的常数；

步骤2.4：更新粒子的pbest和gbest；

步骤2.5：更新粒子的速度和位置；

其中，c₁、c₂为学习因子；r₁、r₂为[0,1]之间的随机数；ω为惯性权重；p_id为个体最优值；g_id为全局最优值；下标i表示第i个粒子；下标d表示粒子维数，d＝2；α＝π/4，m为控制因子；

步骤2.6：若满足终止条件，输出gbest并结束，否则，转步骤2.5；

步骤3：USV将沿着规划出的路径从起点运动到子目标点；

步骤4：若USV行驶到全局的终点，则停止；否则，以此子目标为起点，返回步骤1。

Images