CN114625150B - 基于危险系数和距离函数的快速蚁群无人艇动态避障方法 - Google Patents

Abstract

本发明公开了一种基于危险系数和距离函数的快速蚁群无人艇动态避障方法，采用改进蚁群算法进行全局路径规划，得到起始位置到目标点的可行路径；利用动态窗口法实现局部路径规划，以蚁群算法关键节点为子目标位置进行局部实时避障，直至到达目的点。本发明综合考虑了路径规划安全性和收敛速度问题；改进蚁群算法转移概率，加入危险系数，保证全局规划路径的安全可靠；为了快速实时避障，在动态窗口法评价函数上加入了路径融合距离函数；实现无人艇在复杂水域环境下动态避障，保证无人艇安全可靠地到达目的地。

Description

基于危险系数和距离函数的快速蚁群无人艇动态避障方法

技术领域

本发明属于路径规划算法技术领域，具体涉及一种基于危险系数和距离函数的快速蚁群无人艇动态避障方法。

背景技术

水面无人艇(Unmanned Surface Vessels，USV)可以实现巡航、监测等功能，在军事和民用领域受到了越来越广泛的应用。其中路径规划是无人艇研究领域的一项重要技术，是无人艇保证自身安全，实现自主巡航的基础。

无人水面艇的路径规划可以分为全局路径规划算法和局部路径规划算法。全局路径规划需要提前获取整个水域的环境信息，并在该区域内进行规划；常用的有Dijkstra算法、A*算法、蚁群算法、遗传算法、粒子群优化算法等。其中 Dijkstra算法、A*算法算法复杂度低，但无法很好的处理复杂的路径信息；而蚁群算法是一种智能仿生学的路径规划算法，具有较好的鲁棒性、规划速度也有了一定的提升。

局部路径规划是指在全局未知或者部分未知的水域环境下通过硬件设备自主获取一定范围内的环境信息；常见的局部路径规划算法主要有动态窗口法、人工势场法、快速扩展随机树算法、速度障碍法等。其中人工势场法多用于对静态障碍物进行避障，若目标位置周围不存在障碍物时，具有算法速度较快。动态窗口法能根据无人艇的运动状态信息，并结合无人艇传感器信息实时分析周边障碍物情况，实现了对障碍物的动态避障能力。

本发明采用的路径规划算法是将全局路径规划算法与局部路径规划算法相结合，基于安全蚁群算法和动态窗口法融合的无人艇动态平滑避障算法，该避障算法能在保证无人艇自身安全的前提下较快地实现平滑处理动态障碍物，可以实现基于全局信息的最优可行解。

发明内容

发明目的：针对上述现有技术的不足，本发明提一种基于危险系数和距离函数的快速蚁群无人艇动态避障方法，解决在复杂场景下动态平滑回归避障，规划路径难以保持其路径安全性的问题。

技术方案：本发明所述的一种基于危险系数和距离函数的快速蚁群无人艇动态避障方法，包括如下步骤：

(1)对预先采集到的真实水域地图信息进行栅格化处理，并保存栅格化后的地图信息，确定无人艇航行初始地点和目标点；

(2)在栅格化地图上，启动蚁群算法，根据加入危险系数的转移概率进行全局路径的规划，储存其路径拐点坐标信息，并启动信息素更新策略；

(3)对栅格化地图进行障碍物检测，若发现动态障碍物，则以最近的拐点作为子目标点；如果子目标点在障碍物上，将下一个拐点作为子目标点；并利用加入路径融合距离函数后的动态窗口法进行动态避障；

(4)对步骤(3)寻找子目标点进行循环，直至局部路径规划子目标点为全局路径规划的目标点，则表明该避障算法结束，输出全局最优安全路径。

进一步地，步骤(1)所述栅格化后的地图信息能够显示障碍物以及周围环境信息，每一个栅格用唯一的序列号与其位置坐标相对应，其对应关系为：

其中，(x_i,y_i)为第i个栅格的位置坐标；i为第i个网格的序号；mod()是冗余运算符；ceil()表示取整操作；N_x和N_y分别为行方向和列方向的栅格数，a 为比例系数。

进一步地，步骤(2)所述加入危险系数的转移概率为：

其中，τ_ij(t)为t时刻原始蚁群算法的信息素浓度，η_ij(t)是启发函数，表示t 时刻当前网络位置i移动到第j个栅格的路径启发式信息；ξ(t)为自适应障碍抑制因子，取值范围为[0,1]；α和β分别是信息素和启发式函数的影响因子；加入危险系数DI，将无人艇和障碍物质心之间的距离、无人艇相对障碍物矢面的惯量因子作为碰撞严重程度的主要影响，计算方法如下：

其中，f_cm(D_cm)为无人艇和障碍物质心之间的距离影响因子，

是距离比例因子；D_cmin、D_cmax分别是无人艇质心接近障碍物质心所允许的最小距离和对无人艇安全产生影响的最大距离；无人艇相对障碍物矢面的惯量影响因子：

其中，I_s为障碍物在基坐标系下的惯量矩阵的最大主转动惯量在无人艇矢状面上的分量，I_max为最大允许转动惯量，当I_s超过最大允许转动惯量时，若同障碍物发生潜在碰撞，无人艇将受到严重冲击；

将路径规划阶段总的危险系数定义为上述距离影响因子和惯量影响因子之积：

DI＝f_cm(D_cm)f_I(I_s) (6)

当危险系数DI比较大时，则自适应障碍抑制因子ξ(t)趋近于0，表示无人艇和障碍物之间的危险性较大；当危险系数DI比较小时，则ξ(t)趋近于1，表示无人艇和障碍物之间较为安全。

进一步地，步骤(2)所述信息素更新策略加入自适应挥发因子，具体如下：

其中，Δτ_ij ^k为蚂蚁k在路径(i,j)上的信息素浓度；Q为信息素强度，L_bs为各个路径的最优指标；L_k为区域内最优路径长度；T_k为转移概率搜索时的转弯次数；

和

分别为区域内最优路径长度和转弯次数的加权因子；

τ_ij(t+1)＝(1-ρ)τ_ij(t)+Δτ_ij(t) (9)

其中，τ_ij(t+1)为(t+1)时刻原始蚁群算法的信息素浓度，Δτ_ij(t)为当前迭代路径(i,j)上信息素的增量；加入自适应信息素挥发因子ρ，如式：

其中，k为调整系数；N_c为当前迭代次数；N_max为最大迭代次数。

进一步地，所述步骤(3)通过以下公式实现：

在动态窗口法评价函数上加入了路径融合距离函数：

其中，path(ν,ω)为路径融合距离函数，(x'_i,y'_i)为DWA在第i个局部窗口内基于预测速度轨迹而推演出的局部路径末端坐标；(x_i,y_i)为改进安全蚁群算法所得到的相应拐弯节点坐标；将路径融合距离函数加入到评价函数中，改进后的评价函数为：

G_v,w＝σ(θ·heading(v,w)+κ·dist(v,w)+γ·velocity(v,w)+δ·path(ν,ω)) (12)

其中，heading(v,w)为方位角评价函数表示在当前速度下，模拟轨迹终点方向与目标之间的方位角偏差；dist(v,w)为速度对应轨迹上离障碍物的最近距离；velocity(v,w)为当前速度大小评价函数；path(ν,ω)为路径融合距离函数；σ为平滑系数；θ、κ、γ、δ分别为4项函数的加权系数，表示各项在评价函数中分配的权重比例。

有益效果：与现有技术相比，本发明的有益效果：安全蚁群算法先规划出一条全局路径，能避开静态障碍物；而改进后的动态窗口法(Dynamic window Approach，DWA)能根据全局路径，进行窗口迭代，从而对动态障碍物成功避障；经过避障算法规划后，相比于单一的蚁群算法，避障算法规划整体路径长度基本一致；由此本发明不仅具有安全蚁群算法路径搜索的高效率以及其很大安全指数，还具有动态窗口法能对动态障碍物平滑避障的特性；不仅能有效降低路径搜索长度，还具有较大安全指数，以保证整个搜索过程中无人艇始终远离障碍物并处于低风险状态；本发明还避免了单一算法容易陷入局部最优解这一问题，提高了算法的稳定性；本发明在保证搜索路径安全的前提下，解决了全局路径规划算法实时性较差和局部路径规划算法缺乏全局信息的问题，同时又能找到全局最优解并实现动态平滑避障。

附图说明

图1为本发明的流程图；

图2为路径融合示意图；

图3为蚁群算法对比实验结果图，其中，(a)采用本发明安全蚁群输出路径图；(b)采用原始蚁群算法输出路径图；

图4为采用本发明避障结果图，其中，(a)为本发明提出的安全蚁群算法规划的全局路径图；(b)为本发明无人艇实时避障示意图；(c)为本发明无人艇输出路径图。

具体实施方式

下面结合附图对本发明作进一步的详细说明。

本发明提出一种基于危险系数和距离函数的快速蚁群无人艇动态避障方法，如图1所示，具体包括以下步骤：

步骤1：预处理阶段，无人艇利用采集到的真实水域信息进行栅格化处理，并保存栅格化后的地图信息，确定无人艇航行初始地点和目标点。栅格化地图能够简单有效地显示障碍物以及周围环境信息，每一个栅格都可以用唯一的序列号与其位置坐标相对应；其对应关系为：

其中，(x_i,y_i)为第i个栅格的位置坐标；i为第i个网格的序号；mod()是冗余运算符；ceil()表示取整操作；N_x和N_y分别为行方向和列方向的栅格数；a 为比例系数。

步骤2：启动安全蚁群算法，在栅格化地图上，采用改进安全蚁群算法进行全局路径的规划，并储存其路径拐点坐标信息。

(2.1)参数初始化，释放蚂蚁并设定搜索起点；路径寻优，并根据转移概率选择栅格前进；转移概率为：

其中，τ_ij(t)为t时刻原始蚁群算法的信息素浓度，η_ij(t)是启发函数，表示t 时刻当前网络位置i移动到第j个栅格的路径启发式信息；ξ(t)为自适应障碍抑制因子，取值范围为[0,1]。α和β分别是信息素和启发式函数的影响因子，加入危险系数DI，将无人艇和障碍物质心之间的距离、无人艇相对障碍物矢面的惯量因子作为碰撞严重程度的主要影响，计算方法如下：无人艇和障碍物质心之间的距离影响因子：

其中，

是距离比例因子；D_cmin、D_cmax分别是无人艇质心接近障碍物质心所允许的最小距离和对无人艇安全产生影响的最大距离。无人艇相对障碍物矢面的惯量影响因子：

其中，I_s为障碍物在基坐标系下的惯量矩阵的最大主转动惯量在无人艇矢状面上的分量，I_max为最大允许转动惯量，当I_s超过该值时，若同障碍物发生潜在碰撞，无人艇将受到严重冲击。

将路径规划阶段总的危险系数定义为上述距离影响因子和惯量影响因子之积：

DI＝f_cm(D_cm)f_I(I_s) (6)

当危险系数DI比较大时，则自适应障碍抑制因子ξ(t)趋近于0，表示无人艇和障碍物之间的危险性较大。当危险系数DI比较小时，则ξ(t)趋近于1，表示无人艇和障碍物之间较为安全。从而实现了通过轮盘赌状态转移规则在选择路径时，更偏向于无人艇向远离障碍物方向上的路径进行转移，增强了制导效果。

(2.2)判断是否完成迭代，若未完成则增加蚂蚁数量，继续迭代搜索。若已完成则更新信息素，改进信息素更新方法如下：

其中，Δτ_ij ^k为蚂蚁k在路径(i,j)上的信息素浓度；Q为信息素强度，L_bs为各个路径的最优指标；L_k为区域内最优路径长度；T_k为转移概率搜索时的转弯次数；

和

分别为区域内最优路径长度和转弯次数的加权因子。

τ_ij(t+1)＝(1-ρ)τ_ij(t)+Δτ_ij(t) (9)

其中，τ_ij(t+1)为(t+1)时刻原始蚁群算法的信息素浓度，Δτ_ij(t)为当前迭代路径(i,j)上信息素的增量。通过式(9)可以看到，信息素挥发因子ρ也是影响信息素浓度的重要因素。在蚁群算法利用信息素寻找最优路径的过程中，影响算法性能的重要因素除了信息素浓度的更新规则外，还有信息素挥发因子。为了保持算法在全局规划时的准确性和收敛速度，本发明加入了自适应信息素挥发因子ρ，如式：

其中，k为调整系数；N_c为当前迭代次数；N_max为最大迭代次数。在算法的早期阶段，挥发因子ρ很小，因为N_c很小，相应的每个路径的信息素浓度差很小；蚁群的指导功能削弱，这提高了蚂蚁的全球搜索范围和算法的准确性。随着N_c的越来越大，使得挥发因子ρ迅速增大，各路径的信息素浓度差增大，增强了蚁群的引导功能，提高了蚁群的搜索速度，使算法快速收敛。

步骤3：启动动态窗口法，对栅格化地图进行障碍物检测；若发现动态障碍物，则以最近的拐点作为子目标点；如果子目标点在障碍物上，那就下一个拐点作为子目标点；并利用加入路径融合距离函数后的动态窗口法进行动态避障。

计算路径融合距离函数：

其中，(x'_i,y'_i)为DWA在第i个局部窗口内基于预测速度轨迹而推演出的局部路径末端坐标；(x_i,y_i)为改进安全蚁群算法所得到的相应拐弯节点坐标。

将路径融合距离函数加入到评价函数中，改进后的评价函数为：

G_v,w＝σ(θ·heading(v,w)+κ·dist(v,w)+γ·velocity(v,w)+δ·path(ν,ω)) (12)

其中，heading(v,w)为方位角评价函数表示在当前速度下，模拟轨迹终点方向与目标之间的方位角偏差；dist(v,w)为速度对应轨迹上离障碍物的最近距离；velocity(v,w)为当前速度大小评价函数；path(ν,ω0为路径融合距离函数；σ为平滑系数；θ、κ、γ、δ分别为4项函数的加权系数，表示各项在评价函数中分配的权重比例。

计算最小的改进评价函数，以蚁群算法规划的拐点坐标(x_i,y_i)作为子目标点，如图2所示进行路径融合。进行窗口迭代，若在该阶段动态窗口中无人艇已到达了最近的子目标点，则结束该阶段的局部路径规划。

步骤4：对寻找子目标点进行循环，直至局部路径规划子目标点为全局路径规划的目标点，则表明该避障算法结束，输出全局最优安全路径。

为验证前文所设计算法的可行性和可靠性，实验环境为：WIN10，i5-6300HQ CPU；编译环境为：MATLAB R2020a仿真平台。采用传统蚁群算法与本专利所提安全蚁群算法进行比较分析；从上述路径规划算法的路径搜索长度、安全指数维度为指标进行评价，从而对比阐述本专利改进安全蚁群算法的优劣性。

路径搜索长度能直观展现蚁群算法根据转移概率搜索路径的效率大小。公式如式：

其中，n表示该路径长度中栅格的总个数；(x_i,y_i)表示当前栅格位置； (x_i+1,y_i+1)表示下一栅格位置。

安全指数确保了无人艇在航行时尽可能避免发生碰撞，该值和航线与障碍物距离直接相关。在本文中引入了通过路径的危险网格数，以评估安全性。安全指数定义如式：

其中：

其中，S_p为安全指数；D_io是从栅格路径到最近障碍物的距离；N_risk是全局路径上的危险网格数，定义无人艇路径规划时与障碍物距离小于等于1为危险网格；(x₀,y₀)为路径上最近障碍物坐标。

图3为蚁群算法对比实验结果图，由图3中(a)为采用本发明提出的安全蚁群输出路径图；图3中(b)为采用原始蚁群算法输出路径图；通过仿真结果可以计算路径长度和安全指数两种指标；结果如表1所示：

表1为安全蚁群算法和原始蚁群算法实验对比结果

	安全蚁群算法	原始蚁群算法
			路径搜索长度L<sub>p</sub>	32.5116	40.8701
安全指数S<sub>p</sub>	5.7742	2.0328

从计算结果可以看出，原始蚁群算法所规划路径冗余，且没有考虑到路线的平滑性，导致路径搜索长度L_p相较于本发明改进蚁群算法数值大了许多。此外，安全蚁群算法主要特点是能保证无人艇航行时自身安全性。和原始算法相比由于加入了危险系数这一指标，所以该算法在计算转移概率选择最优路径时都保持了和障碍物一定的安全距离，故而具有最大的安全指数S_p。

由图4中(a)至图4中(c)所示，对于水域环境信息，本发明能由出发位置到目标位置规划出一条可行路径，并且都能成功避障。相比于单一的蚁群算法，本发明避障算法具有安全蚁群算法保证规划路线的安全性，还具有动态窗口法能对动态障碍物平滑避障特性。

本发明经过避障算法规划后，相比于单一的蚁群算法具有安全蚁群算法路径搜索的高效率以及其很大安全指数，还具有动态窗口法能对动态障碍物平滑避障的特性；不仅能有效降低路径搜索长度，避障算法具有较大安全指数，以保证整个搜索过程中无人艇始终远离障碍物并处于低风险状态。此外，改进后的避障算法还避免了单一算法容易陷入局部最优解这一问题，提高了算法的稳定性。

Claims (2)

1.一种基于危险系数和距离函数的快速蚁群无人艇动态避障方法，其特征在于，包括以下步骤：

(1)对预先采集到的真实水域地图信息进行栅格化处理，并保存栅格化后的地图信息，确定无人艇航行初始地点和目标点；

(2)在栅格化地图上，启动蚁群算法，根据加入危险系数的转移概率进行全局路径的规划，储存其路径拐点坐标信息，并启动信息素更新策略；

(3)对栅格化地图进行障碍物检测，若发现动态障碍物，则以最近的拐点作为子目标点；如果子目标点在障碍物上，将下一个拐点作为子目标点；并利用加入路径融合距离函数后的动态窗口法进行动态避障；

(4)对步骤(3)寻找子目标点进行循环，直至局部路径规划子目标点为全局路径规划的目标点，则表明该避障算法结束，输出全局最优安全路径；

步骤(2)所述加入危险系数的转移概率为：

其中，τ_ij(t)为t时刻原始蚁群算法的信息素浓度，η_ij(t)是启发函数，表示t时刻当前网格 位置i移动到第j个栅格的路径启发式信息；ξ(t)为自适应障碍抑制因子，取值范围为[0,1]；α和β分别是信息素和启发式函数的影响因子；加入危险系数DI，将无人艇和障碍物质心之间的距离、无人艇相对障碍物矢面的惯量因子作为碰撞严重程度的主要影响，计算方法如下：

其中，f_cm(D_cm)为无人艇和障碍物质心之间的距离影响因子，

是距离比例因子；D_cmin、D_cmax分别是无人艇质心接近障碍物质心所允许的最小距离和对无人艇安全产生影响的最大距离；无人艇相对障碍物矢面的惯量影响因子：

其中，I_s为障碍物在基坐标系下的惯量矩阵的最大主转动惯量在无人艇矢状面上的分量，I_max为最大允许转动惯量，当I_s超过最大允许转动惯量时，若同障碍物发生潜在碰撞，无人艇将受到严重冲击；

将路径规划阶段总的危险系数定义为上述距离影响因子和惯量影响因子之积：

DI＝f_cm(D_cm)f_I(I_s) (6)

当危险系数DI比较大时，则自适应障碍抑制因子ξ(t)趋近于0，表示无人艇和障碍物之间的危险性较大；当危险系数DI比较小时，则ξ(t)趋近于1，表示无人艇和障碍物之间较为安全；

步骤(2)所述信息素更新策略加入自适应挥发因子，具体如下：

其中，Δτ_ij ^k为蚂蚁k在路径(i,j)上的信息素浓度；Q为信息素强度，L_bs为各个路径的最优指标；L_k为区域内最优路径长度；T_k为转移概率搜索时的转弯次数；

和

分别为区域内最优路径长度和转弯次数的加权因子；

τ_ij(t+1)＝(1-ρ)τ_ij(t)+Δτ_ij(t) (9)

其中，τ_ij(t+1)为(t+1)时刻原始蚁群算法的信息素浓度，Δτ_ij(t)为当前迭代路径(i,j)上信息素的增量；加入自适应信息素挥发因子ρ，如式：

其中，k为调整系数；N_c为当前迭代次数；N_max为最大迭代次数；

所述步骤(3)通过以下公式实现：

在动态窗口法评价函数上加入了路径融合距离函数：

其中，path(ν,ω)为路径融合距离函数，(x'_i,y'_i)为DWA在第i个局部窗口内基于预测速度轨迹而推演出的局部路径末端坐标；(x_i,y_i)为改进安全蚁群算法所得到的相应拐弯节点坐标；将路径融合距离函数加入到评价函数中，改进后的评价函数为：

G_v,w＝σ(θ·heading(v,w)+κ·dist(v,w)+γ·velocity(v,w)+δ·path(ν,ω)) (12)

其中，heading(v,w)为方位角评价函数表示在当前速度下，模拟轨迹终点方向与目标之间的方位角偏差；dist(v,w)为速度对应轨迹上离障碍物的最近距离；velocity(v,w)为当前速度大小评价函数；path(ν,ω)为路径融合距离函数；σ为平滑系数；θ、κ、γ、δ分别为4项函数的加权系数，表示各项在评价函数中分配的权重比例。

2.根据权利要求1所述的基于危险系数和距离函数的快速蚁群无人艇动态避障方法，其特征在于，步骤(1)所述栅格化后的地图信息能够显示障碍物以及周围环境信息，每一个栅格用唯一的序列号与其位置坐标相对应，其对应关系为：

其中，(x_i,y_i)为第i个栅格的位置坐标；i为第i个网格的序号；mod()是冗余运算符；ceil()表示取整操作；N_x和N_y分别为行方向和列方向的栅格数，a为比例系数。

Images