CN109460045B - 动态障碍在线感知下usv基于改进蚁群优化的避碰规划方法 - Google Patents

Abstract

动态障碍在线感知下USV基于改进蚁群优化的避碰规划方法，属于水面无人艇避碰规划技术领域。本发明针对USV在动态未知环境中避碰规划算法的搜索能力不足等问题，提出了一种基于改进蚁群优化算法的USV实时避碰规划方法，首先，为了满足《国际海上避碰规则》，设计反向偏心膨化法对动态障碍进行膨化；其次，基于运动速度模型和改进蚁群优化算法，将动态已知环境中的USV避碰规划转换成为一个瞬时静态已知环境下的多条件目标优化问题；最后，针对于蚁群优化算法收敛速度慢的问题，提出了改进的伪随机比例规则对蚂蚁状态转移进行选择，且借鉴狼群分配原则和最大—最小蚂蚁系统对全局信息素进行更新，避免了搜索陷入局部最优。

Description

动态障碍在线感知下USV基于改进蚁群优化的避碰规划方法

技术领域

本发明属于水面无人艇避碰规划技术领域，具体涉及一种动态障碍在线感知下USV基于改进蚁群优化的避碰规划方法。

背景技术

随着当今科技的迅猛发展，海上智能交通已成为世界各国科技战略装备不可缺少的重要组成部分，对其智能化航行的深入研究具有重大的战略意义。USV作为智能化的海上航行器，由于其具有航速快、体积小、自动化和智能化程度高等特点而吸引了广泛研究，其避碰规划既是USV智能化的重要标志，又是USV自主航行的核心技术，所以USV顺利完成使命任务的重要前提是其能够自主避碰。常用的传统避碰规划方法包括蚁群优化算法、粒子群优化算法、人工免疫算法、遗传优化算法、A*优化算法、人工势场法、Dijkstra优化算法等。对于避碰规划问题，除了考虑安全性和避碰运动平滑性等，系统的实时性也是重要的指标，然而基于上述传统算法的避碰规划系统，存在避碰的实时性与避碰精度相互矛盾的问题。

发明内容

本发明的目的在于提供一种动态障碍在线感知下USV基于改进蚁群优化的避碰规划方法，解决USV在动态未知环境中避碰规划方法的搜索能力不足等问题。

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

动态障碍在线感知下USV基于改进蚁群优化的避碰规划方法，包括如下步骤：

步骤1：构建全局坐标系和局部坐标系，建立AIS系统仿真模型；

步骤2：建立USV碰撞危险度模型，利用AIS提供的当前时刻动态障碍信息，计算USV综合碰撞危险度；

步骤3：建立基于《国际海上避碰规则》的动态障碍避碰模型，利用AIS提供的当前时刻动态障碍信息，根据《国际海上避碰规则》判定USV当前会遇情况，对动态障碍进行反向偏心膨化，生成虚拟障碍；

步骤4：构建基于USV与动态障碍的运动速度模型；

步骤5：考虑USV自身运动能力，采用改进蚁群优化方法对虚拟障碍进行避碰规划，求解出此时的最优值；

步骤6：将AIS系统提供的环境信息及目标点信息输入至基于改进蚁群优化方法的USV动态避碰规划器，获得USV下一时刻的转艏及速度的调整指令。

所述步骤1中全局坐标系采用北东坐标系，地图左上角为原点，正东方向为X轴，正北方向为Y轴；局部坐标系分为艇载坐标系和传感器坐标系，艇载坐标系是以USV为原点所建立的直角坐标系，传感器坐标系是以AIS系统为极点、USV前进方向作为极轴所建立的极坐标系。

所述步骤3中采用基于《国际海上避碰规则》的动态障碍反向偏心膨化法，刚进入USV碰撞危险阶段时，动态障碍向《国际海上避碰规则》要求的避碰方向进行反向偏心膨化，从而使USV的规避动态障碍方向符合《国际海上避碰规则》；当USV避碰规划方案进入USV紧迫阶段时，基于《国际海上避碰规则》的反向偏心膨化程度逐渐减小直至消失，此时USV背离《国际海上避碰规则》，按照最安全的避碰方向规避动态障碍。

所述步骤5中改进蚁群优化方法，具体步骤如下：

步骤5.1：使用基于相对位置检测的可视性判断方法来统计包括起点和终点在内的所有已经经过膨化的环境障碍物顶点的可视点，每一个顶点g_i都定义一个allowed_i链表存储其可视点集；

步骤5.2：初始化τ_ij(0)，给它赋一个较小的正数,历史最优保留代数计数器count=0，进化代数计数器G_count=0，设置历史最优最大保留代数Max和最大进化代数Generation，设置初始历史最优蚂蚁history_best的路径代价为∞，每代蚂蚁种群数为m；

步骤5.3：设置蚂蚁种群编号k=1；

步骤5.4：如果k＞m,转到步骤5.7；否则，把蚂蚁k放置在起始位置g_s上，转到步骤5.5；

步骤5.5：设此时蚂蚁当前位置为g_i，若g_i的可视点链表allowed_i为空，则这只蚂蚁死亡，k=k+1，转到步骤5.4；

步骤5.6：若蚂蚁当前位置为终点g_e，则蚂蚁找到完整路径，k=k+1，转到步骤5.4；否则，将本路径点从allowed_i中删除，以防止蚂蚁向回搜索，并按照状态转移规则寻找下一路径点，转到步骤5.5；

步骤5.7：G_count=G_count+1；若本代蚂蚁中的最优蚂蚁比历史最优蚂蚁找到的路径更优，更新history_best并且count=0，否则count=count+1；

步骤5.8：若count＞Max或G_count≥Generation，优化方法停止，否则对蚁群信息素进行全局更新处理，转到步骤5.3。

所述步骤5中改进蚁群优化方法对虚拟障碍进行避碰规划包括基于方向角权值的状态转移规则、基于狼群分配原则的全局信息素更新模型以及基于最大-最小蚂蚁系统的全局信息素更新模型；

所述步骤5中基于方向角权值的状态转移规则为：

其中τ为信息素浓度函数；allowed_k为蚂蚁k允许做状态转移的i点的可视点集合；η是启发式函数，取值为待转移可视点到目标点距离的倒数；α为信息素的重要程度；β为启发函数的重要程度。

所述步骤5中基于狼群分配原则的全局信息素更新模型为：

τ_ij(t+n)=(1-α)τ_ij(t)+Δτ_ij(t)

其中α为全局信息素挥发系数；L_BEST为目前全局最优路径的长度。

τ_ij(t+1)＝λ×τ_ij(t+n)

其中D为起点到终点的欧式距离。

所述步骤5中基于最大-最小蚂蚁系统的全局信息素更新模型：

其中，τ_min、τ_max为自行设置的信息素浓度上下限。

本发明有益效果在于：

(1)为了满足《国际海上避碰规则》，设计反向偏心膨化法对动态障碍进行膨化；

(2)基于运动速度模型和改进蚁群优化方法，将动态已知环境中的USV避碰规划转换成为一个瞬时静态已知环境下的多条件目标优化问题；

(3)针对于蚁群优化方法收敛速度慢的问题，提出了改进的伪随机比例规则对蚂蚁状态转移进行选择，且借鉴狼群分配原则和最大—最小蚂蚁系统对全局信息素进行更新，避免了搜索陷入局部最优。

附图说明

图1为本发明的USV全局及局部坐标系示意图；

图2为本发明的USV动态避碰规划流程图；

图3为本发明的USV避碰规划方案示意图；

图4为本发明的动态障碍反向偏心膨化效果图；

图5为本发明的USV与动态障碍运动速度模型图；

图6为本发明的改进蚁群优化方法流程图；

图7为本发明的基于避碰规则的USV相遇仿真图；

图8为本发明的基于避碰规则的USV追越仿真图；

图9为本发明的基于避碰规则的USV左交叉仿真图；

图10为本发明的基于避碰规则的USV右交叉仿真图；

图11为本发明的基于避碰规则的USV多动态已知障碍避碰规划仿真图。

具体实施方式

下面结合附图对本发明做更进一步描述。

一种基于改进蚁群优化算法的USV动态避碰规划方法，包括以下步骤：

步骤1：构建全局坐标系和局部坐标系，建立AIS系统仿真模型；

全局坐标系采用北东坐标系，地图左上角为原点，正东方向为X轴，正北方向为Y轴。

局部坐标系分为艇载坐标系和传感器坐标系，其中艇载坐标系是以USV为原点所建立的直角坐标系，传感器坐标系是以AIS系统为极点、USV前进方向作为极轴所建立的极坐标系。

AIS系统仿真模型识别船数不小于300个，数据更新率为1Hz，接收误码率低于20％，且在5级海况下设备能够正常工作，最大探测距离为20海里，探测范围为0°～360°。

步骤2：建立USV碰撞危险度模型；

鉴于USV运动的高速性和安全性，本发明将采取模糊数学和神经网络相互结合的方法对碰撞危险度进行计算，并且对碰撞危险度分成两个部分进行研究，一是空间碰撞危险度，二是时间碰撞危险度，其中前者表示USV发生碰撞的概率，后者表示USV避碰规划时间的紧迫程度。

步骤3：建立基于《国际海上避碰规则》的动态障碍避碰模型；

本发明设计的基于《国际海上避碰规则》的动态障碍反向偏心膨化法，即USV避碰规划方案刚进入USV碰撞危险阶段时，此时USV应按照《国际海上避碰规则》去避让动态障碍，所以动态障碍将向《国际海上避碰规则》要求的避碰方向进行反向偏心膨化，从而使USV的规避动态障碍方向符合《国际海上避碰规则》；当USV避碰规划方案进入USV紧迫阶段时，基于《国际海上避碰规则》的反向偏心膨化程度逐渐减小直至消失，此时USV可背离《国际海上避碰规则》，按照最安全的避碰方向规避动态障碍。

步骤4：构建USV与动态障碍的运动速度模型；

针对AIS系统提供的动态障碍信息，将膨化生成虚拟障碍，此时USV对所有环境信息做瞬间静态化处理即将虚拟障碍看成瞬时膨化后的静态已知障碍，并调用本发明所设计的改进蚁群优化算法即可规划处一条在这瞬间的实时安全航线，而且基于USV与动态障碍的运动速度模型，USV规避动态障碍需同时求解出最优的和来满足USV动态避碰条件，因此动态已知环境中的USV避碰规划可以看成是一个瞬时静态已知环境下的多条件目标优化问题。

步骤5：设计用于实时避碰规划的改进蚁群优化算法；

蚁群优化算法的改进设计包括基于方向角权值的状态转移规则、基于狼群分配原则的全局信息素更新模型以及基于最大-最小蚂蚁系统的全局信息素更新模型。

步骤6：将AIS系统提供的环境信息及目标点信息输入至基于改进蚁群优化算法的USV动态避碰规划器，获得USV下一时刻的转艏及速度的调整指令；

全局坐标系和局部坐标系：

如图1所示，USV所在位置的全局坐标为(x_u,y_u),α_u是USV航行方向与正东方向X_G的夹角，AIS系统安装于艇载坐标系X_UO_UY_U下一点(x_r,0)处，即USV在航行过程中，传感器坐标系P(d_i,θ_i)相对于艇载坐标系X_UO_UY_U的位置是固定不变的，其中Q_i为动态障碍与USV综合感知视域模型的交点(d_i,θ_i)。

USV碰撞危险度模型：

1.空间碰撞危险度

空间碰撞危险度(Space Collision Risk，SCR)是以USV与目标船的最短会遇距离DCPA及其方位、USV与目标船间的距离D_T和两船间的安全距离为标准，表示一种发生碰撞概率的空间衡量。

DCPA的隶属函数：

其中d₁为USV与目标船间的安全会遇距离；d₂为最晚采取避碰时的安全距离。当d₂＜|DCPA|时不需要进行避碰规划；当d₁＜|DCPA|≤d₂时需要计算空间碰撞危险度来判断是否进行避碰规划；当|DCPA|＜d₁时USV与目标船之间存在碰撞风险，需要进行避碰规划。

d₁=1.5l(θ_i)，d₂=2d₁。

2.时间碰撞危险度

时间碰撞危险度(Time Collision Risk，TCR)主要反映了USV与目标船间的运动参数对碰撞危险度的影响，表示某一方到达最晚转舵点的时间紧迫程度，其中运动参数包括：两船航速、速度比、相对速度、相对距离以及USV艇长。

TCPA的隶属函数：

(1)当TCPA＞0时

(2)当TCPA≤0时

基于《国际海上避碰规则》的动态障碍反向偏心膨化法：

1.USV综合碰撞危险度u_dt：

2.如图4所示，反向偏心膨化圆半径R^*：

当u_dt较小时动态障碍反向偏心膨化较大，此时《国际海上避碰规则》影响大，当u_dt较大时动态障碍反向偏心膨化较小，此时《国际海上避碰规则》影响小；

3.如图4所示，反向偏心膨化圆O^*的圆心：

其中ζ为反向偏心膨化圆的偏心角度；当Δv沿弧线转向v_O时，若此转向为逆时针则为左交叉情况，“±”取+；反之则为右交叉情况，“±”取-。

USV与动态障碍的运动速度模型：

如图5所示，图中以USV为中心建立艇载坐标系，动态障碍为安全性膨化圆，其膨化半径为R′，T为安全性膨化圆O′一切点，USV速度为(v_USV,α)，动态障碍速度为(v_O,β)，相对速度为(Δv,ψ)，α＝∠(X_USV,v_USV)，β＝∠(X_USV,v_O)，ψ＝∠(X_USV,Δv)，

η＝∠(v_O,Δv)，λ＝∠(v_O,v_USV)，γ＝∠(USV-O′,Δv)，θ＝(X_USV,USV-O′)，μ＝(USV-O′,USV-T)，其中“∠(a,b)”表示由线a沿弧指向线b的角度，规定其方向逆时针为正、顺时针为负；“(a,b)”表示a与b间的角度，无正负关系。

如图5可知只需保证任意时刻abs(γ)≥μ即可使USV不与动态障碍发生碰撞，因此将相对速度Δv分解成沿USV-O′方向即指向动态障碍的速度分量Δv_o和垂直于USV-O′方向的速度分量Δv_r，并且由v_USV、v_O和Δv可构成的数学关系求解出Δv_r、Δv_o和γ，其中Δv_o是驱使USV向动态障碍靠近的速度分量，而Δv_r是驱使USV远离动态障碍的速度分量。

为了在USV动态避碰规划时，通过实时调整角度γ使其满足abs(γ)≥μ，因此对调整角度γ进行求导：

令

则

其中：

因此可得

假设动态障碍运动不会瞬时突变，则其速度v_O和运动角度β可忽略不计，即dv_O=0，dβ＝0，则上述公式可简化为：

该公式也可以写成：

实时调整角度γ使USV满足动态避碰条件|γ+Δγ|≥μ，即为：

因此动态已知环境中的USV避碰规划可以看成是一个瞬时静态已知环境下的多条件目标优化问题，如下公式所示。

式中f(Δv_USV,Δα)为改进蚁群优化算法的目标优化函数，m₁和m₂分别为Δv_USV和Δα的权值。

此外，所求解Δv_USV和Δα应满足USV实际的运动能力，要考虑USV避碰规划约束条件，即

和

其中

为USV极限加速度，

为USV极限角速度，

为USV最大航速。因此受USV自身运动能力

以及

的限制，USV可能无法在一个算法周期内规避动态障碍，但由于USV在第一时间发现碰撞危险时就开始进行避碰规划，而且此时USV距离动态障碍较远，所以USV有足够的时间对Δv_USV和Δα进行多个算法周期的优化调整，从而实现USV动态避碰。

因此基于动态障碍反向偏心膨化的动态已知环境中的USV避碰规划策略的具体步骤如下：

Step1:利用AIS提供的当前时刻动态障碍信息，计算USV综合碰撞危险度u_dt，判断是否存在碰撞危险，若不存在转到Step5；

Step2:利用AIS提供的当前时刻动态障碍信息，根据《国际海上避碰规则》判定USV当前会遇情况(相遇、追越以及左右交叉)，对动态障碍进行反向偏心膨化，生成虚拟障碍；

Step3:考虑USV自身运动能力

以及

的限制，采用改进蚁群优化算法对虚拟障碍进行避碰规划，求解出此时满足上述公式的Δv_USV和Δα最优值；

Step4:利用AIS提供的下一时刻动态障碍信息，再次计算USV综合碰撞危险度u_dt，判断是否存在碰撞危险，若依然存在转到Step2，否则转到Step5；

Step5:USV动态避碰完成，复航。

改进蚁群优化算法：

基于方向角权值的状态转移规则：

在USV搜索路径的过程中，首先在其二维工作空间模型中定义一个初始方向角ω，初始方向角是起始点和目标点的连线与正东方向的夹角；蚂蚁开始移动后的方向角，即蚂蚁当前位置和目标点的连线与正东方向的夹角称为实时方向角ω_e，因此方向角权值F_q如下所示：

基于方向角权值的状态转移规则如下所示：

式中，τ为信息素浓度函数；allowed_k为蚂蚁k允许做状态转移的i点的可视点集合；η是启发式函数，取为待转移可视点到目标点距离的倒数；α为信息素的重要程度；β为启发函数的重要程度。

基于狼群分配原则的全局信息素更新模型：

研究发现狼群会将捕捉到的大部分猎物分给强壮的狼，尽管会饿死一些弱小的狼。这样能保证强壮的狼在下次捕捉到猎物，不至于使整个狼群饿死，所以能够提高狼群的生存能力。因此，本发明基于狼群分配原则的全局信息素更新模型如下：

τ_ij(t+n)=(1-α)τ_ij(t)+Δτ_ij(t)

式中，α为全局信息素挥发系数；L_BEST为目前全局最优路径的长度。

τ_ij(t+1)＝λ×τ_ij(t+n)

式中，D为起点到终点的欧式距离。

基于最大-最小蚂蚁系统的全局信息素更新模型：

式中，τ_min、τ_max为自行设置的信息素浓度上下限。

改进蚁群优化算法具体步骤如下：

Step1:使用基于相对位置检测的可视性判断方法来统计包括起点和终点在内的所有已经经过膨化的环境障碍物顶点的可视点，每一个顶点g_i都定义一个allowed_i链表存储其可视点集；

Step2:初始化τ_ij(0)，给它赋一个较小的正数,历史最优保留代数计数器count=0，进化代数计数器G_count=0，设置历史最优最大保留代数Max和最大进化代数Generation，设置初始历史最优蚂蚁history_best的路径代价为∞，每代蚂蚁种群数为m；

Step3:设置蚂蚁种群编号k=1；

Step4：如果k＞m,转到Step7；否则，把蚂蚁k放置在起始位置g_s上，转到Step5；

Step5:设此时蚂蚁当前位置为g_i，若g_i的可视点链表allowed_i为空，则这只蚂蚁死亡，k=k+1，转到Step4；

Step6:若蚂蚁当前位置为终点g_e，则蚂蚁找到完整路径，k=k+1，转到Step4；否则，将本路径点从allowed_i中删除，以防止蚂蚁向回搜索，并按照状态转移规则寻找下一路径点，转到Step5；

Step7:G_count=G_count+1；若本代蚂蚁中的最优蚂蚁比历史最优蚂蚁找到的路径更优，更新history_best并且count=0，否则count=count+1；

Step8:若count＞Max或G_count≥Generation，优化算法停止。否则对蚁群信息素进行全局更新处理，转到Step3。

基于避碰规则的USV动态避碰仿真：

针对于高速运动条件下的USV，本小节基于Qt平台首先对基于《国际海上避碰规则》的动态已知环境的避碰规划分为相遇、追越、左交叉和右交叉这四种会遇情况进行仿真验证，如图7～10所示。其中R′=1.2R+d₁，USV巡航速度v_USV=40kn，最大航速v_USVmax=60kn，动态障碍的航速v_O=20kn。

从图7～10可以看出，USV通过建立运动速度模型，以动态障碍的反向偏心膨化圆为避碰区域，在避碰过程中不仅保证了USV与动态障碍的安全，同时也使USV的规避方向符合《国际海上避碰规则》。其中在USV航向调整过程中，动态障碍的反向偏心膨化圆半径及其方向可能会发生突变，这是因为USV会遇情况会随着航向的改变而转变成其他会遇情况而导致的，并且经仿真验证该突变并不会对USV初始避碰规划产生影响。

由于USV在第一时间发现碰撞危险时其避碰规划方案就进入到碰撞危险阶段，而且此时USV距离动态障碍较远，所以本节USV有足够的时间并结合《国际海上避碰规则》对其进行动态避碰，因此本小节USV在相遇和左、右交叉情况下均只通过调节航向实现避碰规划，而在追越情况下同时调节航向和航速且采用USV的最高航速超越动态障碍，直到实现避碰后再恢复到USV的巡航速度。

其次，本小节对基于《国际海上避碰规则》的多动态已知障碍的避碰规划进行仿真验证，如图11所示。其中R′=1.2R+d₁，USV巡航速度v_USV=40kn，最大航速v_USVmax=60kn，动态相遇障碍1的航速v_O1=20kn，动态追越障碍2的航速v_O2=20kn，动态左交叉障碍3的航速v_O3=50kn，为了使避碰规划航迹线更加清晰，仿真中对动态障碍的反向偏心膨化圆不予以显示。

在图11中USV首先只通过调节航向规避相遇障碍1；其次通过同时调节航向和航速规避追越障碍2，且采用USV的最高航速超越障碍2，直到实现避碰后再恢复到USV的巡航速度；然后由于左交叉障碍3航速较快，USV按照巡航速度航行难以实现避碰，所以此时USV在调节航向的同时采取减速策略来完成避碰规划；最后USV先恢复到巡航速度航行一段时间，直到距离终点不远处时，通过自身降速驶向终点。

Claims (1)

1.动态障碍在线感知下USV基于改进蚁群优化的避碰规划方法，其特征在于，包括以下步骤：

步骤1：构建全局坐标系和局部坐标系；

所述的全局坐标系X_GO_GY_G为以地图左上角为原点O_G，以正东方向为X_G轴，以正北方向为Y_G轴的北东坐标系，USV所在位置的全局坐标为(x_u,y_u)，USV航行方向与正东方向X_G的夹角为α_u；所述的局部坐标系分为艇载坐标系X_UO_UY_U和传感器坐标系；所述的艇载坐标系X_UO_UY_U是以USV为原点所建立的直角坐标系，USV搭载的AIS系统在艇载坐标系X_UO_UY_U中的坐标为(x_r,0)，AIS系统在全局坐标系X_GO_GY_G中的坐标为(x_R,y_R)；所述的传感器坐标系是以AIS系统为极点、USV前进方向作为极轴所建立的极坐标系，USV在航行过程中传感器坐标系相对于艇载坐标系X_UO_UY_U的位置是固定不变的；

步骤2：利用AIS系统获取当前时刻动态障碍信息，计算USV综合碰撞危险度u_dt，判断是否存在碰撞危险，若不存在碰撞危险则执行步骤6；

步骤2.1：计算空间碰撞危险度u_dT；

其中，DCPA为USV与动态障碍的最短会遇距离；d₁为USV与动态障碍间的安全会遇距离，d₁＝1.5l(θ_i)；d₂为最晚采取避碰时的安全距离，d₂＝2d₁；

其中，Q_i(d_i,θ_i)为动态障碍与USV综合感知视域模型的交点；

步骤2.2：计算时间碰撞危险度u_tT；

当TCPA＞0时：

当TCPA≤0时：

其中，

步骤2.3：计算USV综合碰撞危险度u_dt；

步骤3：根据《国际海上避碰规则》判定USV当前会遇情况，对动态障碍进行反向偏心膨化，生成虚拟障碍；

反向偏心膨化圆半径R^*：

反向偏心膨化圆O^*的圆心：

其中，ζ为反向偏心膨化圆的偏心角度；当Δv沿弧线转向v_O时，若此转向为逆时针则为左交叉情况，“±”取+；反之则为右交叉情况，“±”取-；

步骤4：考虑USV自身运动能力

以及

的限制，采用改进蚁群优化算法对虚拟障碍进行避碰规划，求解出此时Δv_USV和Δα最优值；其中，

为USV极限加速度；

为USV极限角速度；

为USV最大航速；

步骤4.1：使用基于相对位置检测的可视性判断方法来统计包括起点和终点在内的所有已经经过膨化的环境障碍物顶点的可视点，每一个顶点g_i都定义一个allowed_i链表存储其可视点集；

步骤4.2：初始化τ_ij(0)，给它赋一个较小的正数，历史最优保留代数计数器count＝0，进化代数计数器G_count＝0，设置历史最优最大保留代数Max和最大进化代数Generation，设置初始历史最优蚂蚁history_best的路径代价为∞，每代蚂蚁种群数为m；

步骤4.3：设置蚂蚁种群编号k＝1；

步骤4.4：如果k＞m，转到步骤4.7；否则，把蚂蚁k放置在起始位置g_s上，转到步骤4.5；

步骤4.5：若蚂蚁当前位置g_i的可视点链表allowed_i为空，则这只蚂蚁死亡，k＝k+1，转到步骤4.4；

步骤4.6：若蚂蚁当前位置为终点g_e，则蚂蚁找到完整路径，k＝k+1，转到步骤4.4；否则，将本路径点从allowed_i中删除，并按照状态转移规则寻找下一路径点，转到步骤4.5；

在搜索路径的过程中，首先在其二维工作空间模型中定义一个初始方向角ω，初始方向角是起始点和目标点的连线与正东方向的夹角；蚂蚁开始移动后的方向角，即蚂蚁当前位置和目标点的连线与正东方向的夹角称为实时方向角ω_e，因此方向角权值F_q如下所示：

基于方向角权值的状态转移规则如下所示：

式中，τ为信息素浓度函数；allowed_k为蚂蚁k允许做状态转移的i点的可视点集合；η是启发式函数，取为待转移可视点到目标点距离的倒数；α为信息素的重要程度；β为启发函数的重要程度；

步骤4.7：G_count＝G_count+1；若本代蚂蚁中的最优蚂蚁比历史最优蚂蚁找到的路径更优，更新history_best并且count＝0，否则count＝count+1；

步骤4.8：若count＞Max或G_count≥Generation，优化算法停止；否则对蚁群信息素进行全局更新处理，转到步骤4.3；

对蚁群信息素进行全局更新处理的方法具体为：

τ_ij(t+n)＝(1-α)τ_ij(t)+Δτ_ij(t)

式中，α为全局信息素挥发系数；L_BEST为目前全局最优路径的长度；

τ_ij(t+1)＝λ×τ_ij(t+n)

式中，D为起点到终点的欧式距离；

基于最大-最小蚂蚁系统的全局信息素更新：

式中，τ_min、τ_max为自行设置的信息素浓度上下限；

步骤5：利用AIS提供的下一时刻动态障碍信息，再次计算USV综合碰撞危险度u_dt，判断是否存在碰撞危险，若依然存在，则返回步骤3；否则执行步骤6；

步骤6：USV动态避碰完成，复航。

Images