一种基于改进萤火虫算法的AUV三维航路规划方法

摘要

本发明属于水下潜器三维路径规划技术领域，具体涉及一种基于改进萤火虫算法的AUV三维航路规划方法。本发明包括：对水下潜器三维路径规划进行建模和萤火虫种群初始化；计算目标函数值；计算自适应参数；比较萤火虫之间的亮度，更新萤火虫的位置：添加辅助规划算子；当满足算法迭代停止条件则输出最优路径，水下潜器三维路径规划结束，输出最后一次迭代的最优路径。本发明提出一种基于改进萤火虫算法的AUV三维路径规划方法。该方法相比传统路径搜索算法更加灵活，通过添加辅助规划算子，可以实现了AUV三维路径的快速规划。

说明书

技术领域

本发明属于水下潜器三维路径规划技术领域，具体涉及一种基于改进萤火虫算法的AUV 三维航路规划方法。

背景技术

AUV可广泛应用于海洋学勘测、地形地貌测量、目标搜索、海底管线检测维修等领域，AUV 最大的特点即具有高度的自主性，AUV的这种自主性表现之一是具有基于环境模型的全局规 划能力。水下潜器三维路径规划是给定一个运动体和一个关于环境的描述，当环境建模完成 之后，水下潜器路径规划任务需满足在安全航行区域内，按一定的优化准则（如路径最短、 耗时最少等）以及附加约束（如转向角、航行深度等）搜索一条从指定起点到目标点的最优 路径（或次优路径）。对于传统搜索算法，三维环境带来的计算量非常巨大，导致算法规划速 度显著变慢。另一方面，随着智能算法的兴起，越来越多的学者尝试用智能算法解决优化问 题。由于智能算法较为灵活易于操作，在解决复杂问题时具有更好的适应性和智能性，因此 很多文献把智能算法，如PSO算法、蚁群算法等应用于路径规划，收到了较好的效果。

萤火虫算法（Firefly Algorithm,FA）是由Xin-she Yang于2008年提出的一种新的生 物启发算法。该算法来源于对萤火虫群体行为的简化和模拟，是一种基于群体搜索的随机优 化算法。算法的核心即用萤火虫发出的绝对光强代表目标函数值，萤火虫的位置代表待解决 问题的解。目前萤火虫算法还没有完备的数学理论基础，但其概念较为简单，需要调整的参 数不多，易于编程实现，并且其本身没有复杂的数学操作，对计算机硬件的速度和存储要求 不高。作为一种新兴的优化算法已在诸多领域展现了良好的应用前景。专利ZL 201110257951.3公开了一种基于萤火虫算法的舰船路径规划新方法，该方法是一种二维航路 规划方法，目前还没有文献将萤火虫算法应用于三维航路规划。

发明内容

本发明的目的在于提供一种以提高算法规划速度和规划质量的基于改进萤火虫算法的 AUV三维航路规划方法。

本发明的目的是这样实现的：

（1）对水下潜器三维路径规划进行建模和萤火虫种群初始化：

（1.1）对航行空间抽象建模：

在水下潜器三维路径规划范围内建立全局坐标系Oxyz，以起点和终点建立航行空间 ABCD-EFGH，将X、Y、Z三个方向中跨度最大的方向进行等间隔划分，得到一组相互平行的平 面，平面的个数与航路点的个数相同，每一个平面包含一个待求路径点，将起点、每个平面 内的航路点及终点顺次连接起来得到一条路径；

（1.2）萤初始化火虫算法种群：

解空间P＝[p₁,p₂,...,p_n]∈R^m×n，其中n为萤火虫种群的规模，一只萤火虫代表一条路径， m为萤火虫解的维度，对应为路径的节点个数，令P中的每个向量p_i＝x_ji+y_jj+z_jk，其中 x_j,y_j,z_j∈R^m，将pi在X、Y、Z三个方向进行初始化，设Y向为等距分割方向，则pi在Y向 均匀取点，p_i在X、Z方向随机取点：

$\left(\begin{array}{c}{x}_i=\mathrm{rand},X_{\min }<\mathrm{rand}<X_{\max }\\ y_i=i\times \frac{L_{\mathrm{SD}}}{n-1}+y_s\\ z_i=\mathrm{rand},Z_{\min }<\mathrm{rand}<Z_{\max }\end{array},,\right)$

（2）计算目标函数值：

目标函数值由路径节点直线段长度和进入障碍物的惩罚值构成：

$\mathrm{Value}=\underset{i=1}{\overset{n-1}{\Sigma }}{S}_i+(k_1+k_2)L$

其中S_i代表第i个直线段长度，k₁和k₂分别是进入障碍物的节点数和中间节点数，

（3）计算自适应参数：

采用均衡度来衡量种群的分布：

$\mathrm{dis}\left(S\right)=\frac{1}{\mathrm{Dim}}·\underset{i=0}{\overset{\mathrm{Dim}}{\Sigma }}\sqrt{\underset{l=1}{\overset{n}{\Sigma }}{(\frac{\mathrm{PNum}}{N}-a_{\mathrm{il}})}^2}$

其中,Dim是每个萤火虫的维数，PNum为萤火虫数目，N是维变量取值域的等分数，a_ij是 在第l个等分区域中对应的第i维的维变量个数，其中，PNum≤N，

调整参数：计算均衡分布度dis(S)，令η＝dis(S)，以η调节参数α和γ：

$\left(\begin{array}{c}{\alpha }_i={\alpha }_b+\exp (-\eta )({\alpha }_e-{\alpha }_b)\\ {\gamma }_i={\gamma }_b+\exp (-\eta )({\gamma }_e-{\gamma }_b)\end{array}\right)$

其中α_b、γ_b表示参数的初始值，α_e、γ_e表示参数的最终值，

（4）比较萤火虫之间的亮度，更新萤火虫的位置：

根据萤火虫算法的运行机制，通过两两比较，及时更新每个萤火虫的解空间：

$\left(\begin{array}{c}p\left(i\right)=p\left(i\right)+k·{\beta }_{\mathrm{ij}}(p\left(i\right)-p\left(i\right))+2\alpha ·(\mathrm{rand}-0.5)\\ k=\mathrm{Light}\left(j\right)/\mathrm{Light}\left(i\right)\end{array}\right),$

其中p代表解向量，i代表待更新萤火虫，j代表相比待更新萤火虫较优的萤火虫；

（5）添加辅助规划算子：

（5.1）添加排除算子；

（5.2）添加变异算子；

（5.3）添加收缩算子；

（6）当满足算法迭代停止条件则输出最优路径，水下潜器三维路径规划结束，输出最后 一次迭代的最优路径。

本发明的有益效果在于：

本发明提出一种改进的萤火虫算法，相比基本萤火虫算法寻优精度更高，收敛速度更快， 同时该方法能够使算法适应不同定义域范围的优化问题。本发明提出一种基于改进萤火虫算 法的AUV三维路径规划方法。该方法相比传统路径搜索算法更加灵活，通过添加辅助规划算 子，可以实现了AUV三维路径的快速规划。

附图说明

图1为本发明提出的基于改进萤火虫算法的水下潜器三维路径规划流程图。

图2为本发明中采用的水下潜器三维路径规划环境模型分割图。

图3为本发明中三维路径规划基本方案示意图。

图4为本发明采用的改进萤火虫算法流程图。

图5为本发明中采用的排除算子方法示意图。

图6为本发明采用的收缩算子示意图。

具体实施方式

下面对本发明的基于改进萤火虫算法的水下潜器三维路径规划方法进行详细说明。

本发明公开了一种基于改进萤火虫优化算法的水下潜器三维路径规划方法，属于水下潜 器三维路径规划技术领域。发明中提出萤火虫算法参数自适应计算的方法，提高了萤火虫算 法本身的寻优能力，并使其可以在大范围的空间进行有效的搜索。同时，提出AUV三维路 径规划方案，把改进萤火虫算法应用到其中，并通过在规划中增加排除算子、变异算子和收 缩算子促进算法快速找到最优路径，提高了算法的规划速度和规划质量。步骤主要包括：三 维环境抽象建模和种群初始化、计算目标函数值、计算自适应参数、更新萤火虫位置、添加 辅助规划算子、输出规划结果。相比于传统路径规划算法，本专利提出的路径规划方法更加 高效灵活，可以根据不同的目标进行规划；同时，三维路径规划的实现相比于二维路径规划 更加具有实用性，能够更好的满足实际航行需要。

步骤一：三维环境抽象建模和种群初始化。根据起点和终点确定航行空间范围，对航行 空间进行等距分割，使路径在一个维度上的位置固定，其他维度上的位置随机分布。

步骤二：计算目标函数值。目标函数值由两部分组成，即路径长度和进入障碍物的惩罚 值。路径长度近似为节点之间的直线距离之和，进入障碍物的惩罚值由进入障碍物的节点数 和单位惩罚值的乘积表示。算法中，目标函数值由两部分组成，即路径长度和进入障碍物的 惩罚值。路径长度近似为节点之间的直线距离之和，进入障碍物的惩罚值由进入障碍物的节 点数和单位惩罚值的乘积表示。其中，为了避免节点中间段进入障碍物，在两节点中间再生 成若干子节点，利用子节点和路径节点共同观测路径进入障碍物的情况。

步骤三：计算自适应参数。通过均衡分布度来表达种群的敛散性，进而计算算法的相应 参数，使算法的收敛性和寻优精度显著提高，同时可以根据优化问题的定义域自动调节参数 量级，使算法能够适应广阔的路径规划三维环境。

步骤四：比较萤火虫之间的亮度，更新萤火虫的位置。按照萤火虫的位置更新公式和相 互吸引关系更新每一个萤火虫的位置。

步骤五：添加辅助规划算子。本发明中的路径规划方法添加排除算子、变异算子和收缩 算子。其中排除算子在优化过程中始终调用，以辅助算法跳出障碍物，快速找到无碰路径。 而变异算子和收缩算子的使用要根据种群的均衡分布度来判断，当均衡分布度大于一个定值 时，此时种群多样性较差，使用变异算子适当增加种群的多样性；另一方面，此时算法进入 小范围开发阶段，路径基本形成，调用收缩算子平滑路径，提高规划路径的质量。

步骤六：当满足算法迭代停止条件则输出路径，水下潜器三维路径规划结束。

本发明通过提高萤火虫算法本身的寻优能力，使其可以在大范围的空间进行有效的搜索。 同时，在规划过程中增加排除算子、变异算子和收缩算子，以提高算法规划速度和规划质量。

具体步骤如下：

步骤一：对水下潜器三维路径规划问题进行建模和萤火虫种群初始化。

步骤1.1航行空间抽象建模

在水下潜器三维路径规划范围内建立全局坐标系Oxyz，以起点和终点建立航行空间 ABCD-EFGH，如图2。将X、Y、Z三个方向中跨度最大的方向进行等间隔划分，得到一组相互 平行的平面，平面的个数与航路点的个数相同。每一个平面内包含一个待求路径点，将起点、 每个平面内的航路点及终点顺次连接起来即可得到一条路径。以在Y向跨度最大为例，则把 航行空间在Y向等距分割，则待求路径节点的在Y向坐标固定，X向、Z向坐标自动调整，算 法根据节点之间直线距离之和进行选择、优化，从而找到适合的路径，示意图如图3。

步骤1.2萤火虫算法种群初始化

设有算法的解空间P＝[p₁,p₂,...,p_n]∈R^m×n，其中n为萤火虫种群的规模，一只萤火虫代 表一条路径，m为萤火虫解的维度，对应为路径的节点个数。由于节点应包含三维位置信息， 因此令P中的每个向量p_i＝x_ji+y_jj+z_jk，其中x_j,y_j,z_j∈R^m。将pi在X、Y、Z三个方向进 行初始化。设Y向为等距分割方向，则p_i在Y向均匀取点，p_i在X、Z方向随机取点，具体 计算公式如下：

$\left(\begin{array}{c}{x}_i=\mathrm{rand},X_{\min }<\mathrm{rand}<X_{\max }\\ y_i=i\times \frac{L_{\mathrm{SD}}}{n-1}+y_s\\ z_i=\mathrm{rand},Z_{\min }<\mathrm{rand}<Z_{\max }\end{array}\right)$

步骤二：计算目标函数值。

算法的目标函数值由路径节点直线段长度和进入障碍物的惩罚值构成。公式如下：

$\mathrm{Value}=\underset{i=1}{\overset{n-1}{\Sigma }}{S}_i+(k_1+k_2)L$

其中S_i代表第i个直线段长度，k₁和k₂分别是进入障碍物的节点数和中间节点数。

本算法中的障碍物判断是通过计算路径节点到对应平面的距离来实现的。将环境数据在 经度和纬度上都从小到大进行排列，得到有序排列的数据网格，通过计算节点经纬度所在网 格的位置序号，迅速定位出节点对应的网格平面。计算节点到平面的垂线向量，判断向量的 方向即可知道节点是否在障碍物内。

步骤三：计算自适应参数。

为了达到较好的优化结果，萤火虫算法优化初期应具备大范围搜索的特点，要求算法能 够充分、大范围的搜索，而在算法优化后期应具备小范围开发的特点。通过分析可知，萤火 虫算法中的参数γ影响着算法的收敛，参数α影响着随机位移的步长。在算法优化初期，算 法处于大范围搜索阶段，α和γ的值应较大，以有利于快速搜索；算法优化后期，算法处于 小范围开发阶段，α和γ的值应该较小，以防止错过最优值。而基本萤火虫算法的光吸收系 数γ和随机位移常数α都是固定不变的，且α的范围总是设定为[0,1]，这样就导致了参数适 应性较差，不能满足算法不同阶段的优化需求，也不能满足不同定义域的优化问题。因此提 出可以在搜索过程中自适应调整的参数设置方法。

本专利中改进的萤火虫算法能够根据萤火虫种群分布情况自动调整光吸收系数γ和随机 常数α。种群分布情况需要通过一定的方法来衡量，传统方法是计算目标函数值的方差，该 方法可以在一定程度上反应种群的分布情况，但是在一些特殊情况下仍不能准确的反映。而 如果检测种群中每一维的位置情况则可以从根本上解决观测种群分布不准确的问题。本专利 采用均衡度来衡量种群的分布。均衡分布度的计算公式如下：

$\mathrm{dis}\left(S\right)=\frac{1}{\mathrm{Dim}}·\underset{i=0}{\overset{\mathrm{Dim}}{\Sigma }}\sqrt{\underset{l=1}{\overset{n}{\Sigma }}{(\frac{\mathrm{PNum}}{N}-a_{\mathrm{il}})}^2}$

其中,Dim是每个萤火虫的维数，PNum为萤火虫数目，N是维变量取值域的等分数。a_ij是 在第l个等分区域中对应的第i维的维变量个数。其中，PNum≤N。dis(S)的值越大，种群 越集中。dis(S)值越小，种群越分散。对应到萤火虫算法的位置更新，种群聚集则随机运动 的幅度应减小，光吸收系数应较小；种群分散则随机运动的幅度应较大，光吸收系数也应较 大。按照该原则可以得到参数的调整方法如下：首先计算均衡分布度dis(S)，令η＝dis(S)， 以η调节参数α和γ。参数计算公式为：

$\left(\begin{array}{c}{\alpha }_i={\alpha }_b+\exp (-\eta )({\alpha }_e-{\alpha }_b)\\ {\gamma }_i={\gamma }_b+\exp (-\eta )({\gamma }_e-{\gamma }_b)\end{array}\right)$

其中α_b、γ_b表示参数的初始值，α_e、γ_e表示参数的最终值。

步骤四：比较萤火虫之间的亮度，更新萤火虫的位置。

根据萤火虫算法的运行机制，通过两两比较，及时更新每个萤火虫的解空间，其位置更 新公式如下：

$\left(\begin{array}{c}p\left(i\right)=p\left(i\right)+k·{\beta }_{\mathrm{ij}}(p\left(i\right)-p\left(i\right))+2\alpha ·(\mathrm{rand}-0.5)\\ k=\mathrm{Light}\left(j\right)/\mathrm{Light}\left(i\right)\end{array}\right)$

其中p代表解向量，i代表待更新萤火虫，j代表相比待更新萤火虫较优的萤火虫。在三维路 径规划中，除了固定方向不需要更新，其他两个方向均需要按照上述公式进行位置更新。

步骤五：添加辅助规划算子。

为了加快算法规划进程，提高规划质量，使路径更加平滑可行，本发明在路径规划算法 中添加三种辅助规划算子。不同算子的加入条件不同，具体分以下两种情况：

a、排除算子在整个规划过程中都调用，用来减小算法规划的负担，加快算法收敛。

b、变异算子和收缩算子只有在算法进入小范围开发阶段才调用。算法是否进入小范围开 发阶段可以通过观测种群的聚集情况来判断，种群的聚集情况也通过计算种群的均衡分布度 来判断。当dis(S)大于某定值DIS时就调用收缩算子和变异算子，DIS可以通过实验选出最 适合的值。

（1）排除算子的添加方法

通过计算节点进入障碍物的深度向外排除节点。如图5所示，设A′_i对应的平面是平面ABC， 选出A、B、C三个顶点中最高的点。以B点为最高点为例，设B在Z方向的坐标值为H，A′_i的 Z方向坐标为h，则可以计算出A_i点的Z方向需要抬高的距离s_h，从而得到A_i新的Z方向 坐标计算公式如下：

$\left(\begin{array}{c}s\_h=H-h\\ {Z_A}_i=Z_{A_i^{\prime }}+s\_h\end{array}\right)$

如果排除后的节点的深度小于算法规定的最小水深，则应改变排除策略。通过步骤一的 建模已知路径的节点在一个方向的位置已经固定，因此可以以该向位置为基准，检索在该向 位置上其他方向范围内是否存在同样深度且不在障碍物内部的位置，选择距离节点最近的位 置，从而完成符合水深约束的障碍物排除。

（2）变异算子的添加

当算法优化进入小范围开发阶段时，容易陷入局部最优值中，算法处于停滞状态，因此 需要加入变异算子帮助算法跳出局部最优值，改善停滞现象。因此，当dis(S)>DIS时，随机 改变路径上一小部分连续的节点，暂时扰乱路径，以有机会获得更加优良的路径，公式如下：

$\left(\begin{array}{c}\mathrm{rand}\_n=\mathrm{rand}·(n-\mathrm{kd}-2)+1\\ P(\mathrm{rand}\_n:\mathrm{rand}\_n+\mathrm{kd})=P(\mathrm{rand}\_n:\mathrm{rand}\_n+\mathrm{kd})+\mathrm{range}·(\mathrm{rand}(1,\mathrm{kd}+1)-0.5\mathrm{ones}(1,\mathrm{kd}+1))\end{array}\right)$

其中，rand_n是变异节点的起始点，P(rand_n:rand_n+kd)代表变异节点的位置，range 是位置变异范围。

（3）收缩算子的添加

理论上来讲，路径越平滑长度越短，而对于随机搜索算法来说通过协调各点的位置达到 路径平滑要耗费大量的迭代次数，为了使路径快速的平滑，需要采用有针对性的平滑算子。 本专利借鉴弹性绳算法的思想，当dis(S)>DIS时，引入收缩算子，使路径趋于平滑。收缩算 子的示意图如图6。P′_i-1和P′_i+1为点P_i两侧的临近点。假设P′_i-1和P′_i+1固定，根据胡克定律P_i点 受相邻两个点的弹性拉力，其合力方向为在弹性拉力下，P_i点的理论最远到达 点为B_i，但由于受空气阻力的影响，P_i点的实际最远到达点为P′_i。P_i点的位移和实际到达点 的计算公式如下：

$\left(\begin{array}{c}{P}_i^{\prime }=P_i+\overrightarrow{S_{P_i}}\\ \overrightarrow{S_{P_i}}=k(\overrightarrow{P_i{P}_{i-1}^{\prime }}+\overrightarrow{P_i{P}_{i+1}^{\prime }})\end{array}\right)(i=\mathrm{2,3},...,n-1)$

其中，为点Pi的位移矢量，k∈(0,1)为阻力系数。

步骤六：当满足算法迭代停止条件则输出最优路径，水下潜器三维路径规划结束。 当算法达到停止条件则输出最后一次迭代的最优路径。