CN110320907A - 一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法 - Google Patents

Abstract

本发明提供一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法，首先，利用MKLINK图论技术为无人艇生成海上环境地图模型，然后对蚁群算法的状态转移概率进行改进优化出无人艇的全局最短路径，同时判断是否会有运动碍航物相遇而来，有则调用局部层的椭圆锥碰撞推演算法实时调整无人艇的动态路径。最后，将两种智能避碰方法通过滚动协调配合的原理进行耦合，形成无人水面艇海上航行时的双层智能避碰方法。本发明通过无人水面艇路径规划技术和基于椭圆碰撞锥推演模型的局部避碰技术滚动协同配合，实现无人艇在执行搜救、勘察作业等任务的时候能够自主搜索全局路径和调整局部避开运动碍航物的路径。

Description

一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇 双层避碰方法

技术领域

本发明涉及一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法，属于无人水面艇路径规划与局部路径动态避碰领域。

背景技术

无人水面艇(unmanned surface vehicle，USV)是解决近海海域、海洋岛礁海域海图序列不完整问题的有效科技平台，同时也是提升我国海洋军事装备能力，维护我国海洋权益的重要海洋装备。现如今，随着人工智能算法和大数据库的发展和成熟，USV的很多技术瓶颈和理论研究取得了很大的突破，其研究现状以及发展趋势表明USV在海洋环境监测、海洋气象预报、海洋水文观测、海图绘制、海洋资源勘探与开采、领海监视等军事、科研等领域已经拥有了非常广阔的应用前景。而支撑起无人水面艇迅猛发展的重要技术手段之一便是基于路径规划与局部路径动态避碰的无人水面艇自主航行技术。

发明内容

本发明的目的是为了克服基本蚁群算法存在的缺陷和无人水面艇单一路径规划技术所遇到的无法调整局部路径以避开运动碍航物而提供一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法。

本发明的目的是这样实现的：步骤如下：

步骤一：基于Maklink图论的环境地图模型的构建；

步骤二：针对无人水面艇艏向角偏差进行改进的蚁群算法设计；

步骤三：基于椭圆碰撞锥推演模型的无人水面艇避碰艏向角设计。

本发明还包括这样一些结构特征：

1.步骤一具体包括：

1.1)利用生成的大量Maklink线段来实现无人艇在海上航行时对电子海图进行环境建模；

1.2)将形成障碍物凸多边形的顶点和中线的中点之间的连线以及顶点与边界平行连线称为Maklink线段，根据Maklink自由链接线的生成方法得到的自由链接线上的点为中点；

1.3)通过Maklink线上的中点来构建无向网络线，最终这些Maklink线段和顶点、中点，形成无人水面艇在海洋中航行时的初步地图模型。

2.步骤二具体包括：

2.1)点S为全局路径规划的起点；点T为全局路径规划的终点；P_i-1为当前蚁群所在的点；P_i为蚁群的下一个搜索点；ψ_t为蚁群当前节点连线的方向角；ψ_r为蚁群起点和终点连线的方向角，F_a＝|ψ_r-ψ_t|；

2.2)计算蚁群由节点i到节点j的概率：

式中，χ为艏向角偏差启发因子，体现无人艇的第六自由度r对全局路径规划的重要性，χ越大表示艏向角偏差启发因子对蚂蚁选择下一个节点的权重，越大越不容易偏离到达目的地的方向。

3.步骤三具体包括：

3.1)对运动着的障碍物进行椭圆聚类建模，提出了在二维海洋环境中点与椭圆的碰撞锥推演算法，点A(USV)与椭圆F分别以V_A、V_F运动，α和β是速度方向，椭圆F的长、短轴分别为a和b，C₁、C₂是椭圆的两个焦点：

式中Vr和V_θ分别表示A与F各自的相对速度分量，且进一步化简得到：

(r₀₁V_r01-r₀₂V_r02)²+(r₀₁V_θ01+r₀₂V_θ02)²≤4a²(V² _θ0+V² _r0)

以上条件只在t_m非负值的情况下才是有效碰撞的条件，因此，把t_m＞0代入上式得到：

V_r02V_θ01+V_r01V_θ02≤0

3.2)在所有USV会发生碰撞的角度集合为[α₁,α₂]时，其采用的避碰措施大多是不改变航速，仅改变航向，椭圆碰撞锥模型在计算出碰撞区间后，依据当前航向转向最小角度来决定避碰角度，从而进行USV局部路线调整；

3.3)假设USV此时当前航向角为α₀，如果abs(α₁-α₀)＞abs(α₂-α₀)，那么避碰角度其中为避碰角度，ζ是为了保障避碰安全而人为规定的安全角度限制，大小可以根据USV与运动着的障碍物的尺寸决定，其中，ζ的取值按照下式选取：

式中：S为椭圆障碍物船只与USV长轴尺寸比例，本文中为了简化仿真，取ζ＝π/18，反之，如果abs(α₁-α₀)＜abs(α₂-α₀)，那么，避碰角度则为

4.还包括步骤四：局部层动态路径段避碰结束点的确定；

无人水面艇行驶至A点时这一刻可判断已经成功避开了碍航物并到达了避碰结束点，具体是：

当γ＞π/2时，局部层避碰算法认为USV成功避开了碍航物，而且有如下的条件满足且此时d最小时：

d₁＝d+σ,σ≥d/10

则USV到达避碰结束点A₁；判断USV在局部层避碰算法下的避碰结束点是根据d和γ两个因素来的。

与现有技术相比，本发明的有益效果是：1、本发明通过无人水面艇路径规划技术和基于椭圆碰撞锥推演模型的局部避碰技术滚动协同配合，实现无人艇在执行搜救、勘察作业等任务的时候能够自主搜索全局路径和调整局部避开运动碍航物的路径，根据障碍物的分布情况实施避碰策略，确保无人艇自主航行时的安全性；2、本发明将基本蚁群算法的局部路径启发度和蚂蚁搜寻下一个节点的状态转移概率加以改进，既能解决智能蚁群算法的陷入局部最优解的缺点，还大大提高了无人艇搜寻全局最短路径的效率和智能化，完成了无人艇从搜索附近航行环境，收集并分析障碍物信息，到制定避碰对策，完成对障碍物的躲避的一系列过程，有效提升了无人艇的智能化，减少了操控人员的工作量；3、本发明的碰撞锥推演的可预测模型可以在USV局部路径上有效地对两个未知轨迹的移动目标进行碰撞检测，如两个点之间的碰撞预测、点与圆之间的碰撞预测、点与球体之间的碰撞预测等，其优点是可预测性、实时性、高精度和安全性。但是在海平面复杂环境中USV遇到的运动着的大多数障碍物是船只，而船只的长短轴的比例很悬殊，如果对船只采用圆形障碍物建模，会大大降低USV的避碰效率。因此，本发明局部层避碰方法将对船只等障碍物采用椭圆状建模，从而大大提高USV在局部路径上的避碰效率。

附图说明

图1本发明的双层避碰滚动规划原理图；

图2本发明的基于Maklink图论的无人水面艇环境地图模型；

图3本发明的无人水面艇艏向角偏差示意图；

图4本发明的改进蚁群算法设计流程图；

图5本发明的局部椭圆聚类-碰撞锥推演模型图；

图6本发明的局部动态路径避碰结束点说明图；

图7本发明的无人水面艇双层智能避碰方法规划结果图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

首先，利用MKLINK图论技术为无人艇生成海上环境地图模型，然后对蚁群算法的状态转移概率进行改进优化出无人艇的全局最短路径，同时判断是否会有运动碍航物相遇而来，有则调用局部层的椭圆锥碰撞推演算法实时调整无人艇的动态路径。最后，将两种智能避碰方法通过滚动协调配合的原理进行耦合，形成无人水面艇海上航行时的双层智能避碰方法。

1)基于Maklink图论的环境地图模型的构建：

要保护的特征在于采用Maklink图论的方法进行环境地图建模；

1.4)Maklink图论是利用生成的大量Maklink线段来实现无人艇在海上航行时对电子海图进行环境建模；

1.5)将形成障碍物凸多边形的顶点和中线的中点之间的连线以及顶点与边界平行连线称为Maklink线段，根据Maklink自由链接线的生成方法得到的自由链接线上的点为中点；

1.6)通过Maklink线上的中点来构建无向网络线，最终这些Maklink线段和顶点、中点，形成了无人水面艇在海洋中航行时的初步地图模型；

2)针对无人水面艇艏向角偏差进行改进的蚁群算法设计：

要保护的特征在于通过无人艇的艏向角偏差改进蚁群算法的状态转移概率；

2.1)点S为全局路径规划的起点；点T为全局路径规划的终点；P_i-1为当前蚁群所在的点；P_i为蚁群的下一个搜索点；ψ_t为蚁群当前节点连线的方向角；ψ_r为蚁群起点和终点连线的方向角，F_a＝|ψ_r-ψ_t|；

2.2)计算蚁群由节点i到节点j的概率：

式中，χ为艏向角偏差启发因子，体现无人艇的第六自由度r对全局路径规划的重要性，χ越大表示艏向角偏差启发因子对蚂蚁选择下一个节点的权重，越大越不容易偏离到达目的地的方向；

3)基于椭圆碰撞锥推演模型的无人水面艇避碰艏向角设计：

要保护的特征在于通过椭圆碰撞锥推演算法求出无人艇避开运动碍航物的艏向避碰角度；

3.1)对运动着的障碍物进行椭圆聚类建模，提出了在二维海洋环境中点与椭圆的碰撞锥推演算法，点A(USV)与椭圆F分别以V_A、V_F运动，α和β是速度方向，椭圆F的长、短轴分别为a和b，C₁、C₂是椭圆的两个焦点：

式中Vr和V_θ分别表示A与F各自的相对速度分量，且进一步化简得到如下公式：

(r₀₁V_r01-r₀₂V_r02)²+(r₀₁V_θ01+r₀₂V_θ02)²≤4a²(V² _θ0+V² _r0)

以上条件只在t_m非负值的情况下才是有效碰撞的条件，因此，把t_m＞0代入上式得到：

V_r02V_θ01+V_r01V_θ02≤0

3.2)在所有USV会发生碰撞的角度集合为[α₁,α₂]时，其采用的避碰措施大多是不改变航速，仅改变航向，椭圆碰撞锥模型在计算出碰撞区间后，依据当前航向转向最小角度来决定避碰角度，从而进行USV局部路线调整；

3.3)假设USV此时当前航向角为α₀，如果abs(α₁-α₀)＞abs(α₂-α₀)，那么避碰角度其中为避碰角度，ζ是为了保障避碰安全而人为规定的安全角度限制，大小可以根据USV与运动着的障碍物的尺寸决定，其中，ζ的取值按照下式选取：

式中：S为椭圆障碍物船只与USV长轴尺寸比例，本文中为了简化仿真，取ζ＝π/18，反之，如果abs(α₁-α₀)＜abs(α₂-α₀)，那么，避碰角度则为

本发明的目的可以通过以下技术方案来实现：

1)基于国际上标准的S57电子海图的处理

电子海图中储存着丰富的海洋环境信息，一张电子海图通常存储着三十多个图层，在每一层通过点、线、面的形式对所处环境进行描述。对于无人艇的路径规划，研究中主要关心陆地、障碍物、水深等信息。研究中对电子海图的处理分以下步骤进行，首先是对原始的S57标准的电子海图的查看采用的是eLane Viewer2.0软件，在选定海图区域后采用FME软件对.000格式的电子海图进行转换，得到shapefile格式的分层信息文件，之后采用ArcGIS软件选取需要的图层输出矢量图，最后用MATLAB对叠加了不同信息矢量图格式文件进行处理，成为可以利用的环境模型。

1.1)首先是采用eLane Viewer2.0软件对S57格式的电子海图进行查看，显示出无人艇作业的区域。eLane Viewer2.是一款免费的电子海图查看软件，能够显示符合国际标准的S-57格式的电子海图。这在实际的应用中同样存在现实意义，对于岸上工作人员对当前海洋信息的判断具有明显的帮助。

1.2)然后选取电子海图C1100103中的一部分进行查看，在电子海图中可以查看到磁差、深度范围、海域名等等十分丰富的信息，但是此种信息还不能够直接利用。接下来利用FME软件，对.000格式的电子海图进行转换，得到shapefile格式的分层信息文件。

1.3)在得到了shapefile格式的电子海图后，在ArcGIS软件中打开。最后以济州岛到珍岛之间的海域为例，对其中的电子海图信息提取，得到可供无人水面艇使用的海上环境地图。

2)基于Maklink图论的环境地图模型的构建

本发明采用Maklink图论的方法进行环境地图建模。选取济州岛到珍岛之间的海域当做是海上地图建模的对象，运用Maklink图论法进行建模，整个过程和方法步骤如下：

2.1)Maklink图论是利用生成的大量Maklink线段来在环境地图的二维路径中规划可行空间，比如中线和无向自由连接线。

2.2)都将形成障碍物凸多边形的顶点和中线的中点之间的连线以及顶点与边界平行连线称为Maklink线段，根据Maklink自由链接线的生成方法得到自由链接线上的点为中点。

2.3)有如下的算法流程生成无向网络图。

1.集合中的元素属于障碍物凸多边形的顶点，集合中的点到边界会作各自的垂线并按一定的序号储存在另外的一个集合里。

2.选取集合中的第一条线段。

3.判断线段与凸多边形边界是否相交。若相交，则放弃该线段，不能让其成为Maklink线段，接下来查看集合中的下一条线段，若不相交，进入步骤4。

4.查看Maklink线段与凸多边形的顶点，它们之间会产生的两个外角。两个外角均小于180°的时候，那么这条链接线便是最佳的路线。

5.审查备选连接线中是否有外角是大于180°的。有的话则选择集合中的下一条线段，并返回步骤3；没有的话，则进入步骤6。

6.删除多余的除Maklink线段之外的连接线段。

7.多次循环步骤1～6，一直遍历到所有顶点。

接下来，通过Maklink线上的中点来构建无向网络线，最终这些Maklink线段和顶点、中点，就形成了无人水面艇在海洋中航行时的初步地图模型。

3)针对蚂蚁局部路径启发度进行改进的蚁群算法设计

对蚁群的局部路径启发函数做了如下改进：

蚁群会优先选择转移到下一节点的距离小于均值这样路径长度很大于均值的节点蚂蚁自然就能不会考虑了，这种设计改善了算法过早的陷入局部极小值。同样，当可见度为距离还大时，算法中较差节点被选择的概率就会被增加，这样就保证了不会出现局部极小值的点被选择了。

4)针对无人水面艇艏向角偏差进行改进的蚁群算法设计

计算蚁群由节点i到节点j的概率；

式中，χ为艏向角偏差启发因子，体现无人艇的第六自由度r对全局路径规划的重要性，χ越大表示艏向角偏差启发因子对蚂蚁选择下一个节点的权重，越大越不容易偏离到达目的地的方向。

5)基于椭圆碰撞锥推演模型的无人水面艇避碰艏向角设计

5.1)对运动着的障碍物进行椭圆聚类建模，提出了在二维海洋环境中点与椭圆的碰撞锥推演算法，点A(USV)与椭圆F分别以V_A、V_F运动，α和β是速度方向，椭圆F的长、短轴分别为a和b，C₁、C₂是椭圆的两个焦点：

式中Vr和V_θ分别表示A与F各自的相对速度分量。对上述两式微分，则有：

5.2)根据定义当t＝t_m，此时r达到最小值，即r＝r_m，对于椭圆上的任意一点(以中心点C为例)，点A与该点的最近距离和最近时刻如下：

r² _m＝r² ₀V² _θ0/(V² _r0+V² _θ0)

t_m＝-r₀V_r0/(V² _r0+V² _θ0)

C₁和C₂表示椭圆的两个焦点，r₁和r₂分别代表AC₁和AC₂的长度，V_r1和V_r2分别代表对应于AC₁和AC₂的相对速度。另外，θ₁和θ₂分别代表AC₁和AC₂的角度取向，可以定义V_θ1和V_θ2：

(r₁+r₂)_m＜2a

5.3)当r₁+r₂达到最小值时即r₁+r₂＝(r₁+r₂)_m＝r_m，如果存在一个t_m，则此时V_r1(t_m)+V_r2(t_m)＝0。如果不等式成立(其中a是椭圆的长轴长)，点A会和椭圆发生碰撞。

对AC₁可以得到结果r₁：

r² ₁＝r² _m1+(V² _θ01+V² _R01)(t-t_m1)²

对AC₂也可以得到类似的结果r₂：

r² ₂＝r² _m2+(V² _θ02+V² _R02)(t-t_m2)²

则有：

那么，r_m≤2a的条件可以重新写成：

(r₀₁V_r01-r₀₂V_r02)²+(r₀₁V_θ01+r₀₂V_θ02)²≤4a²(V² _θ0+V² _r0)

以上条件只在t_m非负值得情况下才是有效碰撞的条件。因此，把t_m＞0代入上式得到：

V_r02V_θ01+V_r01V_θ02≤0

5.4)以上两式便是点与椭圆的碰撞判别条件，求出所有会发生碰撞的角度集合为[α₁,α₂]，该集合称之为点与椭圆的碰撞锥。在所有USV会发生碰撞的角度集合为[α₁,α₂]时，其采用的避碰措施大多是不改变航速，仅改变航向。椭圆碰撞锥模型在计算出碰撞区间后，依据当前航向转向最小角度来决定避碰角度，从而进行USV局部路线调整。碰撞锥区间集合N记作：

N＝{α:α₁＜α＜α₂}

其中0＜α₁＜2π,0＜α₂＜2π。

5.5)假设USV此时当前航向角为α₀，如果abs(α₁-α₀)＞abs(α₂-α₀)，那么避碰角度其中为避碰角度，ζ是为了保障避碰安全而人为规定的安全角度限制，大小可以根据USV与运动着的障碍物的尺寸决定，其中，ζ的取值按照下式选取：

式中：s为椭圆障碍物船只与USV长轴尺寸比例，本发明中为了简化仿真，取ζ＝π/18，反之，如果abs(α₁-α₀)＜abs(α₂-α₀)，那么，避碰角度则为

6)局部层动态路径段避碰结束点的确定

在无人水面艇避碰的过程中，若不能准确的判断出避碰结束点，会让无人水面艇的行驶路径达不到能耗最优，在最糟糕的情况下，会导致避碰失败。无人水面艇行驶至A点时这一刻可以判断已经成功避开了碍航物并到达了避碰结束点。具体算法规则是，当γ＞π/2时，局部层避碰算法认为USV成功避开了碍航物，而且有如下的条件满足且此时d最小时：

d₁＝d+σ,σ≥d/10

则USV到达避碰结束点A₁。可见，判断USV在局部层避碰算法下的避碰结束点是根据d和γ两个因素来的。由于USV自身的航迹回归算法，其将会沿着回归算法产生的回归航迹角度前进。当USV打舵回归航迹到一定角度，碍航物又出现在碰撞危险区，使得USV还得根据γ来判断是否在避碰结束点。

附图1是本发明的方法流程图，具体介绍了本方法在无人水面艇双层智能避碰中的工作流程与作用。

实施例1按以下步骤实现：

第1步：附图2首先是采用eLane Viewer2.0软件对S57格式的电子海图进行查看，显示出无人艇需要作业的区域。然后选取电子海图C1100103中的一部分进行查看，在电子海图中可以查看到磁差、深度范围、海域名等等十分丰富的信息，但是此种信息还不能够直接利用。接下来利用FME软件，对.000格式的电子海图进行转换，得到shapefile格式的分层信息文件。在得到了shapefile格式的电子海图后，在ArcGIS软件中打开。得到可供无人水面艇在济州岛到珍岛之间海域的海上环境地图模型。

第2步：通过附图3中的方法对蚁群算法进行改进使无人艇快速收敛到全局最短路径和附图4给出改进蚁群算法步骤设计软件仿真程序，针对步骤1的环境地图模型优化全局最短路径。

第3步：局部层基于椭圆聚类-碰撞锥推演模型的动态路径避碰

附图5和附图6给出了当无人水面艇在全局静态路径上航行时遇到运着的碍航物相遇而来，如何为无人水面艇调整局部路径，改变局部艏向角以避开碍航物，然后继续回到全局段路径上继续安全航行的方法原理。此时当遇到运动着的船只交叉相遇而来，通过算法计算得到USV与船只的碰撞区间为α∈[32.21°,58.58°]，按照USV与碍航物的轨迹知此时两者之间的角度为225°，属于《国际海上避碰规则》中的交叉相遇事件。根据《国际海上避碰规则》的约束，USV向左打舵的艏向角避开碍航物来调整局部路径。

第4步：附图6中，在无人水面艇避碰的过程中，若不能准确的判断出避碰结束点，会让无人水面艇的行驶路径达不到能耗最优，在最糟糕的情况下，会导致避碰失败。无人水面艇行驶至A点时这一刻可以判断已经成功避开了碍航物并到达了避碰结束点。具体算法规则是，当γ＞π/2时，局部层避碰算法认为USV成功避开了碍航物，而且有如下的条件满足且此时d最小时：

d₁＝d+σ,σ≥d/10

认为USV到达避碰结束点A₁。可见，判断USV在局部层避碰算法下的避碰结束点是根据d和γ两个因素来的。由于USV自身的航迹回归算法，其将会沿着回归算法产生的回归航迹角度前进。当USV打舵回归航迹到一定角度，碍航物又出现在碰撞危险区，使得USV还得根据γ来判断是否在避碰结束点。本发明结合几何关系和发生碰撞的条件综合来确定准确、有效的碰撞结束点，当USV到达了避碰结束点后，安全回归到全局层设定航迹上继续自主航行。

第5步：规划的结果如附图7.

对于某型无人水面艇，双层智能避碰系统的初始化如下：

在双层智能避碰算法中，对蚁群寻迹的下一个节点概率作如下修改：

其中第一部分是当碍航物到达USV船载激光雷达扫描范围，第二部分是当碍航物未到达USV船载激光雷达扫描范围，在《国际海上避碰规则》蚂蚁寻迹的下一个节点：

J是USV船载激光雷达扫测到碍航物在安全距离内时根据所调整的艏向角到达的下一个节点。其他参数不变，m＝10,NC＝500，还是在上面提及的三种海洋环境中仿真智能避碰算法的可行性。

信息素选择阈值：pheThres＝0.8，

信息素计算参数：pheCacuPara＝2，

信息素更新参数：pheUpPara＝[0.1,0.0003]，

启发信息参数：qfzPara1＝ones(10,1)*0.5,qfzPara2＝1.1

蚂蚁数量：m＝10，

迭代代数：NC＝500，

链接线点比例参数：h_i＝pathk(l+1,k)/10，

USV实时方向角：ψ_t＝acos(angleend-anglestart)，

USV期望方向角：根据S和T的坐标计算出。

本实例的最终规划结果如附图7所示，并且实现了本发明的发明效果，无人水面艇向目标点航行过程中，通过双层智能避碰系统的协调配合，使得无人水面艇在航行途中遇到静态的、运动着的碍航物都能进行安全自主避碰，顺利到达预设目标点。

实施例2按以下步骤实现：

本实施例与实施例1基本相同，特别之处如下：

所述步骤3中，此时当遇到运动着的船只正面相遇而来，通过算法计算得到USV与船只的碰撞区间为α∈[39.72°,50.28°]，属于《国际海上避碰规则》中的正面相遇事件。根据《国际海上避碰规则》的约束，USV向右打舵的艏向角避开碍航物来调整局部路径。

实施例3按以下步骤实现：

本实施例与实施例2基本相同，特别之处如下：

所述步骤3中，此时当遇到运动着的船只追赶相遇而来，通过算法计算得到USV与船只的碰撞区间为α∈[43.86°,48.14°]，属于《国际海上避碰规则》中的追赶相遇事件。根据《国际海上避碰规则》的约束，此时USV向右打舵的艏向角追赶碍航物进行超越来调整局部路径。

综上，本发明涉及一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法，其包括以下步骤：1.利用国际上标准S57电子海图信息处理方式和Maklink图论技术为无人水面艇生成海上航行的环境地图模型。2.在全局层采用改进蚁群算法，对蚂蚁寻找下一节点的转移概率和局部路径启发度进行改进，为无人水面艇优化出一条从出发点到目的地的最短路径，此路径能有效避开静态的山、礁和岛屿。3.在局部层采用基于椭圆聚类-碰撞锥推演的可预无人水面艇动态避碰模型，为无人水面艇在全局最短路径段航行时实时规划出避开运动碍航物的局部动态路径。4.对提出的无人水面艇双层智能避碰控制算法进行了仿真分析和实例测试，得到所证明的结果。

Claims (5)

1.一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法，其特征在于：步骤如下：

步骤一：基于Maklink图论的环境地图模型的构建；

步骤二：针对无人水面艇艏向角偏差进行改进的蚁群算法设计；

步骤三：基于椭圆碰撞锥推演模型的无人水面艇避碰艏向角设计。

2.根据权利要求1所述的一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法，其特征在于：步骤一具体包括：

1.1)利用生成的大量Maklink线段来实现无人艇在海上航行时对电子海图进行环境建模；

1.2)将形成障碍物凸多边形的顶点和中线的中点之间的连线以及顶点与边界平行连线称为Maklink线段，根据Maklink自由链接线的生成方法得到的自由链接线上的点为中点；

1.3)通过Maklink线上的中点来构建无向网络线，最终这些Maklink线段和顶点、中点，形成无人水面艇在海洋中航行时的初步地图模型。

3.根据权利要求2所述的一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法，其特征在于：步骤二具体包括：

2.1)点S为全局路径规划的起点；点T为全局路径规划的终点；P_i-1为当前蚁群所在的点；P_i为蚁群的下一个搜索点；ψ_t为蚁群当前节点连线的方向角；ψ_r为蚁群起点和终点连线的方向角，F_a＝|ψ_r-ψ_t|；

2.2)计算蚁群由节点i到节点j的概率：

式中，χ为艏向角偏差启发因子，体现无人艇的第六自由度r对全局路径规划的重要性，χ越大表示艏向角偏差启发因子对蚂蚁选择下一个节点的权重，越大越不容易偏离到达目的地的方向。

4.根据权利要求3所述的一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法，其特征在于：步骤三具体包括：

3.1)对运动着的障碍物进行椭圆聚类建模，提出了在二维海洋环境中点与椭圆的碰撞锥推演算法，点A(USV)与椭圆F分别以V_A、V_F运动，α和β是速度方向，椭圆F的长、短轴分别为a和b，C₁、C₂是椭圆的两个焦点：

式中Vr和V_θ分别表示A与F各自的相对速度分量，且进一步化简得到：

(r₀₁V_r01-r₀₂V_r02)²+(r₀₁V_θ01+r₀₂V_θ02)²≤4a²(V² _θ0+V² _r0)

以上条件只在t_m非负值的情况下才是有效碰撞的条件，因此，把t_m＞0代入上式得到：

V_r02V_θ01+V_r01V_θ02≤0

3.2)在所有USV会发生碰撞的角度集合为[α₁,α₂]时，其采用的避碰措施大多是不改变航速，仅改变航向，椭圆碰撞锥模型在计算出碰撞区间后，依据当前航向转向最小角度来决定避碰角度，从而进行USV局部路线调整；

3.3)假设USV此时当前航向角为α₀，如果abs(α₁-α₀)＞abs(α₂-α₀)，那么避碰角度其中为避碰角度，ζ是为了保障避碰安全而人为规定的安全角度限制，大小可以根据USV与运动着的障碍物的尺寸决定，其中，ζ的取值按照下式选取：

式中：s为椭圆障碍物船只与USV长轴尺寸比例，本文中为了简化仿真，取ζ＝π/18，反之，如果abs(α₁-α₀)＜abs(α₂-α₀)，那么，避碰角度则为

5.根据权利要求4所述的一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法，其特征在于：还包括步骤四：局部层动态路径段避碰结束点的确定；

无人水面艇行驶至A点时这一刻可判断已经成功避开了碍航物并到达了避碰结束点，具体是：

当γ＞π/2时，局部层避碰算法认为USV成功避开了碍航物，而且有如下的条件满足且此时d最小时：

d₁＝d+σ,σ≥d/10

则USV到达避碰结束点A₁；判断USV在局部层避碰算法下的避碰结束点是根据d和γ两个因素来的。