一种动态障碍环境下USV基于改进蚁群算法的避碰方法

技术领域

本公开一般涉及水面无人艇避碰规划技术领域，具体涉及一种动态障碍环境下USV基于改进蚁群算法的避碰方法。

背景技术

USV作为海上的智能化工具，USV避碰规划既是自动化的重要标识，也是自助航行的核心，所以USV完成任务的前提是其能够智能化避碰。常用的传统避碰规划方法包括蚁群算法、粒子群算法、遗传优化算法、人工势场法、深度学习方法等等。针对USV智能化避碰，系统方法除了安全度和平滑性之外，实时性也是重要的考核指标，然而基于上述传统方法，系统方法存在避碰的实时性与精度相互矛盾的问题。

关于USV的智能化避碰通常包括对静态障碍和动态障碍进行避碰，而在实际情况下，动态障碍又包括已知动态障碍和未知动态障碍，关于动态障碍，传统的避碰规划方法主要是针对已知动态障碍进行避碰，针对未知动态下的障碍进行避碰往往需要通过AIS设备等对动态障碍运动速度和运动方向信息进行获取，但是存在成本较高的问题，现需要一种仅通过导航器即可获取未知动态障碍信息的方法。

发明内容

鉴于现有技术中的上述缺陷或不足，期望提供一种动态障碍环境下USV基于改进蚁群算法的避碰方法以解决上述问题。

本申请提供一种动态障碍环境下USV基于改进蚁群算法的避碰方法，方法包括以下步骤：

S1：构建全局坐标系和局部坐标系；

S2：采用瞬时法对未知动态障碍进行预测，获得动态障碍的运动速度和运动方向；

S3：计算USV的碰撞危险度，判断是否存在碰撞危险；

S4：采用反向偏心膨化法对所述动态障碍进行膨化；

S5：根据改进蚁群算法获取USV下一时刻的运动速度和运动方向；

S6：重复步骤S2-S5，直至完成避碰。

根据本申请实施例提供的技术方案，步骤S1中，所述全局坐标系为北东坐标系；所述局部坐标系包括船体坐标系和导航器坐标系，所述船体坐标系为以船体USV为原点，以USV前进方向为X轴的直角坐标系；所述导航器坐标系为以导航器为极点，以USV前进方向为极轴的极坐标系。

根据本申请实施例提供的技术方案，步骤S2中，所述瞬时法具体包括：

S21：获取第一时刻动态障碍被导航器探测到最大范围的两个边缘点的导航器坐标，所述导航器坐标为在所述导航器坐标系下的坐标；

S22：根据两个所述边缘点坐标计算第一时刻动态障碍半径的最大值，作为安全膨化圆半径；

S23：根据两个所述边缘点的导航器坐标计算第一时刻动态障碍圆心的导航器坐标；

S24：重复S21-S23获得第二时刻动态障碍的导航器坐标，所述第二时刻为所述第一时刻经过第一时间间隔后的某一时刻；

S25：根据第一时刻动态障碍的导航器坐标和第二时刻动态障碍的导航器坐标计算动态障碍的运动速度和运动方向。

根据本申请实施例提供的技术方案，步骤S22中，根据公式(一)计算第一时刻动态障碍安全性膨化圆半径的最大值：

R

式中，d

根据本申请实施例提供的技术方案，步骤S25中具体包括：

S251：将所述第一时刻动态障碍的导航器坐标转换成第一全局坐标，所述第一全局坐标为第一时刻动态障碍在所述北东坐标系下的坐标，将所述第二时刻动态障碍的导航器坐标转换成第二全局坐标，所述第二全局坐标为第二时刻动态障碍在所述北东坐标系下的坐标；

S252：根据公式(二)和公式(三)计算动态障碍的运动速度：

v

式中，x

根据公式(四)计算动态障碍的运动方向：

式中，β表示动态障碍的运动方向与正东方向的夹角。

根据本申请实施例提供的技术方案，步骤S3中，计算USV的碰撞危险度具体包括计算空间危险度和时间危险度，根据所述空间危险度和时间危险度计算USV综合碰撞危险度。

根据本申请实施例提供的技术方案，步骤S4具体包括：

S41：反向偏心动态障碍膨化圆半径；

S42：反向偏心动态障碍膨化圆圆心。

根据本申请实施例提供的技术方案，步骤S5中，基于双矢量角的状态转移规则优化蚁群算法，根据蚁群算法获取USV下一时刻的航行方向和航行速度。

与现有技术相比，本申请的有益效果在于：采用瞬时法对动态障碍的运动速度和运动方向进行预测，将未知动态障碍转化为已知动态障碍，替代传统方法中通过AIS等设备对动态障碍移动状态进行检测，使得USV仅通过探测到动态障碍的位置即可获得动态障碍的运动速度和运动方向，结合改进的蚁群算法可实现对未知动态障碍的有效避碰。本申请提供的一种动态障碍环境下USV基于改进蚁群算法的避碰方法具有避碰效率高，节省USV上装置成本的优点。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：

图1为本申请提供的一种动态障碍环境下USV基于改进蚁群算法的避碰方法的步骤流程图；

图2为本申请中全局坐标系和局部坐标系示意图；

图3为本申请中未知动态障碍运动预测示意图；

图4为本申请中动态障碍反向偏心膨胀模型示意图；

图5为本申请中USV与动态障碍运动速度模型示意图；

图6为本申请中基于避碰规则的USV相遇仿真图；

图7为本申请中基于避碰规则的USV追越仿真图；

图8为本申请中基于避碰规则的USV左、右交叉仿真图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

请参考图1，本申请提供一种动态障碍环境下USV基于改进蚁群算法的避碰方法，方法包括以下步骤：

S1：构建全局坐标系和局部坐标系；

S2：采用瞬时法对未知动态障碍进行预测，获得动态障碍的运动速度和运动方向；

S3：计算USV的碰撞危险度，判断是否存在碰撞危险；

S4：采用反向偏心膨化法对所述动态障碍进行膨化；

S5：根据改进蚁群算法获取USV下一时刻的运动速度和运动方向；

S6：重复步骤S2-S5，直至完成避碰。

工作原理：采用瞬时法对动态障碍的运动速度和运动方向进行预测，将未知动态障碍转化为已知动态障碍，替代传统方法中通过AIS等设备对动态障碍移动状态进行检测，使得USV仅通过探测到动态障碍的位置即可获得动态障碍的运动速度和运动方向，结合改进的蚁群算法可实现对未知动态障碍的有效避碰。本申请提供的一种动态障碍环境下USV基于改进蚁群算法的避碰方法具有避碰效率高，节省USV上装置成本的优点。

在一优选实施方式中，所述全局坐标系为以界面左下角为原点，X轴为正东方向，Y轴为正北方向的北东坐标系，所述局部坐标系包括船体坐标系和导航器坐标系，所述船体坐标系为以船体USV为原点，以USV前进方向为X轴的直角坐标系；所述导航器坐标系为以导航器为极点，以USV前进方向为极轴的极坐标系。

具体的，如图2所示，USV所在位置的全局坐标为(x

具体的，所述全局坐标系、船体坐标系和导航器坐标系下的点坐标可进行相互转换，所述转换具体为：

如图2所示，若已知障碍在所述导航器坐标系X

也可按照下式将其转换为全局坐标系X

同理，若已知障碍在全局坐标系X

在一优选实施方式中，所述瞬时法具体包括：

S21：获取第一时刻动态障碍被导航器探测到最大范围的两个边缘点的导航器坐标，所述导航器坐标为在所述导航器坐标系下的坐标；

S22：根据两个所述边缘点坐标计算第一时刻动态障碍半径的最大值，作为安全膨化圆半径；

S23：根据两个所述边缘点的导航器坐标计算第一时刻动态障碍圆心的导航器坐标；

S24：重复S21-S23获得第二时刻动态障碍的导航器坐标，所述第二时刻为所述第一时刻经过第一时间间隔后的某一时刻；

S25：根据第一时刻动态障碍的导航器坐标和第二时刻动态障碍的导航器坐标计算动态障碍的运动速度和运动方向。

在一优选实施方式中，根据公式(一)计算第一时刻动态障碍安全性膨化圆半径的最大值：

R

式中，d

具体的，如图3所示，在所述第一时刻t时刻，USV的导航器在A点发现动态障碍O

根据上述运动参数，计算出动态障碍安全性膨化圆在导航器坐标系下的坐标为(d

式中，d

利用坐标变换，可计算出t时刻动态障碍安全膨化圆在北东坐标系下的坐标为(x

式中，x

动态障碍O

v

动态障碍O

式中，β表示动态障碍的运动方向与正东方向的夹角；

本申请中将第一时间间隔Δt看作是瞬时值，可忽略，即认为动态障碍O

y-y

在一优选实施方式中，计算USV的碰撞危险度具体包括计算空间危险度和时间危险度，根据所述空间危险度和时间危险度计算USV综合碰撞危险度。

具体的，空间危险度u

式中，DCPA表示最短会遇距离，d

时间危险度u

当DCPA＞0时，

式中，TCPA表示到达最短会遇的时间，

综合碰撞危险度：

在一优选实施方式中，步骤S4具体包括：

S41：反向偏心动态障碍膨化圆半径；

S42：反向偏心动态障碍膨化圆圆心。

具体的，如图4所示，反向偏心膨化圆半径R

反向偏心膨化圆圆心O

式中，ζ为反向偏心膨化圆的偏心角度；当Δv沿弧线转向v

如图5所示，图中以USV为中心建立船体坐标系，动态障碍为安全性膨化圆O'，其膨化圆半径R'，T为安全性膨化圆O'一切点，USV速度为(v

如图5可知只需abs(γ)≥μ保持在任意时刻即可保证USV无风险航行。

式中，Δv

实时调整角度γ满足abs(γ)≥μ，即|γ+Δγ|≥μ，对γ进行求导：

假设动态障碍运动速度不会突变，即dv

因此动态USV避碰规划问题等同于瞬时静态USV多条件优化问题，如下公式所示：

式中，f(Δv

改进蚁群优化算法：

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

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

针对未知动态障碍基于方向角权值的状态转移规则如下所示：

式中，τ为信息素浓度函数；allowed

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

Step1：使用基于相对位置检测的可视性判断方法来统计包括起点和终点在内的所有已经经过膨化的环境障碍物顶点的可视点，每一个顶点g

Step2：初始化τ

Step3：设置蚂蚁种群编号k＝1；

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

Step5：设此时蚂蚁当前位置为g

Step6：若蚂蚁当前位置为终点g

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

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

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

针对于高速运动条件下的USV，基于Qt平台首先对基于《国际海上避碰规则》的动态一直环境的避碰规划分为相遇、追越、左交叉和右交叉这四种会遇情况进行仿真验证，如图6-8所示。

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

以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。