无人船实时路径规划与编队控制仿真研究

源自：系统仿真学报

作者：宋大雷  干文浩  许嘤枝  曲秀青  曹江丽

摘  要

安全和无碰撞导航是无人船正常航行的基础。通过Unity3D构建高保真的虚拟海洋环境，在无人船建模基础上，提出一种面向未知复杂环境的实时路径规划及编队控制方法。利用激光雷达获取的局部环境信息，在重规划的策略下，结合A-star以及航路抽稀方法完成实时局部路径规划。基于领航-跟随者策略与一致性方法进行编队控制，结合人工势场法完成多无人船的避障航行任务。在多种复杂地图上的仿真实验表明，该仿真系统具有较好的真实性和视觉效果，为无人船模拟试航提供了有效手段。同时，无人船群利用所提算法可以依次抵达预设航路点，自主变换队形实时避障，在工程应用中具有一定理论意义和实用性。

关键词

 无人船 ; 运动仿真 ; 场景模拟 ; 路径规划 ; 编队控制

引言 

随着世界各国对海洋重视程度的加深，水面无人船(unmanned surface vessel，USV)因自主程度高、机动性强等特点，在海洋科学调查、海上搜寻和运输[1-2]等方面得到了广泛应用。同时，随着航行任务复杂度的增加，多无人船协同作业应运而生[3]。

多无人船路径规划与编队控制是无人船作业中需要考虑的核心问题之一[4]，对其进行有效的设计能够提高无人船的工作效率。由于USV的体积、携带的传感器以及水域环境的复杂性使得实施USV的现场实验既困难又耗时，在开发初期，不宜通过实航实验去检验系统设计的正确性。因此,在虚拟环境中进行USV的航行模拟就显得尤为重要,其运动控制方案的模拟以及航行过程的可视化对于分析系统设计的完整性以及USV在环境干扰和自动避碰等需求下的运动情况有着重要意义。

在路径规划与编队算法方面，近年来出现了一些相关研究。文献[5]提出了一种改进的蚁群算法进行路径规划，并采用领航-跟随者方法实现了多机器人编队，但并未考虑群体实时路径规划与编队队形变换问题。文献[6]提出一种基于快速扩展随机树的路径规划算法，并结合领航者-跟随者结构与位置约束实现编队。文献[7]使用人工势场法对多机器人进行路径规划与编队避障，但未进行编队队形变换且在行进的过程中存在一定范围的误差。文献[8]将人工势场法与虚拟结构法相结合，设计了一种编队控制方法。该方法能够灵活实现队形变化，但避障时是使单个航行器沿着障碍物边界划过再恢复为原队形。文献[9]针对未知环境中的无人船编队避障问题，对障碍物进行了建模，然后基于领航-跟随法设计了实时避碰控制器。虽然该方法能够保证在完成跟踪任务的同时完成避障，但是各艘无人船都是自行避障。总的看来，现阶段利用领航-跟随者的编队方法已经能够较好地解决队形保持和轨迹跟踪等问题。而在编队及避障问题的研究中，有着不同的解决策略，如保持原队形进而控制整个群体从障碍物一侧进行规避，或者打散原队形各自避障等等。这类方法大多是基于环境状态已知、障碍物形状规则且分布较为稀疏的情况设计的，对于复杂环境并不是最优的解决方案。

针对上述问题，本文旨在搭建一套高保真的无人船仿真平台，并提出一种面向未知复杂环境的实时路径规划及编队控制策略。所搭建的仿真平台可提供一个全栈的闭环仿真，模拟理想情况或特定环境扰动，再现无人船的航行效果并实时反馈信息。所提出的航行算法，能够引导多无人船在未知环境中进行实时路径规划从而抵达每个预设的航路点，并在航行过程中保持秩序自主编队与避障。

1 仿真平台的构建

1.1   无人船模型

使用一艘真实的USV进行仿真研究，模型如图1所示，结构参数见表1。为了满足USV对象更高的仿真逼真度以及仿真性能，模型被进行了部分简化处理，简化后的船体网格具有23 707个三角面和34 559个顶点。该船采用了双体结构，并在船尾两侧配备2个推进器，矢量角度固定。无人船可以通过控制2个推进器的转速产生偏航运动。

图1   本文所使用的USV模型

表1   USV模型参数

 1.1.1    无人船运动学模型

虽然该USV只能通过控制两侧推进器在海表面运动，但是当其航行时，可能会以各种不同的航速和航向角遭遇各种不同的波浪，从而发生6个自由度的运动，因此本文将关注USV的6自由度(6DOF)运动情况。在这项工作中使用的坐标系如图2所示，其中E0表示惯性坐标系，B0表示机体坐标系，相关变量的描述见表2。

图2   USV坐标系示意图

表2   无人船的坐标变量

USV的状态被可以描述为

(1)

而USV机体坐标系下的速度与角速度到惯性坐标系的转换矩阵可分别表示为

(2)

(3)

由此，USV的运动学模型可以被描述为

(4)

 1.1.2   无人船动力学模型

无人船的动力学模型用来描述无人船在受到力和力矩作用时发生的状态变化[10]。根据刚体在流体中的牛顿-欧拉运动方程，该欠驱动USV的动力学模型可以被描述为

(5)

式中：M为无人船的惯性质量阵，由刚体惯量矩阵MRB和附加质量的惯量矩阵MA构成，可分别通过Solidworks等软件建模以及CFD仿真分析获取流体动力参数得到；类似的，C(v)为向心力和科氏力系数矩阵；D为水阻尼矩阵，无人船在水面受到的阻力可近似表示为与速度平方的相关关系[11]，通过无人船的FLUENT阻力模拟可以完成该关系的拟合；g(η)为恢复力/力矩，表示重力和浮力的共同作用；τ 为无人船的驱动力和驱动力矩矢量；d 为海洋扰动。同时，该USV的驱动力和驱动力矩τ 由左舷和右舷推进器推力Tport 和Tstbd 计算得到。在实际控制任务中，由于需要控制的是差动推进器的转速，因此还需要获得推力与推进器的每分钟转数(RMP)之间的对应关系，通过水动力仿真可以完成这一过程的拟合。

求解式(5)的关键在于求得作用于无人船的各种力和力矩，这一过程在Unity3D中的FixedUpdate函数中完成计算：力的值被传送到AddRelativeForce构件函数，扭矩的值被传送到AddRelativeTorque 构件函数，每帧更新一次相关的矩阵变量并求解USV的实时运动状态，更新周期为0.02 s。

1.2 海洋环境

该USV的用途是在海面进行航行并执行任务，因此海面环境的搭建是主要关注点。当其航行时，主要是受到浪和流这两部分因素的影响。

 1.2.1   浪的构建

对于海表面的模拟，可将海水看作是由多种表面微粒所组成，这些微粒在三维曲面上运动从而形成了表层的结构波浪[12]。这些微粒的振幅可认为是在海平面上某一个点的波高，许许多多这样的微粒的运动构成了整片海域内的波浪高度场。

考虑到波浪谱是现阶段海浪性质研究的重要手段[12]，因此该仿真平台基于Phillips谱模型实现了海浪的绘制，它可以表示为

(6)

式中：A为Phillips常数，根据文献[13]取值为0.008 1；

为风速(影响海浪的浪高)，g 为重力常数；ωD 为风向(二维向量，影响海浪的波动方向)；波数可表示为n=2π/ωl ，ωl 为波长；波数向量为k=(kx,ky)=(ncosθ,nsinθ) ，θ 为向量k 与x 轴正半轴的夹角。

波被设置为14个不同的波浪叠加形成，index=0,1,⋯,13 。由此可得到相应的波长ωl=2−4+index 。对于谱分析模型，在t 时刻位置向量X=(x,y) 处的海水高度场可表示为

(7)

式中：*为共轭复数；

为初始谱模型，由2个独立的高斯随机数εr 和εi 以及谱函数构成：

在仿真过程中，可以由此获得USV所在海面上每一个网格顶点对应的波浪高度hw 、实时的三维波浪流速vw=[uw , vw , ww] 以及波浪法线nw ，从而进一步计算得到USV的实时浮力，以及波浪的对USV的干扰力与力矩。

 1.2.2    流的构建

考虑到USV实际工作时间以及工作区域的有限性，在一段较长时间内特定区域下海流的运动速度、流量与流向通常都是较为稳定的，即海流变化速度是缓慢，而且是稳定无旋的。

海流的存在会改变USV受到的流体动力[14]。设Vc=(Uc , Vc , 0) 为惯性坐标系下的海流速度；vr表示USV相对于流体的运动速度，它在体坐标系中的分量为(ur , vr , wr) ，综合波速，可以得到

(8)

1.3   传感器模型

因为障碍物的高度对USV路径规划的影响很小，因此USV的路径规划可以看作是二维曲面上的导航。在本文中，简化后的Merlin船载激光传感器被用来获取USV周围的障碍物信息，如图3和表3所示。简化后的传感器使用虚拟光线来探测局部环境，从而获取障碍物距离当前USV的位置和角度等信息。同时，传感器一旦探测到局部障碍物，就会存储障碍物的信息并用于避障。

图3   USV上搭载的激光传感器

表3   简化后的船载激光传感器信息

对于仿真平台的操作部分，使用WinForm技术搭建了一个控制平台，用来模拟地面岸站对无人船的指挥操作。其余可视化部分如相机视角、海底地形的搭建、灯光的渲染、航行器航迹与拖尾效果的显示等等，设计流程与文献[15-16]中所提及到的类似，在此不再赘述。

2 算法设计

2.1   总体方案

所设计的算法适用于解决未知环境下的多无人船路径规划与编队控制问题。问题可描述如下：在环境中存在若干未知障碍物(如由潮汐现象出现的岛屿)，无人船群需要组成编队队形，安全的通过障碍物组成的狭长区域或绕开障碍物到达预设的航路点。总体策略采用多智能体分层导航控制方案，整个控制系统分为：目标层、领导层和跟随层。

• (1) 目标层：该层由用户使用控制平台进行输入，即指定一系列USVs航行过程中的关键航路点。

• (2) 领导层：编队领航者根据关键航路点信息实时规划路径，由通信拓扑分析决定相应的编队队形，并在航行过程中实时避障。

• (3) 跟随层：编队跟随者跟随领航者移动并保持相应队形。

图4展示了该算法的一类应用场景。当USV群未检测到周围存在障碍物时，将按照设定队形前进。一旦有USV检测到前进方向上存在障碍物，将改变队形为长蛇阵穿过或沿边缘绕开障碍物(其中Δd表示前后无人船之间的相对距离)。待远离障碍物后，调整回原队形继续前往下一航路点。

图4   路径规划及跟踪示意图

按照USV和障碍物之间的距离，可以把障碍物附近的区域划定为安全区、避障区或者危险区。当USV进入避障区时，触发避障模式，在不进入禁止区的前提下规避该障碍物，如图5所示。其中，Rmax 和Rmin 分别为避障区和危险区的半径，选取 Rmax=80 m，Rmin =20 m；Robs 为障碍物距离USV的最短距离。

图5   无人船避障策略示意图

(1) 当Robs>Rmax 时，路径跟踪模式有效。

(2) 当Rmin≤Robs≤Rmax 时，避碰模式有效。

(3) 当Robs

2.2  领航者的路径规划算法

无人船需要在复杂环境中具备自主航行的能力，其中最先应具备的就是路径规划能力，即主要考虑如何在USV移动空间中找到一个从起始点到终点的安全航行路线。考虑到本文主要研究存在任意形状障碍物环境下的无人船路径规划问题，采用图搜索法对环境进行栅格化的操作可以很好的将障碍物外形纳入规划范围。同时，该方法也是目前公认的最成熟的、安全系数最高的算法[17]。

在此基础上，基于A-Star算法[17]设计实时重规划策略，并结合PID控制方法完成路径规划和路径跟踪任务。该算法适用于在未知障碍物分布的情形下，实时规划出局部的无碰撞路线。确定航行过程中必须途径的航路点位置后，会生成一条预先规划好的路径，无人船群依次途经每一个航路点；同时，当航行过程中无人船搭载的激光传感器探测到障碍物后，会重新进行局部动态路径规划以避开障碍物。该算法的总体流程图如图6所示。

图6   路径重规划及跟踪流程图

其中，栅格粒度大小需要根据无人船运行环境的范围进行设计，与多无人船群体间共享的全局二维栅格地图map[x, y] 大小相关。较大的粒度意味着环境划分比较粗糙，但算法执行效率会有较大提高。相反，较小的粒度意味着环境划分更加精细，规划出的路径也会更加精细，但也增加了算法的规划时间。同时由于环境的未知性，共享栅格地图map[x, y] 在起始没有先验信息，只有在探索到新的环境信息后(根据每艘USV传感器采集的信息)才会被逐渐填充地形信息数据。

在每一次到达新的必经航路点(关键点，人为设置得到)、规划出来的路径点(规划算法得到)或者传感器探测到新的障碍物时，需要再次执行新的A*规划算法，从而得到一条符合当前状态的航行路径。之后，采用道格拉斯-普克抽稀算法(Douglas-Peucher)进行航路优化，减少无人船群体多余的转向机动。

得到优化后的航行路径后，使用双闭环控制器完成对领航无人船的航行控制，控制框图见图7。

图7   双闭环控制策略

该控制器主要由位置环和速度环串联构成。

• (1) 对于位置环，根据规划出的下一路径点坐标P(x, y)，可计算出相应的回转角度偏差eψ 和位置偏差ep 构成该环的输入项，输出得到USV系统在体坐标系下的期望进退线速度uR 和期望回转角速度rR 。

• (2) 对于速度环，根据位置环的输出并结合USV系统当前时刻的真实进退线速度u 和回转角速度r 构成速度偏差eu 和角速度偏差er 项，最终计算得到USV差速推进器的转速控制量nport 和nstbd ，完成对USV航行系统的控制。

2.3  无人船编队控制算法

多无人船编队问题主要考虑信息交互策略以及队形控制方案的设计。其中信息交互策略中的分布式方法对通信设备和计算设备的要求都较低，被认为是最优的信息交互策略。队形控制方案中的领航-跟随法因理论成熟且易于拓展的特点得到了广泛应用。本文将使用图论方法确定无人船编队的通讯拓扑结构，然后采用领航者-跟随者的分布式控制结构结合群集一致性方法解决编队控制问题。

 2.3.1   系统一致性

基于群集一致性的编队控制算法关键思想在于控制好领航者和跟随者之间的相对位置和速度等状态量，当这些状态量达到稳态时就实现了编队队形[18]。在本文中，通过通信图协同控制USV群动态系统。编队中信息交互用G=(X, E) 表示。其中，X={x1 , x2 , ⋯, xN} 表示N个节点的集合，EE为网络拓扑图的边集，集合E中的元素(pi , pj) 表示从节点pj 指向pi 的边，记为eji 。A(G)=[aij]∈Rn×n 用于表示交互图G的邻接矩阵，满足如下关系式：

(9)

 2.3.2   基于领航者的行为一致性方法

首先建立跟随者的数学模型：

(10)

式中：ξi 和δi 分别为第i 艘USV跟随者的状态信息量(如回旋角、位置信息、速度等)和控制输入量(如回旋角速度、速度、加速度等)。

对于领航者的数学模型可表示为

(11)

在任何初始条件下，当跟随者的状态满足：

(12)

该USV群体就形成了具有领航者的一致性。对于基于领航者的群集一致性可表示为

(13)

式中：δi 为无人船i 的速度变化控制量；

为无人船i 和j 之间需要保持的距离。而权重项设置为

最后得到第i (i>0) 艘跟随者无人船在t 时刻，惯性坐标系下x 和y 轴方向上的期望速度分量：

(14)

式中：

且具有最大值δmax ，在仿真中δmax 取为2；表示速度变化的朝向；K0 和γ 均为常数，仿真中分别取为1和0.5。

 2.3.3    基于人工势场法的船体间避碰

为避免无人船之间的碰撞，引入人工势场中的斥力场。斥力frep 可表示斥力场Urep 的梯度，在x轴方向上的无人船i 与无人船j 之间的斥力关系可表示为

(15)

式中：η为斥力尺度因子，仿真中取为10；lij 为无人船i 和j 之间的距离；Rdetect 表示斥力场的影响半径，即当无人船之间相距较远时，它们之间就不存在斥力影响。

同理可得y 轴方向上的船之间的斥力关系。最后叠加得到第i (i>0) 艘无人船在t 时刻，惯性坐标系下x 和y 轴方向上的期望速度分量：

(16)

3 仿真实验与分析

为验证USV运动仿真的真实性，路径规划及编队算法的有效性和可靠性，以及仿真平台的实用性，进行了如下三类实验。

3.1   无人船运动模型验证实验

USV的位置、速度以及角速度被初始化置0，然后被投放到一片海域内进行随波漂流实验。通过调整风速ωs 大小，得到的仿真效果如图8所示。

图8   不同风速大小下的仿真效果

将三种风速下USV的运动状态以及漂流轨迹绘制出来，得到的仿真结果如图9和10所示。

图9   不同风速大小下的USV的运动状态变化情况

图10   不同风速大小下的USV中心处受到的波浪变化情况

从仿真效果可以看出，该仿真平台可以良好的模拟出USV在不同风速大小的下运动状态，符合预期效果。

3.2   路径规划及编队算法测试实验

在该实验中，利用Python环境进行了算法测试。以多无人船编队通过狭长水道为任务场景，进行仿真。该无人船群的初始状态信息如表4所示。

表4   无人船集群初始状态信息

设置航行过程中必经航路点的数量为4，具体位置信息如下：

• (1) 航路点1：(200, 300) m；

• (2) 航路点2：(760, 620) m；

• (3) 航路点3：(1 360, 1 480) m；

• (4) 航路点4：(1 900, 1 900) m。

仿真步长设置为0.02 s，按照图4所示的起始和结束编队队形进行航行，仿真结果如图11所示。

图11   无人船通过狭长水道的路径规划及编队仿真

从图11(a)可以看出，从0 s开始无人船集群迅速组成了起始编队结构向航路点1移动；在到达航路点1后又向航路点2移动，在靠近航路点2时，无人船探测到前方存在障碍物，迅速变换队形为长蛇阵继续移动；从航路点2向航路点3移动过程中，由于障碍物始终位于无人船群附近，队形始终保持为长蛇阵；从航路点3向航路点4移动过程中，无人船集群到达空旷区域后变换回原队形继续移动。从图11(b)可以看出，在第240~730 s的过程中，无人船群探测到了障碍物并变换队形为长蛇阵，每个无人船之间保持固定间距20 m移动。

进一步在更加复杂的群岛区域进行了仿真，设置航行过程中必经航路点的数量为4，具体位置信息如下：

• (1) 航路点1：(440, 60) m；

• (2) 航路点2：(1 060, 1 140) m；

• (3) 航路点3：(1 640, 1 540) m；

• (4) 航路点4：(1 900,1 900) m。

仿真结果如图12及表5所示。

图12   无人船群岛区域路径规划及编队仿真

表5   群岛区域下的无人船航行性能

图12和表5可以看出，当无人船群在群岛区执行路径规划及编队任务时，所提算法能够能够自主地规划出一条安全的航行路径，并且满足实时避障要求，体现出了更高的实用性。

3.3   仿真系统综合实验

无人船作为水面智能任务平台，可以用于完成环境监测、海洋调查、巡逻搜救及侦查等多项任务。在航行过程中规避途经岛礁或者其它障碍物，以较短路径安全抵达指定地点是USV应具备的基本能力之一。本小节使用搭建好的高保真仿真平台以及无人船运动模型进行具体任务下的综合仿真验证。

设计一个无人船集群在多个岛礁区域中的侦查任务场景，无人船群需要到达预设好的若干个位置点。同时由于潮汐现象岛礁环境具有未知性，无人船群还需要在航行过程中不断收集环境信息进行自主路径规划和群体避障。在该场景中，使用Unity3D中的地形编辑器来构建了一个近乎真实的岛屿场景，并添加了Mesh碰撞体以完成航行器的碰撞检测。使用3艘USV进行仿真，初始和结束队形均设为边长40 m的三角阵。设置4个关键航路点，仿真结果如图13~15所示。

图13   USV群航行过程

图14   USV航行起始及终止时的编队队形

图15   平静海况下的USV群整体航行轨迹

图14中USV周围的圆圈代表避障传感器，当其变为红色时代表USV探测到了障碍物。从图15航行轨迹可以看出，USV在航行过程中不断根据实时状态信息切换队形，最终在保证群体安全避碰的前提下依次通过了预设的4个关键航路点，完成了此次的编队航行任务。

除此之外，为验证算法在不同环境下的适用性，在环境中施加定常海流和波浪作为海洋扰动项继续实验。仿真中海流流速被设置为0.5 m/s，沿南偏西45°方向，风速大小为15 km/h，得到的编队整体轨迹如图16所示。

图16   海洋干扰下的USV群整体航行轨迹

从图16可以看出，虽然群体轨迹有往南偏西方向偏移的趋势，但无人船集群仍能完成路径规划及编队控制任务。

4 结论

针对无人船航行系统开发的需要，设计并实现了一套针对多无人船的三维视景仿真系统。建立了USV在浪流干扰下的运动数学模型，并仿照真实的船舶控制环境搭建了多无人船运动可视化仿真平台。同时针对复杂未知环境下多无人船群的航行问题，提出多算法融合的实时路径规划及编队控制方法。该方法基于机载激光传感器收集周围环境信息，使用路径重规划策略，结合A-star以及航路抽稀方法完成无人船群体在未知环境中的实时路径规划。然后，基于领航-跟随者策略与一致性方法进行编队控制，结合人工势场法完成船体间避障功能。该融合方法解决了多无人船在未知环境下的实时路径规划以及变换队形进行有序避障的问题，具有较大的灵活性和可拓展性。

在此基础上，设计了具体的应用场景进行仿真验证。结果表明，该仿真系统能够较好地模拟USV的实际航行状况，以及多USV的路径规划以及编队行为，具有较高的仿真精度，在测试无人船性能以及验证航行器算法方面有着很强的实用性。同时也验证了所提出的路径规划及编队算法，可以使得USV群在有效规避障碍物的同时，准确地到达每一个预设的航路点，且在不同环境中都能保持一定的有效性与稳定性。这种方法在多无人船的海上运输、搜救等任务中有着潜在的应用价值。

声明:公众号转载的文章及图片出于非商业性的教育和科研目的供大家参考和探讨，并不意味着支持其观点或证实其内容的真实性。版权归原作者所有，如转载稿涉及版权等问题，请立即联系我们删除。

“人工智能技术与咨询” 发布