一种内河无人船自主避碰方法

技术领域

本发明涉及内河无人船智能航行技术领域，具体涉及一种无人船自主动态避碰方法。

背景技术

无人水面船(USV)是一种智能化、无人操作的水面舰船，USV借助自身搭载的传感器感知环境信息、建立作业环境模型即可在复杂、恶劣的水域按照预设任务进行作业。近年来，随着军用和民用方面的需求加深，进一步推动了USV在货物运输、水文监测、航道测量、军事巡逻领域的应用。在复杂的水面环境中，USV的航行受到多种障碍物的威胁，因此，自主避碰技术是USV的核心技术之一。

USV自主避碰是利用船舶周围的局部环境信息，结合传感器的信息进行态势感知，主动避开动态障碍物，生成可行、安全的路径，是USV的研究重点。针对USV自主避碰，国内外常用的算法有模型预测控制算法、人工势场算法、速度障碍法、深度学习算法以及各类改进变种算法等。但是，常见的这些算法都存在一些不足。例如在面临拥挤水域(如内河流域)的情况下会面临失效的风险，有些算法是为了适应某些特定的理想区域而设计的，不具有普适性。相比于以上的算法，动态窗口法作为一种局部路径规划算法，其工作流程比较简单，常用于USV实时动态避碰。动态窗口法是在每一次迭代中，算法可以计算出下一次迭代的所有可行速度组。

专利CN114839968A提出一种基于动态窗口法与优化LPA*算法。通过动态窗口法，构造路径全局最优的评价函数，进行局部路径动态规划，达到全局最优路径前提下的局部动态规划能力。但是该方法仅适用理想环境，在复杂环境下(内河航行环境)避碰能力会面临失效的风险。专利CN114326702A提出了一种基于模型预测控制的内河无人船避碰方法，该方法采用模型预测控制方法，综合考虑内河航行规则、无人船运动学约束、航道条件等因素的影响的问题，但是模型预测控制计算量大，避碰的实时性难以保证。

目前大多数USV的避碰是基于国际海事避碰规则，基于内河避碰规则的研究较少；大部分研究是为了适应某些特定的理想区域而设计的，难以适应复杂的内河航行环境，不具有普适性。

发明内容

发明目的：为了克服现有技术的不足，本发明提供一种内河无人船自主避碰方法，该方法可以解决现有技术普适性和实时性差的问题。

技术方案：本发明提供内河无人船自主避碰方法，包括以下步骤：

S1初始化环境信息和动态化窗口信息，所述初始化环境信息包括：无人水面船的起点和目的地信息、动态障碍物信息和静态障碍物信息，所述动态化窗口信息为根据无人水面船的特性和当前速度v以及角速度w，得到一系列速度对(v,w)，进而得到无人水面船在动态窗口中运动的时间间隔内的速度约束；

S2判断动态障碍物和静态障碍物与无人水面船是否存在碰撞风险，若不存在碰撞风险，则保持航向和航速航行，并更新无人水面船的位置；否则，若所述无人水面船与所述静态障碍物存在碰撞风险，则无人水面船进行位置更新，保持安全距离避让；若所述无人水面船与所述动态障碍物存在碰撞风险，则对无人水面船和对方船只的会遇局面进行划分，并判断无人水面船是否为让路船，如果无人水面船为让路船，则跳转进行步骤S3，否则，若无人水面船为直航船，则保持航向和航速航行，并更新无人水面船的位置；

S3设置最大迭代次数，并开始迭代计算并判断是否采取避让行动，随后创建避碰时机模型和避让幅度模型以及复航时机模型，进而得到避碰规则函数，进入步骤S4；

S4简化速度搜索窗口，选择下一时间段最佳速度矢量并进行位置更新；

S5判断无人水面船是否已到达目的地。

进一步的，包括：

所述步骤S1中，所述动态障碍物信息包括对方船只纵向航速、航向、当前位置；静态障碍物信息指其位置和形状信息；所述速度约束表示为：

其中，v

进一步的，包括：

所述步骤S2中，若所述无人水面船与所述动态障碍物存在碰撞风险判定方法为：

当无人水面船与对方船只对驶相遇时，计算会遇安全距离d

所述对无人水面船和对方船只的会遇局面进行划分，包括：当无人水面船与对方船只形成不同会遇局面时，根据内河避碰规则，将两船相遇的情形分为对驶相遇、交叉、追越三种情形：

两艘船舶在对遇局面下，应当互以左舷会船，以同时满足两船的航向夹角小于等于5°和两船的艏向线有交叉两个条件判定两船属于对驶相遇；

同时满足以下三点条件对方船为追越船：无人水面船处于对方船只正横后大于22.5°的位置；V

满足以下三点条件，则判断两船处于交叉相遇局面：不属于对驶相遇局面和追越局面；对方船只在无人水面船的右舷方向；|Ψ

进一步的，包括：

所述判断无人水面船是否为让路船的方法为：

当无人水面船与对方船只的航向交叉且有碰撞危险时，若两船的航向角度差

进一步的，包括：

所述步骤S3具体包括：

S31当无人水面船与对方船只对驶相遇时，如果判断有碰撞风险，此时，建立避碰时机模型为(d

S32根据内河避碰规则，无人水面船需要以左舷会船的方式来避碰对方船只，此时向右转向△C，确立转向幅度模型△C＝w

S33根据内河避碰规则，在无人水面船和对方船只无碰撞风险时，无人水面船方可复航；在对驶相遇情形中，当对方船只位于无人水面船正横位置时，无人水面船开始复航，加速驶往目的地；

S34当无人水面船与对方船只处于追越局面时，建立的避碰时机模型为(d

S35当无人水面船与对方船只处于交叉相遇局面时，避碰时机模型和避碰幅度模型与另外两种情形相同，且在该情形下，当对方船只位于无人水面船横荡方向上时候，无人水面船开始复航。

进一步的，包括：

所述步骤S3包括：

S36将避碰时机模型、避碰幅度模型、复航时机模型融入规则评价函数之中，建立新的目标函数：

新的动态窗口目标函数为：

G(v,w)＝σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w)+η·g

其中，heading(v,w)表示速度对(v,w)对应的轨迹末端与目标位置之间的角度。dist(v,w)表示机器人与障碍物之间的距离，其功能是使机器人远离障碍物。velocity(v,w)表示速度到(v,w)中心线速度的值，目的是使机器人以更快的速度移动到目标位置。α，β，γ，η为对应函数的权重因子，σ为调节参数。

S37对避碰时机模型、避碰幅度模型、复航时机模型进行归一化处理，分别得到三种情形对应的规则函数：

其中，K

综合三种情形对应的规则函数得到避碰规则函数g

g

进一步的，包括：

所述步骤S4包括：

S41无人水面船减速制动的搜索空间V

无人水面船制动距离约束：当在局部环境中躲避障碍物时，确保无人水面船的安全，在最大减速度的约束下，可在碰撞障碍物前将速度降低到0米/秒，制动约束如下：

其中，dist(v,w)表示速度对(v,w)对应的运动轨迹和障碍物之间的最短距离；

S42对于运动轨迹已知的动态障碍物，如果无人水面船在未来某一时刻与它们碰撞，则相应的预测轨迹将被丢弃，在丢弃所有带有障碍物的轨迹后，剩余的预测轨迹用于无人水面船导航，采用新的动态窗口目标函数评估这些剩余轨迹，并将选择最佳轨迹来引导无人水面船；

S43根据目标函数，选择最优速度矢量，也就是选择使避碰规则的动态窗口法的目标函数值最大的避让速度矢量(v,w)作为下一刻的执行速度；

S44更新无人水面船的位置：如果无人水面船为直航船，则保持航向和航速航行，并更新无人水面船的位置；为避免无人水面船频繁调整航行状态，对(△v,△w)设置阈值，如果(△v,△w)小于阈值，则保持上一时间段的航向和航速航行。

进一步的，包括：

所述步骤S5中，判断无人水面船是否已到达目的地决策如下：

如果无人水面船未到达目的地，则执行步骤S3～S4进行下一次迭代，并更新动态窗口，直至无人水面船到达目的地，提示避让障碍物成功；

如果达到设置的最大迭代次数，无人水面船还未到达目的地，结束方法，并提示避让障碍物失败。

有益效果：(1)本发明针对传统DWA在面对动态障碍物时实时性不高的问题，对速度窗口进行简化，降低算法计算的复杂度，从而提高算法效率，以实现在多船情况下USV避免碰撞。(2)本发明针充分考虑中华人民共和国内河避碰规则以及USV操纵特性，本文引入了新的规则评价函数，重新设计了DWA的评价函数，划分了船只相遇情形，充分考虑避碰规则和动、静态障碍物等因素的影响进行研究，使USV在相遇时能够做出符合内河避碰规则的避碰动作。

附图说明

图1为本发明所述的基于改进DWA的USV自主避碰算法流程图；

图2为中华人民共和国内河避碰规则下两船会遇情况与职责划分图，其中，图2(a)为船舶对驶相遇局面示意图，图2(b)为船舶追越局面示意图，图2(c)为船舶交叉相遇局面示意图；

图3为本发明所述的规则要求的船舶操纵示意图，其中，图3(a)为船舶对驶相遇具有碰撞风险时，建立的模型示意图，图3(b)为船舶追越局面时，建立的模型示意图，3(c)为船舶交叉相遇局面时，建立的模型示意图；

图4为本发明所述的预测轨迹和避碰示意图，其中，图4(a)为USV减速制动的搜索空间V

图5为对驶相遇局面下的的避碰算法过程示意图，其中，图5(a)为传统DWA避碰过程示意图，图5(b)为本发明改进DWA避碰过程示意图；

图6为追越局面下的避碰过程示意图，其中，图6(a)为传统DWA避碰过程示意图，图6(b)为本发明改进DWA避碰过程示意图；

图7为交叉相遇局面下的避碰过程示意图，其中，图7(a)为传统DWA避碰过程示意图，图7(b)为本发明改进DWA避碰过程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明设计的基于改进DWA的USV自主避碰算法在给定时间窗内能达到的速度矢量，并根据避碰规则约束，选择符合避碰规则要求的速度矢量进行避碰，过程包括碰撞危险判断、会遇局面划分、选择最佳速度矢量等。

如图1所示，本发明提供一种基于改进DWA的USV自主避碰算法，包括如下步骤：

步骤S1、初始化环境和动态窗口信息；

步骤S11、初始化环境信息主要包括USV的起点和目的地信息以及动、静态障碍物信息。动态障碍物信息主要包括对方船只纵向航速、航向、当前位置等，静态障碍物信息主要指其位置和形状信息。

步骤S12、初始化动态窗口信息是指根据USV的动力学与运动学特性以及USV当前速度v和角速度w，在速度采样空间中对v,w进行离散采样，得到一系列速度对(v,w)，进而得到一系列预测轨迹。

并可以得到USV在动态窗口中运动的时间间隔内的速度约束为:

式中，v

步骤S2、判断动、静态障碍物与USV是否存在碰撞危险并对USV和对方船只的会遇局面进行划分，最后判断USV是否为让路船；

步骤S21、当USV与对方船只对驶相遇时，首先，计算会遇安全距离d

如果判定USV与静态障碍物存在碰撞危险，则进行位置更新，保持安全距离避让。

如果判定USV与对方船只存在碰撞危险，则会进行步骤S22，根据避碰规则对会遇局面进行划分。

步骤S22、当判定USV与对方船只存在碰撞危险，此时根据避碰规则对会遇局面进行划分。

如图2所示，当USV与对方船只形成不同会遇局面时，应采取符合中华人民共和国内河避碰规则的行动。根据内河避碰规则，将两船相遇的情形分为对遇、交叉、追越三种情形。

船舶会遇场景的判定方法具体如下：

1)对驶相遇

“内河避碰规则”第十条规定两艘船舶在对遇局面下，应当互以左舷会船。本文以同时满足两船的航向夹角小于等于5°(即180°-|Ψ

2)追越

“内河避碰规则”第十一条规定两艘船舶在形成追越局面下相应的操控要求，本文选取以下三点条件判断某船是否为追越船，船舶追越局面示意图如图2(b)所示。

(1)USV处于对方船只正横后大于22.5°的位置，即Ψ>112.5°。

(2)V

(3)对方船只的航向角在USV的航向角左右10°范围内。

3)交叉相遇

“内河避碰规则”第十二条规则规定两艘船舶在形成交叉会遇局面下相应的操控要求，本文按满足以下三点条件，则判断两船处于交叉相遇局面。

(1)不属于对驶相遇局面和追越局面。

(2)对方船只在USV的右舷方向，即Ψ≤180°。

(3)|Ψ

船舶交叉相遇局面示意图如图2(c)所示，根据规则可知该会遇情形为交叉相遇局面，对方船只位于USV的右舷方向，并且|Ψ

步骤S23、判断USV是否为让路船具体如下：

当USV与对方船只的航向交叉且有碰撞危险时，如果航向角度差

如果USV为让路船，则会进行步骤S3，进行迭代计算。

如果USV为直航船，则保持航向和航速航行，并更新USV的位置。

步骤S3、开始迭代i＝1并判断是否采取避让行动，随后创建避碰时机模型和避让幅度模型和复航时机模型，并简化速度搜索窗口，选择下一时间段最佳速度矢量并进行位置更新；

步骤S31、如果USV为让路船，则进行迭代计算，应采取避碰行动。此时i＝1，在每一次迭代过程中，需要结合避碰的不同阶段，分别建立USV的避碰时机模型、避让幅度模型和复航时机模型。

如图4所示，当USV与对方船只对驶相遇时，如图3(a)所示，如果判断有碰撞风险，此时，建立避碰时机模型为(d

S32、根据内河避碰规则，USV需要以左舷会船的方式来避碰对方船只，此时向右转向△C，转向幅度确定如图△C所示，可以确立转向幅度模型△C＝w

步骤S33、根据内河避碰规则，在USV和对方船只无避碰风险时，USV方可复航。在对驶相遇情形中，当对方船只位于USV正横位置时，USV开始复航，加速驶往目的地。

当USV与对方船只处于追越局面时，建立的避碰时机模型为(d

如图3(b)所示，可得避碰幅度模型为△C＝w

步骤S34、将三种模型融入规则评价函数之中，建立新的目标函数。

新的动态窗口目标函数为：

G(v,w)＝σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w)+η·g

其中，heading(v,w)表示速度对(v,w)对应的轨迹末端与目标位置之间的角度。dist(v,w)表示机器人与障碍物之间的距离，其功能是使机器人远离障碍物，velocity(v,w)表示速度到(v,w)中心线速度的值，目的是使机器人以更快的速度移动到目标位置；

对避碰时机模型、避碰幅度模型、复航时机模型进行归一化处理，可以分别得到三种情形对应的规则函数：

其中，K

综合三种情形对应的规则函数可以得到避碰规则函数g

g

步骤S35、如图4所示，USV减速制动的搜索空间V

其中，dist(v,w)表示速度对(v,w)对应的运动轨迹和障碍物之间的最短距离。如图4(b)所示，放弃有障碍物一侧的轨迹。对于运动轨迹已知的动态障碍物，如果USV在未来某一时刻与它们碰撞，则相应的预测轨迹将被丢弃，如图4(c)所示。在丢弃所有带有障碍物的轨迹后，剩余的预测轨迹可用于USV导航。目标函数评估这些剩余轨迹，并将选择最佳轨迹来引导USV。因此，无需计算到障碍物的最小距离和制动距离，从而提高了算法的效率，降低了系统的计算复杂度。

步骤S36、根据目标函数，选择最优速度矢量，也就是选择使避碰规则的动态窗口法的目标函数值最大的避让速度矢量(v,w)作为下一刻的执行速度。

步骤S37、更新USV的位置。如果USV为直航船，则保持航向和航速航行，并更新USV的位置。为避免USV频繁调整航行状态，对(△v,△w)设置阈值，如果(△v,△w)小于阈值，则保持上一时间段的航向和航速航行。

步骤S4、判断USV是否已到达目的地。

判断USV是否已到达目的地的决策具体如下：

如果未到达目的地，则进行下一次迭代，更新动态窗口，直至USV到达目的地，提示避让障碍物成功，避碰算法完成。

如果达到设置的最大迭代次数时USV还未到达目的地，算法也会结束，提示避让障碍物失败。

如图5所示，为对遇局面下采用改进动态窗口法的USV自主避碰算法的避让过程。从图5中可看出，改进的DWA的避让幅度更大，幅度大，改进后的曲线弯曲程度更大，说明USV的避让角度更大，幅度更大，安全性更高，在对驶相遇局面下两艘船舶均采取以左舷会船的形式进行有效避碰，符合中华人民共和国内河避碰规则。其中的横纵坐标是距离单位。

如图6为追越场景下两种算法的避碰过程。从图6中可看出，改进的DWA的避让幅度更大，在追越局面下己方船采取以左舷会船的形式进行有效避碰，符合中华人民共和国内河避碰规则。

如图7所示，改进的DWA的避让幅度更大，在交叉相遇局面下己方船采取以左舷会船的形式进行有效避碰，符合中华人民共和国内河避碰规则。即图中两艘船舶轨迹出现重叠，但是两艘船舶到达重叠点的时间不同，所以不会发生碰撞。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明实施例进行各种改动和变型而不脱离本发明实施例的精神和范围。这样，倘若本发明实施例的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。