CN108876065B - 一种基于轨迹单元的无人艇运动规划方法 - Google Patents

Abstract

本发明公开了一种基于轨迹单元的无人艇运动规划方法，该方法包括以下步骤：1)确定无人艇运动路径的起点和终点；2)确定无人艇所处的实时路径点和艏向角；3)根据无人艇所处的实时路径点和艏向角，以及无人艇运动的轨迹单元，求取可达路径点；4)对所有的可达路径点进行路径代价计算，获得下一个路径点位置和对应的艏向角；5)判断该路径点是否为运动路径的终点，若为终点，则输出最终路径，否则，将该路径点和对应的艏向角作为无人艇所处的实时路径点和艏向角，转入步骤2)。本发明方法建立了无人艇轨迹单元模型，并且提出了基于轨迹单元的无人艇运动规划算法，使得规划的路径在实际航行过程中更贴近实际的航行要求。

Description

一种基于轨迹单元的无人艇运动规划方法

技术领域

本发明涉及海事智能交通技术，尤其涉及一种基于轨迹单元的无人艇运动规划方法。

背景技术

人工智能的发展与海洋资源的开发使得水面无人艇(USV)越来越多地承担起各种水上任务。而作为无人艇自主性的重要一部分，路径规划是能否完成各项水上任务的前提。但是由于无人艇可控程度较低、自由度较高，所以当其作为研究对象时，应从质点转变为欠驱动刚体来进行研究。相应地，其路径规划问题也应从路线规划转变为运动规划。与前者不同的是，运动规划不仅考虑了规划空间的约束，同时还对规划行为进行了详尽地讨论。而规划行为与研究对象的运动和动力学约束有关。

发明内容

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

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

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

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

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

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

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

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

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

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

规则三：轨迹段所能达到的路径点和所能改变的艏向一一映射；

规则四：所有轨迹段的轮廓形状为方格或基于方格，以适应栅格地图；

3.3)根据步骤3.1)和3.2)完成轨迹单元的建模过程，并建立轨迹单元集；

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

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

按上述方案，所述步骤3)种求取可达路径点，方法如下：

根据无人艇所处的实时路径点和艏向角，以及无人艇运动的轨迹单元，利用轨迹单元的几何特征，获得从实时路径点所能到达的路径点和相应的船艏向；

根据所能到达的路径点，搜索可达的内层路径点；所述内层路径点为地图上以所处的实时路径点为中心的8个临近点；

判断是否有路径点关联，若有则搜索可达的外层路径点；所述外层路径点为地图上以所处的实时路径点为中心的内层路径点的外层关联点；即地图上以所处的实时路径点为中心的内层路径点向外扩展一层的路径点。

按上述方案，所述步骤4)中的代价计算通过代价函数f(x)表示，具体如下：

f(x)＝g(x)+h(x)

其中，g(x)表示初始点到当前点的实际代价函数，是研究对象实际走过路径的代价值；h(x)为当前点到终点的启发式代价函数，是对剩下还未走路径的估计值；

其中实际代价函数g(x)由距离代价函数d(x)和转舵代价函数s(x)构成，启发式代价函数h(x)由曼哈顿距离表示。

本发明产生的有益效果是：

1.本发明通过建立一种无人艇运动规划的轨迹单元模型，进而有效解决无人艇运动规划的两大关键问题，分别是无人艇动力学约束表达问题和无人艇动力学约束与空间搜索的结合问题。

模型利用无人艇的运动轨迹来解决无人艇动力学约束表达问题。无人艇的运动轨迹不仅能够完整地表达其动力学约束，而且各约束间的联系也被包含在轨迹曲线中；

考虑到空间搜索是空间离散化的过程，模型根据无人艇的轨迹特点以及运动规划的搜索需求，提出了四条轨迹离散化规则。规则不仅将连续的轨迹离散为轨迹段，使每一步子空间搜索都包含了无人艇动力学约束；并且还使得这些轨迹段被拼接后仍然保持最终轨迹的连续性，从而解决了动力学约束与空间搜索的结合问题。

2、本发明建立了无人艇轨迹单元模型，并且提出了基于轨迹单元的无人艇运动规划算法，使得规划的路径在实际航行过程中更贴近实际的航行要求。

附图说明

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

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

图2是本发明实施例的作用于船体的流体动力示意图；

图3是本发明实施例的无人艇左右旋回圈分析示意图；

图4是本发明实施例的路径点与艏向的对应示意图；

图5是本发明实施例的轨迹单元生成示意图；

图6是本发明实施例的栅格地图周围网格的遍历示意图；

图7是本发明实施例的无人艇艏向的类型示意图；

图8是本发明实施例的路径点和可能的艏向示意图；

图9是本发明实施例的初始艏向为0°时，轨迹单元的生成过程示意图；

图10是本发明实施例的空间全覆盖性验证示意图；

图11是本发明实施例的轨迹单元抽象化示意图；

图12是本发明实施例的所有轨迹单元抽象化示意图；

图13是本发明实施例的轨迹段曲线及距离计算示意图；

图14是本发明实施例的轨迹段的标准化示意图；

图15是本发明实施例的轨迹段的标准化示意图；

图16是本发明实施例的自由空间下无人艇路线规划与运动规划示意图。

具体实施方式

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

如图1所示，一种基于轨迹单元的无人艇运动规划方法，包括以下步骤：

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

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

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

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

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

关于MMG模型，本发明利用MMG模型对无人艇进行运动建模。

1)、基于MMG模型的无人艇运动建模

如图2所示为作用于船体的流体动力(力矩)示意图，为了简化问题，在建模前需要做以下假设：

假设1、以无人艇的平面运动(三自由度)为研究重点，忽略其运动过程中的垂荡、横摇和纵摇。

假设2、不考虑风、浪、流等自然环境对无人艇运动的影响。

假设3、只考虑船舶主机正转的情况。并且当无人艇航行稳定后，螺旋桨转速保持不变。

针对假设3，有必要进行补充说明。当船舶处于在航状态时，船体会受到水流的阻力。从而使螺旋桨的负载增加，导致转速(n)降低。此时，速度控制器将提高主机功率，以抵消螺旋桨增加的载荷，并维持原来的转速。在上述过程中，主机在转速控制器的调节下，控制模型可表示为：

其中T_D是时间常数，I_E是整个系统的惯性矩，k_p是速度控制器的增益，n_r是命令转速，K是主传动杆到转矩输出的增益，Q_P是螺旋桨吸收力矩。

因此，无人艇在前进(X)、横移(Y)、转艏(N)下的三自由度运动模型可表示为：

其中I、H、P、R分别是惯性、粘性、螺旋桨和舵产生的力(力矩)。

(1)惯性模型

船舶在航行过程中会引起周围流体的扰动，从而使流体介质产生附加动量(动量矩)。其中，附加惯性矩可表示为：

式中m_x和m_y为X轴和Y轴方向上的附加质量；α_x为m_y作用于重心在X轴的坐标值，J_zz为作用于Z轴方向的惯性附加力矩。因此附加动量和动量矩可表示为：

式中u和v分别为X轴和Y轴方向的速度，r是艏摇角速度。对式子(1-4)在相应方向上求微分即得到流体的惯性力和惯性力矩：

(2)粘性模型

船舶所受到的粘性流体动力和力矩与船体几何特征、船舶运动状态和流体物理特性有关。因此当船型和流场不变时，作用于船体上的粘性动力和力矩主要取决于船舶当前的运动状态(即u、v和r)：

式中，X(u)为船舶的直航阻力；X_vvv²、X_vrvr、X_rrr为船舶在流体介质中运动引起的粘性阻力；Y_vv、Y_rr和N_vv、N_rr分别为线性横向水动力及力矩，Y_nlH和N_nlH则为非线性项。

通常，船舶在流体介质中运动时，漂角的大小对作用于船体上的粘性力和力矩有较大的影响。由于本文不考虑风、浪、流对船舶的影响(假设2)，所以船舶在航行过程中不会产生漂角。于是本文借助贵岛公式来估算非线性水动力和力矩(Y_nlH和N_nlH)：

将(1-6)式和(1-7)式联立，即可以得到最终的结果：

在实际应用中为了简化问题，有时采用二阶泰勒展开式来表示粘性流体动力和力矩的近似值：

式中，为X0为船舶的直航阻力，Δu为X轴方向上速度的改变量，Xu、Yv、Yr、Nv和Nr为流体动力的导数。

(3)螺旋桨模型

螺旋桨是船舶动力的主要来源，在提供推力的同时，螺旋桨还会产生横向的作用力(力矩)。但相比于舵产生的影响，该横向力(力矩)的作用很小可以忽略不计。因此，舵模型可以表示为：

式中T是螺旋桨产生的推力，t_P为推力减额系数。推力减额系数是推力减额与推力的比值，它由很多因素决定，如船舶和螺旋桨的大小。而螺旋桨产生的推力T与螺旋桨盘面直径D_P、转速n、流体密度ρ等因素有关。推力计算式可近似估算为：

T＝ρn²D_p ²K_T(J_P) (1-11)

式中K_T为推力系数；J_P为进速系数。两者又可分别表示为

式中a_i(i＝0,1,2)是回归系数；w_P是伴流系数，产生于船舶斜航时螺旋桨上。

(4)舵模型

舵模型是模型中最重要的一部分，因为它决定了船舶运动的方向，进而决定了船舶的运动轨迹。一般将水流对舵的正压力按照三个自由度分解为纵向阻力、横向横移力以及转舵力矩后，可得到舵模型：

式中δ为舵角；t_R为阻力减额系数；a_H为校正因子；x_H为舵的受力作用点至船舶重心的纵向距离；F_N为舵的正压力，它可表示为：

式中U_R为流体进入舵的有效舵速；A_R为有效舵面积；α_R为有效冲角；f_a为升力的斜率。

为了方便控制船舶从而得到需要的轨迹，还需要建立舵的控制模型：

式中δ_E为命令舵角；T_E为时间常数，约为2.5秒。

2)、无人艇运动轨迹分析

为了验证该模型的准确性，有必要对模型产生的运动轨迹进行分析。

图3为在一定转速下，无人艇操相同舵角后左右两舷的旋回圈轨迹。从图中可以看到，左舷的旋回圈半径比右舷略大(R_P>R_S)。该现象实属正常，因为一艘船舶的质量很难做到在左右两舷保持绝对的平均。所以一般情况下，船舶的重心不在其中心线上。这也证明了该模型符合实际情况。并且从图中可知，两者的差别并不是很大。因此为了简化问题，本发明在建立无人艇轨迹单元时，认为两舷的运动轨迹是相同的，即具有左右对称性。

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

规则一：在轨迹段初始和结束时刻，无人艇的运动状态稳定且保持一致；(即舵角为0且速度一致)

为了使离散后的轨迹段被拼接后仍然保持连续性，一个可行的做法是在轨迹段初始和结束时刻，使无人艇的运动状态稳定且保持一致。具体而言有两个要求：(1)舵角为0；(2)速度一致。

舵角为0是为了维持航行的稳定性。当舵固定在一定的舵角时，船舶会始终处于旋回运动状态。此时船舶的各项运动参数实时发生改变，不利于进行状态转换操作。而当舵角为0时，船舶处于直航状态。在没有风、浪、流等环境干扰的情况下，此时船舶会稳定在某一航向上。这种稳定状态有利于在轨迹拼接时，从当前状态平稳地转移到下一个状态。

而速度保持一致不仅保证了在轨迹拼接过程中，速度大小相同；而且也使得在拼接点上，前后轨迹曲线的曲率保持一致。因为不同的船速对应不同的旋回半径。而当速度一定时，旋回半径保持不变，曲率也不会发生变化。

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

轨迹单元是指不可分割的量，因此一个单元中的轨迹段应该尽可能的简单。而判断轨迹复杂程度的标准是其转向的次数，对于无人艇而言即转舵次数。因此，一个轨迹段的转舵次数不应太多；否则拼接后的轨迹将产生大量转向点，这对于实际应用显然是不恰当的。本规则要求在生成轨迹段的过程中，转舵次数不应该超过一次。

规则三：轨迹段所能达到的路径点和所能改变的艏向一一映射；

一个路径点对应一个艏向不仅使不同的路径点有且仅有唯一的代价值，而且在所有必要的艏向都被考虑到的前提下，不存在冗余的搜索点。

根据两者的映射关系，路径点和艏向一一对应如图4。

规则四：所有轨迹段的轮廓形状为方格或基于方格，以适应栅格地图；

由于不同的轨迹段产生不同的路径点和艏向，轨迹段的轮廓形状(外接矩形)在理论上是多样的。但是在路径搜索算法中，往往会采用栅格地图来对规划空间进行处理。为了适应栅格地图，轨迹段的基准轮廓形状应为方格，轨迹超过一个方格的，其形状也应该基于基本方格。假如所有的轨迹段轮廓都不是基于一个基准的轮廓网格，当这些轨迹被拼接后就不能完全覆盖规划空间，这不利于路径的搜索。

综上所述，无人艇运动轨迹离散化规则为轨迹单元的生成提出了具体的要求。规则一起到优化轨迹段的作用，它要求轨迹段起始和最终状态保持一致，保证了轨迹段被拼接后，最终轨迹的连续性。规则二起到优化无人艇操纵的作用，它限制了每一段轨迹的操舵次数，保证了最终轨迹的稳定性，避免了无人艇的频繁操舵。规则三起到优化路径搜索的作用，它选择了路径点与船舶艏向一一映射的关系，在简化了搜索过程的同时，保证了周围所有可能的情况被考虑到。规则四起到优化规划空间的作用，它规定以方格为轨迹段的基础轮廓，适应了栅格地图的环境建模方法，保证了规划空间完全被覆盖。实质上，轨迹离散化规则是船舶运动轨迹到轨迹单元的桥梁，它为接下来轨迹单元的建立打下了坚实基础。轨迹单元的生成过程如图5。

3.3)根据步骤3.1)和3.2)完成轨迹单元的建模过程，并建立轨迹单元集；

在路径规划算法中，一般利用栅格地图作为环境建模的基本方法。在网格搜索过程中，一次当前搜索往往要遍历周围八个网格(如图6(1)所示)。由于传统的路径规划(路线规划)将研究对象视为质点，所以当前点周围的八个网格都能够直接到达(如图6(2)所示)。

但是在运动规划过程中，由于考虑了研究对象的动力学约束，当前点所能到达的周围网格是由当前运动状态所决定的。具体而言，对于无人艇运动规划问题，当前节点所能到的周围节点是由当前船艏向决定的。不同的初始艏向对应可达节点的位置和该位置上对应的船艏向是不同的。因此，对于当前节点周围的八个网格，无人艇的船艏向有八个可能的方向。而根据对称性，这八个艏向可以分为两大类(如图7所示)：类型一，水平垂直艏向，即艏向分别为0°、90°、180°和270°(0°为真北方向，图7(1))；类型二，斜45°艏向，即艏向分别为45、135°、225°和315°(图7(2))。

接下来以0°艏向为代表，来分别介绍类型的建立过程。

1)如图8所示为基于轨迹单元建模：

当无人艇初始艏向为0°时，由于方向135°、180°和225°与初始艏向相反，因此轨迹段的目标艏向应为0°、45°、90°、270°和315°；对于轨迹段最终可达的周围网格而言，由于受到最小旋回半径的约束，节点d、e、f、g和h不能通过一次的转舵操纵到达。因此最终的可达路径点只可为a、b和c(如图8(1)所示)。节点b位于初始点的正前方，其艏向应该维持初始艏向不变，为0°。节点a和c分别位于初始点的左前方和右前方，因此相应船艏向应为270°和90°。此时当前节点周围八个网格都被讨论完，但是艏向45°和315°未被节点分配。考虑到这两个方向分别为北偏西和北偏东，将周围点向外扩展一层，选择节点i和j为艏向45°和315°的路径点(如图8(2)所示)。

假设无人艇当前状态为S_start(x₀,y₀,ψ₀,v₀,δ₀)，经过t时间后的状态为S_end(x_t,y_t,ψ_t,v_t,δ_t)。当初始艏向为0°时，轨迹单元数学模型可表示为：

TC＝{S_start→S_end} (1-16)

式中，x_i和y_i为无人艇的位置坐标，ψ_i为船艏向，v_i为船速，δ_i为舵角，a为网格边长。式子(1-17)为生成轨迹单元的约束，式子(1-18)为初始艏向为0°时轨迹单元的五个不同艏向和路径点的计算。

基于上述模型，初始艏向为0°时，轨迹单元的生成过程如图9所示。由于单元ob没有转舵操纵，其轨迹段为保持艏向为0°的直线段。单元oa、oc、oi和oj最终的艏向都有改变，因此他们均有转舵操纵。单元oa和oc的艏向改变均为90°，而在无人艇的运动建模中，规定了相同舵角下左右两舷舵效相同，因此，命令舵角的绝对值均为δ₁。单元oi和oj的艏向改变均为45°，由于此时艏向改变小于oa和oc，所以命令舵角δ₂应小于δ₁(即，δ₂<δ₁)。值得注意的是，在有操舵的轨迹单元中，每段轨迹均有回舵的操作(红色的回舵点)。该操纵是为了保证无人艇到达路径点后舵角为0，此时运动状态稳定并与初始状态一致。

从该类轨迹单元的生成过程可以发现：

1)轨迹段初始和最终状态的舵角均为0，并且由于无人艇螺旋桨转速恒定(假设3)，在稳定航行后(舵角为0)，无人艇航速不变。因此，满足规则1。

2)所有需要改变航向的轨迹单元生成过程，均只有一次操舵，因此满足规则2。

3)对于每个可达的路径点，有且仅有唯一的艏向与之对应，因此满足规则3。

4)基本轨迹单元(oa、ob和oc)的轮廓形状为方格，超过单个方格的单元(oi和oj)形状均以该基本方格为基准扩展，因此满足规则4。

因此，轨迹单元满足轨迹离散化规则，生成的轨迹段具备无人艇运动规划要求。

2)、如图10所示为轨迹单元空间全覆盖验证：

经过前两小节的介绍，轨迹单元模型的生成过程已经展示完全，并且两种类型的单元均满足轨迹离散化规则。但是在利用该模型提出搜索算法之前，还需对其能否完全覆盖搜索空间进行验证。同样根据对称性，以艏向为0°和45°为代表验证轨迹单元模型的空间全覆盖性。

如图10所示，图(1)和图(2)分别是初始艏向为0°和45°时轨迹单元到达周围点的情况(当前点为蓝点，周围点为绿点)。从图中可知，两种状态下都可以通过一个或多个轨迹单元的拼接直接或间接到达周围全部的八个点。因此，搜索空间内任意一个网格点都可以通过轨迹单元的组合到达，没有遗漏点的存在。轨迹单元具有全覆盖性。

3)、轨迹单元控制表：

基于以上轨迹单元的建模过程，一套完整的轨迹单元集可以建立。为了在规划过程中了解如何具体控制无人艇以达到相应的轨迹，为运动控制提供参考，有必要建立能表示操纵细节的轨迹单元控制表(如表1-1至1-8所示。其中，H为航向，RA为舵角(左舷为负，右舷为正)，CRA为命令舵角，EH为回舵)。

表1-1 0°初始艏向的轨迹单元控制表

表1-2 45°初始艏向的轨迹单元控制表

表1-3 90°初始艏向的轨迹单元控制表

表1-4 135°初始艏向的轨迹单元控制表

表1-5 180°初始艏向的轨迹单元控制表

表1-6 225°初始艏向的轨迹单元控制表

表1-7 270°初始艏向的轨迹单元控制表

表1-8 315°初始艏向的轨迹单元控制表

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

前述步骤已经将无人艇动力学约束和动力学约束与规划空间约束相结合这两大问题解决，建立起了轨迹单元模型。此处基于该模型，提出了无人艇运动规划算法，从而解决无人艇该怎样到达目标点。

该算法的主要思想是利用轨迹单元的几何特征，抽象出它们所能到达的路径点和相应的船艏向。于是在路径的搜索过程中，无人艇的动力学约束将被转换为这些可达路径点和对应的艏向。因此，该算法将运动规划问题转化为几何学的节点搜索问题。

1、轨迹单元抽象化

轨迹单元为一系列具有不同运动状态的轨迹曲线集合。而路径搜索关心的是在当前运动状态下，无人艇能够到达的节点位置和对应的方向。因此，在搜索算法提出之前，需要对轨迹单元的几何特征进行抽象化处理。

类型一轨迹单元(以初始航向0°为示例)的抽象化过程如图11所示。当初始艏向为0°时，无人艇所能到达的路径点为a、b、c、i和j。所以此时的节点搜索应该只限于这五个点。相应地，五个路径点对应五个不同的艏向。

类型二轨迹单元(以初始航向45°为示例)的抽象化过程类似。当初始艏向为45°时，无人艇所能到达的路径点为c、j和k。所以此时的节点搜索应该只限于这三个点。同理三个路径点对应三个不同的艏向。

总体分析，两种类型下所有八个方向的轨迹单元抽象化过程如图12所示。从图中可知，当前节点需要一次搜索周围16个路径点。这16个点可分为两类：内层点(八个绿色的点)和外层点(八个蓝色的点)。内层点为传统路径规划的周围八个节点，并且每个节点对应唯一的艏向。外层点位于内层点的向外一层，并且这些点均是航向改变45°的结果(即，0°→45°、45°→0°、45°→90°，等)。

因此在一个循环搜索过程中，算法会首先根据初始艏向来判断哪些点是可访问的，然后根据相应的策略选择一个最好的路径点。然而在该搜索过程中，应该如何确定内外点的搜索顺序？显然在自由空间下，内外点的顺序对最终的结果没有影响；但在有障碍物的环境下，不同的搜索顺序对能否找到一条安全的路径有关键性的作用。

2、自由空间下路径点选择策略

路径点的选择是决定最终路径优劣的关键。在众多优化策略中，启发式算法由于快速可行、简单有效并且易于修改，常常用于解决路径规划问题^[93]。该算法的核心是代价函数f(x)：

f(x)＝g(x)+h(x) (2-1)

可以从式子(2-1)看出，该代价函数由两部分组成。其中g(x)表示初始点到当前点(已搜索到的点)的实际代价函数，是研究对象实际走过路径的代价值；h(x)为当前点到终点的启发式代价函数(或者估计代价函数)，是对剩下还未走路径的估计值。代价函数f(x)的值越小，所选路径点越好。因此，本文利用启发式算法的思想，对无人艇的可达路径点进行最优评估。

1)实际代价

在传统的路径规划中，由于没有考虑动力学约束，规划路径的优劣一般用距离值来评判，距离越短，路径越优。因此实际代价函数一般为距离函数。然而对于无人艇而言，路径的好坏除了与距离大小有关，还与实际操舵次数有关。因为考虑到实际操纵与控制，一个较优的规划路径不仅是路径本身得到了优化，而且在操纵控制上也易于实现。一般而言，转舵次数越少，规划路径越容易控制。因此，无人艇运动规划的实际代价函数由距离代价函数d(x)和转舵代价函数s(x)构成。

(1)距离代价

距离代价是常见的评判路径优劣的指标，它决定了最终路径的距离大小，而这对于某些实际问题(如节省时间和燃油)有直接的影响。在计算实际距离大小之前，首先应该对轨迹单元进行分析。

根据对称性，八个方向上的轨迹单元可分为五类曲线：0°→0°、0°→45°、0°→90°、45°→45°和45°→0°。但是，由于曲线45°→0°可以被看作是曲线0°→45°的逆过程，所以就距离而言，这两种曲线应该被视为同一类。因此按照距离标准，所有轨迹单元包括四类不同的曲线段(如图13(1))。

由于轨迹单元0°→0°和45°→45°均为直线段，两者的距离可直接用两点间的欧氏距离求解：

轨迹单元0°→45°和0°→90°为曲线段，两者的距离不能用精确的求距公式计算。因此本文利用圆弧拟合来近似估计。如图14(2)所示，曲线段0°→90°(黄色)占据一个网格，并且该曲线曲率较大，可用一个半径为网格边长的四分之一圆弧来近似求解(黄色虚线)。曲线段0°→45°(橙色)占据两个网格，并且该曲线曲率较小，因此拟合圆弧的圆心在网格外部。经过计算，当半径为2.5倍网格边长，圆心角为53°时所对应的圆弧能够较好地拟合该曲线。因此，曲线段的距离求解可表示为：

式中θ和r分别为拟合圆弧的圆心角和半径。

然而，上述讨论的距离仅仅是实际值，对于代价而言，还需对其进行标准化处理。如图14(1)所示，轨迹单元0°→0°(蓝色)、45°→45°(紫色)和0°→90°(黄色)均占据一个网格。当在相同起点(点o)和终点(点d)的情况下，轨迹单元45°→45°和0°→90°均只需一个轨迹段，而0°→0°则需要两段。故在规划过程中，两个0°→0°单元与一个45°→45°或一个0°→90°单元等效。对于轨迹单元0°→45°，由于其占据两个网格，所以将网格扩展为3×3后，四类轨迹单元的等效图如图14(2)所示。

从图中不难看出，在相同起点(点A)和终点(点B)的情况下，六段0°→0°单元、三段45°→45°或0°→90°单元与0°→45°单元有相同的路径效果。同时还可以看到，这与前三段轨迹单元之间的关系一致。因此，最终的轨迹代价可表示为：

d(ψ₀→ψ_t)＝dv(ψ₀→ψ_t)·a_d (2-4)

式中，dv为实际的轨迹段距离值；a_d为距离代价系数，并且四类轨迹单元a_d的比例关系为，6:3:3:2。

(2)转舵代价

转舵代价决定了操纵的复杂性，而操纵的难易程度决定了运动控制的好坏。轨迹单元的初始艏向与最终艏向的改变量可以量化为实际的转舵代价值。因此，转舵代价可表示为：

式中，Δψ_max为所有轨迹单元中，最大艏向改变量；a_T为转舵代价系数。

由式子(4-5)可知，由于轨迹单元0°→0°和45°→45°没有艏向改变，因此转舵代价为0；而单元0°→90°和0°→45°的艏向分别改变了90°和45°，所以两者的转舵代价为a_T和0.5a_T(所有轨迹单元中，最大艏向改变量为90°，即Δψ_max＝90°)。

因此总的实际代价可表示为：

g(ψ₀→ψ_t)＝d(ψ₀→ψ_t)+s(ψ₀→ψ_t) (2-6)

此外，对于当前点周围不可达的路径点，人为设置这些点的实际代价值为正无穷。所以无论这些点的启发式代价如何，总的代价值(实际代价加启发式代价)依然为正无穷，在路径点选择时可自动忽略。

2)、启发式代价

启发式代价是对未来路径代价的估计，它评估了哪些路径点最有可能朝向目标点。因此设置启发式代价的目的是为了引导搜索方向朝向目标，从而节省搜索时间，减少计算量。但是，启发式代价对整个代价函数的影响不能太强。因为无论选择的路径点有多么好，判断都是在当前状态下发生的。而该点是否在接下来是时间内仍然是最好的，无从得知。

基于以上分析，本文选择曼哈顿距离作为启发式代价的估计。在某些路径规划算法中，有将欧式距离作为启发式代价的做法(如图15(1))。但由于两节点间的欧氏距离比曼哈顿距离短，欧氏距离不能很好地表示不同路径点的启发式代价区别。并且欧氏距离在计算过程中有开方运算，这与启发式代价降低计算量的原始初衷相悖。因此，选择曼哈顿距离较为合理。

在规则3的保证下，不同的艏向点具有不同的曼哈顿距离值，并且艏向角越靠近目标点，曼哈顿距离值越小。例如，当前艏向为0°时(如图15(2)所示)，可达路径点a、b、c、i和j均有不同的曼哈顿距离值。显然，路径点j(艏向为45°)是最靠近目标(绿色八角形，x)的节点，而此时该点的曼哈顿距离值也最小。因此，启发式算法可以表示为：

h(ψ₀→ψ_t)＝|x_g-x_t|+|y_g-y_t| (2-7)

式中，x_g和y_g为终点的位置坐标。

在上述讨论过程中，路径点的艏向角与目标和当前点之间的方向角的差值也可以作为启发式代价的判断依据。该差值越小，表示备选路径点越靠近目标。显然在自由空间下，该判断策略是准确无误的，选择的路径点能够保证最终的路径是最优的。但是在有障碍物的环境下，该策略容易使得规划的路径陷入局部最优。

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

关于本发明方法的应用：一个示例。

对比实现结果如图16所示。由于运动规划考虑了无人艇的船艏向，因此八个不同的初始艏向对应八个不同的规划结果(蓝点为起点，绿点为终点)。

图16的(1)为路线规划结果。由于没有考虑无人艇的动力学约束，规划的路径仅由路径点的连线所组成。从路线本身的角度来看，该结果为起点到终点的最短路线。但从实际的规划控制角度来看，该结果没有多少参考意义。

图16的(2)-(9)为运动规划结果。从整体上看，每个规划结果都有两到三个规划阶段：初始状态的调整(红色方框)，最优路径的执行(绿色方框)和最终状态的调整(蓝色方框)。其中，图16的(3)由于初始状态正好为最优，所以不必进行多余的调整；图16的(6)由于经过初始状态调整后航向正好朝着目标，所以不必进行最终状态的调整。图16的(2)和16的(4)的规划路径类似，在初始阶段分别向左右两舷操一个小角度舵角，使无人艇调整到最优状态。图16的(5)和16的(9)的规划路径类似，在初始阶段分别向左右两舷连续操两个小舵角来调整无人艇的运动状态。图16的(6)和16的(8)的规划路径类似，在初始阶段分别向左右两舷先操一个大舵角，使船艏向改变90°；然后操一个小舵角调整到最佳状态。图16的(7)无人艇的初始艏向角与目标位置角正相反，因此需要首先进行旋回操纵，将船首转到朝向目标角度，再进行路径优化。

因此，该对比规划路径证明了基于轨迹单元的无人艇运动规划算法能够在自由空间下规划出一条正确合理的运动路径。该规划路径不仅能够满足无人艇的动力学约束，使得每一步航行都合理有效；并且还能保证其为最优结果。

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

Claims (3)

1.一种基于轨迹单元的无人艇运动规划方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

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

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

规则三：轨迹段所能达到的路径点和所能改变的艏向一一映射；

规则四：所有轨迹段的轮廓形状为方格或基于方格，以适应栅格地图；

3.3)根据步骤3.1)和3.2)完成轨迹单元的建模过程，并建立轨迹单元集；

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

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

2.根据权利要求1所述的基于轨迹单元的无人艇运动规划方法，其特征在于，所述步骤3)种求取可达路径点，方法如下：

根据无人艇所处的实时路径点和艏向角，以及无人艇运动的轨迹单元，利用轨迹单元的几何特征，获得从实时路径点所能到达的路径点和相应的船艏向；

根据所能到达的路径点，搜索可达的内层路径点，作为可达路径点；所述内层路径点为地图上以所处的实时路径点为中心的8个临近点。

3.根据权利要求1所述的基于轨迹单元的无人艇运动规划方法，其特征在于，所述步骤4)中的代价计算通过代价函数f(x)表示，具体如下：

f(x)＝g(x)+h(x)

其中，g(x)表示初始点到当前点的实际代价函数，是研究对象实际走过路径的代价值；h(x)为当前点到终点的启发式代价函数，是对剩下还未走路径的估计值；

其中，实际代价函数g(x)由距离代价函数d(x)和转舵代价函数s(x)构成，启发式代价函数h(x)由曼哈顿距离表示。

Images