CN102175245B - 一种基于海流历史统计信息的水下潜器路径规划方法 - Google Patents

Abstract

本发明公开了一种基于海流历史统计信息的水下潜器路径规划方法，包括确定航行区域，将航行区域栅格化、利用海流历史统计数据库生成航行区域内的海流场、利用电子海图作为环境场，将航行区域内的障碍物、岛屿、浅水区进行简化合并，生成禁航区、将海流信息和禁航区信息按照栅格进行存储、构造路径评价函数、利用粒子群算法进行最优路径搜索、输出路径，路径规划结束几个步骤。本发明利用海流历史统计数据库生成接近真实值的海流场，在充分考虑海流影响的情况下，设计了考虑航行安全性、经济性、光滑性相结合的路径评价函数，利用粒子群优化算法作为路径搜索算法进行水下潜器全局路径规划，以规划出更接近实际航行路线的潜器航行路径。

Description

一种基于海流历史统计信息的水下潜器路径规划方法

技术领域

本发明属于水下潜器路径规划领域，尤其是一种基于海流历史统计信息的的水下潜器路径规划方法。

背景技术

路径规划问题是水下潜器智能控制的关键技术之一。随着人们对海洋资源的日趋关注，水下潜器在海洋开发、军事侦察及援潜救生中发挥着越来越重要的作用，人们对水下潜器实现智能控制和自主导航的要求和期望也越来越高，路径规划技术在一定程度上直接关系到水下潜器智能水平的高低。由于水下潜器经常工作在大范围开放的三维空间环境中，受到各种海洋环境要素的影响。这种情况下路径规划方法已经不仅仅是避障问题，还要充分考虑各种海洋要素对潜器航行的影响。

目前针对路径规划的研究主要集中在路径规划的方法上，如路径优化方法、障碍物规避方法等。例如申请号为200910100613.1的专利申请一种基于粒子群优化算法的移动机器人路径规划方法，该专利对机器人路径利用粒子群优化算法进行规划；最后对规划的路径进行深度优先搜索。申请号为201010123511.4的专利提出一种基于双群协同竞争粒子群和Ferguson样条的机器人路径规划方法，该方法主要解决现有技术中存在的规划路径平滑性差且收敛速度缓慢的问题。这些方法在实验室仿真下已经取得不错的效果，但是离实际应用还有很大的距离，主要原因之一就是没有考虑实际的航行环境或者考虑较少。由于海流的流幅多数比较宽，流速也比较大，对航海会产生比较明显的影响。潜器在有海流存在的海域航行，必须及时修正航向、航速，否则会改变计划航线、加长航程，甚至会出现航海事故或影响任务的完成。同时潜器在航行过程中如果能充分利用海流的影响，避开逆流并沿着顺流航行则能大大节省燃料。因此在路径规划中如果充分考虑海流的影响，不仅能够保证规划出安全可靠的航行路线，而且能够节省燃料。目前国内外已经有一些学者开始海流影响下的路径规划技术研究。例如徐玉如，姚耀中在《考虑海流影响的水下机器人全局路径规划研究》一文中将海流因素作为遗传算法的评价因子，在路径规划的层面上考虑海流的影响。毛宇峰，庞永杰在《改进粒子群在水下机器人路径规划中的应用》一文中设计了综合路径长度、海流和转向费用的适应度函数，使算法很好地适应海流的变化，很大程度减小了海流对水下机器人能量消耗和控制的不利影响。但是上述两篇论文在路径规划中采用的海流均是由流函数生成的，与实际海流存在较大差别。利用这种海流数据进行路径规划虽然在一定程度上考虑了海流的影响，但是由于这种海流与实际海流存在较大的差距，因此生成的路径与实际航线偏差较大。海流的流动具有相对稳定性，在较长时间内流动方向、速率和流动路径大致相似。海流历史统计数据是完全根据实测资料进行统计的结果，包括最大、最小、平均值等多个统计项目结果，因此在没有实测信息的情况下，可以利用历史观测统计资料来近似表示当前流场。

发明内容

本发明的目的是为了解决传统路径规划中考虑海流因素较少的缺陷，提出了一种基于海流历史统计信息的水下潜器路径规划方法。本发明利用海流历史统计数据库生成接近真实值的海流场，在路径规划中充分考虑了海流的影响，使规划出的路径更接近水下潜器实际航行路线。

本发明的目的是利用海流历史统计数据库生成海流场，提供一种充分考虑海流影响的路径规划的方法。其特征在于，该方法包含以下要点：

步骤1.确定航行区域，将航行区域栅格化；

步骤2.利用海流历史统计数据库生成航行区域内的海流场；

步骤3.利用电子海图作为环境场，将航行区域内的障碍物、岛屿、浅水区进行简化合并，生成禁航区；

步骤4.将海流信息和禁航区信息按照栅格进行存储；

步骤5.构造路径评价函数；

步骤6.利用粒子群算法进行最优路径搜索；

步骤7.输出路径，路径规划结束。

本发明的优点在于：

本发明利用海流历史统计数据库生成接近真实值的海流场，在充分考虑海流影响的情况下，设计了考虑航行安全性、经济性、光滑性相结合的路径评价函数，利用改进粒子群优化算法作为优化算法进行水下潜器全局路径规划，以规划出更接近实际航行路线的潜器航行路径。

附图说明

图1为本发明所述水下潜器路径规划流程图。

图2为本发明说书改进粒子群优化算法流程图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明是一种基于海流历史统计信息的水下潜器路径规划方法，该包括环境建模、海流场生成、路径评价函数设计、改进粒子群算法设计等关键内容，具体流程如图1所示，包括以下：

步骤1.确定航行区域，将航行区域栅格化。

航行区域为以路径起始点和终止点连线为对角线的矩形区域。沿经度和纬度方向分别以10′为单位进行等间隔划分，生成大小为10′X10′的栅格区域。

步骤2.利用海流历史统计数据库生成航行区域内的海流场。

海流历史统计数据库存储有海流的12个月份历史统计信息，海流信息是经过累年月平均统计得到，具有一定的权威性和很大的参考价值。海流历史统计数据库由25个表组成，分别为月份表、12个月份的海流统计信息北偏量表和12月份的东偏量表。海流信息以网格的形式分布，经纬度的分辨率均为10′，每一个网格的海流以东偏量u和北偏量v来表示，分别存放在海流数据库中的东偏量表和北偏量表中。已知某位置海流的东偏量u和北偏v，则该处的流向θ和流速

的计算公式为：

$\left|{\stackrel{\→}{V}}_c\right|=\sqrt{u^2+v^2}$

$\mathrm{\θ}=\left\{\begin{array}{c}\arctan \left(\frac{u}{v}\right),v\≠0\\ \frac{\mathrm{\π}}{2},v\end{array}\right.---\left(2\right)$

航行区域内海流场的生成方法如下：首先连接海流历史统计数据库，数据库连接成功后在月份表中查询当前日期对应的数据表名。遍历步骤1中航行区域划分的栅格，计算每一个栅格的经纬度值，然后根据数据表名在相应的东偏量表和北偏量表中查询当前经纬度对应的海流的北偏量和东偏量值，并把该值存在海流结构体Struct oceanCurrent中。海流结构体的具体定义如下：

Struct oceanCurrent

{

float oceancurrentU；

float oceancurrentV；

}

其中，oceancurrentU为该栅格点内的海流东偏量，oceancurrentV为该栅格点内的海流北偏量。

步骤3.利用电子海图作为环境场，将航行区域内的障碍物、岛屿、浅水区进行简化合并，生成禁航区。

水下潜器航行中的障碍物主要有岛屿、岛礁、浅水区、沉船。电子海图是水下潜器的“眼睛”，因此潜器航线规划可以建立在准确的海图信息基础上。利用shapefile格式电子海图信息作为静态环境场，将环境中的障碍物、浅水区、岛屿以多边形形式进行表达。多边形围成的区域为禁航区，路径不能通过，其他区域为可行域，路径可以通过。具体处理方法及定义如下：

安全区：该区域中潜器航行没有任何危险

禁航区：该区域存在不利于潜器安全航行的要素，会危及潜器航行安全。

步骤4.将海流信息和禁航区信息按照栅格进行存储。

对于步骤1中划分的每一个栅格，定义数据结构struct PosInfor来存储该栅格的位置信息、海流信息以及是否可航。PosInfor的具体定义如下：

struct PosInfor

{

float lon；

float 1at；

float oceancurrentU；

float oceancurrentV；

bool bNoPassing；

}

其中lat为栅格点的纬度，lon为栅格点的经度，oceancurrentU为栅格点内的海流东偏量，oceancurrentV为栅格点内的海流北偏量，bNoPassing表示该栅格是否可航，1表示不可航，0表示可航。

步骤5.构造路径评价函数；

在现有的水下潜器路径规划中，要么是时间固定的能量消耗最优路径研究，要么是不考虑能量消耗的时间最优路径研究。而在实际的航行中，能量消耗、航行时间均需要考虑。另外对于具备特定任务的水下潜器来说，航行的隐蔽性和安全性也是需要考虑的因素之一。因此本发明设计了一个综合考虑能量消耗、路径长度、安全性、光滑度等四个评价指标的路径性能评价函数，具体包括以下几个步骤：

(1)能量消耗

潜器航行过程中的能量消耗主要由两部分组成。一部分用来克服惯性的加速力F_a，另一部分用来克服牵引潜器运动的牵引力F_d。

$E_{\mathrm{energy}}=\underset{p}{\∫}(F_a\left(t\right)+F_d\left(t\right))V_r\left(t\right)d_t---\left(3\right)$

$V_r\left(t\right)=|{\stackrel{\→}{V}}_a\left(t\right)-{\stackrel{\→}{V}}_c\left(t\right)|---\left(4\right)$

F_a(t)＝w_aV_r(t)                                    (5)

F_d(t)＝w_dV² _r(t)                        (6)

其中，E_energy表示潜器克服惯性力和牵引力需要消耗的能量，V_r(t)为相对速度，即马达需要提供的速度，为绝对速度，即潜器的航行速度。

为t时刻的海流速度。w_a，w_d为调和常数。对于整个航路的能量消耗来说，克服加速力的能量消耗可以忽略不计，因此w_a可以取0。由n条路径段组成的航路的能量消耗可以离散化为：

$E_{\mathrm{sum}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{E}_i---\left(7\right)$

每条航路段又可以划分成m条子航路段，划分时要确保每条子航路段上的海流值的最大值与最小值偏差不大于航路段上海流平均值的十分之一。

$E_i=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{E}_{\mathrm{ij}}=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}(w_d{V^2}_{r,j}+w_a{V}_{r,j})V_{r,j}\mathrm{\Δ}{t}_j\≈\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{w}_d{V^3}_{r,j}\mathrm{\Δ}{t}_j---\left(8\right)$

其中E_sum为整条航路等能量消耗，E_i为第i个航路段内的能量消耗，E_ij为第i个航路段内的第j个子航路段的能量消耗，Δt_j为第j个子航路段花费的时间，w_a，w_d为调和常数。V_r，j为第j个子航路段内的潜器相对速度。

(2)路径长度

对于由n个节点的路径来说，共有n-1条路径段，路径长度就是将这些路径段的长度相加，即：

$E_{\mathrm{length}}=\underset{i=0}{\overset{n-1}{\mathrm{\Σ}}}\mathrm{\Δ}{L}_i---\left(9\right)$

$\mathrm{\Δ}{L}_i=\sqrt{{(y_i-y_{i-1})}^2+{(x_i-x_{i-1})}^2}---\left(10\right)$

其中，E_length表示整条航路的长度，ΔL_i为第i条航路段的长度，x_i，y_i为第i个路径点的坐标，x_i-1，y_i-1为第i-1个路径点的坐标，1≤i≤n。

(3)安全性

为了保证潜器航行的安全性，规划航路禁航区等必须保持一定的安全距离。规划航线不穿过禁航区并且与禁航区的距离大于安全距离时该航线是安全的。取距离航线最近的禁航区与航线的距离L_min作为评价标准，L_min越大则航线越安全。

$E_{\mathrm{safty}}=\left\{\begin{array}{c}\frac{K}{L_{\min }},L_{\min }\≤L_{\mathrm{safe}}\\ 0,L_{\min }>L_{\mathrm{safe}}\end{array}\right.---\left(11\right)$

其中，E_safty表示安全代价值，L_min为距离航线最近的禁航区或者障碍区与航线的距离，L_safe为潜器与禁航区应该保持的最小安全距离，由潜器的尺寸确定，取潜器长度的6-15倍，本发明中取10倍，K为一个常数，它的取值为900L_safe至1100L_safe，本发明中取1000L_safe。

(4)光滑性

光滑度是指路径中每一个路径点处路径偏转角度的大小。路径从出发点到目的点所偏转的总角度越小，路径越平滑，潜器转向的角度越小，路径质量越高。因此设计路径平滑度应等于路径所有路径点的偏转角度之和，即：

$E_{\mathrm{smooth}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\θ}}_i---\left(12\right)$

其中，E_smooth表示光滑度代价值，θ_i表示第i段路径相对于第i-1段路径的偏转角度。

在上述四个评价指标中，能量消耗越大、路径越长、距离航线最近的禁航区到航线的距离L_min越小、偏转角之和越大的路径为较差的路径，因此衡量路径优劣的评价函数可以设计成：

E(p)＝w_energyE_energy+w_saftyE_safty+w_lengthE_length+w_smoothE_mooth        (13)

其中w_energy、w_safty、w_length、w_smooth为权系数，具体取值由对路径的要求确定。当能量消耗为主要考虑对象时w_energy的值要大于其他三个系数；当安全性为主要考虑对象时w_safty的值要大于其他三个系数；当路径长度为主要考虑对象时w_length的值要大于其他三个系数；当路径光滑度为主要考虑对象时w_smooth的值要大于其他三个系数。E(p)越小，路径越好。

步骤6.利用粒子群算法进行最优路径搜索；

PSO的优点在于简单同时又有深刻的智能背景，既适合科学研究，又特别适合工程应用。PSO算法的主要缺点是易陷入局部极小点。近几年来已提出了多种PSO改进，其中把PSO中的参数设置为时变参数是一种新的参数调整策略。用对惯性系数和学习因子的改进来提高粒子群算法的寻优能力。改进的目的是提高粒子在早期搜索过程中的全局搜索能力，并最终促使粒子在搜索的最终阶段收敛于全局最优解。本专利采用时变参数粒子群算法进行最优路径搜索。

步骤6.1.确定粒子群的参数、最大迭代次数、种群大小，完成粒子群初始化。

步骤6.2.根据步骤5确定的路径评价函数计算每个粒子的适应值。

步骤6.3.更新每个粒子的最优适应值和整个群体的最优适应值。

如果粒子当前位置适应度小于其个体最优位置的适应度值，则以当前位置适应度值作为该粒子的最优适应。如果所有个体最优位置中出现适应度值低于整个群体的最优适应值，则以所有个体最优位置中出现的适应度值代替整个群体的最优适应度值。

步骤6.4.判断当前迭代次数是否达到步骤6.1设定的最大迭代次数，如何达到则执行步骤7。否则转向步骤6.5。

步骤6.5.计算当前粒子群惯性系数和学习因子。

(1)惯性系数

在基本公式中，惯性系数w表示粒子惯性的大小。w较大时具有较强的全局收敛能力，w较小时具有较强的局部搜索能力。而在实际的优化问题中，一般希望先采用全局搜索，使算法快速收敛于某一个区域，然后再采用局部搜索来获得高精度的解。因此在搜索初期应该具有较大的w，以加大粒子的全局收索能力；在搜索后期w应该较小，以提高粒子的局部搜索能力。因此随时间而改变的w可以设计成：

$w=w_{\max }-(w_{\max }-w_{\min })\frac{i}{N}---\left(14\right)$

其中，w_max为最大的惯性系数，值为0.75到1.05，本发明中取0.9，w_min为最小的惯性系数，取值为0.3到0.55，本发明中取0.4，i为当前迭代次数，N为总的迭代次数。

(2)学习因子

c₁越大粒子的种群多样性越好。c₂越大，粒子的相似性越好。为了防止算法过早收敛，提高种群的多样性，在优化的初期应该强调个体认知经验，因此应该加大c₁。在优化后期应该提高群体经验系数c₂以加快收敛。根据这个规则将c₁，c₂设计成随时间改变的参数，在初始阶段c₁＞c₂，使粒子趋于种群最优；另一方面，在搜索的后期，c₁＜c₂，使粒子趋于全局最优解。

$c_1=(c_{1f}-c_{1c})\frac{i}{N}+c_{1c}---\left(15\right)$

$c_2=(c_{2f}-c_{2c})\frac{i}{N}+c_{2c}---\left(16\right)$

其中c_1f、c_1c、c_2f、c_2c为常数，c_1f、c_2c的取值范围为0.3到0.7，c_1c、c_2f的取值范围为2.2到2.8，本发明中取c_1f＝0.5、c_1c＝2.5、c_2f＝2.5、c_2c＝0.5，i为当前迭代次数，N为总的迭代次数。

步骤6.6.更新粒子的位置和速度，并转向步骤6.2。

粒子的位置和速度的更新公式如下：

v_id(t+1)＝w*v_id(t)+c₁*(p_id-x_id(t))+c₂*(p_gd-x_id(t))

x_id(t+1)＝x_id(t)+v_id(t+1)

其中v_id(t)、v_id(t+1)分别为第i个粒子t和t+1时刻的粒子速度，x_id(t)、x_id(t+1)分别为第i个粒子t和t+1时刻的粒子位置，p_id为第i个粒子的个体最优适应值，p_gd为整个群体的最优适应度值，w，c₁和c₂的值由步骤6.5求得。

步骤7.输出路径，路径规划结束。

Claims (8)

1.一种基于海流历史统计信息的水下潜器路径规划方法，其特征在于，包括以下几个步骤：

步骤1.确定航行区域，将航行区域栅格化；

航行区域为以路径起始点和终止点连线为对角线的矩形区域，沿经度和纬度方向分别以10′为单位进行等间隔划分，生成大小为10′X10′的栅格区域；

步骤2.利用海流历史统计数据库生成航行区域内的海流场；

在海流历史统计数据库的月份表中查询当前日期对应的数据表名，遍历步骤1中航行区域划分的栅格，计算每一个栅格的经纬度值，然后根据数据表名在相应的东偏量表和北偏量表中查询栅格当前经纬度对应的海流的北偏量和东偏量值，则生成航行区域内的海流场；

步骤3.利用电子海图作为环境场，将航行区域内的障碍物、岛屿、浅水区进行简化合并，生成禁航区；

利用电子海图信息作为静态环境场，将环境中的障碍物、浅水区、岛屿以多边形形式进行表达，多边形围成的区域为禁航区，路径不能通过，其他区域为可行域，路径能够通过；

步骤4.将海流信息和禁航区信息按照栅格进行存储；

对于步骤1中划分的每一个栅格，存储该栅格的位置信息、海流信息以及禁航区信息；

步骤5.构造路径评价函数；

具体包括以下几个步骤：

(1)能量消耗

潜器航行过程中的能量消耗主要由两部分组成；一部分用来克服惯性的加速力F_a，另一部分用来克服牵引潜器运动的牵引力F_d；

$E_{\mathrm{energy}}=\underset{p}{\∫}(F_a\left(t\right)+F_d\left(t\right))V_r\left(t\right)d_t---\left(3\right)$

$V_r\left(r\right)=|{\stackrel{\→}{V}}_a\left(t\right)-{\stackrel{\→}{V}}_c\left(t\right)|---\left(4\right)$

F_a(t)＝w_aV_r(t)    (5)

F_d(t)＝w_dV² _r(t)    (6)

其中，E_energy表示潜器克服加速力和牵引力需要消耗的能量，V_r(t)为相对速度，即马达需要提供的速度，

为绝对速度，即潜器的航行速度；

为t时刻的海流速度；w_a，w_d为调和常数；由n条路径段组成的航路的能量消耗离散化为：

$E_{\mathrm{sum}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{E}_i---\left(7\right)$

每条航路段划分成m条子航路段，划分时确保每条子航路段上的海流值的最大值与最小值偏差不大于航路段上海流平均值的十分之一；

$E_i=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{E}_{\mathrm{ij}}=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}(w_d{V^2}_{r,j}+w_a{V}_{r,j})V_{r,j}\mathrm{\Δ}{t}_j\≈\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{w}_d{V^3}_{r,j}\mathrm{\Δ}{t}_j---\left(8\right)$

其中，E_sum为整条航路的能量消耗，E_i为第i个航路段内的能量消耗，E_ij为第i个航路段内的第j个子航路段的能量消耗，Δt_j为第j个子航路段花费的时间，V_r，j为第j个子航路段内的潜器相对速度；

(2)路径长度

对于有n个节点的路径，则共有n-1条路径段，路径长度为路径段的长度相加，即：

$E_{\mathrm{length}}=\underset{i=0}{\overset{n-1}{\mathrm{\Σ}}}\mathrm{\Δ}{L}_i---\left(9\right)$

$\mathrm{\Δ}{L}_i=\sqrt{{(y_i-y_{i-1})}^2+{(x_i-x_{i-1})}^2}---\left(10\right)$

其中，E_length表示整条航路的长度，ΔL_i为第i条航路段的长度，x_i，y_i为第i个路径点的坐标，x_i-1，y_i-1为第i-1个路径点的坐标，1≤i≤n；

(3)安全性

取距离航线最近的禁航区与航线的距离L_min作为评价标准，L_min越大则航线越安全；

$E_{\mathrm{safty}}=\left\{\begin{array}{c}\frac{K}{L_{\min }},L_{\min }\≤L_{\mathrm{safe}}\\ 0,L_{\min }>L_{\mathrm{safe}}\end{array}\right.---\left(11\right)$

其中，E_safty表示安全代价值，L_min为距离航线最近的禁航区与航线的距离，L_safe为潜器与禁航区应该保持的最小安全距离，K为常数；

(4)光滑性

路径平滑度应等于路径所有路径点的偏转角度之和，即：

$E_{\mathrm{smooth}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\θ}}_i---\left(12\right)$

其中，E_smooth表示光滑度代价值，θ_i表示第i段路径相对于第i-1段路径的偏转角度；

则路径评价函数为：

E(p)＝w_energyE_energy+w_saftyE_safty+w_lengthE_length+w_smoothE_smooth    (13)

其中w_energy、w_safty、w_length、w_smooth为权系数；

步骤6.利用粒子群算法进行最优路径搜索；

采用时变参数粒子群算法进行最优路径搜索，具体包括以下几个步骤；

步骤6.1.确定粒子群的参数、最大迭代次数、种群大小，完成粒子群初始化；

步骤6.2.根据步骤5确定的路径评价函数计算每个粒子的适应值；

步骤6.3.更新每个粒子的最优适应值和整个群体的最优适应值；

如果粒子当前位置适应值小于其个体最优位置的适应值，则以当前位置适应值作为该粒子的最优适应值；如果所有个体最优位置中出现适应值低于整个群体的最优适应值，则以所有个体最优位置中出现的适应值代替整个群体的最优适应值；

步骤6.4.判断当前迭代次数是否达到步骤6.1设定的最大迭代次数，如果达到则执行步骤7；否则转向步骤6.5；

步骤6.5.计算当前粒子群惯性系数和学习因子；

(1)惯性系数

惯性系数w为：

$w=w_{\max }-(w_{\max }-w_{\min })\frac{i}{N}---\left(14\right)$

其中，w_max为最大的惯性系数，w_min为最小的惯性系数，i为当前迭代次数，N为总的迭代次数；

(2)学习因子

学习因子c₁、c₂为；

$c_1=(c_{1f}-c_{1c})\frac{i}{N}+c_{1c}---\left(15\right)$

$c_2=(c_{2f}-c_{2c})\frac{i}{N}+c_{2c}---\left(16\right)$

其中c_1f、c_1c、c_2f、c_2c为常数，i为当前迭代次数，N为总的迭代次数；

步骤6.6.更新粒子的位置和速度，并转向步骤6.2；

粒子的位置和速度的更新公式如下：

v_id(t+1)＝w*v_id(t)+c₁*(p_id-x_id(t))+c₂*(p_gd-x_id(t))

x_id(t+1)＝x_id(t)+v_id(t+1)

其中：v_id(t)、v_id(t+1)分别为第i个粒子的t和t+1时刻的粒子速度，x_id(t)、x_id(t+1)分别为第i个粒子的t和t+1时刻的粒子位置，p_id为第i个粒子的个体最优适应值，p_gd为整个群体的最优适应值；

步骤7.输出路径，路径规划结束。

2.根据权利要求1所述的一种基于海流历史统计信息的水下潜器路径规划方法，其特征在于，所述的步骤2中，通过栅格海流的东偏量u和北偏v，得到该处的流向θ和流速

为：

$\left|{\stackrel{\→}{V}}_c\right|=\sqrt{u^2+v^2}---\left(1\right)$

$\mathrm{\θ}=\left\{\begin{array}{c}\arctan \left(\frac{u}{v}\right),v\≠0\\ \frac{\mathrm{\π}}{2},v=0\end{array}\right.---\left(2\right)$

3.根据权利要求1所述的一种基于海流历史统计信息的水下潜器路径规划方法，其特征在于，所述的步骤3中，电子海图为shapefile格式电子海图。

4.根据权利要求1所述的一种基于海流历史统计信息的水下潜器路径规划方法，其特征在于，所述的步骤5(1)中，w_a＝0。

5.根据权利要求1所述的一种基于海流历史统计信息的水下潜器路径规划方法，其特征在于，所述的步骤5(3)中，L_safe的值取潜器长度的6-15倍。

6.根据权利要求1所述的一种基于海流历史统计信息的水下潜器路径规划方法，其特征在于，所述的步骤5(3)中，K的取值为900L_safe至1100L_safe。

7.根据权利要求1所述的一种基于海流历史统计信息的水下潜器路径规划方法，其特征在于，所述的步骤6.5中，w_max取值为0.75到1.05，w_min取值为0.3到0.55。

8.根据权利要求1所述的一种基于海流历史统计信息的水下潜器路径规划方法，其特征在于，所述的步骤6.5中，所述的c_1f、c_2c的取值范围为0.3到0.7，c_1c、c_2f的取值范围为2.2到2.8。

Images