CN112965475A - 一种基于动态导航船域和避碰规则的障碍物避碰方法 - Google Patents

Abstract

本发明涉及一种基于动态导航船域和国际海上避碰规则的障碍物避碰方法，其包括：根据无人船传感器和船舶自动识别***，获取无人船和干扰船的基本信息，确定无人船和干扰船相对速度和相对方位角β，并基于避碰规则确定船舶间会遇局面；根据会遇船舶尺度、操纵性和相对速度、相对方位角，分别以无人船和干扰船为中心建立动态导航船域；判断无人船和干扰船的船域是否相交，若相交，则具有碰撞风险，则进行避碰操作，根据避碰规则，确定遭遇单干扰船和多干扰船时避碰操作方式，确定局部避碰路点，规划避碰路径并判断何时结束避碰。本发明使无人船能够沿预先规划的路径航行的同时规避障碍物，为船舶安全航行提供基础。

Description

一种基于动态导航船域和避碰规则的障碍物避碰方法

技术领域

本发明涉及无人船自主动态避碰方法，尤其涉及一种基于动态导航船域和避碰规则的障碍物避碰方法。

背景技术

无人船可在许多科学、民用和军事领域中进行各种应用，具有能耗低、劳动力成本低的优点，许多国家都在大力发展无人船。据研究，大约50％的海上事故是由人为错误引起的，另外30％的事故是由人类发现和预防的。使用智能防撞***，无人船可以避免人为失误，减少损失。无人船路径规划分为全局路径规划和局部路径规划，其中局部路径规划根据传感器动态检测到的障碍物信息通过避碰算法确定局部避碰路径，是无人船避碰***核心。

当前无人船避碰通常基于船域或最小会遇距离(DCPA)判断碰撞风险，确定何时开始避碰。DCPA方法相当于利用圆形船域来判定碰撞风险，因其未能考虑船舶会遇状态，从而不能充分估计船舶碰撞危险及进行避碰决策。船域是船舶航行时在其四周必须保持的安全区域。在不同遭遇状态下利用船域对本船和干扰船进行安全判断有四种标准，包括：(a)自身船域不被侵犯；(b)不侵犯对方船域；(c)双方船域互不侵犯；(d)双方船域不相交。基于这些标准，利用船域可以判断碰撞风险并确定进行避碰操作的时间。然而，现有关于船域的研究较少回答如何进行局部避碰路径规划的问题。

早期的统计船域通常基于船长确定而忽略了船速和遭遇方式等因素建立，为静态模型，没有考虑船舶在不同船速下航行时对碰撞危险的影响。此外，船舶操纵性能对船舶避碰时间有很大影响，操纵性能差的船舶应该保持更大的安全距离。由此提出了解析的动态船域，如四元船域，该船域为由四个半径定义的组合椭圆，半径的尺寸综合考虑了船舶操纵性、船速和航向等信息。但该船域由四个半径确定，直接用于避碰路径规划较为复杂。

此外，传统避碰算法当无人船与目标船间距超过一固定距离时才结束避碰，这将导致避碰轨迹过长，增加运行成本。另外，为使船舶间能对避碰操作达成一致的理解，所有海上航行船舶都应遵守《国际海上避碰规则》(简称“避碰规则”)的规定，否则将由于不能理解对方的避碰行为而引起碰撞危险。

发明内容

(一)要解决的技术问题

为了解决现有技术的上述问题，本发明提出一种基于动态导航船域(Dynamicnavigation ship domain,DNSD)和避碰规则的障碍物避碰方法。在船域的设计上充分考虑了无人船操纵特性、船舶尺度、会遇船舶间相对速度和相对方位角等因素影响以及船艏和船尾安全会遇距离的不同，形成了尺寸可变的动态导航船域，实时地反映船舶避碰存在的风险。此外，通过基于避碰规则的不同会遇局面确定避碰操作并判断何时结束避碰操作，为船间动态避碰提供决策依据，降低航行成本。

(二)技术方案

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

101、通过无人船传感器和船舶自动识别***，获取无人船和干扰船的基本信息，确定无人船和干扰船相对速度和相对方位角β，并基于避碰规则确定船舶间会遇局面；

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

103、判断无人船和干扰船的船域是否相交，若相交，则具有碰撞风险，进行避碰操作，根据避碰规则，确定遭遇单干扰船和多干扰船时避碰操作方式，确定局部避碰路点，规划避碰路径并判断何时结束避碰。

作为本发明方法的一种改进，101包括：所述船舶传感器和自动识别***获取的信息包括干扰船的船长、船速、航向、位置等，以及无人船自身船速、航向、位置、船长和无人船操纵性能指标：滞距、重心横向移动距离、重心纵向移动距离等。

作为本发明方法的一种改进，101还包括：基于避碰规则确定船舶间的会遇局面包括追越局面、对遇局面和交叉局面。为了确定会遇局面，定义了无人船和干扰船之间的相对方位角β。对遇局面选择为以干扰船航向为中心的30度角，交叉局面为每侧97.5度角，其余范围视为追越情况。相对方位角β计算如下：

其中

是无人船确定的最危险干扰船的航向；(x_TS,y_TS)为无人船确定的最危险干扰船船位；(x_OS,y_OS)是无人船自身船位。

作为本发明方法的一种改进，102包括：根据会遇船舶尺度、操纵性和相对速度、相对方位角，分别以干扰船和无人船为中心建立动态导航船域，动态导航船域由一个半椭圆和一个半圆组成，它们仅由两个半径(包括R_f和R_s)确定。该模型易于实现局部避障路径规划。为了便于动态导航船域的确定，在地球坐标系原点处的

坐标系中建立了动态导航船域。

和

轴分别朝向与x和y轴相同的方向。动态导航船域可以表达如下：

其中sgn(·)是符号函数，定义为:

椭圆和圆半径的确定考虑了会遇船舶尺度、操纵性和相对速度、相对方位角和会遇局面。动态半径描述如下：

式中，

是无人船与干扰船的相对速度，其中^vOS为无人船的速度；^vTS为干扰船的速度；||·||₂为二范数。C为常数；Re为无人船的滞距，当两船的距离小于滞距之和时，认为无法避让，大约为船长的1～2倍；B是船的宽度；DT为旋回初径是无人船转向180°时重心的横向距离，大约为船长的3～6倍；AD为近距又称纵距是无人船从转向开始旋转90°时重心的纵向前进距离，大约为DT的0.6～1.2倍；

是动态安全域系数，其值取决于无人船与干扰船的相对方位角β，相对方位角在0°时，此时相对速度最大，AD对动态安全域的影响比较大，故此时的ki比较大，相对方位角在180°时，此时相对速度最小，DT对动态安全域的影响比较大，故此时ki的系数比较小。s为取值范围为0.8～1.2的常数，其值取决于会遇船舶主尺度，可以对ki的系数实现微调。

作为本发明方法的一种改进，103包括：当无人船的动态导航船域和干扰船的动态导航船域相交时，且随时间变化距离逐渐缩短时认为两船存在碰撞风险，此时应当执行避碰操作，并根据会遇局面和避碰规则规划避碰点；反之，继续执行路径跟踪。

为了判断船域是否相交，需要对无人船和干扰船的动态导航船域进行操作。首先，应围绕原点旋转

坐标中定义的动态导航船域，使

轴指向船舶的航向。然后，将船舶域转换为船体坐标系o_bx_by_bz_b的原点。坐标旋转矩阵定义为：

然后可以得到船域曲线：

其中(x_DNSD,y_DNSD)是地球坐标系中动态导航船域的坐标。

根据避碰规则，对于对遇和交叉局面，无人船应向右转向避碰；对于追越局面，无人船既可向右也可向左转向进行避碰。

确定追越局面避碰操纵转向的方法为：通过无人船位置与干扰船位置的连线来划分避碰操作区域，如果下一全局目标点在连线的右边，无人船从右舷通过；否则，从左舷通过。

作为本发明方法的一种改进，103还包括：根据干扰船的位置和DNSD计算避碰点的位置；无人船的避碰点的计算公式为：

或

其中，R＝r₁×R_s(OS)+r₂×R_s(TS),是一个调整航路点的参数，它由r₁和r₂调整，其范围在(0-1)之间。

作为本发明方法的一种改进，103还包括：计算无人船何时结束避碰从而切换为路径跟踪模式。当无人船已经通过干扰船动态导航船域的半椭圆和半圆的交界线，并且无人船和干扰船之间的当前距离大于前一时刻的距离(d_t>d_t-1)时，避障模式应切换到路径跟随模式，以便无人船跟踪下一目标点。

(三)有益效果：

本发明的有益效果是：

(1)使用无人船和干扰船动态导航域是否相交用来决定碰撞风险以及何时开始避碰，并考虑了无人船和干扰船的船速、航向、位置、船长等因素以及无人船的操纵性能指标(滞距、重心横向移动距离、重心纵向移动距离)等因素，使碰撞风险能够根据会遇状态的变化而变化，有效避免了避碰操作太早导致的无效避碰路径过长，造成成本增加，以及由于避碰操作太晚导致避碰失败。

(2)基于避碰规则和无人船以及干扰船的动态导航船域确定避碰点，方法简单，有效的避免了无人船与干扰船碰撞风险；

(3)以无人船远离干扰船的动态导航船域，而非行驶到避碰点来判断何时结束避碰，有效解决了结束避碰太晚导致无效避碰路径过长而导致的避碰能耗增加。

附图说明

图1是基于动态导航船域和避碰规则的障碍物避碰方法流程图；

图2是基于国际海事避碰规则的会遇状态；

图3是动态导航船域示意图；

图4是基于避碰规则的无人船与干扰船避让情景；

图5是无人船避碰路径点示意图；

图6是结束避障示意图；

图7是单个干扰船情况下无人船避碰示意图；

图8是多个干扰船情况下无人船避碰示意图；

图9单个干扰船动态导航船域变化示意图；

图10多个干扰船的动态导航船域变化示意图；

具体实施方式

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

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

本发明在无人船避碰的基础上，综合考虑干扰船对无人船的影响，提出了一种基于动态导航船域和避碰规则的障碍物避碰方法。针对无人船在海洋环境航行中遇到的干扰船实时建立适应当前航行环境的船域，建立的船域模型可根据无人船与干扰船会遇状态及无人船操纵特性的不同而实时变化，从而可以为无人船安全航行提供基础。

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

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

步骤二：根据会遇船舶尺度、操纵性和相对速度、相对方位角，分别以无人船和干扰船为中心建立动态导航船域；

步骤三：根据确定的动态导航船域判断无人船和干扰船的动态导航船域是否相交，如果相交执行步骤四，如果没有执行步骤六；

步骤四：根据避碰规则和动态导航船域计算避碰点，并根据相对方向角β及避碰规则选择合适的避碰点，从而得到避碰路径；

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

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

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

在步骤一通过无人船传感器和船舶自动识别***，获取无人船和干扰船基本信息。

在本发明的一个实例中，所述船舶传感器和自动识别***获取的信息包括干扰船的船速、航向、位置、船长等，同时包括无人船自身船速、航向、位置、船长和通过旋回实验确定的操纵性能指标(滞距、重心横向移动距离、重心纵向移动距离)。

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

在本发明的一个实施例中，根据避碰规则确定的会遇局面包括追越局面、对遇局面和交叉局面。为了确定会遇局面，定义了无人船和干扰船之间的相对方位角β。对遇局面选择为以无人船航向为中心的30度角，交叉局面为每侧97.5度角，其余角视为超车情况，图2是基于避碰规则的不同会遇局面。相对方位角β计算如下：

其中

是无人船确定的最危险干扰船的航向；(x_TS,y_TS)为无人船确定的最危险干扰船船位；(x_OS,y_OS)是无人船自身船位。

在步骤二中根据会遇船舶尺度、操纵性和相对速度、相对方位角，分别以无人船和干扰船为中心建立动态导航船域。在本发明的一个实施例中，选择出一个最危险的干扰船，对无人船来说至关重要，因此，本发明利用最近会遇距离方法确定无人船中最具有危险的干扰船，若无人船中最具有危险的干扰船有多个，无人船会随机选择一个最具有危险的干扰船。

在本发明的一个实施例中，图3为动态导航船域示意图。船域是指无人船航行安全范围，一旦无人船进入到干扰船的船域中将具有极大的碰撞危险。由于会遇双方船舶的船长、船速、航向、位置，以无人船自身操纵特性等因素都将影响避碰响应距离，从而影响着安全域的大小。比如说，相对船速越大避碰响应距离越大，船域也就越大。相同会遇条件下船艏和船尾的安全距离也是不同的，船艏的安全会遇距离长，船尾的安全会遇距离短。因此不能把船域定义为一个固定的圆形区域或者椭圆区域而是能够根据会遇状态和无人船自身操纵特性进行实时变化的船域。本发明动态导航船域设计充分考虑了无人船操纵特性、船舶尺度、会遇船舶间相对速度和相对方位角等因素影响以及相同会遇条件下船艏和船尾安全会遇距离的不同，形成了尺寸范围可变的动态导航船域。

本发明涉及的动态导航船域由一个半椭圆和一个半圆组成，根据会遇船舶尺度、操纵性和相对速度、相对方位角，分别以无人船和干扰船为中心建立动态导航船域。该模型易于实现局部避障路径规划。为了便于动态导航船域的确定，在地球坐标系原点处的

坐标系中建立了动态导航船域。

和

轴分别朝向与x和y轴相同的方向。具体的动态导航船域公式可以写成如下：

其中sgn(·)是符号函数，定义为:

椭圆和圆半径的确定考虑了会遇船舶尺度、操纵性和相对速度、相对方位角和会遇局面。动态船域半径描述如下：

式中，

是无人船与干扰船的相对速度，其中^vOS为无人船的速度；^vTS为干扰船的速度；||·||₂为二范数。C为常数；Re为无人船的滞距，当两船的距离小于滞距之和时，认为无法避让，大约为船长的1～2倍；B是船的宽度；DT为旋回初径是无人船转向180°时重心的横向距离，大约为船长的3～6倍；AD为近距又称纵距是无人船从转向开始旋转90°时重心的纵向前进距离，大约为DT的0.6～1.2倍。

是动态安全域系数，其值取决于无人船与干扰船的相对方位角β，相对方位角在0°时，此时相对速度最大，AD对动态安全域的影响比较大，故此时的ki比较大，相对方位角在180°时，此时相对速度最小，DT对动态安全域的影响比较大，故此时ki的系数比较小。s为取值范围为0.8～1.2的常数，其值取决于会遇船舶主尺度，可以对ki的系数实现微调。

在步骤三中根据确定的动态导航船域判断无人船动态导航船域是否与干扰船的动态导航船域相交，如果相交执行步骤四，如果没有执行步骤六。

在本发明的一个实例中，当无人船和干扰船的动态导航船域相交时，且随时间变化距离逐渐缩短时认为此时应当进行避碰，无人船从路径跟踪模式切换到避碰模式；否则，无人船保持路径跟踪模式，以固定的速度向下一路径点航行。无人船是否应当避碰，可以用如下公式进行描述：

当无人船和干扰船的动态导航船域不相交时RSK＝0，此时应当执行路径跟踪模式，否则应当执行避碰模式，执行避碰模式之后确定避碰点。

为了判断船域是否相交，需要对无人船和干扰船的动态导航船域进行操作。首先，应围绕原点旋转

坐标中定义的动态导航船域，使

轴指向船舶的航向。然后，将船舶域转换为船体坐标系o_bx_by_bz_b的原点。坐标旋转矩阵定义为：

然后可以得到船域曲线：

其中(x_DNSD,y_DNSD)是地球坐标系中动态导航船域的坐标。

在步骤四中根据避碰规则和动态导航船域计算避碰点并根据相对方向角β及避碰规则选择合适的避碰点，从而得到避碰路径；

在本发明一个实例中，图4为基于避碰规则无人船与干扰船避让情景；对于对遇和交叉局面，无人船应向右转向避碰；对于追越局面，无人船既可向右也可向左转向进行避碰。

确定追越局面避碰操纵转向的方法为：通过无人船位置与干扰船位置的连线来划分避碰操作区域，如果下一全局目标点在连线的右边，无人船从右舷通过；否则，从左舷通过。

图5为发明的一个实例根据干扰船的位置和干扰船的动态导航船域计算避碰点位置的示意图；

无人船的避碰点的计算公式为：

或

其中，R＝r₁×R_s(OS)+r₂×R_s(TS),是一个调整航路点的参数，它由r₁和r₂调整，其范围在(0-1)之间，其中r₁和r₂的值可以通过实验确定。在本发明中，我们已经证明了在交叉和迎面情况下，当r₁＝0.8和r₂＝0.9时，以及在超车情况下，r₁＝0.5和r₂＝0.6时，避碰性能最佳。(x_wp1,y_wp1)和(x_wp2,y_wp2)为确定的避碰点，之后根据避碰规则和无人船与干扰船之间的相对方位角选择一个合适的避碰点。

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

在避障模式下，无人船将根据避碰规则和重新规划的路径轨迹进行避障操作，从而使碰撞风险逐渐降低。因此，需要判断何时完成避障模式并切换回路径跟踪模式。避障操作是在无人船和干扰船的动态导航船域相交时执行的，如果在船域不重叠的情况下切换出这种模式，将导致一条长而不必要的避障路径。因此，我们使用更合理的方法来决定何时完成避障模式。如图6所示，当无人船已经通过干扰船确定的动态导航船域的半椭圆和半圆的交界线，并且无人船和干扰船之间的当前距离大于前一时刻的距离(d_t>d_t-1)时，避障模式应切换到路径跟踪模式，以便无人船将跟踪下一目标点。

具体地，在本发明的一实施例中对单个干扰船和多个干扰船以及无人船的运动参数设置的如表1所示：OS代表无人船，TS代表干扰船。

表1无人船和干扰船运动参数设置

图7为单个干扰船的避碰效果图，交叉会遇局面的模拟结果如图7(a)所示。可以看出，在t＝6s时，无人船和干扰船的动态导航船域相交，无人船切换到避障模式，并规划避让路径确定新的路径点，无人船根据避碰规则进行避让机动。此外，我们可以看到，在t＝11.5s时，无人船被评估为安全的，然后切换回原始路径跟踪模式。这大大加快了回避效率。最后当t＝31.5s时，无人船到达目标点。类似地，追越和对遇局面的模拟结果分别如图7(b)和(c)所示。

图8为多个干扰船的避碰效果图，无人船在t＝7s时开始切换避障模式，在t＝12.5s时，它成功地避开了TS1并切换到路径跟踪模式。无人船在在t＝18s与TS2相遇，并开始避碰，在t＝24s时，成功地避开了TS2。最后，无人船在t＝28s时与TS3相遇，并开始避碰，在t＝41s时成功避开TS3，在t＝47s时通过路径跟踪到达目的地。当遇到多干扰船的情况下，无人船会使用DCPA法检测最危险的干扰船，若有碰撞危险，则开始规避这一干扰船，规避结束后开始复航。当检测到与第二个干扰船有碰撞危险时，开始规避第二个干扰船，当检测到与第三个干扰船有碰撞危险时，开始规避第三个干扰船。

图9和图10分别为单个干扰船和多个干扰船的动态导航船域变化示意图，从图中可以看出动态导航船域随着时间的变化在不断发生变化。当无人船和干扰船处于高碰撞风险时，为了降低碰撞风险，执行更可靠的避碰机动，根据相应的会遇局面，增大动态导航船域的R_f和R_s。当无人船处于相对较低的碰撞风险时，船舶域尺寸将缩小，以提高避碰效率。

Claims (3)

1.一种基于动态导航船域和避碰规则的障碍物避碰方法，其特征在于，其包括：

101、通过无人船传感器和船舶自动识别***，获取无人船和干扰船的基本信息，确定无人船和干扰船相对速度和相对方位角β，并基于避碰规则确定船舶间会遇局面，所述步骤101包括：

船舶基本信息包括干扰船的船长、船速、航向、位置等，以及无人船自身船速、航向、位置、船长和无人船操纵性能指标如：滞距、重心横向移动距离、重心纵向移动距离等；根据避碰规则确定船舶间的会遇局面，包括追越局面、对遇局面和交叉局面；

102、根据会遇船舶尺度、操纵性和相对速度、相对方位角，分别以无人船和干扰船为中心建立动态导航船域；若无人船附近有多个干扰船时，则利用最近会遇距离方法确定干扰船中最具有危险的干扰船，之后确定动态导航船域；若无人船中最具有危险的干扰船有多个，无人船会随机选择一个最具有危险的干扰船；

103、判断无人船和干扰船的动态导航船域是否相交，若相交，则具有碰撞风险，进行避碰操作，根据避碰规则，确定遭遇单干扰船和多干扰船时避碰操作方式，确定局部避碰路点，规划避碰路径并判断何时结束避碰，所述步骤103包括：

若无人船的动态导航船域和干扰船的动态导航船域相交时，且随时间变化距离逐渐缩短时认为两船存在碰撞风险，则认为两船存在碰撞风险，此时应当执行避碰操作，并根据会遇局面和避碰规则规划避碰点；反之，继续执行路径跟踪；根据避碰规则，对于对遇和交叉局面，无人船应向右转向避碰；对于追越局面，无人船既可向右也可向左转向进行避碰；

确定追越局面避碰操纵转向的方法为：通过无人船位置与干扰船位置的连线来划分避碰操作区域，如果下一全局目标点在连线的右边，无人船从右舷通过；否则，从左舷通过；

根据干扰船的位置和动态导航船域计算避碰点的位置，无人船的避碰点的计算公式为：

或

其中，R＝r₁×R_s(OS)+r₂×R_s(TS)，是调整航路点的参数。

计算无人船何时结束避碰从而切换为路径跟踪模式的方法为：当无人船已经通过干扰船动态导航船域的半椭圆和半圆的交界线，并且无人船和干扰船之间的当前距离大于前一时刻的距离(d_t>d_t-1)时，避障模式应切换到路径跟随模式，以便无人船跟踪下一目标点。

2.根据权利要求1所述的一种基于动态导航船域和避碰规则的障碍物避碰方法，其特征在于：所述102还包括：

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

其中sgn(·)是符号函数，定义为:

动态导航船域的确定考虑了会遇船舶尺度、操纵性和相对速度、相对方位角和会遇局面；动态半径描述如下：

式中，R_f和R_s分别为动态导航船域的两个半径；

是无人船与干扰船的相对速度，其中^vOS为无人船的速度；^vTS为干扰船的速度；||·||₂为二范数；C为常数；Re为无人船的滞距；B是船的宽度；DT为旋回初径是无人船转向180°时重心的横向距离；AD为近距又称纵距是无人船从转向开始旋转90°时重心的纵向前进距离；

是动态安全域系数。

3.根据权利要求1所述的一种基于动态导航船域和避碰规则的障碍物避碰方法，其特征在于：所述103还包括：

根据确定的船域，判断船域是否相交应当进行如下操作：首先，应围绕原点旋转

坐标系中定义的动态导航船域，使

轴指向船舶的航向；然后，将船域转换为船体坐标系o_bx_by_bz_b的原点；坐标旋转矩阵定义为：

然后可以得到船域曲线：

其中(x_DNSD,y_DNSD)是地球坐标系中动态导航船域的坐标。

Images