CN114355899A - 一种基于动态导航船域与速度障碍的无人船避碰方法 - Google Patents

Abstract

本发明涉及一种基于动态导航船域与速度障碍的无人船避碰方法，其包括：根据会遇船舶尺度、操纵性和相对速度、相对方位角，建立起以干扰船为中心的动态导航船域；基于建立的动态导航船域设计基于动态导航船域的风险评估模型以判断无人船和干扰船间碰撞风险，若具有碰撞风险，则根据避碰规则、基于集合制导原理以及速度障碍物法决定避让航向、避让路点以及避让速度，规划避碰路径并判断何时结束避碰；本发明使无人船能够沿预先规划的路径航行的同时规避障碍物，为船舶安全航行提供基础。

Description

一种基于动态导航船域与速度障碍的无人船避碰方法

技术领域

本发明涉及无人船自主动态避碰方法，具体涉及一种基于动态导航船域与速度障碍的无人船避碰方法。

背景技术

在过去几十年中，无人船(Unmanned Surface Vessel,USV)在科学、民用和军事等领域得到广泛应用。无人船在海上航行时易受到洋流等环境干扰以及海上动、静态障碍物的影响，并且由于局部路径规划是保证USV在海上安全航行及作业的必要条件，已成为USV领域研究热点。

局部路径规划是根据实时检测到的干扰船信息确定局部回避路径，因此也称为避碰。一种常用的局部路径规划方法为速度障碍法(Velocity Obstacle,VO)，该方法首次应用于机器人领域，之后被Kuwata等人结合《国际海上避碰规则》(COLREGs)应用于无人船领域。然而，对于在复杂多动态干扰船中运动的USV，将导致连续的微小转向，加剧无人船磨损，避让效果不够理想。由此，基于Moe等人[1]提出的集合控制方法，Myre[2]提出无人船动态避碰集合制导(Set-based Guidance,SBG)方法。该方法克服了多干扰船避碰时由VO引起的航向振荡，然而该方法认为无人船以恒定速度行驶，将导致避碰效果不理想甚至失败。

此外，影响避碰效果的一个重要因素是避碰操纵时机的确定。为确定避碰操纵时机引入风险评估模型，当风险大于阈值时，无人船应从路径跟踪模式切换至避碰模式。为评估风险需建立船域，SBG反应区域和安全区域相当于固定半径的圆形船域，不能反映无人船和干扰船的速度及不同遭遇情况的影响。

因此，需要综合考虑无人船操纵性能、会遇船舶间相对方位角、相对速度、外界干扰以及避碰规则进行无人船避碰决策及局部路径规划。

参考文献：

[1]Moe S.,Teel A.R.,Antonelli G.,et.al,Experimental results for set-based control within the singularity-robust multiple task-priority inverskinematics[C].2015 IEEE Conference on Robotics and Biomimetics,Zhuhai,China,2015,pp.1233-1239.

[2]Myre H.,Collision avoidance for autonomous surface vehicles usingvelocity obstacle and set-based guidance[D].Norwegian University of Scienceand Technology,2016.

发明内容

(一)要解决的技术问题

为了解决现有技术的上述问题，本发明提出一种基于动态导航船域与速度障碍的无人船避碰方法。使用无人船参数、航速及无人船与干扰船的相遇情况，建立基于动态导航船域的风险评估模型，用于评估无人船航行时的风险，当风险大于阈值时开始避碰操作，包括避碰操作方式以及避碰路点设计。然后，使用VO生成最优避碰速度并控制无人船跟踪该速度。

(二)技术方案

为了达到上述目的，本发明采用的主要技术方案包括：

101、通过无人船传感器和船舶自动识别系统，获取无人船和干扰船的基本信息，并基于无人船和干扰船相对方位角ψ以及避碰规则确定船舶间会遇局面，包括追越局面、对遇局面和交叉局面；对遇局面选择以干扰船航向为中心的30度角，交叉局面为每侧97.5度角，其余范围视为追越情况；

102、根据会遇船舶尺度、操纵性和相对速度、相对方位角，以干扰船为中心建立动态导航船域；

103、根据最近会遇时间(TCPA)、最近会遇距离(DCPA)、船速比、相对方位、相对距离(D)、船舶操纵性、海上能见度等情况建立基于动态导航船域的风险评估模型(CRI)；

104、基于建立的动态导航船域风险评估模型判断无人船风险度是否大于阈值，若大于阈值，则进行避碰操作，根据避碰规则，确定避碰操作方式以及避碰路点并判断何时结束避碰；

步骤102包括：根据会遇船舶尺度、操纵性、相对速度以及相对方位角，以干扰船为中心建立动态导航船域，表达式如下：

其中s(i)是反映相遇情况的系数：

式中：R_f、R_s分别为动态导航船域长轴半径和短轴半径；N_f，N_s为考虑了干扰船对动态导航船域影响的参数；L是船长；AD是进距，无人船从开始转向90度时重心的纵向前进距离；DT是旋回初径，无人船从开始转向180度时重心的横向距离；ΔV是本船和干扰船的相对速度；Ks为速度比例系数；V是本船的速度；ψ是相对方位角；

步骤103包括：根据最近会遇时间(TCPA)、最近会遇距离(DCPA)、船速比、相对方位、相对距离(D)、船舶操纵性、海上能见度等情况建立基于动态导航船域的风险评估模型(CRI)，使风险度能随实际情况的变化而变化，表达式如下：

U_CRI＝w₁×U_DCPA+w₂×U_TCPA+w₃×U_D

式中：U_CRI、U_DCPA、U_TCPA、U_D分别表示CRI、DCPA、TCPA和D的隶属度函数值；w₁、w₂、w₃分别表示隶属度函数的权重值；

隶属度函数计算公式可以表达如下：

其中：

DCPA＝||(p₁+v₁×TCPA)-(p₂+v₂×TCPA)||

式中：p₁和p₂分别为两船位置矢量；v₁和v₂分别为两船速度矢量；d₁为不同会遇形势下船舶领域边界，本文d₁＝R_f，d₂＝Kd₁，K为系数，由船舶状态不稳定性及两船行动不协调和设备误差等因素确定，一般取K＝2；t₁为最早避让时间，t₂为最晚避让时间；D₃为最晚避让距离，D₄是可以采取措施的避让距离。K₃由能见度决定，在能见度不良时可以适度扩大K₃的值，能见度良好时可适当减小，一般情况下K₃等于1；K₂取决于当前水域情况，一般水域时，K₂等于1，在船舶密度较大的复杂水域，可以适当减小K₂的值；

隶属度函数值的确定，按照重要性从大到小排序为DCPA>TCPA>D，采用层次分析法对隶属度函数值w₁、w₂、w₃的权重进行确定。

步骤104包括：根据干扰船和无人船的位置，结合动态导航船域计算避碰点的位置，无人船避碰点WT₁由下式确定：

式中：C为比例因子；(x_OS，y_OS)和(x_TS，y_TS)分别为无人船和干扰船的位置。

同理避碰点WT₂，WT₃，WT₄均可以计算出，之后分别延长WT₃和WT₁所在的直线，直到产生交点wp₁，该交点就是其中一个避碰路点，同理可以计算出另一个避碰路点wp₂；之后根据速度障碍法中碰撞锥的概念设计碰撞锥以及根据避碰点确定的航向，通过对无人船速度离散化，建立速度评价函数对无人船速度进行选取，碰撞锥表达式为：

VO_A|B＝{v_A|＝v_BA·p_l≥0∩v_AB·p_r≥0}

其中：

式中：VO_A|B是P_A和P_B形成碰撞锥；P_AB是P_B相对于P_A的相对位置；v_AB是P_B相对于P_A的相对速度；p_l和p_r分别为碰撞锥的左边界和右边界形成的指向P_B的矢量；R是坐标旋转矩阵，将矢量P_AB转到p_l和p_r对应的方向，γ是中心线和圆锥体边缘之间的角度，R_f是由船域确定的长轴半径；

基于建立的碰撞锥的模型，将参考速度空间进行离散化以建立速度评价函数F，对速度进行选取；速度评价函数公式为：

其中：

式中：norm(·)是将·归一化为(0～1)的数；u_i是离散化的速度(本船要选择的前进速度)；Q是一个二阶正定矩阵；

是0或1的常数，当u_i在碰撞锥内

为1，否则

为0。

(三)有益效果：

本发明的有益效果是：

1、使用动态导航船域风险评估模型决定碰撞风险以及何时开始避碰，并考虑了无人船和干扰船的船速、航向、位置、船长等因素以及无人船的操纵性能指标等因素建立动态导航船域，使碰撞风险能够根据会遇状态的变化而变化，有效避免了避碰操作太早导致的无效避碰路径过长，造成成本增加，以及由于避碰操作太晚导致避碰失败。

2、当避碰路点确定后，使用VO算法根据实际情况选择合适速度，克服基于SBG避碰方法速度为恒值的不足；防止避碰速度太快无法及时躲避干扰船以及速度太慢导致追越等局面无法躲避干扰船。

附图说明

图1是基于动态导航船域与速度障碍的无人船避碰方法流程图；

图2为避碰路点位置计算原理；

图3为多个干扰船的避碰效果图；

图4为洋流3节时无人船速度变化曲线；

图5为CRI变化曲线；

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。

本发明实例所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本发明实施例中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在限制本发明。本发明实例所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。

本发明在无人船避碰的基础上，综合考虑干扰船对无人船的影响，提出一种基于动态导航船域与速度障碍的无人船避碰方法。针对无人船在海洋环境航行中遇到的干扰船实时建立适应当前航行环境的基于动态导航船域的风险评估模型以及使用速度障碍物法为无人船提供可变的航速，从而可以为无人船安全航行提供基础。

图1为本发明实施例提供的一种基于动态导航船域与速度障碍的无人船避碰方法的流程图，如图1所示，该方法包括以下步骤：

步骤一：通过无人船传感器和船舶自动识别系统，获取无人船和干扰船的基本信息，并基于无人船和干扰船相对方位角ψ以及基于避碰规则确定船舶间会遇局面；

步骤二：根据会遇船舶尺度、操纵性和相对速度、相对方位角，建立以干扰船为中心的动态导航船域，并建立基于动态导航船域的风险评估模型；

步骤三：基于建立的动态导航船域风险评估模型判断无人船风险度是否大于阈值，若大于阈值，执行步骤四；如果没有大于阈值，执行步骤六；

步骤四：根据避碰规则和动态导航船域计算避碰点以及根据相对方向角ψ及避碰规则选择合适的避碰点，并根据避碰点以及速度障碍物法计算无人船避让航速；

步骤五：判断是否结束避碰，如果没有继续朝着避碰点前进，反之，转向步骤六；

步骤六：执行路径跟踪模式，判断是否到达目标点。如果没有则转向步骤一，否则，整个过程结束。

以下对图1所示实施例的各个步骤的具体实现进行详细阐述：

在步骤一中确定无人船和干扰船间相对速度和相对方位角ψ，并基于避碰规则确定船舶间会遇局面；

在本发明的一个实施例中，根据避碰规则确定的会遇局面包括追越局面、对遇局面和交叉局面。对遇局面选择以干扰船航向为中心的30度角，交叉局面为每侧97.5度角，其余范围视为追越情况；

在步骤二中根据会遇船舶尺度、操纵性和相对速度、相对方位角，建立以干扰船为中心的动态导航船域，并建立基于动态导航船域的风险评估模型；

在本发明的一个实施例中，由于会遇双方船舶的船长、船速、航向、位置以及无人船自身操纵特性等因素都将影响避碰响应距离，从而影响着船域大小。比如说，相对船速越大避碰响应距离越大安全域也就越大。不同的会遇局面也影响着船域大小。比如说，对遇局面要求的安全域大，其次是交叉局面，最后是追越局面。因此不能把船域定义为一个固定的圆形区域而是能够根据会遇状态和无人船自身操纵特性进行实时在线变化。本发明设计的动态导航船域充分考虑无人船操纵特性、船舶尺度、会遇船舶间相对速度和相对方位角等因素影响，形成了尺寸范围可变的动态导航船域，表达式为：

其中s(i)是反映相遇情况的系数：

式中：R_f、R_s分别为动态导航船域长轴半径和短轴半径；N_f，N_s为考虑了干扰船对动态导航船域的影响因素；L是船长；AD是进距，无人船从开始转向90度时重心的纵向前进距离；DT是旋回初径，无人船从开始转向180度时重心的横向距离；ΔV是本船和干扰船的相对速度；Ks为速度比例系数；V是本船的速度；ψ是相对方位角；

在本发明一个实施例中，定义一个基于动态导航船域的风险评估模型(CRI)，根据船域的情况实时的对无人船周围目标进行风险分析。影响风险评估的因素有很多，一般包括最近会遇时间(TCPA)、最近会遇距离(DCPA)、船速比、相对方位、相对距离(D)、船舶操纵性、海上能见度等情况。其中，TCPA和DCPA是影响风险评估关键因素。因此，根据TCPA、DCPA、船速比、相对方位、相对距离、船舶操纵性、海上能见度等情况建立基于船域风险评估模型；其公式如下：

U_CRI＝w₁×U_DCPA+w₂×U_TCPA+w₃×U_D

式中：U_CRI、U_DCPA、U_TCPA、U_D分别表示CRI、DCPA、TCPA和D的隶属度函数值；w₁、w₂、w₃分别表示隶属度函数的权重值；

隶属度函数计算公式可以表达如下：

其中：

DCPA＝||(p₁+v₁×TCPA)-(p₂+v₂×TCPA)||

式中：p₁和p₂分别为两船位置矢量；v₁和v₂分别为两船速度矢量；d₁为不同会遇形势下船舶领域边界，本文d₁＝R_f，d₂＝Kd₁，K为系数，由船舶状态不稳定性及两船行动不协调和设备误差等因素确定，一般取K＝2；t₁为最早避让时间，t₂为最晚避让时间；D₃为最晚避让距离，D₄是可以采取措施的避让距离。K₃由能见度决定，在能见度不良时可以适度扩大K₃的值，能见度良好时可适当减小，一般情况下K₃等于1；K₂取决于当前水域情况，一般水域时，K₂等于1，在船舶密度较大的复杂水域，可以适当减小K₂的值。

其隶属度函数值的确定，按照重要性从大到小排序为DCPA>TCPA>D，采用层次分析法对隶属度函数值w₁、w₂、w₃的权重进行确定在本发明中，w₁、w₂、w₃权重参数分别为0.6333、0.2605、0.1062。

步骤三：基于建立的动态导航船域风险评估模型判断风险度是否大于阈值，如果大于阈值执行步骤四，如果没有执行步骤六；

在本发明的一个实例中，避碰时机是影响避碰效果的一个重要因素。避碰时机过早，会导致无人船过早偏离原先航迹，导致无效航迹过长；避碰时机过晚会导致无人船躲避不及时，导致无人船与干扰船构成紧迫局面，甚至危险局面以致相撞。因此，满足一定风险时，无人船应当执行避碰操作。在本发明中，认为当风险大于阈值ζ＝0.1时，无人船开始避碰操作。

步骤四：根据避碰规则和动态导航船域计算避碰点并根据相对方向角ψ及避碰规则选择合适的避碰点，并根据避碰点以及速度障碍物法计算无人船避让航速；

在本发明的一个实例中，无人船需要根据干扰船及动态导航船域计算避碰路点。图2为避碰路点位置计算原理图，其公式为：

式中：C为比例因子，(x_OS，y_OS)和(x_TS，y_TS)分别为无人船和干扰船的位置。

同理WT₂，WT₃，WT₄均可以计算出，之后分别延长WT₃和WT₁所在的直线，直到产生交点wp₁，该交点就是其中一个避碰路点，同理可以计算出另一个避碰路点wp₂；在本发明中，选择追越局面、对遇局面、交叉局面比例因子C各为1。

在计算出避碰路点从而决定航向之后，需要根据实际航行环境确定无人船航速，防止避碰速度太快无法及时躲避干扰船以及速度太慢导致追越等局面无法躲避干扰船。为设计碰撞锥，首先选择上述避碰点确定的航向，通过对速度离散化，建立速度评价函数对速度进行选取。其碰撞锥表达式为：

VO_A|B＝{v_A|＝v_BA·p_l≥0∩v_AB·p_r≥0}

其中：

式中：VO_A|B是P_A和P_B形成碰撞锥；P_AB是P_B相对于P_A的相对位置；v_AB是P_B相对于P_A的相对速度；p_l和p_r分别为碰撞锥的左边界和右边界形成的指向P_B的矢量；R是坐标旋转矩阵，将矢量P_AB转到p_l和p_r对应的方向，γ是中心线和圆锥体边缘之间的角度，R_f是由船域确定的长轴半径；

基于建立的碰撞锥模型，将参考速度空间进行离散化以建立速度评价函数F，对速度进行选取；速度评价函数公式为：

其中：

式中：norm(·)是将·归一化为(0～1)的数；u_i是离散化的速度，也就是本船要选择的前进速度；Q是一个二阶正定矩阵；

是0或1的常数，当u_i在碰撞锥内

为1，否则

为0。

图3为多干扰船的避碰效果图；单独的黑色形状为无人船(OS)；按照相遇时间，黑色形状及其实心的封闭区域分别代表干扰船1(TS1)、干扰船2(TS2)和干扰船(TS3)及其各自动态导航船域。4个黑色的点表示OS的目标点。从图3中可以明显的看出在12s时OS遇到TS1形成交叉局面完成大幅度转向动作，在18.5s时TS1避碰结束；在52s时，OS到达第一个设置目标点，之后转向第二个设置目标点前进；在77s时OS对对遇局面构成风险的TS2进行避碰操作，在89s时避碰结束；在125s时，OS到达第二个设置目标点，之后转向第三个设置目标点前进；在137.5s对追越局面构成风险的TS3进行避碰，并在168s避碰结束，在188.5s到达设置的第三个目标点。仿真结果表明，该算法对不同局面的场景完成避碰操作。

三个干扰船的初始坐标分别为(350,-60)、(1100,1400)和(500,1200)，三个干扰船的速度分别为15m/s、9m/s和6m/s，航向分别为144°、243°和0°，路径规划的下一路径点为(550,275)、(890,1200)、(1900,1200)。当遇到多干扰船的情况下，无人船使用基于动态导航船域风险评估模型评估最危险的干扰船，若有碰撞危险，则开始规避这一干扰船，规避结束后开始复航。当检测到与第二个干扰船有碰撞危险时，开始规避第二个干扰船，当检测到与第三个干扰船有碰撞危险时，开始规避第三个干扰船。

图4表示洋流为3节时OS速度变化曲线；从图中可以看出OS在避碰过程中能够根据实际的情况不断选择避碰速度，验证所提出的碰撞锥能够根据实际情况选择不同的速度。在8.8s时，OS的速度开始减小，说明此刻计算的避碰点在碰撞锥的范围内，OS根据速度评价函数选择合适的速度，之后速度缓慢增加；在第73s时，速度减小说明躲避TS2，在77s速度开始增加至设定的速度说明此时完成转向速度不在碰撞锥内；在第135s速度增加开始执行超车动作，随后OS速度减缓至设置速度15m/s到达目标点。

图5表示洋流为3节时，在基于动态导航船域风险评估的前提下，本发明和无速度改变方法CRI变化曲线，从图中可以看出传统CRI曲线值要比本发明曲线CRI值高一些，尤其是在追越和交叉局面下CRI达到了1并且保持一段时间，这说明，此时OS进入到紧迫局面。从图中可以看出本发明CRI没有达到1，说明在避碰时并没有造成紧迫局面。

Claims (2)

1.一种基于动态导航船域与速度障碍的无人船避碰方法，其特征在于，其包括：

101、通过无人船传感器和船舶自动识别系统，获取无人船和干扰船的基本信息，并基于无人船和干扰船相对方位角ψ以及避碰规则确定船舶间会遇局面，包括追越局面、对遇局面和交叉局面；对遇局面选择以干扰船航向为中心的30度角，交叉局面为每侧97.5度角，其余范围视为追越情况；

102、根据会遇船舶尺度、操纵性和相对速度、相对方位角，以干扰船为中心建立动态导航船域，表达式如下：

其中s(i)是反映相遇情况的系数：

式中：R_f、R_s分别为动态导航船域长轴半径和短轴半径；N_f，N_s为考虑了干扰船对动态导航船域的影响参数；L是船长；AD是进距，无人船从开始转向90度时重心的纵向前进距离；DT是旋回初径，无人船从开始转向180度时重心的横向距离；ΔV是本船和干扰船的相对速度；Ks为速度比例系数；V是本船的速度；ψ是相对方位角；

103、根据最近会遇时间(TCPA)、最近会遇距离(DCPA)、船速比、相对方位、相对距离(D)、船舶操纵性、海上能见度等情况建立基于动态导航船域风险评估模型(CRI)，使风险度能随实际情况的变化而变化，表达式如下：

U_CRI＝w₁×U_DCPA+w₂×U_TCPA+w₃×U_D

式中：U_CRI、U_DCPA、U_TCPA、U_D分别表示CRI、DCPA、TCPA和D的隶属度函数值；w₁、w₂、w₃分别表示隶属度函数的权重值；

隶属度函数计算公式可以表达如下：

其中：

式中：p₁和p₂分别为两船位置矢量；v₁和v₂分别为两船速度矢量；d₁为不同会遇形势下船舶领域边界，本文d₁＝R_f，d₂＝Kd₁，K为系数，由船舶状态不稳定性及两船行动不协调和设备误差等因素确定，一般取K＝2；t₁为最早避让时间，t₂为最晚避让时间；D₃为最晚避让距离，D₄是可以采取措施的避让距离；K₃由能见度决定，在能见度不良时可以适度扩大K₃的值，能见度良好时可适当减小，一般情况下K₃等于1；K₂取决于当前水域情况，一般水域时，K₂等于1，在船舶密度较大的复杂水域，可以适当减小K₂的值；

其隶属度函数值的确定，按照重要性从大到小排序DCPA>TCPA>D，采用层次分析法对隶属度函数值w₁、w₂、w₃的权重进行确定；

104、基于建立的动态导航船域风险评估模型判断无人船风险度是否大于阈值，若大于阈值，则进行避碰操作，根据避碰规则，结合动态导航船域确定避碰操作方式以及避碰路点并判断何时结束避碰。

2.根据权利要求1所述的一种基于动态导航船域与速度障碍的无人船避碰方法，其特征在于：所述104包括：

根据干扰船和无人船的位置，结合动态导航船域计算避碰点的位置，无人船避碰点WT₁由下式确定：

式中：C为比例因子；(x_OS，y_OS)和(x_TS，y_TS)分别为无人船和干扰船的位置；

同理避碰点WT₂，WT₃，WT₄均可以计算出，之后分别延长WT₃和WT₁所在的直线，直到产生交点wp₁，该交点就是其中一个避碰路点，同理可以计算出另一个避碰路点wp₂；之后根据速度障碍法中碰撞锥的概念设计碰撞锥以及根据避碰点确定的航向，通过对无人船速度离散化，建立速度评价函数对无人船速度进行选取；其碰撞锥表达式为：

VO_A|B＝{v_A|＝v_BA·p_l≥0∩v_AB·p_r≥0}

其中：

式中：VO_A|B是P_A和P_B形成碰撞锥；P_AB是P_B相对于P_A的相对位置；v_AB是P_B相对于P_A的相对速度；p_l和p_r分别为碰撞锥的左边界和右边界形成的指向P_B的矢量；R是坐标旋转矩阵，将矢量P_AB转到p_l和p_r对应的方向，γ是中心线和圆锥体边缘之间的角度，R_f是由船域确定的长轴半径；

基于建立的碰撞锥的模型，将参考速度空间进行离散化以建立速度评价函数F，对速度进行选取；速度评价函数公式为：

其中

式中：norm(·)是将·归一化为(0～1)的数；u_i是离散化的速度(本船要选择的前进速度)；Q是一个二阶正定矩阵；

是0或1的常数，当u_i在碰撞锥内

为1，否则

为0。

Images