CN103744428A - 一种基于邻域智能水滴算法的水面无人艇路径规划方法 - Google Patents

Abstract

本发明涉及水面无人艇路径规划技术领域，具体涉及一种基于邻域智能水滴算法的水面无人艇路径规划方法。本发明包括：（1）对水面无人艇路径规划进行环境建模；（2）利用智能水滴算法根据已知水面无人艇作业区域静态障碍物、行进目标以及路径评价函数在作业区域栅格阵中进行离线全局路径规划，得到全局离线最优路径。本发明针对基本IWD方法存在的易陷入局部最优解导致方法停滞及收敛速度较慢的问题进行了改进，在基本IWD方法基础上引入了最优解邻域扩张机制及全局最优强调机制，得到NIWD方法，可以避免方法陷入局部最优导致早熟，提高了方法寻优的收敛速度。

Description

一种基于邻域智能水滴算法的水面无人艇路径规划方法

技术领域

本发明涉及水面无人艇路径规划技术领域，具体涉及一种基于邻域智能水滴算法的水面无人艇路径规划方法。

背景技术

随着作战方式的变革，无人作战系统是现代武器装备的发展趋势之一，水面无人艇（USV）可承担情报收集、海上侦察监视、精确打击、搜捕、水文地理勘察、反恐、海洋封锁支持等功能，因此发展迅速。自主航线规划是实现USV自治航行的重要环节。

USV路径规划代表性方法主要有人工势场法，遗传算法，人工神经网络，快速随机搜索树算法（RRT）。使用人工势场法只能解决局部空间的避障问题,它缺乏全局信息，很容易陷入局部最小值，导致产生振荡或停滞不前的路径；遗传算法对新空间的探索能力较差，较难构造合适的适应度函数，且较易收敛到局部最优解；使用人工神经网络较难获得具有代表性的训练样本，在USV复杂的作业空间内不易实现；RRT算法的随机性导致结果只能概率完备，在USV动态作业空间该算法缺乏稳定性且易出现偏离最优路径。

受自然启发兴起的智能算法最近几年发展迅猛，研究者开始利用某些智能算法解决无人系统路径规划问题。智能水滴算法（Intelligent Water Drops，IWD）首次由Shah-Hosseini于2007年提出^[12]。IWD算法模拟了自然界当中河流与河床相互作用的动态过程，即河流总可以根据周围河床环境选择一条最优的路径流向湖泊或海洋。由于IWD算法具有正反馈机制和较强的鲁棒性，被成功应用于无功优化调度问题，灰度图像处理，分布式网络服务攻击抵抗等工程问题。尽管IWD算法在各领域取得广泛的应用，但是IWD算法仍然存在易陷入局部最优解导致算法早熟并且收敛速度较慢的问题。将其利用在USV路径规划问题时需要针对这些问题做进一步改进。

发明内容

本发明目的是提供一种有效的、快速的基于邻域智能水滴算法的水面无人艇路径规划方法。

（1）对水面无人艇路径规划进行环境建模：

（1.1）对水面无人艇路径规划的作业区域建立对应的栅格工作区，作为智能水滴搜索最优路径的搜索区域；

在二维平面中进行路径规划，S为起始点，T为目的地，在水面无人艇作业区域内建立全局直角S-XY，其中原点为S，以方向为X轴正向，以垂直于方向为Y轴；

对水面无人艇作业区域进行栅格化，得到作业区域栅格阵，以S为栅格阵起点，栅格阵方向与坐标系S-XY方向一致，基准栅格尺寸为l＝v·Δt，其中v为水面无人艇预期运行速度大小，Δt为水面无人艇实时运动规划周期，任何一个栅格中心点都可以用栅格坐标(r_i,c_i)唯一标识，其中(r_i，c_i)=(x_i/l，y_i/l)，(x_i，y_i)为该栅格中心点在坐标系S-XY中的位置坐标；

将水面无人艇作业区域内的静态障碍物所覆盖的栅格标志为1，表示障碍栅格，不满一个栅格的按照一个栅格处理，其余栅格标志为0，表示自由栅格，将每一个障碍栅格中心点坐标置于障碍点集合V_obstacle{ }中；

在作业区域栅格阵中进行路径规划，从S到T的候选路径可以表示为：path＝{S,p₁,p₂,…,T}，其中路径点p_i为栅格节点坐标，其在坐标坐标系S-XY坐标为(r_i,c_i)，S与T坐标分别为(0,0)及(r_T,0)；

（1.2）确定评价智能水滴搜索得到的路径的代价函数：

f(path)＝α·dist(path)+β·smooth(path)

其中α、β为权值，值大小表示对相应的代价子函数的重视程度；

为路径长度代价子函数，d(p_i,p_i+1)为连接路径点p_i及p_i+1的路径长度，n为路径点数目；

为路径平滑度代价子函数，

为连接第i个路径点的两条路径段矢量之间的偏转角度，δ为调节系数；

（2）利用智能水滴算法根据已知水面无人艇作业区域静态障碍物、行进目标以及路径评价函数在作业区域栅格阵中进行离线全局路径规划，得到全局离线最优路径path_TBest：

（2.1）初始化静态参数：水滴数量N，任意两个栅格节点间的初始含沙量InitGSoil、迭代次数r＝0，全局最优路径path_Best，全局最优路径代价函数值f(path_TBest)以及最大迭代次数r_max；

（2.2）初始化动态参数：每滴水滴初始含沙量InitDSoil，水滴初始速度InitVel，第r代最优路径代价函数值f(path_IBest)以及第r代最优路径path_IBest；

（2.3）将所有水滴都置于作业区域中栅格阵的起点S位置；对所有水滴重复执行步骤(2.4)—(2.8)直至所有水滴都行进至栅格阵横坐标为r_T-1，则第r次迭代结束，执行步骤(2.9)；

（2.4）根据当前水滴h，即第h个水滴所处栅格节点的邻路径情况选择前进路径：

计算水滴h在作业区域栅格当前节点p_i＝(r_i,c_i)位置时选择下一所有可能节点p_i+1＝(r_i+1,c_w)的概率，(r_i+1,c_w)表示栅格阵中横坐标为r_i+1且不属于障碍点集合V_obstacle{ }的节点，即处于p_i节点的水滴h的所有的下一个可能的节点；

（2.5）水滴h由p_i节点到达p_i+1节点后，更新水滴的速度vel^IWD(t)；

（2.6）水滴h由p_i节点到达p_i+1节点后，计算水滴h所经过的路径含沙量增量；

（2.7）水滴h由p_i节点到达p_i+1节点后，更新p_i节点到达p_i+1节点间路径含沙量；

（2.8）水滴h由p_i节点到达p_i+1节点后，更新水滴含沙量；

（2.9）计算当前迭代每个水滴h得到的路径的代价函数f(path_h)，计算第r代最优路径代价函数值

最小路径代价对应的水滴经过的路径保存为path_IBest；

（2.10）更新当代最优路径含沙量；

（2.11）对当代最优路径进行邻域扩张，得到最优解邻域集合V_Iextend，并更新V_Iextend中路径含沙量；

（2.12）将第r代最优路径代价函数值f(path_IBest)与当前全局最优路径代价函数值f(path_TBest)相比较,若f(path_IBest)＜＝f(path_TBest),更新当前全局最优路径代价函数值f(path_TBest)＝f(path_IBest)，更新当前全局最优路径path_TBest＝path_IBest，并更新当前全局最优路径含沙量；

（2.13）若r＝r_max，则搜索到全局最优路径，否则更新迭代代数r＝r+1，重新执行步骤(2.2)。

本发明的有益效果在于：

（1）针对基本IWD方法存在的易陷入局部最优解导致方法停滞及收敛速度较慢的问题进行了改进，在基本IWD方法基础上引入了最优解邻域扩张机制及全局最优强调机制，得到NIWD方法，可以避免方法陷入局部最优导致早熟，提高了方法寻优的收敛速度；

（2）首次提出将NIWD方法用于USV路径规划，充分利用NIWD的正反馈的机制和对较优可行路径有较强的记忆功能的特点进行USV路径规划。本发明提出的方法能够在USV复杂的作业环境中快速高效地搜索到最优路径，满足USV作业工程实际需要，提高了USV的自治能力。

附图说明

图1为本发明的USV路径规划方法的整体步骤流程示意图；

图2为本发明中对USV路径规划的作业区域建立对应的栅格工作区示意图；

图3为本发明USV分别采用基本IWD方法及NIWD方法在11*11栅格图中寻找全局最优航线结果及收敛速度示意图。

具体实施方式

下面结合附图对本发明的技术方案做更详细地说明。

本发明针对基本IWD方法易陷入局部最优解导致方法早熟且收敛速度慢的问题进行改进，在基本IWD方法基础上增加最优解邻域扩展机制以及全局最优强调机制，提出邻域智能水滴（Neighborhood Intelligent Water Drops，NIWD）方法。并利用NIWD方法提供一种有效的、快速的USV路径规划方法，解决了现有技术存在的问题，满足USV作业工程实际需要。

在基本IWD方法执行机制中，水滴按照路径含沙量的均匀随机分布的概率来选则下一路径，若在某一次迭代时多数水滴选择同一局部最优路径，则该路径上的含沙量会大幅减少，而水滴在寻优时倾向于含沙量较小的路径，那么很有可能出现在以后的迭代过程中，该路径被选择的可能性逐代增大，水滴探索其他可行路径的能力逐代减退，方法陷入局部最优解，导致方法早熟。本发明引入最优解邻域扩展机制，使得最优解邻域的路径被选择的概率增大，增加解的多样性，从而避免方法陷入局部最优；

基本IWD算法收敛速度较慢，本发明针对这一缺陷引入全局最优强调机制，采用阶梯递增的方式更新全局最优解路径含沙量，即随着迭代次数的增加，全局最优解路径含沙量减少的幅度增大，使水滴搜索全局最优解能力增强，从而大大加快了方法收敛速度。

基于NIWD方法的USV路径规划方法可以充分利用NIWD方法的寻优特性快速有效地规划出满足USV复杂的作业环境的最优路径。

本发明提出的一种基于邻域智能水滴方法的USV实时路径规划方法，如图1所示，具体以下几个步骤：

步骤1：对USV路径规划问题进行环境建模：

（1.1）对USV路径规划的作业区域建立对应的栅格工作区，作为智能水滴搜索最优路径的搜索区域；

在二维平面中进行路径规划，S为起始点，T为目的地。在USV作业区域内建立全局直角S-XY，其中原点为S，以

方向为X轴正向，以垂直于

方向为Y轴。

对该作业区域进行栅格化，得到作业区域栅格阵，以S为栅格阵起点，栅格阵方向与坐标系S-XY方向一致，基准栅格尺寸为l＝v·Δt，其中v为USV预期运行速度大小，Δt为USV实时运动规划周期，任何一个栅格中心点都可以用栅格坐标(r_i,c_i)唯一标识，其中(r_i，c_i)=(x_i/l，y_i/l)，(x_i,y_i)为该栅格中心点在坐标系S-XY中的位置坐标。

将USV作业区域内的静态障碍物（如岛屿、暗礁等）所覆盖的栅格标志为1，表示障碍栅格，不满一个栅格的按照一个栅格处理。其余栅格标志为0，表示自由栅格，USV可以在自由栅格中选择路径点。将每一个障碍栅格的中心点置于障碍点集合V_obstacle{ }中。

在作业区域栅格阵中进行智能水滴路径搜索，从S到T的候选路径可以表示为：path＝{S,p₁,p₂,…,T}，其中路径点p_i为栅格节点，其在坐标坐标系S-XY坐标为(r_i,c_i)，S与T坐标分别为(0,0)及(r_T,0)。

（1.2）确定评价智能水滴搜索得到的路径的代价函数，代价函数包括对路径的长度及平滑度衡量。代价函数如下式所示：

f(path)＝α·dist(path)+β·smooth(path)  （1）

其中α、β为权值，值大小表示对相应的代价子函数的重视程度。

为路径长度代价子函数，d(p_i,p_i+1)为连接路径点p_i及p_i+1的路径长度，n为路径点数目。

为路径平滑度代价子函数，为连接第i个路径点的两条路径段矢量之间的偏转角度，δ为调节系数。

步骤2：利用智能水滴算法根据已知USV作业区域静态障碍物（如岛屿、暗礁等）、行进目标以及路径评价函数在作业区域栅格阵中进行离线全局路径规划，如图2所示。

（2.1）初始化静态参数：水滴数量N，任意两个栅格节点间的初始含沙量InitGSoil、迭代次数r，全局最优路径path_Best，全局最优路径代价函数值f(path_TBest)以及最大迭代次数r_max。通常情况下水滴数量应和栅格化后的作业区域的栅格节点数相同，本发明实施例中取N＝10000，任意两栅格节点间初始含沙量InitGSoil=10000，代次数r＝0，全局最优路径path_Best＝{ }，全局最优路径代价函数值f(path_TBest)＝∞，最大迭代次数为r_max＝200。

（2.2）初始化动态参数：每滴水滴初始含沙量InitDSoil，水滴初始速度InitVel，第r代最优路径代价函数值f(path_IBest)以及第r代最优路径path_IBest。本发明实施例中每滴水滴初始含沙量InitDSoil=0，水滴初始速度InitVel=4，第r代最优路径代价函数值f(path_IBest)＝∞。

（2.3）初始将所有水滴都置于作业区域中栅格阵的起点S位置；对所有水滴h重复执行步骤(2.4)—(2.8)执行直至水滴h行进至栅格阵横坐标为r_T-1，表示水滴h完成路径搜索，转至步骤(2.9)。

（2.4）根据当前水滴h（即第h个水滴）所处栅格节点的邻路径情况选择前进路径，具体是：

水滴h在作业区域栅格当前节点p_i位置坐标为(r_i,c_i)时选择下一节点p_i+1位置坐标为(r_i+1,c_i+1)的概率为 $P\left(p_{i+1}\right)=\frac{f\left(\mathrm{soil}(p_i,p_{i+1})\right)}{\underset{(r_{i+1},c_w)\∉V_{\mathrm{obstacle}}\left\{\right\}}{\mathrm{\Σ}}f\left(\mathrm{soil}(p_i,(r_{i+1},c_w))\right)},$ 其中，(r_i+1,c_w)表示栅格阵中横坐标为r_i+1且不属于障碍点集合V_obstacle{ }的任意一个节点，也就是处于p_i节点的水滴h的所有的下一个可能的节点。需要计算水滴h从p_i位置到所有下一可能节点的概率。soil(p_i,(r_i+1,c_w))表示p_i节点与(r_i+1,c_w)节点间的路径含沙量。作业区域栅格任意两节点间的初始含沙量均为InitGSoil。

其中，f(soil(p_i,p_i+1))为从p_i节点到p_i+1节点的路径含沙量的相关函数值，式中，ε_s为用户预先给出的很小的正数，为避免含沙量的相关函数的分母为零的情况出现，本发明实施例中设置ε_s=0.0001。

表示对从p_i节点到所有下一个可能的节点(r_i+1,c_w)的路径含沙量的相关函数值求和，每个f(soil(p_i,(r_i+1,c_w)))的求取方式和f(soil(p_i,p_i+1))的相同。

g(soil(p_i,p_i+1))用来确保将p_i节点与p_i+1节点间的路径含沙量转换为正数，表达式为：

$g\left(\mathrm{soil}(p_i,p_{i+1})\right)=\left\{\begin{array}{cc}\mathrm{soil}(p_i,p_{i+1})& \mathrm{if}\min \left(\mathrm{soil}(p_i,(r_{i+1},c_w))\right)>0\\ \mathrm{soil}(p_i,p_{i+1})-\min \left(\mathrm{soil}\left(p_i(r_{i+1},c_w)\right)\right)& \mathrm{else}\end{array}\right.;;$

其中，min(soil(p_i,(r_i+1,c_w)))表示当前节点p_i与所有下一可能节点(r_i+1,c_w)间的路径含沙量的最小值。

水滴倾向于选择含沙量较少的路径作为下一搜索路径，也就是选择计算的最大概率对应的栅格节点作为水滴h当前前进路径的下一个节点；若最小概率对应的节点有多个，则随机选取其中的一个节点作为水滴h前进路径的下一个节点。

（2.5）水滴h由p_i节点到达p_i+1节点后，按照下式更新水滴的速度vel^IWD(t)：

${\mathrm{vel}}^{\mathrm{IWD}}(t+1)={\mathrm{vel}}^{\mathrm{IWD}}\left(t\right)+\frac{a_v}{b_v+c_v\·{\mathrm{soil}}^2(p_i,p_{i+1})};$

vel^IWD(t+1)为t+1时刻水滴h的速度，vel^IWD(t)为t时刻水滴h的速度；速度更新与p_i节点与p_i+1节点间路径含沙量成反比例关系，其中a_v、b_v、c_v为速度调节参数，a_v＝c_v＝1，b_v＝0.01，本发明实施例中初始的水滴h的速度vel^IWD(t)为水滴初始速度InitVel=4。soil(p_i,p_i+1)表示p_i节点与p_i+1节点间的路径含沙量，初始值为InitGSoil。

（2.6）水滴h由p_i节点到达p_i+1节点后，利用下式计算水滴h所经过的路径含沙量增量：

$\mathrm{\Δsoil}(p_i,p_{i+1})=\frac{a_s}{b_s+c_s\·{\mathrm{time}}^2(p_i,p_{i+1};{\mathrm{vel}}^{\mathrm{IWD}}(t+1))};$

其中式中a_s、b_s、c_s为路径含沙量调节参数，a_s＝c_s＝1，b_s＝0.01；HUD(p_i+1)为反向启发式函数，该函数为水滴下一位置的选择提供参考，本发明中， $\mathrm{HUD}\left(p_{i+1}\right)=d(p_i,p_{i+1})=\sqrt{{(r_i-r_{i+1})}^2+{(c_i-c_{i+1})}^2},$ 即连接节点p_i及p_i+1的路径长度。

（2.7）水滴h由p_i节点到达p_i+1节点后，按照下式更新p_i节点到达p_i+1节点间路径含沙量：soil(p_i,p_i+1)＝(1-ρ_n)·soil(p_i,p_i+1)-ρ_n·Δsoil(p_i,p_i+1)；其中ρ_n为路径含沙量更新参数，本发明中ρ_n取0.9。

（2.8）水滴h由p_i节点到达p_i+1节点后，按照下式更新水滴含沙量：

soil^IWD＝soil^IWD+Δsoil(p_i,p_i+1)；

其中，soil^IWD表示水滴的含沙量，初始值为InitDSoil=0。

（2.9）当前第r次迭代结束后，根据式（1）计算当前迭代每个水滴h得到的路径的代价函数f(path_h)，计算第r代最优路径代价函数值

最小路径代价对应的水滴经过的路径保存为path_IBest。

（2.10）当前第r次迭代结束后，更新当前全局路径含沙量，作为下一次迭代的初始路径含沙量，按照下式进行更新：

$\mathrm{soil}(p_i,p_j)=(1+{\mathrm{\ρ}}_{\mathrm{IWD}})\·\mathrm{soil}(p_i,p_j)-{\mathrm{\ρ}}_{\mathrm{IWD}}\·\frac{1}{N_{\mathrm{IB}}-1}\·{\mathrm{soil}}_{\mathrm{IB}}^{\mathrm{IWD}},\∀(p_i,p_j)\∈{\mathrm{path}}_{\mathrm{IBest}},$

其中N_IB为第r代最优搜索路径所包含的节点个数，为第r代最优水滴的含沙量;ρ_IWD为含沙量调节参数，本发明中取值为0.9。

（2.11）对当代最优路径path_IBest进行邻域扩张，得到最优解邻域集合V_Iexten。以p_i＝(r_i,c_i)∈path_IBe为t例， $p_i^{\mathrm{up}}=(r_i,c_{i-1})\∉V_{\mathrm{obstacle}}\left\{\right\}$ 及 $p_i^{\mathrm{down}}=(r_i,c_{i+1})\∉V_{\mathrm{obstacle}}\left\{\right\}$ 为p_i路径点邻域，则

更新V_Iextend中路径含沙量，作为下一次迭代的初始路径含沙量，按照下是进行更新：

$\mathrm{soil}(p_i,p_j)=(1+{\mathrm{\ρ}}_{\mathrm{IWD}})\·\mathrm{soil}(p_i,p_j)-a_N\·\frac{1}{N_{\mathrm{IB}}-1}\·{\mathrm{soil}}_{\mathrm{IB}}^{\mathrm{IWD}},\∀(p_i,p_j)\∈V_{\mathrm{Iextend}}$

其中α_N为邻域调整系数，α_N＝k_N(δ)^(-r)，k_N为邻域更新系数，取k_N∈(0～1)，δ为衰减基数，取δ∈(1～1.2)。

（2.12）将第r代最优路径代价函数值f(path_IBest)与全局最优路径代价函数值f(path_TBest)相比较,若f(path_IBest)≤f(path_TBest),更新当前全局最优路径代价函数值

更新当前全全局最优路径path_TBest＝path_IBest，并按照下式更新当前全局最优路径含沙量。

$\mathrm{soil}(p_i,p_j)=(1+{\mathrm{\ρ}}_{\mathrm{IWD}})\·\mathrm{soil}(p_i,p_j)-a_T\·\frac{1}{N_{\mathrm{IB}}-1}\·{\mathrm{soil}}_{\mathrm{IB}}^{\mathrm{IWD}},\∀(p_i,p_j)\∈{\mathrm{path}}_{\mathrm{TBest}}$

其中α_T为全局调整系数，α_T＝k_T(δ)^(r)，k_T为邻域更新系数，取k_T∈(0～1)，δ为全局调整基数，取δ∈(1～2)。

（2.13）若r＝r_max，则搜索到全局最优路径，否则更新迭代代数r＝r+1，转(2.2)执行。

图3为基本IWD方法及NIWD方法在11*11栅格图中寻找全局最优航线结果及收敛速度示意图。

Claims (1)

1.一种基于邻域智能水滴算法的水面无人艇路径规划方法，其特征在于：

（1）对水面无人艇路径规划进行环境建模：

（1.1）对水面无人艇路径规划的作业区域建立对应的栅格工作区，作为智能水滴搜索最优路径的搜索区域；

在二维平面中进行路径规划，S为起始点，T为目的地，在水面无人艇作业区域内建立全局直角S-XY，其中原点为S，以

方向为X轴正向，以垂直于

方向为Y轴；

对水面无人艇作业区域进行栅格化，得到作业区域栅格阵，以S为栅格阵起点，栅格阵方向与坐标系S-XY方向一致，基准栅格尺寸为l＝v·Δt，其中v为水面无人艇预期运行速度大小，Δt为水面无人艇实时运动规划周期，任何一个栅格中心点都可以用栅格坐标(r_i,c_i)唯一标识，其中(r_i，c_i)=(x_i/l，y_i/l)(x_i,y_i)为该栅格中心点在坐标系S-XY中的位置坐标；

将水面无人艇作业区域内的静态障碍物所覆盖的栅格标志为1，表示障碍栅格，不满一个栅格的按照一个栅格处理，其余栅格标志为0，表示自由栅格，将每一个障碍栅格中心点坐标置于障碍点集合V_obstacle{ }中；

在作业区域栅格阵中进行路径规划，从S到T的候选路径可以表示为：path＝{S,p₁,p₂,…,T}，其中路径点p_i为栅格节点坐标，其在坐标坐标系S-XY坐标为(r_i,c_i)，S与T坐标分别为(0,0)及(r_T,0)；

（1.2）确定评价智能水滴搜索得到的路径的代价函数：

f(path)＝α·dist(path)+β·smooth(path)

其中α、β为权值，值大小表示对相应的代价子函数的重视程度；

为路径长度代价子函数，d(p_i,p_i+1)为连接路径点p_i及p_i+1的路径长度，n为路径点数目；

为路径平滑度代价子函数，

为连接第i个路径点的两条路径段矢量之间的偏转角度，δ为调节系数；

（2）利用智能水滴算法根据已知水面无人艇作业区域静态障碍物、行进目标以及路径评价函数在作业区域栅格阵中进行离线全局路径规划，得到全局离线最优路径path_TBest：

（2.1）初始化静态参数：水滴数量N，任意两个栅格节点间的初始含沙量InitGSoil、迭代次数r＝0，全局最优路径path_Best，全局最优路径代价函数值f(path_TBest)以及最大迭代次数r_max；

（2.2）初始化动态参数：每滴水滴初始含沙量InitDSoil，水滴初始速度InitVel，第r代最优路径代价函数值f(path_IBest)以及第r代最优路径path_IBest；

（2.3）将所有水滴都置于作业区域中栅格阵的起点S位置；对所有水滴重复执行步骤(2.4)—(2.8)直至所有水滴都行进至栅格阵横坐标为r_T-1，则第r次迭代结束，执行步骤(2.9)；

（2.4）根据当前水滴h，即第h个水滴所处栅格节点的邻路径情况选择前进路径：

计算水滴h在作业区域栅格当前节点p_i＝(r_i,c_i)位置时选择下一所有可能节点p_i+1＝(r_i+1,c_w)的概率，(r_i+1,c_w)表示栅格阵中横坐标为r_i+1且不属于障碍点集合V_obstacle{ }的节点，即处于p_i节点的水滴h的所有的下一个可能的节点；

（2.5）水滴h由p_i节点到达p_i+1节点后，更新水滴的速度vel^IWD(t)；

（2.6）水滴h由p_i节点到达p_i+1节点后，计算水滴h所经过的路径含沙量增量；

（2.7）水滴h由p_i节点到达p_i+1节点后，更新p_i节点到达p_i+1节点间路径含沙量；

（2.8）水滴h由p_i节点到达p_i+1节点后，更新水滴含沙量；

（2.9）计算当前迭代每个水滴h得到的路径的代价函数f(path_h)，计算第r代最优路径代价函数值

最小路径代价对应的水滴经过的路径保存为path_IBest；

（2.10）更新当代最优路径含沙量；

（2.11）对当代最优路径进行邻域扩张，得到最优解邻域集合V_Iextend，并更新V_Iextend中路径含沙量；

（2.12）将第r代最优路径代价函数值f(path_IBest)与当前全局最优路径代价函数值f(path_TBest)相比较,若f(path_IBest)＜＝f(path_TBest),更新当前全局最优路径代价函数值f(path_TBest)＝f(path_IBest)，更新当前全局最优路径path_TBest＝path_IBest，并更新当前全局最优路径含沙量；

（2.13）若r＝r_max，则搜索到全局最优路径，否则更新迭代代数r＝r+1，重新执行步骤(2.2)。

Images