CN103760907A - 一种基于布谷鸟搜索算法的水下潜器三维路径规划方法 - Google Patents

Abstract

本发明提出一种基于布谷鸟搜索算法的水下潜器三维路径规划方法，属于水下潜器三维路径规划技术领域，具体包括：对水下潜器三维空间路径规划问题进行建模、初始化布谷鸟搜索算法、更新鸟窝位置、选出全局最优位置并判断终止条件、输出最优路径五个基本步骤。本发明基于布谷鸟搜索算法，提出一种基于布谷鸟搜索算法的水下潜器三维路径规划方法，很好地利用了布谷鸟搜索算法的简单高效性及全局搜索能力。相对于传统的水下潜器路径规划方法来说具有更好的智能性和适应性，相对于其他智能优化算法而言具有更好的灵活性、更易实现；路径规划过程在三维环境中进行，比二维环境下规划出的路径更具实用性，能够更好地满足水下潜器航行的需要。

Description

一种基于布谷鸟搜索算法的水下潜器三维路径规划方法

技术领域

本发明属于水下潜器三维路径规划技术领域，具体涉及一种基于布谷鸟搜索算法的水下潜器三维路径规划方法。

背景技术

水下潜器三维路径规划是实现水下潜器自动化、智能控制的关键技术之一。水下潜器三维路径规划是给定一个运动体和一个关于环境的描述，当环境建模完成之后，水下潜器路径规划任务需满足在安全航行区域内，按一定的优化准则（如路径最短、耗时最少等）搜索一条从指定起点到目标点的最优路径（或次优路径）。由于水下潜器工作在大范围开放的三维空间环境中，这为路径规划方法的求解带来很大的困难。传统的水下潜器路径规划方法在解决复杂和非线性化的路径规划问题中常常缺乏鲁棒性和自适应性，计算过于复杂，对不确定性因素很难做出正确响应，在面对复杂多变的规划环境时往往显得无能为力。而新兴的智能优化算法，尤其是启发式智能优化算法，如粒子群算法、蚁群算法、萤火虫算法等，具有更好的智能性和适应性。如申请号为CN200910100613.1的专利公开了一种基于粒子群优化算法的移动机器人路径规划方法，专利ZL201110257951.3公开了一种基于萤火虫算法的舰船路径规划新方法，申请号为CN201210487442的专利公开了一种用于AUV实时路径规划的免疫遗传算法。上述三个专利的路径规划过程均在二维环境下进行，而水下潜器的航行空间是三维环境，因此二维环境中规划出的航路缺少适应性和实用性。另外，相对于布谷鸟搜索算法而言，免疫遗传算法计算较为复杂，灵活性较差。

布谷鸟搜索算法是在2009年被英国剑桥大学学者Yang Xin-she和Deb Suash提出的（Cuckoo search via Levy.2009）。该算法基于布谷鸟的寻窝产卵行为，并结合了一些鸟类的Levy飞行行为，通过利用一些标准测试函数和随机测试函数进行大范围的对比试验，结果表明由布谷鸟搜索算法获得的最优解远远优于由蜂群算法和遗传算法获得的最优解。布谷鸟搜索算法简单易行，参数少，在解决特殊问题时无需重新匹配大量参数，因此，相对于其他启发式智能优化算法而言，布谷鸟搜索算法在解决许多最优化问题时更胜一筹。现有文献对布谷鸟搜索算法的研究处于初级阶段，西安工程大学的王凡等对布谷鸟搜索算法理论进行了探究（基于CS算法的Markov模型及收敛性分析.2012），建立了布谷鸟搜索算法的Markov链模型，分析该Markov链的有限齐次性，在此基础上分析鸟窝位置的群体状态转移过程，指出随机序列将进入最优状态集，同时证明了布谷鸟搜索算法满足随机搜索算法全局收敛的2个条件，通过仿真模拟验证了该算法的收敛性，对布谷鸟搜索算法的改进及实际应用提供了理论依据。英国斯旺西大学学者S.Walton，O.Hassan，K.Morgan，M.R.Brown对布谷鸟搜索算法进行了改进（Modified cuckoo search：A new gradient free optimization algorithm.2011），首先将原始的布谷鸟搜索算法中的步长控制量设置为随迭代数增加而减小的变量，促使了更多的局部搜索结果更加靠近最优解；其次在鸟蛋之间加入了信息交换，加快了最优化的收敛速度。但关于布谷鸟搜索算法应用方面的研究仍然缺乏，其中，Yang Xin-She和DebSuash曾将该算法应用于简单的工程设计实践中（Engineering optimisation by cuckoosearch.2010）。目前尚无文献将布谷鸟搜索算法应用于水下潜器三维路径规划中。

发明内容

本发明的目的在于提供一种具有更好适用性的基于布谷鸟搜索算法的水下潜器三维路径规划方法。

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

步骤一：对水下潜器三维空间路径规划问题进行建模，对水下潜器三维空间路径规划的环境建模，确定评价路径的适应度函数；

在水下潜器三维路径规划范围内建立全局坐标系Oxyz，其中O(x_O,y_O,z_O)表示水下潜器的出发点，P(x_P,y_P,z_P)为目标点，障碍物用包括其自身的最小外接球体代替，表示为其中Q_j表示第j个障碍物的球心位置，r_j表示第j个障碍物的半径，在Ox轴方向取长度|AD|，其中|AD|的长度为目标点P到y轴的垂直距离，在O_y轴方向取长度|AB|，|AB|的长度为目标点P到x轴的垂直距离，在Oz轴方向取长度|AE|，|AE|的长度为目标点P到z轴的垂直距离，构造一个立方体区域ABCD-EF-GH，该立方体区域即为水下潜器路径规划空间；

做过球心Q_i且垂直于x轴的平面Ψ，并在其左右两侧按步长λ做平行于平面Ψ的平面，寻找一条从起始点O到目标点P的无碰撞路径，即寻找一个路径点的集合P＝{O,P₁,P₂,...,P_i,...,P},i＝1,2,...,m，其中m为路径点的个数，相邻点连接的路径与障碍物无碰撞，同时使从起始点到目标点的路径长度为最短；

水下潜器从起始点O(x_O,y_O,z_O)到目标点P(x_P,y_P,z_P)之间的路径就由其间的各个分层上的路径点连接组成，水下潜器从点O出发，到达平面Π₁，找到第一个路径点P₁(x₁,y₁,z₁)，依次找到平面Π_i上的点P_i(x_i,y_i,z_i)，将这些点连接形成一条从起始点O(x_O,y_O,z_O)到目标点P(x_P,y_P,z_P)的路径，

采用路径长度作为评价路径的适应度函数，路径长度为：

$D=D_{{\mathrm{OP}}_1}+\underset{i=1}{\overset{m-1}{\mathrm{\Σ}}}{D}_{P_i{P}_{i+1}}+D_{P_{m}p}$

其中， $D_{P_i{P}_{i+1}}=\sqrt{{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_i)}^2},$ 表示点P_i和点P_i+1之间的距离，将点O作为点P₀，点P作为点P_m+1，则适应度函数为：

$D=\underset{i=0}{\overset{m}{\mathrm{\Σ}}}\sqrt{{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_i)}^2};$

步骤二：初始化布谷鸟搜索算法；

（1）初始化布谷鸟搜索算法的参数，具体包括种群大小n、布谷鸟的鸟蛋被巢主鸟发现的概率R_a，其中R_a∈[0,1]，R_a＝0.25；

（2）初始化鸟窝的位置，在搜索空间内产生n个鸟窝位置

每个鸟窝位置代表一条备选路径，对每个鸟窝进行检验，查看是否遇障：

$\frac{\left|\mathrm{\&xi;}\right|}{\left|{\mathrm{\&theta;}}_{i+1}\right|}<r_j,j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,k$

其中： $\mathrm{\&xi;}=\left(\left|\begin{array}{cc}{y}_j^{\&prime;}-y_{i+1}& z_j^{\&prime;}-z_{i+1}\\ y_{i+1}-y_i& z_{i+1}-z_i\end{array}\right|\left|\begin{array}{cc}{z}_j^{\&prime;}-z_{i+1}& x_j^{\&prime;}-x_{i+1}\\ z_{i+1}-z_i& x_{i+1}-x_i\end{array}\right|\left|\begin{array}{cc}{x}_j^{\&prime;}-x_{i+1}& y_j^{\&prime;}-y_{i+1}\\ x_{i+1}-x_i& y_{i+1}-y_i\end{array}\right|\right),$

θ_i＝[x_i-x_i-1y_i-y_i-1z_i-z_i-1]为第i段路径的向量，(x'_j,y'_j,z'_j)为障碍物的中心坐标，r_j表示第j个障碍物的半径，k表示障碍物的个数，若鸟窝位置不满足上述条件，该鸟窝代表的路径含有经过障碍物的路径段，则舍弃该鸟窝，并补充新的鸟窝到鸟窝种群中，直到所有鸟窝均不经过障碍物区域，找出初始全局最优位置

并保留到下一代；

步骤三：更新鸟窝位置：

步骤3.1利用Levy飞行过程更新鸟窝位置：

（1）初始化布谷鸟搜索算法的参数：具体包括种群大小n、布谷鸟的鸟蛋被巢主鸟发现的概率R_a，其中R_a∈[0,1]，R_a＝0.25；

（2）初始化鸟窝的位置：在搜索空间内随机产生n个鸟窝位置

每个鸟窝位置代表一条备选路径，对每个鸟窝进行检验，查看其是否遇障：

$\frac{\left|\mathrm{\&xi;}\right|}{\left|{\mathrm{\&theta;}}_{i+1}\right|}<r_j,j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,k$

其中： $\mathrm{\&xi;}=\left(\left|\begin{array}{cc}{y}_j^{\&prime;}-y_{i+1}& z_j^{\&prime;}-z_{i+1}\\ y_{i+1}-y_i& z_{i+1}-z_i\end{array}\right|\left|\begin{array}{cc}{z}_j^{\&prime;}-z_{i+1}& x_j^{\&prime;}-x_{i+1}\\ z_{i+1}-z_i& x_{i+1}-x_i\end{array}\right|\left|\begin{array}{cc}{x}_j^{\&prime;}-x_{i+1}& y_j^{\&prime;}-y_{i+1}\\ x_{i+1}-x_i& y_{i+1}-y_i\end{array}\right|\right),$

θ_i＝[x_i-x_i-1y_i-y_i-1z_i-z_i-1]为第i段路径的向量，(x'_j,y'_j,z'_j)为障碍物的中心坐标，r_j表示第j个障碍物的半径，k表示障碍物的个数，鸟窝位置不满足上述条件，说明该鸟窝代表的路径含有经过障碍物的路径段，则舍弃该鸟窝，并补充新的鸟窝到鸟窝种群中，直到所有鸟窝均不经过障碍物区域，找出初始全局最优位置

并保留到下一代；

步骤3.2根据被发现概率更新鸟窝位置：

产生服从均匀分布的随机数u∈（0,1），与布谷鸟的鸟蛋被巢主鸟发现的概率R_a=0.25对比，保留K_t中被发现概率小于ε的鸟窝位置，而对被发现概率大于λ的鸟窝位置进行改变，得到一组新的鸟窝位置，再对这组新的鸟窝位置进行测试，与上一步中得到的K_t中的每个鸟窝位置的测试值进行比较，用测试值大的鸟窝位置代替测试值小的鸟窝位置，得到一组鸟窝位置 $S_t={(s_1^{\left(t\right)},s_2^{\left(t\right)},\&CenterDot;\&CenterDot;\&CenterDot;,s_n^{\left(t\right)})}^T;$

步骤四：选出全局最优鸟窝位置，判断是否满足终止条件：

利用适应度函数测试步骤三中最后得到的一组鸟窝位置S_t，选出最后得到的S_t中最优的鸟窝位置

并判断是否满足最大迭代次数，若满足，则终止循环，否则返回步骤三继续进行迭代更新，直到搜索满足终止条件为止，最终得到的最优鸟窝位置

即为需要寻找的最优路径；

步骤五：输出步骤四中得到的全局最优路径，水下潜器三维空间路径规划结束。

本发明的有益效果在于：

第一，本发明提出一种基于布谷鸟搜索算法的水下潜器三维路径规划方法，采用布谷鸟搜索算法，比经典的水下潜器路径规划方法更加灵活、高效，比其他智能水下潜器路径规划方法如粒子群算法等更加简单、易行，具有更好的适用性。

第二，本发明提出一种基于布谷鸟搜索算法的水下潜器三维路径规划方法，水下潜器路径规划过程在三维环境中进行，比二维环境中规划出的路径更具实用性，能够为水下潜器提供更加合理的最优路径，更好的满足水下潜器路径规划的实际需要。

附图说明

图1为本发明采用的布谷鸟搜索算法流程图。

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

图3为本发明中采用的水下潜器三维路径选择示意图。

图4为本发明提出的基于布谷鸟搜索算法的水下潜器三维路径规划流程图。

具体实施方式

下面结合附图对本发明进行详细描述。

本发明公开了一种基于布谷鸟搜索算法的水下潜器三维路径规划方法，主要包括以下步骤：

步骤一：对水下潜器三维路径规划问题进行建模。对水下潜器三维路径规划的环境建模，确定评价路径的适应度函数。

步骤二：初始化布谷鸟搜索算法。初始化算法参数和鸟窝位置，对空间模型进行分割，检查鸟窝位置是否遇障，选出初始全局最优位置。

步骤三：更新鸟窝位置。首先利用Levy飞行过程更新鸟窝位置，对比步骤二中得到的鸟窝位置，取较优位置；然后抛弃被发现概率较大的鸟窝位置，与经过Levy飞行过程更新后的鸟窝位置对比，取更优位置。

步骤四：选出全局最优鸟窝位置，判断是否满足终止条件。

步骤五：输出全局最优路径，水下潜器三维路径规划结束。

其中的具体内容包括：

步骤一：对水下潜器三维路径规划问题进行建模。

在水下潜器三维路径规划范围内建立全局坐标系Oxyz，其中O(x_O,y_O,z_O)表示水下潜器的出发点，P(x_P,y_P,z_P)为目标点，障碍物用包括其自身的最小外接球体代替，表示为其中Q_j表示第j个障碍物的球心位置，r_j表示第j个障碍物的半径，这样做的好处是可以使水下环境更加近似于水下潜器的工作环境。如图2所示，在Ox轴方向取长度|AD|，其中|AD|的长度为目标点P到y轴的垂直距离，在Oy轴方向取长度|AB|，|AB|的长度为目标点P到x轴的垂直距离，在Oz轴方向取长度|AE|，|AE|的长度为目标点P到z轴的垂直距离，构造一个立方体区域ABCD-EF-GH，该立方体区域即为水下潜器路径规划空间。

为了利用上述水下潜器路径规划空间进行路径规划，需要对其进一步分割。做过球心Q_i且垂直于x轴的平面，并在其左右两侧按一定的步长λ做平行于该平面的平面，其中步长λ可以根据球的半径大小进行调整。这样就将三维环境空间划分为许多垂直于x轴的平面，水下潜器路径规划的目的就是寻找一条从起始点O到目标点P的无碰撞路径，即寻找一个路径点的集合P＝{O,P₁,P₂,...,P_i,...,P},i＝1,2,...,m，其中m为路径点的个数，相邻点连接的路径与障碍物无碰撞，同时使从起始点到目标点的路径长度为最短。

这样，水下潜器从起始点O(x_O,y_O,z_O)到目标点P(x_P,y_P,z_P)之间的路径就由其间的各个分层上的路径点连接组成。水下潜器从点O出发，到达平面Π₁，找到第一个路径点P₁(x₁,_y1,z₁)，类似地依次找到平面Π_i上的点P_i(x_i,y_i,z_i)，将这些点连接便形成了一条从起始点O(x_O,y_O,z_O)到目标点P(x_P,y_P,z_P)的路径，如图3所示。

本专利采用路径长度作为评价路径的适应度函数，路径长度的计算方法如下：

$D=D_{{\mathrm{OP}}_1}+\underset{i=1}{\overset{m-1}{\mathrm{\&Sigma;}}}{D}_{P_i{P}_{i+1}}+D_{P_{m}p}$

其中， $D_{P_i{P}_{i+1}}=\sqrt{{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_i)}^2},$ 表示点P_i和点P_i+1之间的距离，若将点O作为点P₀，点P作为点P_m+1，则适应度函数可以表示为：

$D=\underset{i=0}{\overset{m}{\mathrm{\&Sigma;}}}\sqrt{{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_i)}^2};$

步骤二：初始化布谷鸟搜索算法。

布谷鸟搜索算法的初始化包括以下两部分内容：

（1）初始化布谷鸟搜索算法的参数。具体包括种群大小n、布谷鸟的鸟蛋被巢主鸟发现的概率R_a，其中R_a∈[0,1]。R_a的大小需要根据待解决的问题来确定，对于大多数优化问题而言，R_a的取值范围为0.15到，在本专利中采用R_a＝0.25。

（2）初始化鸟窝的位置。在搜索空间内随机产生n个鸟窝位置

每个鸟窝位置代表一条备选路径。对每个鸟窝进行检验，查看其是否遇障，检验规则如下：

$\frac{\left|\mathrm{\&xi;}\right|}{\left|{\mathrm{\&theta;}}_{i+1}\right|}<r_j,j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,k$

其中： $\mathrm{\&xi;}=\left(\left|\begin{array}{cc}{y}_j^{\&prime;}-y_{i+1}& z_j^{\&prime;}-z_{i+1}\\ y_{i+1}-y_i& z_{i+1}-z_i\end{array}\right|\left|\begin{array}{cc}{z}_j^{\&prime;}-z_{i+1}& x_j^{\&prime;}-x_{i+1}\\ z_{i+1}-z_i& x_{i+1}-x_i\end{array}\right|\left|\begin{array}{cc}{x}_j^{\&prime;}-x_{i+1}& y_j^{\&prime;}-y_{i+1}\\ x_{i+1}-x_i& y_{i+1}-y_i\end{array}\right|\right),$

θ_i＝[x_i-x_i-1y_i-y_i-1z_i-z_i-1]为第i段路径的向量，(x'_j,y'_j,z'_j)为障碍物的中心坐标，r_j表示第j个障碍物的半径，k表示障碍物的个数。若鸟窝位置不满足上述条件，说明该鸟窝代表的路径含有经过障碍物的路径段，则舍弃该鸟窝，并补充新的鸟窝到鸟窝种群中，直到所有鸟窝均不经过障碍物区域。找出初始全局最优位置

并保留到下一代。

步骤三：更新鸟窝位置。

步骤3.1利用Levy飞行过程更新鸟窝位置。

保留上代鸟窝的最优位置

其中t为正整数，并利用下式对其他的鸟窝位置进行更新，得到一组新的鸟窝位置：

$s_i^{(t+1)}=s_i^{\left(t\right)}+\mathrm{\&alpha;}\&CirclePlus;\mathrm{Levy}\left(\mathrm{\&lambda;}\right),i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,n$

其中，

表示第i个鸟窝在第t代的鸟窝位置；

为点对点乘法；α表示步长控制量，本专利中取α＝ο(1)；Levy(λ)为Levy随机搜索路径，且有Levy～u＝t^-λ,(1＜λ≤3)，它随平均值的无限变化而无限变化。

对这组新的鸟窝位置进行适应度函数测试，与上代产生的一组鸟窝位置

进行对比，用测试值较好的鸟窝位置代替测试值较差的鸟窝位置，即保留较好的鸟窝位置，抛弃较差的鸟窝位置，并通过Levy飞行过程建立一组全新的鸟窝，从而得到一组较好的鸟窝位置这种更新鸟窝位置的方法相当于遗传算法中的精英主义，它确保了最好的鸟窝位置被保留到下一代，而且不会有被逐出种群的危险。

步骤3.2根据被发现概率更新鸟窝位置。

一个巢主鸟能够发现外来鸟蛋的概率是R_a，若果外来鸟蛋被发现，巢主鸟可将该鸟蛋丢弃，或者干脆抛弃这个鸟窝，并在其它位置建立一个全新的鸟窝。这个过程的实现方式如下：产生服从均匀分布的随机数u∈（0,1），与布谷鸟的鸟蛋被巢主鸟发现的概率R_a=0.25对比，保留K_t中被发现概率较小的鸟窝位置，而对被发现概率较大的鸟窝位置进行随机改变，从而得到一组新的鸟窝位置。再对这组新的鸟窝位置进行测试，与上一步中得到的K_t中的每个鸟窝位置的测试值进行比较，用测试值较好的鸟窝位置代替测试值较差的鸟窝位置，从而得到一组更好的鸟窝位置 $S_t={(s_1^{\left(t\right)},s_2^{\left(t\right)},\&CenterDot;\&CenterDot;\&CenterDot;,s_n^{\left(t\right)})}^T.$

步骤四：选出全局最优鸟窝位置，判断是否满足终止条件。

利用适应度函数测试步骤三中最后得到的一组鸟窝位置S_t，选出最后得到的S_t中最优的鸟窝位置

并判断是否满足最大迭代次数，若满足，则终止循环，反之，则返回步骤三继续进行迭代更新，直到搜索满足终止条件为止。最终得到的最优鸟窝位置

即为需要寻找的最优路径。

步骤五：输出最优路径，水下潜器三维路径规划结束。输出步骤四中得到的最优路径，水下潜器三维路径规划过程结束。

Claims (1)

1.一种基于布谷鸟搜索算法的水下潜器三维空间路径规划方法，其特征在于，包括以下步骤：

步骤一：对水下潜器三维空间路径规划问题进行建模，对水下潜器三维空间路径规划的环境建模，确定评价路径的适应度函数；

在水下潜器三维路径规划范围内建立全局坐标系Oxyz，其中O(x_O,y_O,z_O)表示水下潜器的出发点，P(x_P,y_P,z_P)为目标点，障碍物用包括其自身的最小外接球体代替，表示为

其中Q_j表示第j个障碍物的球心位置，r_j表示第j个障碍物的半径，在Ox轴方向取长度|AD|，其中|AD|的长度为目标点P到y轴的垂直距离，在O_y轴方向取长度|AB|，|AB|的长度为目标点P到x轴的垂直距离，在Oz轴方向取长度|AE|，|AE|的长度为目标点P到z轴的垂直距离，构造一个立方体区域ABCD-EF-GH，该立方体区域即为水下潜器路径规划空间；

做过球心Q_i且垂直于x轴的平面Ψ，并在其左右两侧按步长λ做平行于平面Ψ的平面，寻找一条从起始点O到目标点P的无碰撞路径，即寻找一个路径点的集合P＝{O,P₁,P₂,...,P_i,...,P},i＝1,2,...,m，其中m为路径点的个数，相邻点连接的路径与障碍物无碰撞，同时使从起始点到目标点的路径长度为最短；

水下潜器从起始点O(x_O,y_O,z_O)到目标点P(x_P,y_P,z_P)之间的路径就由其间的各个分层上的路径点连接组成，水下潜器从点O出发，到达平面Π₁，找到第一个路径点P₁(x₁,y₁,z₁)，依次找到平面Π_i上的点P_i(x_i,y_i,z_i)，将这些点连接形成一条从起始点O(x_O,y_O,z_O)到目标点P(x_P,y_P,z_P)的路径，

采用路径长度作为评价路径的适应度函数，路径长度为：

$D=D_{{\mathrm{OP}}_1}+\underset{i=1}{\overset{m-1}{\mathrm{\&Sigma;}}}{D}_{P_i{P}_{i+1}}+D_{P_{m}p}$

其中， $D_{P_i{P}_{i+1}}=\sqrt{{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_i)}^2},$ 表示点P_i和点P_i+1之间的距离，将点O作为点P₀，点P作为点P_m+1，则适应度函数为：

$D=\underset{i=0}{\overset{m}{\mathrm{\&Sigma;}}}\sqrt{{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_i)}^2};$

步骤二：初始化布谷鸟搜索算法；

（1）初始化布谷鸟搜索算法的参数，具体包括种群大小n、布谷鸟的鸟蛋被巢主鸟发现的概率R_a，其中R_a∈[0,1]，R_a＝0.25；

（2）初始化鸟窝的位置，在搜索空间内产生n个鸟窝位置

每个鸟窝位置代表一条备选路径，对每个鸟窝进行检验，查看是否遇障：

$\frac{\left|\mathrm{\&xi;}\right|}{\left|{\mathrm{\&theta;}}_{i+1}\right|}<r_j,j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,k$

其中： $\mathrm{\&xi;}=\left(\left|\begin{array}{cc}{y}_j^{\&prime;}-y_{i+1}& z_j^{\&prime;}-z_{i+1}\\ y_{i+1}-y_i& z_{i+1}-z_i\end{array}\right|\left|\begin{array}{cc}{z}_j^{\&prime;}-z_{i+1}& x_j^{\&prime;}-x_{i+1}\\ z_{i+1}-z_i& x_{i+1}-x_i\end{array}\right|\left|\begin{array}{cc}{x}_j^{\&prime;}-x_{i+1}& y_j^{\&prime;}-y_{i+1}\\ x_{i+1}-x_i& y_{i+1}-y_i\end{array}\right|\right),$

θ_i＝[x_i-x_i-1y_i-y_i-1z_i-z_i-1]为第i段路径的向量，(x'_j,y'_j,z'_j)为障碍物的中心坐标，r_j表示第j个障碍物的半径，k表示障碍物的个数，若鸟窝位置不满足上述条件，该鸟窝代表的路径含有经过障碍物的路径段，则舍弃该鸟窝，并补充新的鸟窝到鸟窝种群中，直到所有鸟窝均不经过障碍物区域，找出初始全局最优位置

并保留到下一代；

步骤三：更新鸟窝位置：

步骤3.1利用Levy飞行过程更新鸟窝位置：

（1）初始化布谷鸟搜索算法的参数：具体包括种群大小n、布谷鸟的鸟蛋被巢主鸟发现的概率R_a，其中R_a∈[0,1]，R_a＝0.25；

（2）初始化鸟窝的位置：在搜索空间内随机产生n个鸟窝位置

每个鸟窝位置代表一条备选路径，对每个鸟窝进行检验，查看其是否遇障：

$\frac{\left|\mathrm{\&xi;}\right|}{\left|{\mathrm{\&theta;}}_{i+1}\right|}<r_j,j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,k$

其中： $\mathrm{\&xi;}=\left(\left|\begin{array}{cc}{y}_j^{\&prime;}-y_{i+1}& z_j^{\&prime;}-z_{i+1}\\ y_{i+1}-y_i& z_{i+1}-z_i\end{array}\right|\left|\begin{array}{cc}{z}_j^{\&prime;}-z_{i+1}& x_j^{\&prime;}-x_{i+1}\\ z_{i+1}-z_i& x_{i+1}-x_i\end{array}\right|\left|\begin{array}{cc}{x}_j^{\&prime;}-x_{i+1}& y_j^{\&prime;}-y_{i+1}\\ x_{i+1}-x_i& y_{i+1}-y_i\end{array}\right|\right),$

θ_i＝[x_i-x_i-1y_i-y_i-1z_i-z_i-1]为第i段路径的向量，(x'_j,y'_j,z'_j)为障碍物的中心坐标，r_j表示第j个障碍物的半径，k表示障碍物的个数，鸟窝位置不满足上述条件，说明该鸟窝代表的路径含有经过障碍物的路径段，则舍弃该鸟窝，并补充新的鸟窝到鸟窝种群中，直到所有鸟窝均不经过障碍物区域，找出初始全局最优位置

并保留到下一代；

步骤3.2根据被发现概率更新鸟窝位置：

产生服从均匀分布的随机数u∈（0,1），与布谷鸟的鸟蛋被巢主鸟发现的概率R_a=0.25对比，保留K_t中被发现概率小于ε的鸟窝位置，而对被发现概率大于λ的鸟窝位置进行改变，得到一组新的鸟窝位置，再对这组新的鸟窝位置进行测试，与上一步中得到的K_t中的每个鸟窝位置的测试值进行比较，用测试值大的鸟窝位置代替测试值小的鸟窝位置，得到一组鸟窝位置 $S_t={(s_1^{\left(t\right)},s_2^{\left(t\right)},\&CenterDot;\&CenterDot;\&CenterDot;,s_n^{\left(t\right)})}^T;$

步骤四：选出全局最优鸟窝位置，判断是否满足终止条件：

利用适应度函数测试步骤三中最后得到的一组鸟窝位置S_t，选出最后得到的S_t中最优的鸟窝位置

并判断是否满足最大迭代次数，若满足，则终止循环，否则返回步骤三继续进行迭代更新，直到搜索满足终止条件为止，最终得到的最优鸟窝位置

即为需要寻找的最优路径；

步骤五：输出步骤四中得到的全局最优路径，水下潜器三维空间路径规划结束。

Images