基于USVSim的无人艇综合仿真平台设计与验证

范易杰，杨少龙，黄 金，向先波

(华中科技大学 船舶与海洋工程学院, 武汉 430074)

1 引言

近年来，随着无人装备向低成本、高效率和智能化方向发展，无人艇广泛应用于海上侦察、海上搜救、排险救灾、水文监测等领域，代替人类完成高风险、高强度的作业任务。据统计，2022年中国无人船市场可达数十亿美元。然而，在无人艇研发过程中，为了降低无人艇研制风险，不仅需要在相对安全的场景中测试，也需要在恶劣的海洋环境中测试。此外，无人艇执行多样化任务需要搭载不同载荷，新的传感器与执行器都需要校准与性能测试，这些使得无人艇研发周期变长、风险变高，而且受物理海洋环境约束，实艇自主航行控制器难以开展极端工况测试。

为了解决上述问题，在实艇试验前使用仿真平台对算法进行验证可以测试算法缺陷并提供丰富的验证数据集。传统纯数字仿真虽然能够快速验证数学公式形式的算法，但是存在建模难以十分准确、模拟跟环境交互传感器过于复杂、难以测试极端工况等问题。基于虚拟引擎的仿真器更加逼近真实环境，而且相比于二维数据图表，三维视景仿真效果更加直观，同时个性化、多样化的需求对仿真器可扩展性要求更高。

昆士兰理工大学在2016年开发了高保真自主水面车辆和水上环境模拟器AMSVS，该模拟器通过真实操作环境中生成高保真传感器数据，并模拟ASV运动动力学，作为硬件在环仿真的接口，基于OpenGL技术极大改善视觉仿真的质量。Ricardo等研发了一种根据物理定律设计风浪流的多机器人仿真器Kelpic，该仿真器支持无人机、无人车、无人艇及无人水下航行器等无人设备多机协同仿真。USVSim仿真器通过改进Freefloating插件优化环境干扰力和浮力，同时提供制导、导航和控制的模块化组件，实现了高保真场景下的水面无人艇仿真测试。

综上，针对无人艇开发周期长、控制及感知模块难以综合测试等问题，本文提出设计一款针对无人艇快速开发与验证且面向教学与科研的高保真仿真实验平台设计，本平台对于无人艇教学与科研领域具有重要意义和应用价值。同时，充分发挥现有仿真工具进行升级改造将更有助于模拟器加速应用落地和快速上手使用。本文将Matlab与ROS结合，应用于水面无人艇仿真，使实验平台既有Matlab操作简单、运算快速的特点，也具有ROS插件丰富，自主可控的优点。使用本文开发的实验平台不仅可以实现纯数字仿真，也可以跨系统联合仿真，引入USVSim拥有的环境干扰插件和丰富的环境交互传感器接口，为半实物仿真设计打下基础。同时，本平台应用于教学场景时，学生可只聚焦于Matlab平台开发控制算法，不用开发ROS端，或者深入学习ROS中内部话题通信机制以及物理建模与运动控制。应用于无人艇复杂环境交互研究时，由于ROS有丰富的插件与对外接口，同时Gazebo平台拥有ODE、Bullet、Dart、Simbody等多种物理引擎，使模型可以具有质量、阻力、浮力等属性，进而组合形成多样的场景及行为，模拟各种工况下无人艇的受力情况，更加贴近实际，数据与结论也更具有说服力。

2 实验平台架构

本实验平台面向快速开发且自主性高的任务场景，设计流程分为3部分，如图1所示。首先，Matlab作为纯数字仿真器验证控制算法的快速收敛性、稳定性以及准确性，在Matlab端通过数学模型建立被控对象与环境扰动。其次，通过Solidworks完成装配体模型，确定无人艇各部分零件的构型和几何关系。最后，将无人艇的艇体模型、运动模型、扰动模型导入到ROS中。

图1 平台总体设计流程框图Fig.1 Block diagram of overall platform design flow

本平台由2部分组成，分别是基于Matlab开发的控制器和基于ROS开发的仿真平台，如图2所示。

图2 仿真平台结构框图Fig.2 Simulation platform structure block diagram

平台各部分通过网络交互信息，Matlab作为人机显示界面通过TCP/IP协议下发任务指令，Gazebo作为显示与仿真端，在导入被控对象运动学、动力学、坐标系以及三维模型后，在ROS框架下通过话题的形式实时交互，同时使用ROS强大的生态系统中已开源的各类机器人的感知、规划、控制等功能包，如相机、激光雷达等传感器，也可以接入真实的设备或者控制器联合仿真。在系统初始化时重力、浮力、环境干扰力通过插件的形式作用到模型上，使得仿真更加接近实际湖上试验。

3 基于PID和视线角制导的路径跟踪设计

Matlab作为常用的仿真软件，其强大的运算能力、丰富的函数以及简单易学习的特点在仿真上有很强的优势。本文设计的控制器充分利用了Matlab上述优点，并实现了如下功能：

1) 仅使用Matlab便可快速验证算法可行性

2) 作为人机交互界面，负责下发指令以及分析ROS返回的数据。

3.1 被控对象运动学模型

在Matlab中验证控制算法时，需要先建立被控对象的模型。针对无人艇的运动学模型问题，Fossen提出了完整的六自由度水面无人艇模型，并依据操纵性试验结果得出水动力参数。定义以地球表面某一点为坐标原点的惯性坐标系和以船体重心为坐标原点的附体坐标系，模型坐标系如图3所示。

图3 无人艇模型坐标系示意图Fig.3 Diagram of coordinate system of Unmanned surface vehicle model

对水平面运动产生影响的主要因素有纵荡、横荡、艏摇3个方向作用力。受力包括粘性水动力、惯性水动力、环境干扰力和动力装置产生的推力力矩，所以将六自由度模型简化为三自由度运动模型(沿轴的前进速度，沿轴的横移速度，绕轴的艏摇角速度)，得到无环境扰动下的模型表达式，如式(1)所示。

(1)

式中:、为无人艇在大地坐标系-下的坐标；、分别为无人艇的前向速度和侧向速度；为航向角；为角速度；、、分别为无人艇的惯性质量及其附加质量矩阵的对角项；为前向推力；为扭矩；、、、||、||、||均为水动力导数，根据无人艇自身动力学参数进行计算。建立被控对象模型后，验证控制算法在无扰动下运行效果。

3.2 控制与制导算法设计

为了验证平台仿真实际效果，本文使用基于LOS制导律和PID控制的无人艇路径跟踪算法，将路径跟踪任务分为4个层级-任务层、制导层、控制层和执行层。任务层负责规划并下达航点任务，制导层用于产生期望航向，控制层负责根据期望和实际的航向偏差产生控制信号，使无人艇以较好的性能跟踪给定的期望航向，执行层则根据控制信号驱动无人艇执行器，使无人艇产生相应的状态变化，流程如图4所示。

图4 无人艇路径跟踪原理流程框图Fig.4 Flowchart of unmanned surface vehicle path tracking principle

惯性坐标系下，航线由航路点(=1,2,3,…)构成，当无人艇与航线终点的距离、航向符合到点判断逻辑时，路径跟踪器切换到下一航线。航线+1在惯性坐标系下的方向为，无人艇位置为()，以无人艇为圆心，设定半径为的视线圆，以视线圆与+1的交点()为期望位置。根据航线与无人艇的位置关系，计算出期望航向，无人艇在直线+1上的垂足为()，无人艇到+1的距离为()，根据反正弦函数可以求出()()和()()之间的夹角

(2)

期望航向为

(3)

当无人艇与航线间的距离大于时，直接取无人艇到航线的垂线方向作为期望航向，完整的期望航向表示为：

(4)

其中，的数值影响控制器的响应速度，无人艇在相同位置下，越小则期望航向越趋于垂直航线方向，系统对误差的响应越快，无人艇位置会快速收敛，同时也会带来振荡现象，制导原理如图5所示。

图5 视线角制导原理示意图Fig.5 Schematic diagram of line-of-sight guidance

产生期望航向后，由控制器根据期望航向产生对无人艇的控制信号。以视线角制导算法产生的期望航向与无人艇当前航向之间的偏差为PID控制器的输入，根据航向偏差进行比例、积分、微分运算，为防止长时间积分使积分项过大导致控制器失效，对于积分运算进行了饱和限制，控制流程如图6所示。

图6 无人艇航向控制器控制流程框图Fig.6 Block diagram of control flow of unmanned surface vehicle heading controller

离散系统的PID算式，该PID算式(5)为位置式PID算法：

(5)

式中；()为舵角控制指令；()为航向偏差；为比例系数；=为积分系数，为积分时间常数；=为微分系数，为微分时间常数。

首先在Matlab中测试上述算法的快速性、准确性和稳定性，搭建Simulink仿真模块，在收敛性和跟踪误差满足预期后，使用代码自动生成技术将模型编译成为C++文件放入ROS功能包，在ROS中验证存在扰动情况下的跟踪误差是否满足要求。

4 基于ROS开发的仿真平台

基于ROS架构的无人艇仿真平台主要包括：被控对象的运动学模型、动力学模型、几何模型、环境扰动模型和通信接口等。ROS既可以接收坐标和经纬度，并自动规划轨迹跟踪航点，也可以接收航向和力矩，测试控制效果。仿真软件Gazebo启动时自动加载环境、模型、作用力，在接收位置与速度信息后运动。

4.1 无人艇三维模型

在Gazebo中加载无人艇物理模型有两种方式，一种是直接编写URDF模型文件，另一种是先通过SolidWorks建立三维模型，再通过SolidWorks to URDF Exporter插件将模型文件导出为URDF文件。本文使用SolidWorks定义各零件的质量、位置关系等属性，通过插件输出URDF文件和STL网格文件后，在Gazebo中加载并显示的效果如图7所示。

图7 无人艇三维模型效果图Fig.7 3D model of unmanned surface vehicle

4.2 无人艇动力学模型

根据刚体动力学和无人艇受力，可以将无人艇附体坐标系下的运动描述为式(6)：

(6)

式中:、和是作用于艇体的粘性水动力和力矩；带有下标的变量是无人艇推进装置产生的作用力和力矩；带有下标Wind、Wave、Current的变量分别对应风、浪、流3种环境干扰力和力矩；下标和Buoyancy分别代表重力和浮力在该方向产生的力和力矩；、是、轴运动的附加质量；为无人艇绕轴旋转的转动惯量；是绕轴的附加惯性矩。

无人艇在各自由度上的受力可以表示为：

(7)

带有下标Model的项为无人艇模型换算得到的等效力和力矩，包括惯性水动力、粘性水动力和推进器推力；下标Wind、Current、Wave、G、Buoyancy分别代表：风、水流、波浪、重力、浮力在各个方向所产生的力和力矩。

结合式(6)和式(7)可以得到无人艇各方向上的作用力表达为式(8)。

(8)

力的加载由Gazebo插件实现，当物体的link文件中挂载了某一插件时，在仿真时该插件就会自动运行，向link施加力或者其他操作。式(8)中的质量、附加质量、水动力导数等系数为常数，需要通过回归式计算，再通过实际试验修正。各方向的速度和角速度可以在插件程序中使用物理引擎提供的指针来获取。

4.3 环境扰动模型

在风、浪、流等环境干扰的作用下，无人艇在航行中的航向、位置等状态会发生偏离，对运动控制产生较大影响，为了使仿真平台可以更加贴合真实环境，需要为仿真平台添加风、浪、流环境干扰模型，同时由于环境扰动力之间的耦合关系比较复杂，所以本文将风浪流扰动解耦，分别建立数学模型，再由Gazebo计算出干扰力力矩作用在无人艇上。

本文参考Blendermann等提出的风力计算式(9)，计算风对无人艇的作用力

(9)

式中：为无人艇的总长度；为空气密度；为无人艇水线以上的侧投影面积；为无人艇水线以上的正投影面积；()、()和()分别为、方向的风压力系数以及绕轴的风压力矩系数。

Daidola等提出的二阶波浪干扰力式(10)得出波浪对无人艇纵荡、横荡、艏摇3个自由度运动的影响采用插件的形式写成功能包文件，由仿真软件在初始化阶段调用。

(10)

在仿真平台启动时直接运行插件，加载作用力到模型上，可以通过修改功能包中的模型文件改变环境扰动力。

5 仿真平台效果与实船验证

5.1 平台仿真结果

为了验证本文提出的基于USVSim的仿真试验平台与Matlab交互的效果，本节采用视线角制导算法航点跟踪进行测试。本次测试实验使用两台计算机，一台运行Matlab程序，另一台运行USVSim仿真器，两者通过以太网连接使计算机在同一网段下，通过ping命令确认通信正常。仿真运行时，Matlab端发送期望航点坐标，通过Publish模块发送给communication_upway_points话题,Gazebo中的三维模型接收话题中的航点信息规划航迹，同时仿真器将位姿信息存储到话题communication_to_ui里面，Matlab端接收话题信息并显示，通信模块如图8所示。

图8 Simulink通信模块框图Fig.8 Simulink communication module

本节仿真起点默认(8，-4)，目标点(50，0)，ROS接收目标点信息后跟踪曲线如图9，跟踪误差曲线如图10，无人艇在接收到航点任务后从起点开始向目标点航行，横向偏距不断减小，33 s到达目标航点。

图9 仿真跟踪曲线Fig.9 Simulation tracking curve

图10 跟踪误差曲线Fig.10 Tracking error curve

5.2 湖上验证结果

为了验证仿真试验平台结果的准确性，采用自行研发的1.75 m双桨双舵无人艇进行湖上测试，无人艇如图11所示。

图11 无人艇实物图Fig.11 Photoview of unmanned surface vehicle

某一航次以(-40，-25)为起点、(90，-5)为终点，无人船艏向309.4°、初始航向303°、速度3 kn的制导任务实验结果如图12、图13所示，规划行线与航迹基本一致，稳态横向偏距约0.5 m。

图12 湖上实验跟踪曲线Fig.12 Experimental tracking curve on the lake

图13 跟踪误差曲线Fig.13 Tracking error curve

由图9、图10、图12、图13可知，仿真曲线相对平稳，参数合适时横向偏距没有超调，为一条平滑曲线，而湖上试验航路跟踪任务有小幅波动，30 s后横向偏距收敛到0 m。仿真与物理实验规律基本相符，说明设计的基于USVSim的仿真平台能够模拟无人船的运动特性及验证控制算法功能。综上，基于USVSim的无人艇综合仿真系统可作为一种快速开发平台，适用于无人艇运动控制系统的仿真与测试验证。

6 结论

提出了基于USVSim的仿真平台与Matlab的无人艇综合仿真系统，能够在Matlab初步验证后快速修改移植到本平台，在原有仿真架构上没有增加复杂程序，但在扩展性和显示效果上更加优异。相比于数值二维曲线，三维交互显示结果拥有直观感受，利于科研与教学。平台通过Simulink将控制器与ROS解耦，将仿真后控制算法直接植入物理控制器(如树莓派)，可与ROS中模拟的逼真环境与传感器(如激光雷达、摄像头)交互，实现更加真实的仿真测试，有效释放物理试验风险，降低测试成本，缩短开发周期。