基于Gazebo的水下机器人模型构建与运动仿真

周善旻 马佳杰

浙江大学舟山海洋研究中心 浙江 舟山 316000

针对传统仿真平台对水下机器人静力学、动力学模拟困难的问题，提出基于Gazebo的水下机器人模型构建与运动仿真方法。首先，采用Fossen方法构建机器人水动力模型，通过CFD仿真实现主要参数辨识，建立推进器模型后得到推力分配矩阵；其次，在Gazebo平台下联合UUV仿真包及相关插件实现水下机器人传感器、推进器及水动力仿真；最后，采用基于PID的艏向控制及定深控制程序实现水下机器人在仿真环境下的运动控制，为后续复杂定位及控制算法仿真测试和优化提供基础。

引言

随着我国加快海洋开发步伐，在十九大报告中明确提出要建设海洋强国，将该战略纳入国家大战略，海洋权益上升到前所未有的高度。海洋工程装备在中国制造2025规划中是十大重要领域之一，水下机器人也逐步成为研究热点。

对水下机器人来说，自主航行控制、水下定位、轨迹跟踪等关键技术均需要不断的下海试验，存在成本高、机会少、风险大的问题，迫切需要合适的水下机器人运动仿真平台实现相关算法测试及调优。

Matlab中Simulink为模型构建提供了图形化界面，甚至也提供与ROS通信的包，但是缺乏3维实时可视化能力，且生成的代码需要进一步移植才能使用在嵌入式系统中；Unity是世界领先的游戏开发实时引擎，可以做出逼真的模型，其图形可视化也优于其他仿真平台，但是其缺乏在工程应用的文档支持，并很难在linux中使用。

本文采用通用机器人仿真平台Gazebo构建水下机器人模型进行运动控制仿真，首先，其Gazebo基于ROS框架，应用统一通信规范，可以很好的应用在嵌入式系统中；其次，提供仿真测试实时3维可视化能力；最后，构建水动力模型后，可联合UUV仿真包模拟水动力和静水力，并以各插件形式实现水下机器人各传感器和推进器仿真，提供系统化、规范化、可配置化的仿真子功能支持。

1 仿真平台简介

ROS是针对机器人开发的软件编写框架，提供标准化的用户、计算机操作系统和计算机外部设备之间交互通信方式，已被广泛应用于各种机器人平台应用开发过程中。同时，ROS也提供了大量的工具和广泛的库文件，用以配置、自检、调试机器人平台，实现机器人操作控制及感知功能。

Gazebo作为通用机器人仿真平台，一方面集成动力学仿真模块、传感器仿真模块及环境扰动测试模块，可以准确描述水下机器人系统；另一方面，统一ROS应用程序交互接口，便于算法测试、优化和部署，便于程序移植。

2 水下机器人模型构建

2.1 机器人水动力模型建模

建立正确的水动力模型是建立水下仿真平台的基础，具体可分为对运动学和动力学的研究。运动学主要研究运动过程中速度位姿之间的关系，而动力学是分析具体引起水下机器人运动的力，具体包括推进器推力、波浪力、粘性力及惯性力等。

本文采用轮机工程学会(SNAME)术语公报的体系，采用北－东－地坐标系(North－East－Down(NED)coordinate system)作为惯性坐标系，以机器人重心位置为载体坐标系原点。

载体坐标系和惯性坐标系

对比基于泰勒展开式展开的常见的水下航行器建模方法，采用Fossen水动力模型描述航行器水动力特性，并以刚体动力学方程的矢量形式表达。

其中η表示在NED惯性坐标系下机器人的位置，JΘ代表从载体坐标系到惯性坐标系之间的转换关系，是水下机器人的运动学方程；ν为机器人在载体坐标系下的速度，M˙为惯性系数矩阵，C为科氏力系数矩阵，D为阻尼系数矩阵，g为恢复系数矩阵，τ为附加力，是水下机器人的动力学方程。

为得到较为精确的系数用以模型构建及仿真，利用ICEM设立计算域和边界层，细分网格，进行壁面影响仿真；通过Fluent仿真水下航行器在直线运动和回转运动的水动力，拟合仿真曲线得到水下机器人的的惯性系数、一阶粘性系数、二阶粘性系数及耦合系数。

2.2 推进器模型构建及推力分配

水下机器人本体要实现复杂的对接、航行等功能，需要有很强的运动控制能力，为了实现6个自由度的全驱动运动控制，需要简单构建推进器模型，并采用矢量控制方法计算推力分配。

由于推进器本身具有惯性，不能立刻响应所需的控制输入，因此必须构建较为精确的推进器模型。其中Ts为时间常数，通常由推进器技术手册提供，同时拟合实际推力与转速曲线得到推力转换系数krotor，构建简单的推进器模型。

水下机器人总共有6个推进器，如下图所示，其中1号、2号和3号为垂推，提供升沉力；5号和6号为主推，提供进退力；4号为侧推，同5号6号实现横移功能。通过这6台推进器实现6自由度全向运动控制。

水下机器人推进器分布

计算推进器各推力矢量时，取螺旋桨桨面中心为受力点。(按0.005m精度)，获得推进器的单位推力矢量:

将每个推进器的单位推力转化为相对于原点的力和力矩，可以获得推进器到载体推力矢量变换关系，反解推进器推力分配矩阵Mprop:

3 Gazebo与UUV联合仿真

现阶段有较多开源仿真平台适用于空中及地面机器人，相比而言适用于水下机器人的平台较少，其主要原因是海洋环境复杂且机器人在水下水动力很难模拟。

SWARMS为Gazebo开发了水下仿真包，称为UUV仿真包，扩展了通用机器人仿真平台Gazebo在水下的仿真能力。UUV仿真包主要包括对典型的水下传感器仿真功能、对推进器的动力学模型仿真功能，对机器人本体的水动力仿真。另外，还可以通过插件形式实现扩展机器人在水下的静水力，传感器噪声设置等功能。在仿真过程中，海洋环境通过SDF进行文件初始化，需要直接通信的对象包括机器人、传感器、推进器等以通用机器人描述语言URDF文件初始化，每个实例对象还需要提供STL几何文件描述对象大小及碰撞体积。

在Gazebo中，水下机器人可以描述为固定在主体模型框架base_link下各link的集合，具体包括6个推进器、1个IMU、1个DLV和2个摄像机，他们都被添加到机器人配置文件中，以相对于的机器人主体模型框架base_link的方向和位置来描述并分别命名为thruster0_link、DVL_link、IMU_link和camera0_link。所有传感器和执行机构都是默认为Gazebo和UUV仿真包里的插件，只要配置相关设备参数，与各自设备相对应。

4 水下机器人运动控制仿真

水下机器具备高度非线性、强耦合性和大惯性等特点，自主航行控制作为其核心关键技术，一直是备受关注的研究热点，对于其位置和姿态的可靠控制是其能在水下平稳作业的前提。通过UUV仿真包配置基于Fossen水动力模型参数，以插件形式实现重力和浮力及各传感器输出及配置噪声，以ROS节点形式订阅控制程序直接下发的控制量，可以较好实现水下机器人运动控制仿真。

通过基于PID的艏向控制及定深控制程序验证水下机器人运动控制仿真的有效性。控制程序接收来自UUV仿真包插件发布的里程计话题/Odom，跟踪艏向误差和深度误差，最终发布控制量话题/thruster_input；Gazebo仿真平台订阅控制量话题/thruster_input并作用于机器人的base_link，施加具体的力和力，同时不断更新当前载体坐标系下速度及方向，水动力插件同步更新机器人受到的水动力，最终实现水下机器人定艏向及定深度的运动控制仿真。在运动控制仿真过程中，可以通过基于ROS的RVIZ插件订阅轨迹话题实时可视化运动过程，通过rqt工具实时监控ROS下各话题内容，通过rosbag工具保存控制输出数据便于进一步优化控制参数。

5 结语

以水下机器人为研究对象，针对传统仿真平台对水下机器人静力学、动力学模拟困难的问题，提出基于Gazebo的模型构建与运动仿真方法。仿真平台主要基于ROS框架，通过配置UUV仿真包水动力插件中Fossen水动力模型参数，配置各传感器插件于机器人本体坐标系base_link的相对位置及噪声，配置推进器插件中的时间常数和推力转换系数及推力分配矩阵实现水动力仿真、推进器仿真及传感器仿真，并通过艏向控制 及定深控制程序测试运动控制仿真过程，便于进一步算法测试和优化。