无人水面艇自主航行控制仿真系统研究*

程 宇，付悦文，李 鲁

（1.海军装备部驻武汉地区第五军事代表室，武汉 430000；2.江苏自动化研究所，江苏 连云港 222000；3.中国船舶工业贸易有限公司，北京 100000）

0 引言

无人水面艇具有成本低、体积小、航时长、速度快、智能化等优点，已经成为当今军事和民用领域发展的重点，特别是军事领域［1］。国内外学者对无人艇技术的研究较为全面，尤其在无人艇相关算法研究成果较为丰富，但是对仿真系统环境研究较少，普遍为单一特定功能，结构较为简单，未结合实际应用场景［6］。本文所研究的自主航行仿真系统具有全局航路、无人艇操纵性运动模型、环境信息、避障和航行控制等功能模块，提供了相对完备的自主航行仿真系统，系统可实现模拟无人艇水上航行运动和环境信息，支持无人艇探测巡逻、目标跟踪等任务推演，也可回放实海域历史数据，为无人艇提供算法调试环境、数据分析处理功能等，为无人艇开发和试验提供一个多功能的仿真验证平台，可以有效减少海上调试试验周期，为海上试验做好充分保障和准备。

1 无人水面艇自主航行控制仿真系统方案

无人水面艇自主航行控制仿真系统数据通信方案的流程如图1 所示，各模块之间主要通过UDP协议进行数据通信。

图1 无人水面艇自主航行控制仿真系统通信方案Fig.1 Communication scheme of autonomous navigation control simulation system of USV

无人水面艇自主航行控制仿真系统流程方案如图2所示，显控模块显示规划航路、无人艇导航信息、航行态势信息等，环境信息模块实时地输出周围航行目标信息至避障和全局规划，全局规划航路输出至避障和航行控制模块，避障对航行态势进行安全评估，若发生航行安全，进行避障规划，根据当前情况输出避障解算的期望航速、航向以及规划航路传给航行控制模块，航行控制模块根据规划航路、期望航速、航向输出相应的舵角、油门传给无人水面艇模型，无人水面艇操纵性运动模型根据舵角、油门和环境干扰，实时输出当前的姿态和位置信息给到避障模块，直到任务结束。

图2 无人艇自主航行控制仿真系统流程方案Fig.2 The process scheme of autonomous navigation control simulation system of USV

2 无人水面艇自主航行控制仿真系统构成

2.1 显控模块

2.1.1 地图显示

显控模块是在开源栅格地理数据库GDAL和开源跨平台开发框架Qt 的基础上实现的。遥感图像一般较大，无法全部读入内存，结合Qt 的图形视图框架，可以只读取当前视图内对应的图像数据，并完成显示图像功能，减少计算机的内存消耗，提高计算资源的利用效率。另外，遥感图像除了包含原始的光栅数据外，还包含一些与之相关的信息，如图像尺寸、波段数、空间投影信息等，在使用GDAL函数读取图像数据之前，可以先将这些信息读取出来。遥感卫星地图相比海图具有更清晰直观的显示效果、更低的价格成本以及开发时间，同时显示范围更大，也可支持内河的显示。针对实际环境中存在的固定碍行区可导入实海域试验数据进行标注，或导入先验高精度激光雷达探测生成的栅格地图，基本满足无人艇仿真环境的功能实现，因此，采用了遥感卫星地图，如下页图3所示，为日照帆船基地港口地图。

图3 显控界面图Fig.3 Display and control interface

遥感图像数据集的地理参照坐标系统采用WKT 描述，GDAL 使用仿射坐标系来描述遥感图像像素坐标与地理坐标之间的关系，如式（1）所示。

2.1.2 全局航路规划

A*算法又称为启发式搜索算法［3］，适用于在空间内对已知起点至终点最小代价路径的求解，航路规划选用A*规划算法时，对其规划空间进行网格化，网格密度越高，规划精度就越高，计算量和运行时间也就越大，一般根据实际需求进行网格密度设定，网格数据填入网格中心点的经纬度以及网格坐标，通过计算代价函数计算最优代价航路。代价函数中加入启发函数，使得规划更有目的性，更快速地到达终点目标，避免遍历所有节点，从而提高搜索效率。A*算法的代价函数组成如下：

其中，f（n）为在节点n的总代价；g（n）是从初始节点到当前节点n的实际代价；h（n）是从当前节点到目标节点的估计代价，因为h（n）为启发函数，当函数h（n）满足条件时，如式（3）所示，A*算法存在最优解且是唯一解。

其中，h*（n）是节点n到目标节点的真实最小代价。

在栅格化的搜索空间中，A*算法通过计算当前节点代价与到周围节点的代价之和，选取周围最小代价的节点加入到集合中，作为下一节点进行扩展，同时更新代价函数，继续下一轮的扩展计算，直到找到满足式（3）条件的节点为止。A*算法的基本流程：首先建立open 表和close 表，当前节点周围可通行点存放在open 表中［10］，已经访问过的节点存放在close表中，open表为优先队列，升序排列，队列中第1 个节点为最优节点，并将其放入close 表中存放，同时作为下一步的起始节点，当搜索到目标点或者open表为空时搜索结束。A*算法流程图如图4所示。

图4 A*算法流程图Fig.4 A*algorithm flow chart

A*规划的路径点相隔过密且折线较多，不适合直接作为无人艇的航路，可对其原始规划航路进行优化，剔除多余航路点且不影响航行安全，将折线变为直线，保证不会出现锯齿阶梯状规划航路，路径光顺度提升，符合无人艇航行控制的要求，有利于无人艇任务执行。本文采用的优化方法为：规划路径点中选取连续点Pi、Pi+1、Pi+2，若Pi与Pi+2之间的连线不穿过任何障碍物，则删去Pi+1；然后，连续判断Pi与Pi+2后续节点连线，连线不碰撞任何障碍物，则删去Pi+2；依次类推，直到Pi与后续节点直连穿过障碍物，并选取后续3 个节点赋值给Pi、Pi+1、Pi+2点，重复上述操作，直到遍历完规划路径点。

2.1.3 任务推演

任务推演是任务规划系统的主要功能之一，它是衔接任务规划和任务执行的重要环节之一。当完成任务规划后，通过任务推演系统对将要进行的任务航路，通过本文的仿真系统进行动态反复的预先演示。通过演示结果，评判任务规划的合理与否，并进行任务的调整。在提前预设的侦察背景下，根据任务规划中预先设定的任务以及航路，对有可能影响任务执行结果的不同关键环节进行连续重复的演练、调整，直到满足任务执行的条件。

本仿真系统基于无人艇的功能需求，对其任务进行设计，以任务规划为基础，设计具备包括警戒巡逻、疑似目标跟踪或伴行、返航航行、定点搜索、目标确认等任务场景的推演能力，可辅助人员确认任务执行的合理性，图5为某区域巡逻任务规划。

图5 巡逻任务规划Fig.5 Patrol mission planning

2.1.4 数据回放

将从无人艇实海域测试网络采集记录得到的网络包通过网络仿真系统数据回放功能在地面实验室环境下进行分析回放，可以真实再现艇上航行数据状态及网络状态，从而不需要在艇载软件开发调试过程在艇上与设备连接，同时也可以回放真实场景用来测试自主航行算法软件，缩短了算法软件调试开发时间，有效节约试验资源，提高软件开发集成效率。主要利用WinPcap 提供的API 函数，对无人艇上交换机设备采集的pcap 格式IENA 网络包进行解析回放，数据回放功能主要包括：

1）数据回放功能：能够打开保存到本地的历史网络报文数据包，将数据包存储的网络报文按照时间顺序通过网络回放发出。

2）报文方案设定功能：具体数据添加、修改、系数、删除、导入等功能，根据外部提供正确数据包添加界面显示等操作。

3）报文解析功能：根据添加或导入数据包信息，解析正确数据包，并可查看解析后数据包格式等信息。

4）视频解析功能：根据正确视频数据包格式，选择性转换需要视频格式播放。

2.2 无人艇操纵性运动模型模块

在复杂环境外力作用下，无人艇为六自由度运动，为了降低计算量和系统复杂度，纵荡、横荡、艏摇的影响可以忽略，采用MMG 分离建模［9］，建立以下三自由度无人艇操纵性运动方程：

式中，x为横坐标；y为纵坐标；u为纵向速度；v为横向速度；r为艏向角速度；ψ为艏向；m为质量；Izz为转动惯量；xg为中心重心距；[XH YH NH]为水动力；[XP YP NP]为推力；[XR YR NR]为舵力；[XE YE NE]为外力；外力主要由风、浪、流3种环境力组成，其中

2.3 环境信息模块

无人艇仿真系统中环境信息模块主要分为内部和外部信息，内部信息主要包括人工设置或模拟传感器信息以及环境干扰信息，外部信息包括无人艇实海域录取的传感器数据信息，通过数据回放实现数据场景重现。

2.3.1 内部模拟信息

1）人工设置障碍物

通过仿真系统可设置静态多边形障碍物、动态多边形障碍物、禁航区域、随机障碍物、目标等对无人艇自主航行过程进行人工约束，其中，障碍物可设置其位置、大小、属性、运动速度、方向等，为无人艇自主航行仿真提供不同的测试场景和航行需求，同时也支持典型场景的任务设定。

2）感知模拟器

模拟无人艇航行过程中对目标的探测能力，周围存在的目标通过模拟器探测感知获得，获得的感知数据均输出到数据融合功能模块中，不断测试数据融合软件的稳定性和准确性，更能较为准确模拟实际环境中的系统状态。

导航雷达模拟器2 s 扇扫一次设定范围内的目标，雷达盲区为100 m，设置20%的随机虚假目标，同时对雷达数据加入随机噪声，更为真实地模拟实际雷达数据。雷达数据主要包括批号、位置、速度、航向、反射面积、DCPA、TCPA。

激光雷达模拟器一秒一次更新周围200 m 以内目标信息，输出目标信息为点云聚类处理后的数据，主要包括目标距离、方位、航速。

光学识别模拟器的视角可设置，一秒一次更新光学视角范围内的识别目标，逐一地对雷达提供的目标信息进行识别确认，同时给出目标属性，对识别后目标不再进行确认。

3）环境干扰信息

无人艇所处的海洋环境复杂，易受到周围环境影响，针对环境干扰力所带来的问题，建立海洋环境干扰模型，主要考虑风、浪、流3个方面的干扰。

a）风力模型

风对船舶的影响主要作用在船舶水线以上的船体，从而影响船舶的姿态。风的作用大小由风速、风向决定，其公式如下：

式中，ρa为空气密度；Ar、AL为水线以上船体的正投影面积和侧投影面积；L为船长；VW为相对风速；CWX、CWY和CWN为风力系数。

b）浪力模型

波浪对船体的干扰力一般分为两种，一种是一阶波浪力，也称高频干扰力，其主要由高频小幅波浪产生，与波高成正比，影响船舶的艏向；另外一种是二阶波浪力，也称波浪漂移力，与波高的平方成正比，影响船舶的航迹。这里考虑二阶波浪力，其公式如下：

式中，ρw为水密度；g为重力加速度；L为船长；为波长；hw为有义浪高；θ为波浪遭遇角；CXW（）、CYW（）和CNW（）为漂移力系数。

c）流力模型

对于流对船舶运动干扰的计算，一般考虑恒值海流下的运动模型。一般情况下，流对船舶作用力计算与风的计算相似，其公式如下：

式中，ρC为流体密度Afw、Asw分别为水线以下船体的正投影面积和侧投影面积；β为漂角；L为船长；VC为流速；CX（β）、CY（β）和CN（β）为流的作用力矩系数。

2.3.2 外部信息

外部输入信息主要包括实海域录取的传感器设备网络报文数据，通过回放软件，将网络报文接入到仿真系统中，实时的态势信息主要包括导航雷达信息、AIS 信息、光学识别信息、激光雷达信息，在仿真系统中，通过实海域态势对无人艇自主航行控制算法进行测试，更具可靠性和适用性，有利于实艇算法的移植和实用性测试。

2.4 航行控制模块

航行控制模块主要解决3 个方面的技术问题：航向、航速和航路跟踪的控制问题，拟将技术分为3层功能实现。

1）导引层根据任务航路对无人艇位置进行约束，驱动无人艇收敛在期望航路上，解算生成导引控制指令，发送至控制层。

2）控制层根据导引层生成的航速航向控制指令，通过控制算法解算，生成执行机构控制指令，发送至执行层。

3）执行层根据执行机构控制指令，生成匹配的动力指令，发送至无人艇操纵性模型模块中，实现转向和速度的改变，继而更新无人艇的导航信息。

由此，航行控制模块的导引、控制、执行3 个功能流程中如图6 所示，对每个功能层进行算法针对性设计，在航速控制、航向控制、航路跟踪中分别采用滑模—神经网络控制算法（SMCNN）、自抗扰控制算法（ADRC）、LOS 航路跟踪算法，为无人艇自主航行提供基础。

图6 航行控制结构图Fig.6 Navigation control structure chart

视线（line of sight，LOS）算法是一种常用的无人艇航路跟踪控制算法［4］，具有不依赖于模型、参数少、抗干扰强等特点，应用范围较为普遍。LOS通过计算当前航线上的导引点［5］，引导无人艇向导引点航行，从而驱动无人艇收敛于规划航路上。

图7 为基于LOS 导引的无人艇航路跟踪原理图，坐标系为北向坐标系，α为当前航路AB 的方向，θ为无人水面艇的航向，β为漂角，ψ为期望航向，u（t）为无人艇航速，h为水面无人艇当前位置PUAV到在航路上投影点PT的距离［7］，Δ（t）为前视距离，与无人艇当前航速和跟踪误差有关，是点PT到导航点PLOS之间的距离。本文在原有LOS 算法的基础上进行改进［12］，加入对外部环境干扰观测，对视线角计算进行补偿，It为自适应补偿项；同时考虑无人艇高速收敛航路过程中出现震荡问题，加入微分环节，其中，dh为跟踪误差的微分，λ、λ1、kp、ρ、Δcst为可调参数，Δmin为常值，通常设定为1.5 倍艇长，由公式求得：

图7 基于LOS导引的无人艇航路跟踪原理图Fig.7 Schematic diagram of route tracking for USV base on LOS guidance algorithm

无人艇在高速航行时，水动力所产生的动升力能将艇体上抬，艇体抬起后，阻力降低，尤其是兴波阻力大量降低。这样的航行特性使其阻力非线性特性明显，纵向运动比常规排水船要复杂，阻力参数随运动状态变化较大，模型参数随时都在变化［8］，因此，很难对其进行精确建模，而且外界环境干扰的不确定性，常规的PID 控制方法难以对这种不确定非线性系统得到较好的控制效果。

针对无人艇航行运动特点，仿真系统航行控制模块中航向控制默认采用经典自抗扰控制算法（ADRC），该控制算法分为跟踪微分器（TD）、扩张状态观测器（ESO）、非线性状态误差反馈（NLSEF）3个模块，进行非线性转换，将非线性系统转化为线性系统，实现动态系统的反馈线性化。跟踪微分器将误差进行平缓过渡，同时计算微分量；扩张状态观测器（ESO）的作用是估计系统状态和不确定干扰的观测估计值；利用安排非线性状态误差反馈（NLSEF）对系统状态进行控制。无人艇航向易受到外界环境的干扰，自抗扰控制算法能有效地估计扰动和状态值，补偿干扰带来的误差。

航速控制采用滑模—神经网络相结合的控制算法（SMCNN），滑模控制的优点是对干扰和模型动态具有很强的鲁棒性，控制算法实现简单，同时针对航速控制的特点，设计采用神经网络方法解决滑模抖振问题，将两种算法结合设计了一种滑模—神经网络航速控制算法。

2.5 避障模块

避障模块主要分为反应式避障和动态规划航路避障两个模式，两个模式可以随时切换，算法模块也可随时替换，本文主要介绍反应式避障算法。海上航行需要遵守海事规则，尤其是两船或多船会遇时，海事规则根据相对方位角来区分和定义会遇局面，主要分为3种会遇情况，如图8所示。

图8 无人艇会遇场景定义Fig.8 Definition of USV encounter scene

速度障碍法（velocity obstacle，VO）作为反应式避障算法中的应用较多的方法［2］，具有响应速度较快、计算量小的特点，因此，本文采用该算法设计无人艇避障模块。当无人艇前端传感器发现碍航物时，无人艇根据碍航物信息采用速度避障法进行规避，该算法的核心主要在无人艇的速度空间中，根据碍航物运动信息形成障碍物态势区域，为无人艇和碍航物的预测碰撞区，对无人艇的速度进行适当改变，从而避免未来一段时间内出现在预测避碰区，局面采取算法输出一系列可航行无碰撞的速度向量，若发现障碍物为动态船只，则基于国际海事规则的同时结合船只的运动状态，对航行会遇态势进行决策判断，分为对遇、左交叉、右交叉、追越4个会遇态势，对遇局面采取右侧避障的原则；交叉会遇采取从对方尾部避让的原则，禁止从对方船艏通过；追越采取最优原则，从靠近的一边通过。根据上述海事规则、艇体操纵性、偏航距离等约束条件，结合最优化算法计算，从可航行速度向量范围中，得到一个最优速度向量，作为无人艇下一时刻的期望速度向量。当判断无人艇无碰撞危险之后，快速回归到目标航路上。

建立动态障碍物的空间避碰模型［11］，如图9 所示，其中，无人艇位于O点，当探测到周围存在动态障碍物时，本艇采取转向保速的方法对动态障碍物进行避障。首先对障碍物进行膨胀化处理，根据探测到障碍物大小信息向外膨胀1～2 倍船长做膨胀圆，从O点向膨胀圆做两条切线［13］，切点为a、b点，VO为无人艇的速度，VT表示动态障碍物的速度，VR表示无人艇相对于障碍物的相对速度。若VR的速度方向在碰撞区∠aOb的范围内，则存在碰撞危险，需要调整无人艇航速航向从而改变相对速度方向远离碰撞区。

图9 无人艇避障原理图Fig.9 Schematic diagram of obstacle avoidance for USV

无人艇进行动态避障的过程中，实时判断是否可以回归航路。如图10所示，无人艇位于点0位置，点2为目标点，当前航路L由点1、点2组成，点0到航路L的垂点和点0到点2组成的角度范围ψ=［θ1，θ2］，当范围ψ在可航行安全航向范围内时，可以回归航路。

图10 航路回归判断示意图Fig.10 Schematic diagram of judgment of return expectation of route

速度障碍法流程如图11所示，具体步骤如下：

图11 速度障碍法流程图Fig.11 Flow chart of velocity obstacle avoidance method

1）艇载传感器实时收取航行态势信息，对周围障碍物信息进行预处理；

2）对障碍物进行危险判断；

3）根据航行态势解算当前可航行安全航向和航速集合；

4）根据评价函数和最优算法选取最优航向和航速；

5）无人艇按照海事规则选取最优的航向和航速航行；

6）实时判断是否可以回归航路，若是，进入下一步，否则返回第1）步；

7）无人艇复航，返回至目标航路。

3 自主航行仿真试验

在项目算法开发测试的背景下，为了减少实海域自主航行算法调试试验次数、提高试验效率，提前在本文设计的仿真系统环境中验证自主航行相关算法的可行性和可靠性，通过仿真试验不断优化和改进算法以及模拟任务顺利执行，以下为无人艇全局航路规划、自主避障、航行控制算法和任务推演的仿真试验。

仿真试验地图为谷歌卫星地图，地图位置为日照黄海近岸区域，仿真试验以某中型无人艇为对象进行数学建模，具体参数如下页表1所示。

表1 无人艇参数Table 1 Parameters of USV

环境干扰参数如表2所示。

表2 风浪流参数Table 2 Wind-wave-current parameters

全局航路规划设置栅格大小为10 m*10 m，栅格地图范围5 km*5 km；无人艇自主避障无人艇航速为18 kn，航路中同时规避一个静态一个动态障碍物，动态障碍物航速为10 kn，与无人艇进行左交叉航行，障碍物膨胀大小100 m，DCPA 阈值为300 m，TCPA 阈值为30 s；航行控制中跟踪航路为直线和曲线的结合，航路跟踪控制参数设置kp=0，ρ=2，λ=0.2，λ1=0.1，k=2，Δcst=20；航速航向控制算法对比仿真试验均在相同环境参数条件下进行；目标跟踪伴随仿真实验，人工设置目标航速为10 kn，无人艇起始航速为18 kn，与目标跟踪距离为500 m，伴随距离200 m，仿真试验结果如图12～图13所示。

图12 全局航路规划Fig.12 Global route planning

图13 全局航路规划平滑处理Fig.13 Smoothing process of global route planning

图12 全局航路规划结果，设置了3 个预置障碍物；图13 为对全局规划的路径进行优化处理，可以看出经处理后路径点减少，更加平滑。

图14为自主避障结果，同时规避了一个静态和一个动态障碍物，可以看出无人水面艇在遇到静态障碍物后能够按照正确的方向进行避障，面对不遵守海事规则的船只也具备应急避障能力，遵守但不受限于海事规则。

图14 自主避障Fig.14 Autonomous obstacle avoidance

图15、下页图16 为目标跟踪任务，图17 为目标伴随任务，无人艇正常巡航阶段，人工设置待跟踪目标，目标航向、航速、大小可根据需求设置，无人艇感知系统探测到目标，规划跟踪航路，快速抵进目标后方，若为目标跟踪则维持设定500 m 的距离保持跟踪，若为目标伴随，则保持到目标右侧200 m 位置，持续获取目标的特征，供人员对其进行危险判断。

图15 抵近目标阶段Fig.15 Approaching the target stage

图16 目标跟踪阶段Fig.16 Target tracking stage

图17 目标伴随Fig.17 An adjoint target

图18为航路跟踪结果，完成对任意曲线的航路跟踪控制，跟踪精度达到1 m以内。

图18 航路跟踪Fig.18 Route tracking

图19 为航向控制对比仿真试验，主要对比PID和ADRC 对于航向的控制效果，ADRC 存在5°的超调，收敛时间相比较短；图20 为航速控制算法对比响应图，SMCNN算法具备更快的末端收敛性。

图19 航向控制对比仿真试验Fig.19 Course control comparison simulation test

图20 航速控制对比仿真试验Fig.20 Navigation speed control comparison simulation test

图21为无人艇实海域某航次录取的试验数据，仿真系统通过数据回放功能将当时海上航行态势场景重新输出，1 377 批号目标航速为6～7 kn，航向为180°左右，仿真系统接收到的实海域航行态势数据，结合无人艇操纵性数学模型、自主避障、航行控制等模块进行场景重现自主避障仿真试验，验证避障算法针对实海域复杂场景的可靠性和适应性。

图21 回放真实场景仿真试验Fig.21 Replay real scene simulation test

从上述仿真试验结果可以看出，本文设计的无人艇自主控制仿真系统，具备规划、避障、控制等功能仿真验证，以及任务推演、数据回放存储能力，且与实际航行数据交互，最大程度上验证相关软件算法的可行性和可靠性。

4 结论

本文主要研究了无人水面艇自主航行控制仿真系统，该仿真系统包含了5个模块，分别为显控模块、无人艇操纵性运动模型模块、环境信息模块、避障模块和航行控制模块，基于一致的数据接口，5 个模块中+随时进行不同算法替换以及参数更改。仿真试验结果分析表明，该仿真系统具备对无人艇自主航行控制进行功能测试和数据分析能力，可提高无人艇实艇工程开发效率和调试进度，为无人艇自主航行任务演示提供数据分析和场景重现平台。