CN111123903A

CN111123903A - 一种基于圆形轨迹单元的无人艇避障方法

Info

Publication number: CN111123903A
Application number: CN201811281086.4A
Authority: CN
Inventors: 文元桥; 杜哲; 顾尚定; 周春辉; 肖长诗; 黄亮; 张帆
Original assignee: Wuhan University of Technology WUT
Current assignee: Wuhan University of Technology WUT
Priority date: 2018-10-30
Filing date: 2018-10-30
Publication date: 2020-05-08
Anticipated expiration: 2038-10-30
Also published as: CN111123903B

Abstract

本发明公开了一种基于圆形轨迹单元的避障方法。该方法能够很好的将无人艇的动态特性与避障过程结合。首先，圆弧网格允许无人艇候选航向和路径点在弧上有连续变化，这与连续运动曲线有较好的相关性。其次，利用圆形网格树搜索的结构，解决了空间完全覆盖问题。轨迹单元将轨迹约束添加到空间搜索过程中，实现了运动曲线和避障的结合。最后，通过标准化规则解决了轨迹的离散化和维持最终路径连续性的问题。本发明方法提供了一种新的运动规划避障方法，应用该方法能很好地避碰静态和动态障碍物，避碰路径不仅符合无人艇的动态特性，而且为无人艇的操纵提供操舵指令，从而助力实现水面无人艇精确运动规划。

Description

一种基于圆形轨迹单元的无人艇避障方法

技术领域

本发明涉及智能航海领域，具体是一种基于圆形轨迹单元的无人艇避障方法。

背景技术

对于无人驾驶车辆、船舶等，安全是执行任务首要解决的问题。避免碰撞是安全行驶的关键。对于欠驱动系统，如无人地面车辆(UGV)，无人驾驶飞行器(UAV)和无人驾驶水面无人艇(USV)，避碰的研究需要考虑运动学和动力学约束。因此，在无人驾驶中讨论的一个关键问题是运动规划避碰。一般来说，无人艇的运动规划避碰中存在两个问题需要解决：地图建模和避碰算法。

地图建模是避碰算法的基础。如果没有适当和准确的地图表示，则避碰算法无法充分发挥作用。水面无人艇与无人驾驶车辆或无人机相比，水面无人艇所处的环境更复杂。首先，无人车或无人机的运动总是处于一种媒介中。但对于水面无人艇而言，其中作用环境有两种媒介，分别是水和空气。其次，无人艇运动有六个自由度，但只有两个控制变量(舵角和主发动机转速)。此外，由于水面的惯性，阻力和响应时间大于地面或空中的惯性，所以相应的阻力更大和响应时间更长。进一步的，如果在一些特殊的航行环境中，如港口和近海区域，可航行水域的交通密度很高，这给无人艇的运动控制造成了更大的困难。因此，应用于无人车或无人机的避碰算法方法不适合水面无人艇。针对现有的水面无人艇驾驶的技术缺陷，提出一种适应现有复杂水域的无人艇运动规划避碰方法，助力实现水面无人艇精确运动规划。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种基于圆形轨迹单元的无人艇避障方法。

本发明解决其技术问题所采用的技术方案是：一种基于圆形轨迹单元的无人艇避障方法，包括以下步骤：

1)在地图划分圆网格树，生成圆形树状地图，确定无人艇运动路径的起点和终点；

2)确定无人艇所处的实时路径点和艏向角；

3)根据无人艇所处的实时路径点和艏向角、无人艇运动的轨迹单元以及障碍物所处的路径点，求取可达路径点；

所述轨迹单元为圆形轨迹单元,所述轨迹单元的生成方式如下：

3.1)利用MMG模型对无人艇进行运动建模，获得无人艇的运动轨迹；

3.2)采用无人艇轨迹离散化规则对无人艇的运动轨迹进行约束；包括以下离散化规则：

规则一：在轨迹段初始和结束时刻，无人艇的运动状态稳定且保持一致；

规则二：每段轨迹的转舵次数不超过一次；

规则三：所有轨迹段的起点和终点之间的距离是相同的；(对应圆网格半径)

规则四：所有轨迹段适应圆网格树地图；树的每一层中，当前点始终是圆的中心，并且下一个候选路径点总是位于该圆的圆弧上；

3.3)根据步骤3.1)和3.2)完成轨迹单元的建模过程，根据不同的目标航向产生运动曲线，并将90度作为最大航向变化，建立圆形轨迹单元库；

4)对所有的可达路径点进行路径代价计算，获得下一个路径点位置和对应的艏向角；

5)判断该路径点是否为运动路径的终点，若为终点，则输出最终路径，否则，将该路径点和对应的艏向角作为无人艇所处的实时路径点和艏向角，转入步骤2)。

按上述方案，所述圆网格树地图中，通过船舶领域确定圆网格的半径，圆形网格的半径是USV的船舶领域的大小，即椭圆形船舶领域的长轴。

按上述方案，所述步骤4)中下一个可达路径点的轨迹通过舵角δ，位置(x，y)和艏向角θ共同生成。

按上述方案，所述步骤4)中下一个可达路径点的轨迹中，舵角通过艏向角计算，具体如下：

θ＝aδ³+bδ²+cδ+d；

其中a，b，c和d是拟合系数。

按上述方案，所述步骤4)中下一个可达路径点的轨迹中，舵角通过位置计算，将位置坐标(x，y)转换表示为极坐标(R，α)，通过α计算航向角,再根据航向角获得舵角，具体如下：

航向和中心角的关系函数：

其中，点C(x_c，y_c)是当前节点，点D(x_d，y_d)是目的地。

按上述方案，所述步骤4)中，若下一个可达路径点的轨迹存在障碍物，则采用圆形覆盖障碍物，通过确定当前节点和覆盖障碍物圆形的切点绕过障碍物。

按上述方案，所述步骤4)中，切点的选取如下：确定当前节点和覆盖障碍物圆形的两个切点和假设无障碍物的目标角，选取两个相切角和目标角之间的差异较小的那个角对应的切点。

本发明产生的有益效果是：本发明利用圆形网格树的结构，解决了空间完全覆盖问题，实现了运动曲线和避障的组合。通过标准化规则解决了轨迹的离散化和维持最终路径连续性的问题。因此，运动避障方法以圆网格轨迹单元为基本单元来搜索水域并进行动态避障。实验结果表明，能很好地避免静态和动态障碍。避碰路径不仅符合USV的动态特性，而且为转向指令提供了参考。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的方法流程图；

图2是本发明实施例的网格图搜索方向示意图；

图3是本发明实施例的圆圈图搜索方向示意图；

图4是本发明实施例的网格化地图示意图；

图5是本发明实施例的圆形地图示意图；

图6是本发明实施例的矩形重叠地图示意图；

图7是本发明实施例的同心重叠结构示意图；

图8是本发明实施例的树重叠结构示意图；

图9是本发明实施例的生成圆形轨迹单元的过程示意图；

图10是本发明实施例的圆形轨迹单元库示意图；

图11是本发明实施例的利用船舶领域确定圆网格的半径示意图；

图12是本发明实施例的运动曲线示意图；

图13是本发明实施例的极坐标系与直角坐标系之间的关系转换示意图；

图14是本发明实施例的舵角、航向和中心角关系示意图；

图15是本发明实施例的轨迹路径搜索示意图；

图16是本发明实施例的避免碰撞切点示意图；

图17是本发明实施例的避免多重障碍物碰撞的策略示意图；

图18是本发明实施例的维持航向避免碰撞示意图；

图19是本发明实施例的避免碰撞示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，一种基于圆形轨迹单元的无人艇避障方法，包括以下步骤：

1)确定无人艇运动路径的起点和终点；

2)确定无人艇所处的实时路径点和艏向角；

所述轨迹单元的生成方式如下：

规则二：每段轨迹的转舵次数不超过一次；

规则三：所有轨迹段的起点和终点之间的距离是相同的；(半径)

圆网格树地图中，通过船舶领域确定圆网格的半径，即椭圆形船舶领域的长轴。

以下为本发明的具体实施方式及原理说明。

网格地图是USV避碰中使用最广泛的地图建模方法。但显然，网格图不适合与连续运动曲线相关联，因为在每个搜索循环中只有八个候选节点(离散化节点)，如图2。

为了使离散化的节点成为一组连续节点(曲线段)，应该改变网格的形状。当基本网格是圆形并且候选节点在弧上时，节点的数量是无限的并且它们可以形成连续的曲线，如图3。因此，本发明的主要思想是提出一种基于圆形轨迹单元的无人艇动态避障方法，以解决USV运动规划问题中动态约束与搜索算法的结合，从而助力实现水面无人艇的精确运动规划。

在提出运动规划算法之前存在如下需要解决的问题。

(1)、动态约束的表示。运动曲线不仅包含所有动态特性，还包含这些约束的相互作用。因此，轨迹是一种很好的表示方式，轨迹的产生方式是第一个问题。

(2)、轨迹的标准化。虽然轨迹是表示动态约束的好方法，但最终目的是为了避免碰撞。因此，有必要建立一套规则来标准化这些轨迹。该规则必须保持轨迹简单，以使路径易于实现，并且还使它们保持耦合从而使最终路径保持连续。

(3)、圆形地图的覆盖范围。网格化地图广泛用于路径规划问题的原因是它可以完全覆盖研究空间(图4)。但是对于圆形地图由于弧线(图5，白色阴影)有一些没有覆盖区域。因此，如何解决这一缺陷是地图建模的关键，也是路径搜索精度的前提。

因此，为了解决上述三个问题，将建立圆形轨迹单元模型。首先，建立产生USV轨迹的船舶建模数学模型。其次，建立一个轨迹标准化规则来约束轨迹。此外，提出一个圆形轨迹单元树来解决全空间覆盖问题。最后，构建无人艇运动规划避碰算法。

在建立无人艇运动规划数学模型中，采用MMG数学模型进行无人艇运动数学模型的建立。将流体力和力矩分解成作用于船体，螺旋桨和舵等，对船体的影响分为惯性和粘性。因此，MMG模型由惯性模型，粘性模型，螺旋桨模型和舵模型组成。

在建立数学模型的过程中设定了三个假设：

假设1：考虑无人艇的运动只受水平面力的影响，即只受前进、横移、艏摇影响，忽略无人艇的横摇、纵摇以及垂荡等的影响。

假设2：假设无人艇在行驶过程中没有风浪流等外部因素的影响。

假设3：假设无人艇在航行过程中主机转速恒定。即当船舶处于在航状态时，船体会受到水流的阻力。从而使螺旋桨的负载增加，导致转速(n)降低。此时，速度控制器将提高主机功率，以抵消螺旋桨增加的载荷，并维持原来的转速。

因此，该模型基于三个自由度：考虑无人艇的运动只受水平面力的影响，即只受前进、横移、艏摇影响：

其中I，H，P，R分别表示惯性，粘度，螺旋桨和方向舵的力(或力矩)。

(1)惯性模型

船舶的运动会引起周围流场的扰动，形成流体中等动量(力矩)或附加动量(力矩)。通过计算相应方向的差值，可以获得流体惯性力和力矩。

其中，m_x和m_y分别是沿x轴和y轴的附加质量；α_x是沿着x轴的坐标；J_zz是z轴的额外惯性矩；u和v分别是沿x轴和y轴的速度；r是偏航的角速度。

(2)粘度模型

粘性力和力矩与船体的几何特征、流体的物理性质和船舶的运动状态有关。当前两个因素不变时，运动状态起决定性作用。通过使用二阶泰勒展开，可以表示粘性力和力矩的近似值。

X₀是直航导致的阻力；Δu是沿x轴的速度变化；N_r，X_u，Y_v，Y_r，N_v和N_r是流体动力学的衍生物。

(3)螺旋桨模型

螺旋桨是无人艇的主要动力来源。除产生推力外，它还可以产生横向力和力矩，但是与螺旋桨产生的主动力相比，侧向力和力矩可以忽略不计。因此构建螺旋桨模型。

其中，t_P是推力相关系数；T是螺旋桨直径、转速、水密度和前进速度有关的推力。

(4)舵模型

舵模型是相当重要的部分，因为主要的方向控制力和力矩是由它产生的。同时，无人艇的机动性和轨迹也由它决定。舵的法向力可分解为纵向阻力和横向力。

其中δ是舵角；t_R是阻力衰减系数；a_H是修正系数；x_H是重心与力点之间的距离，是由舵产生的侧向力；F_N是舵的正常力。

根据设定的假设分别建立无人艇的惯性模型、粘性模型、舵模型、螺旋桨模型并进一步建立无人艇运动规划的圆形轨迹单元以及建立相关的轨迹单元库。标准化规则是建立轨迹单元模型的关键。标准化规则不仅是运动曲线和避障算法之间的桥梁，而且当这些离散的轨迹段相互拼接时，也保证最终轨迹连续。

标准化轨迹段有三个规则：

规则1：运动状态在开始和结束时刻是一致且稳定的。这个规则是为了保证离散化的轨迹在最终轨迹拼接时能够使得运动轨迹连续。

规则2：轨迹单元的运动过程中除了一次回舵之外，只有一次转舵操作。作为构成最终轨迹的基本单位，轨迹单元应该是尽可能简单。

规则3：所有单元格的起点和终点之间的距离是相同的。此规则是为后续的圆形运动规划模型做准备。

上述提到圆形地图的覆盖范围是地图建模的关键。但是如果圆圈彼此相切，则会有一些没有覆盖的区域，因此它们必须重叠。有三种重叠圆的方法：矩形重叠，同心重叠和树重叠。

矩形重叠类似于网格图(图6(a))。两个圆之间的距离是半径，相邻的圆相互交叉的通过圆心。但是，此种结构忽略了许多情况。从图6(b)可以看出，除蓝点外，没有其他候选点。当轨迹是图6(b)中所示的红色曲线时，地图无法提供下一个航路点。因此，这种结构是不合适的。

同心重叠是一组具有不同半径和具有相同中心的圆(图7(a))。这种结构解决了上述问题，所有情况都可以在圆弧上找到。并且所有航点的位置都可以通过极坐标定位(中心是原点)。

但是，这种结构必须满足一个要求：每个航向的反向延伸线必须通过圆心。从图7(b)可以看出，当轨迹是从o到d时，如果位置d中的航向是蓝色箭头，则可以通过使用极坐标来计算下一个候选点集的位置。但根据轨迹的切线，真正的航向是红色箭头。该航向的反向延伸线显然不是通过圆心。因此，下一个候选点集难以计算。

因此，该结构对轨迹的要求很高，并且也是不合适的。

实际上，树重叠是最好的选择。此结构利用了单元树的结构。在树的每一层中，当前点始终是圆的中心，并且下一个候选点集总是位于该圆的圆弧上(图8(a))。因此，首先通过船舶坐标(移动坐标)系统计算位置，然后转换为直角坐标系。

当将所有可能的候选点从第一层连接到底部时，节点和边缘组成树状(图8(b))。因此，这种结构不仅涵盖了所有可能的情况，而且还有一种简单的方法来计算位置。本发明将利用它来构建圆形运动规划模型。

根据上述可知，如图9，在生成圆形轨迹单元的过程中有三个状态和两个阶段。开始时，舵角为0，速度是一定的螺旋桨推力(u＝u0，v＝0)下的稳定速度，即状态A.第一阶段是姿态调整，舵角从0变为一定的值δ0。在这个阶段，舵角固定在δ0上，USV处于转向过程中。当它处于状态B时，调整结束。但目前的状态是不稳定的，因为舵角仍然有效。所以USV进入第二阶段，即姿势稳定。在该阶段，舵角从特定值δ0变为0并保持不变。USV正在逐步稳定进程。经过一段时间后，USV最终在状态C上，舵角为0，速度与状态A相同(根据假设3)。速度变化曲线也可以在图9中看到。很明显，速度总是在整个过程中发生变化。但在状态A和C中，值是相同的。

因此，根据不同的目标航向产生运动曲线，并将90度作为最大航向变化，最终生成圆形轨迹单元库(图10)。

从图10中可以看出，虽然左舷和右舷侧的航向变化相同，但舵角的绝对值也不同(35°和31°)。实际上，它源于船舶的真实特征。通常，船的质量在左舷和右舷不能严格平衡。所以重心不在中心线上。在无人艇在水面航行时，左舷和右舷之间的舵效不同。

对于圆网格的半径，本文采用船舶领域的概念来确定。船舶领域是一个安全区域，用于保护船舶与周围其他船舶的碰撞。船舶领域通常是椭圆形，其长轴是船长的4到8倍。因此，圆形网格的半径是USV的船舶领域的大小(如图11所示)。

上述分别介绍了地图和动力学建模，接下来将介绍运动规划算法。其规划算法的主要思想是利用圆形轨迹单元模型在无人艇的每个搜索循环中规划出最佳运动曲线。首先，设计了一个在线轨迹生成器，用于表示运动规划的结果。生成器将搜索过程中产生的运动参数作为基本输入，实时生成运动曲线。然后，进行关键参数求解。挑选出圆形轨迹单元模型中的三个角度，以确定无人艇的位置，航向和操纵。接下来，针对自由和障碍水域，分别建立最优搜索策略。

运动规划的期望结果是规划出实际的运动曲线。因此轨迹生成器是整个算法中的必要环节，特别是在路径搜索过程中。因此，设计在线轨迹发生器很重要。在设计轨迹生成器之前，需要考虑影响运动曲线的因素。根据USV的数学模型，当速度固定时，有三个因素会影响运动曲线的特征：舵角(δ)，位置(x和y)和航向(θ)。

从图12(a)可以看出，舵角决定了曲线的形状。角度越大，舵效果越好，舵效果越好意味着转弯圈越小。因此，舵角与曲线的半径有关。图12(b)是位置的影响。该因子改变曲线的初始位置并使其具有平移变换。图12(c)是航向影响。不同的初始航向使USV具有不同的参考方向，使得曲线具有旋转变换。

因此，根据上述分析，轨迹生成器的输入为：δ，x，y和θ。当前位置和航向决定了USV的初始状态。命令舵角确定运动曲线的形状。在状态调整和稳定阶段后，USV达到最终状态。由上述的轨迹库可得，可以从轨迹单元库中找到相应的轨迹。

可以看出，位置和航向是迭代数据。在每个搜索循环中，来自最后一个循环的输出数据将更新当前循环的输入数据。因此，只要确定USV的原始运动规划状态，就可以容易地计算这三个输入(x，y和θ)。但是，作为运动曲线形状的决策输入，舵角的确定对于该轨迹生成器来说是最重要的。因此，如何计算舵角是下一个讨论的问题。

解决舵角的方法是找到它与其他参数之间的关系。根据上述中的圆网格轨迹单元(图10)，舵角与航向之间存在一定的函数关系。

经过实验数据，我们发现，航向与舵角有正相关关系。为了验证该关系的正确性，计算相关系数。相关系数是变量之间线性相关的度量。公式可表示为：

其中Cov(X，Y)是X和Y的协方差，Var[X]和Var[Y]分别是X和Y的方差。

因此，可以计算舵角和航向角的相关系数：

结果表明，舵角和航向角确实具有正相关关系。利用函数拟合方法，可以表达关系函数，本文采用三次函数拟合舵角与航向的关系：

θ＝aδ³+bδ²+cδ+d (7)

其中a，b，c和d是拟合系数。类似地，舵角也可以用航向(δ＝f(θ))表示。

除了舵角和航向之外的关系，不同的舵角也会导致不同的航路点。因此，舵角和位置之间存在间接关系。为了使这种间接关系更加清晰，有必要将位置数据转换为另一个参数。

从图13中可以看出，极坐标系是处理圆相关问题的常用方法。一旦确定了半径和中心角，就可以将位置表示为(R，α)。假设直角坐标系的中心坐标为(xc，yc)，则从极坐标系到直角坐标系的变换可表示为：

当R固定时，位置(xt，yt)由中心角α确定。

因此，中心角确定位置，航向角决定方向，舵角决定操纵。利用上述函数拟合方法，三个角度可以相互转换(图14)。解决舵角的问题可以用解决航向或中心角的问题来代替。

根据上述可知，解决舵角的关键是计算航向或中心角，中心角确定路径点的位置，但在圆形网格中，位置与路径没有多大直接关系。航向角决定了路径方向，如果确定了目的地，则最佳方向是朝向它。因此，航向角可以用作判断路径的指标。

如图15(a)所示，点C是当前节点，点D是目的地，圆上扇形阴影是可导航区域(轨迹单元库)。假设点T是下一个最佳节点，其对应的航向θt朝向D点，则可以通过T和D的坐标计算角度：

并且点T的协调可以通过等式(9)计算：

另外，通过使用航向和中心角的关系函数，θt也可以表示为：

θ_t＝f(α) (12)

结合等式(10)，(11)和(12)可以得到唯一变量的方程：

因此，可以确定中心角α，并且还解决其他两个角。

然而，在某些情况下，由于轨迹单元的限制，不能在一个步骤中直接达到航向角。如图15(b)所示，当目的地超出轨迹单元覆盖范围时，θt超过最大航向角θm(θt>θm)。因此，首先，USV必须控制最大舵角以获得可导航区域的边缘(Tm)，并且最大航向变化。然后，将边缘点作为当前点以执行上述搜索步骤。

实际上，轨迹单元的最大航向变化是90°。因此，目的地位置有四种不同的情况：上方左右的D1和D2，下方左右的D3和D4。当它位于D1和D2上时，只需要一个转向就可以将航向改为目的地。当它低于D3和D4时，将有两个步骤。首先，增加一个大的舵角使航向变化90°，因此航向和目的地之间的角度是相关的。然后，选择最佳航向，使其朝向目的地。因此，在搜索之前，应首先判断当前节点和目的地之间的相对位置。

在障碍水域中，避免碰撞成为首要任务。有很多方法可以解决这个问题。考虑到圆网格的特点以及航向在路径搜索过程中的重要作用，本文采用圆形覆盖障碍物，找到绕过它的最佳切点。所以主要思想是比较目的地和相切点之间的角度。

但是，如果通过上述的方式确定航向角，则计算将很复杂。由于不同的航向对应不同的航路点，由径向构成的角度不是基于相同的点，并且它们不能直接比较。如图16(a)所示，四条不同的线来自圆弧上的不同点。为了简化计算过程，以当前点(中心点)为基点。

从图21(b)可以看出，N指向正北，原点C(x_C，y_C)，目的地D(x_D，y_D)和障碍物O(x₀，y₀)的位置是已知的。并且圆网和障碍物的半径分别为R和r。因此，当前状态下的相切点A₁和B₁可以通过角度分别确定：

∠NCA₁＝∠NCO-∠A₁CO

∠NCB₁＝∠NCO+∠B₁CO

∠NCO能够通过C点和O点计算得到：

∠A₁COis等于，并且∠A₁CO和∠B₁CO可以通过C点和O点以及r计算得到：

于是，∠NCA₁和∠NCB₁表示如下：

最佳的航向角是是两个相切角和目标角(即∠NCD)之间的差异较小的那个角，如图16(b)所示，∠A₁CD和目标航向角的差距大小相对∠B₁CD和目标航向角的差距大，所以选择∠NCB₁作为下一个点的航向角，点T₁也可通过相关函数计算出来。

但由于简化，节点T1不在CB1的线上。如果航向固定在这个方向上，USV仍有可能撞到障碍物。因此，应继续上述过程。从图16(c)可以看出，在确定下一个节点后，T1成为当前节点。基于该节点，可以计算新的相切点(A₂和B₂)并且可以确定新的航向(在节点T₂上)。很明显，从T₂到T₁B₂的距离比从T₁到CB₁的距离短。这意味着可以通过连续跟踪障碍来减少误差。

当涉及多个障碍时，搜索过程更加复杂。首先，应计算每个障碍物圆的相切点，找出最小和最大相切角，然后，与目标角度进行比较，选择角度差较小的相切点。因此，具有所选相切点的圆是当前聚焦的障碍物。在USV绕过障碍物后，算法重新计算其余障碍物中的最佳切点。

如图17所示，有三个障碍物在航行水域中。首先在远点C₁计算了六个相切点，其中点M和点N是最大和最小的两个相切角对应的切点。∠MC₁D大于∠NC₁D，所以选择N点作为下一个节点，当航行到C₂时，已经通过O₁和O₂障碍物，需要通过的障碍物是O₃，此时的O₃是单个障碍物，可采取和上面类似的方法绕过O₃。

对于移动障碍物，USV会有两种情况：保持航向或转向。所以在采取行动之前，应该提前作出判断。

假设障碍物保持其航向和速度，USV保持其速度(稳定速度)但可以改变航向。如果USV不必改变其航向，则M是USV航向延伸线与障碍物的交点。当USV首先到达点M时(如图18(a)所示)，为了避免碰撞，临界情况是障碍物位于位置Ox(圆圈Ox与圆圈M相切)。因此，根据此刻的运动和几何关系：

l_o＝t·V_O (15)

结合等式(14)，(15)和(16)可以得到障碍物(VO)的速度必须符合的条件：

其中x_m,y_m是节点M的坐标，x_c,y_c是节点C的坐标，x_o,y_o是节点O的坐标，Vs是USV的速度，R和R₀分别是圆形单元的半径和障碍物的半径。

类似地，当障碍物首先到达点M时(如图18(b)所示)，为了避免碰撞，临界情况是USV位于位置C_x(圆C_x与圆M相切)。所以V_O必须遵守：

因此，如果V_O符合等式(17)和(18)，则USV可以保持其航向。但是，当V_O介于两个值之间时，USV必须具有转向以避免碰撞。基于圆形网格，本文提出了一种将动态障碍转化为静态障碍的方法。

如图19所示，以节点M为中心，Rx为半径虚拟化静态障碍物(红色圆盘)。当动态障碍物到达节点O_x时，USV位于节点C_x处，并且圆C_x在点P处与圆M相切。为避免碰撞，O_x和C_x之间的距离必须符合：

C_x可以通过CC_x(l_s)和∠NCC_x计算得到：

l_s和∠NCC_x通过下式得到：

节点O_x的坐标可以通过OO_x(l₀)和障碍物的航向(θ_o)来计算：

l₀可以通过下式求解的时间来确定：

l_o＝V_O·t (24)

结合等式(19)-(25)可以得到Rx的最小值。

通过以上算法即可实现水面无人艇进行自由水面无人艇运动规划以及动态障碍物情况下进行避障运动规划。

本发明能很好地避碰静态和动态障碍物，避碰路径不仅符合无人艇的动态特性，而且为无人艇的操纵提供较为精确的操舵指令，从而助力实现水面无人艇精确运动规划。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims

1.一种基于圆形轨迹单元的无人艇避障方法，包括以下步骤：

2)确定无人艇所处的实时路径点和艏向角；

规则二：每段轨迹的转舵次数不超过一次；

2.根据权利要求1所述的基于圆形轨迹单元的无人艇避障方法，其特征在于，所述圆网格树地图中，通过船舶领域确定圆网格的半径，圆形网格的半径是USV的船舶领域的大小，即椭圆形船舶领域的长轴。

3.根据权利要求1所述的基于圆形轨迹单元的无人艇避障方法，其特征在于，所述步骤4)中下一个可达路径点的轨迹通过舵角δ，位置(x，y)和艏向角θ共同生成。

4.根据权利要求3所述的基于圆形轨迹单元的无人艇避障方法，其特征在于，所述步骤4)中下一个可达路径点的轨迹中，舵角通过艏向角计算，具体如下：

θ＝aδ³+bδ²+cδ+d；

其中a，b，c和d是拟合系数。

5.根据权利要求4所述的基于圆形轨迹单元的无人艇避障方法，其特征在于，所述步骤4)中下一个可达路径点的轨迹中，舵角通过位置计算，将位置坐标(x，y)转换表示为极坐标(R，α)，通过α计算航向角,再根据航向角获得舵角，具体如下：

航向和中心角的关系函数：

其中，点C(x_c，y_c)是当前节点，点D(x_d，y_d)是目的地。

6.根据权利要求1所述的基于圆形轨迹单元的无人艇避障方法，其特征在于，所述步骤4)中，若下一个可达路径点的轨迹存在障碍物，则采用圆形覆盖障碍物，通过确定当前节点和覆盖障碍物圆形的切点绕过障碍物。

7.根据权利要求6所述的基于圆形轨迹单元的无人艇避障方法，其特征在于，所述步骤4)中，切点的选取如下：确定当前节点和覆盖障碍物圆形的两个切点和假设无障碍物的目标角，选取两个相切角和目标角之间的差异较小的那个角对应的切点。