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

权利要求书

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

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

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

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

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

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

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

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(α₂-α₀)，那么，避碰角度则为

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

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

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

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

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

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

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

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

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