基于真实扰动数据的分布式大型高速AUV高保真仿真系统

技术领域

本发明涉及的是一种水下机器人领域的技术，具体是一种高保真度的基于真实海流、海浪扰动数据的分布式大型高速水下机器人性能测试与算法验证的三维可视化仿真系统。

背景技术

大型高速水下机器人(Autonomous UnderwaterVehicle，AUV)在实际海域的性能测试与智能算法(环境感知、规划与决策、控制算法等)验证工作中需要花费大量的人力、财力与物力，且作业测试环境存在极大的不确定性，如海流、海浪干扰等，很难保障工作人员与作业设备的安全。

发明内容

本发明针对大型高速AUV系统研发与算法验证难度高、代码多平台移植性差、仿真系统保真度低的问题，提出一种基于真实扰动数据的分布式大型高速AUV高保真仿真系统，既可在具有真实海流、海浪扰动的虚拟海洋环境中测试AUV的开环性能，亦可验证人工智能相关算法的可靠性与有效性，降低系统研发与算法验证的难度；且分布式架构利于多硬件系统松耦合的实现，便于代码跨平台移植与测试；可实现水下虚拟环境、大型高速AUV虚拟试验艇以及虚拟传感器的动态可视化，同时可实现实时数据与运动轨迹的可视化，高保真度地模拟水下真实试验场景有利于提高人-机“沟通”效率，使结果表达更加直观，方便用户从多角度观察并分析大型AUV的运动状态，进一步提高硬件系统开发与算法研究的效率与灵活度。

本发明是通过以下技术方案实现的：

本发明涉及一种基于真实扰动数据的分布式大型高速AUV高保真仿真系统，包括：大型AUV虚拟试验艇子系统、水下三维虚拟环境子系统、运动求解子系统、数据管理子系统以及可视化子系统，其中：大型AUV虚拟试验艇子系统在刚体惯性、重力、浮力、惯性类水动力、向心力、科氏力、阻力、推力、舵力以及外界环境干扰力的相互作用下等比模拟真实大型高速AUV物理样机的运动状态，并在物理引擎的作用下为本体附加碰撞属性；水下三维虚拟环境子系统通过大范围的水下环境的渲染、动静态障碍可视化模型的搭建以及真实海流、海浪扰动的产生高保真模拟大型高速AUV的真实水下环境；运动求解子系统描述水下虚拟环境中所有物体固联坐标系间的转换关系，并求解大型AUV的运动方程，同步更新其位姿与速度状态；数据管理子系统管理仿真过程中生成的各种数据，支持写入和读取数据操作，同时可管理与调配节点间数据通信问题，支持接收和发送数据操作；可视化子系统加载并动态显示水下三维环境与大型AUV虚拟试验艇模型，同步更新实时数据曲线并可视化AUV运动轨迹。

所述的大型AUV虚拟试验艇子系统包括：运动关联模块、运动感知模块、运动决策模块以及运动控制模块，其中：运动关联模块以连杆和关节为基本单元表征大型AUV各部件的拓扑结构关系；运动感知模块将虚拟传感器组件返回的数据转换为动态环境信息与大型AUV高速航行过程中的状态信息，并输出至运动决策模块；运动决策模块根据运动感知模块反馈的信息进行规划策略(如路径规划或控制决策)并与运动控制模块相互配合完成指定任务；运动控制模块接收运动决策模块下达的指令，快速地做出响应，解算执行器动作，并精准、高品质地完成期望动作。

所述的拓扑结构关系包括：大型AUV各部件的外观纹理与几何碰撞模型以及质量与惯量物理属性；关节按照父-子级关系将上下连杆进行关联，父连杆可以同时作为其他连杆的子级，同样，子连杆也可以同时作为其他连杆的父级。

所述的虚拟传感器组件是指：通过仿真系统抽象出来的与实际物理传感器功能相同或类似的传感器件，可对其相关属性参数进行设置。

所述的传感器包括：用于测量大型AUV的航行里程的里程计、用于采集水下的图像信息的虚拟摄像头、用于采集最近障碍物到传感器的距离的单波束声呐、用于采集返回一组距离数据的多波束声呐、用于采集大型AUV的工作深度数据的深度计、用于采集AUV相对于世界坐标系的位置与姿态数据的IMU、用于采集AUV绝对位置数据的GPS、用于采集AUV距离海底高度以及航速数据的DVL以及用于采集力和力矩数据的力传感器。

所述的指定任务包括：水下导航、自主避障、目标追踪、多体协同或其组合。

所述的运动控制模块包括：位置控制器、姿态控制器、速度控制器，其中：根据大型AUV接收运动决策模块下达的指令，快速地做出响应，精准、高品质地完成期望动作。

所述的位置控制器，以参考位置为输入，以AUV搭载的位置传感器采集到的数据作为反馈，通过调整推进器转速和方向舵舵角，驱动大型AUV至参考位置；所述的姿态控制器，以参考姿态为输入，以AUV搭载的姿态传感器采集到的数据作为反馈，通过调整方向舵舵角，驱动大型AUV至参考姿态；所述的速度控制器，以参考速度为输入，以AUV当前速度为反馈，通过调节推进器转速驱动大型AUV至参考速度。

所述的外观纹理与几何碰撞模型通过SolidWorks、3D MAX构建视觉模型以及具有碰撞网格的几何模型，所述的视觉模型具有形状、颜色、材质、纹理属性。

所述的大型AUV虚拟试验艇子系统以常规单刚体AUV为原型，做等长径比放大处理，其本体为单刚体结构，尾部配有隧道推进器以及十字方向舵，所述的十字方向舵包括垂直舵和水平舵，通过摆动方向舵可调节AUV的俯仰姿态和偏航姿态。

所述的水下三维虚拟环境子系统包括：具有碰撞物理属性的海底模型、动静态物体模型以及真实海洋扰动，其中：海底模型包括平地、海山、海沟地形，静态物体包括沉船、生锈铁柱，动态物体包括游鱼、水下潜器、悬浮物，真实海洋扰动包括海流和海浪的扰动力和力矩。

所述的海底、动静态物体模型通过SolidWorks、3D MAX和World Machine软件构建，并按照简化后的模型建立对应的几何碰撞模型，从而减少物理引擎求解复杂环境碰撞模型的计算量。

所述的运动求解子系统包括：坐标系转换求解模块与大型AUV运动方程求解模块，其中：坐标系转换求解模块根据某一坐标系下位置、速度信息，进行坐标系转换处理，得到另一坐标系下位置、速度信息；大型AUV运动方程求解模块根据AUV执行器指令信息，进行动力学解算，得到AUV的位姿、速度状态信息。

所述的坐标转换求解模块包括：动、静坐标系转换求解单元，旨在通过坐标系来标定水下环境中的物体，其中：动坐标系转换求解单元标定移动物体，即描述移动物体固联坐标系与参考系(任选)之间的坐标转换关系，例如大型AUV机体坐标系与世界坐标系之间的转换关系，方向舵固联坐标系与机体坐标系之间的转换关系等；静坐标系转换求解单元描述不同固定参考系之间的坐标转换关系，比如海底模型固联坐标系与世界坐标系之间的转换等。

所述的大型AUV运动方程求解模块包括：运动学单元和动力学单元，利用ODE45对运动方程进行求解，同步更新大型AUV的状态信息，其中：运动学单元描述大型AUV在世界坐标系中的状态表示；动力学单元描述大型AUV在水动力、环境干扰力以及执行器力的作用下，其速度状态相对于自身固联坐标系的变化情况。

所述的状态表示是指：航行体是一个理想的刚体，其所受的所有力学作用等效为一个合外力(矩)；在地球上定义的惯性系不受地球自转影响。在满足上述假设条件下，根据欧拉第一、第二定律，建立航行体在机体坐标系下的刚体动力学方程为：

所述的数据管理子系统包括：数据读写模块与通信管理模块，其中：数据读写模块执行数据的写入和读取操作，具体地，写入操作将传感器数据、大型AUV位姿和速度状态数据以及时间戳数据以.bag格式文件存储到外部存储器中，便于后期离线数据分析处理；读取操作将外部存储器中三维可视化仿真系统的相关数据读出，并支持数据回放功能，即在可视化子系统重现数据所对应的动态图像仿真过程。通信管理模块管理并调配机器人操作系统(Robot Operating System，ROS)的节点(进程)，一个子系统的实现由一个或多个节点组成。

所述的三维可视化仿真系统基于ROS系统的通信架构，系统由多个节点组成，每个节点以独立可执行文件(通常为C++编译生成的可执行文件、Python脚本)的形式被执行，负责一个单独的功能，如动力学求解、运动决策与控制等，众多节点以分布式的方式通过通信管理模块进行管理和调配。

所述的节点，其相互之间的通信采用发布和订阅话题的方式实现，具体地，在节点通信过程中，发布者节点和订阅者节点需在通信管理模块中进行注册，然后发布者发布话题，订阅者在通信管理模块的指挥下订阅相关话题，在接收消息后对消息进行处理，从而建立起订阅者-发布者之间的通信。消息路由不需要节点之间直接连接，同一个话题可以有多个订阅者，也可以有多个发布者。

所述的可视化子系统包括：水下环境配置模块、主相机模块、窗口配置模块以及数据可视化模块，其中：水下环境配置模块设定水下环境颜色、能见度、颜色衰减程度、最大水深等参数；主相机模块，拍摄其视场范围内的画面，如海下场景、海平面、天空等画面，并在窗口中进行显示；窗口管理器模块配置窗口横纵比、分辨率、着色器开关、轨迹显示、Bullet物理引擎开关等功能；数据可视化模块显示AUV实时轨迹、相关状态量随时间变化曲线以及传感器实时数据信息。

技术效果

与现有技术相比，本发明采用分布式架构，利于多硬件系统松耦合的实现，即代码不需修改便可移植至其他硬件系统；通过考虑物体的碰撞属性，可通过虚拟传感器实现大型AUV虚拟试验艇与虚拟环境的信息交互，为环境感知、运动决策与控制等智能算法的研究与有效性验证提供可靠的环境信息数据。与现有技术相比，本发明为未来智能算法走向工程应用打下基础，可为真实AUV的结构设计与优化以及软硬件系统的开发提供参考，同时可在外场试验前进行规划和控制器参数的整定，提高外场测试过程中工作人员与设备的安全性。

附图说明

图1为本发明高保真度仿真系统可视化图；

图2为本发明系统示意图；

图3为本发明三维可视化仿真系统中各模型间的关联图；

图4为本发明大型AUV虚拟试验艇模型运动坐标系示意图。

具体实施方式

如图1所示，为本实施例涉及的一种基于真实扰动数据的分布式大型高速AUV高保真仿真系统，包括：大型AUV虚拟试验艇子系统、水下三维虚拟环境子系统、运动求解子系统、数据管理子系统以及可视化子系统，其中：大型AUV虚拟试验艇子系统包括运动关联模块、运动感知模块、运动决策模块以及运动控制模块，水下三维虚拟环境子系统包括海底地形、动静态物体以及真实海流、海浪扰动，运动求解子系统包括坐标系转换求解模块与大型AUV运动方程求解模块，数据管理子系统包括数据读写模块与通信管理模块，可视化子系统包括水下环境配置模块、主相机模块、窗口配置模块以及数据可视化模块。

所述的大型AUV虚拟试验艇子系统与水下三维虚拟环境子系统中的模型以OBJ格式通过XML格式文件加载至可视化子系统，实现模型可视化；大型AUV虚拟试验艇子系统中运动感知模块的传感器数据、运动控制模块的控制输入数据以及水下三维虚拟环境中各物体的状态数据一起流入数据管理子系统，实现数据的存储与传输；数据管理子系统与运动求解子系统之间进行双向数据传输，运动求解子系统读取数据管理器的AUV控制输入、水下各物体的状态等数据，经解算将AUV运动状态、水下各物体固联坐标系转换关系等数据传入数据管理子系统；数据管理子系统将相关数据传入可视化子系统，驱动模型移动，并实现数据的可视化。

本实施例中大型AUV虚拟试验艇子系统以常规单刚体AUV为原型，通过设置AUV长度来等长径比任意缩放模型，并同时自动配置相应尺寸AUV的动力学参数，速度设定为14节以上。三维仿真系统优选实施例中大型AUV虚拟试验艇的结构如图4所示。

所述的运动关联模块描述艇身与各执行器之间的拓扑关系，艇身为单刚体结构，执行器包括垂直舵、水平舵以及推进器，垂直舵和水平舵采用正交方式安装在艇身后端，推进器安装至艇身尾部。

所述的运动感知模块描述大型AUV上搭载多种用于感知环境和自身状态的传感设备。

所述的大型AUV虚拟试验艇子系统与水下三维虚拟环境子系统中的模型由SolidWorks、3D Studio Max以及World Machine软件搭建，导出具备纹理、材质、几何碰撞等属性的OJB格式文件，通过URDF和XML格式描述，并在可视化子系统中显示。

如图3所示，所述的三维可视化仿真系统的坐标系转换求解模块描述水下环境中各物体固联坐标系之间的转换关系，本实施例中的物体包括各种海底地形、动静态物体、AUV艇身及其各部件，所有物体固联的坐标系通过直接或间接方式相连接，不能出现断连情况，遵守上述约束，坐标系转换求解模块方可正常工作。

如图4所示，所述的运动方程求解模块求解大型AUV的运动方程，并实时更新其运动状态，该运动方程包括运动学方程和动力学方程，图中机体坐标系固定于艇身中心位置，且与世界坐标系一同满足东-北-地心建系标准；运动方程中数学符号的描述如表1所示。

表1运动方程数学符号描述

所述的运动学方程，通过以下方式建立：机体坐标系原点相对于世界坐标系的位置矢量为η＝[X,Y,Z,φ,θ,ψ]

所述的动力学方程，通过以下方式建立。建立大型AUV动力学方程之前，需定义如下假设条件：大型AUV是一个理想的刚体，其所受的所有力学作用等效为一个合外力(矩)；在地球上定义的世界坐标系不受地球自转影响。在满足上述假设条件下，根据欧拉第一、第二定律，大型AUV的刚体动力学方程为：

将上式整理为矢量表达式为：

根据基尔霍夫方程，可求出由附加质量效应引起的惯性类水动力为：

当AUV重心和浮心不重合，且大小不相等时，重力浮力的合力在机体坐标系下为：

综上，大型AUV在水下环境受到的动力学方程为：

最后，采用ODE45对上述运动学、动力学方程求解，即可获得大型AUV实时的状态变量。

所述的通信管理模块通过话题实现节点之间的通信，接收来自大型AUV虚拟试验艇子系统、水下三维虚拟环境子系统的数据，并与运动求解子系统进行数据的双向通信，然后将所有数据再以话题的方式发布给可视化子系统，完成显示操作。数据读写模块通过rosbag指令将所有话题的消息统一打包成.bag格式文件，存储至外接机械硬盘内，用于后期离线数据分析处理等工作，提高数据的安全性与可重复利用性；类似地，该模块也可通过rosbag指令读取外接机械硬盘中三维仿真系统的相关数据，并支持数据回放功能，即在可视化子系统重现数据所对应的仿真过程。

所述的节点包括：运动方程求解节点、运动决策节点、运动控制节点、运动感知节点、可视化节点。在节点通信过程中，节点需在通信管理模块中进行注册，然后才可以发布和订阅相关话题，所述的话题包括：AUV控制量话题、AUV状态量话题、各种传感器相关话题、参考路径话题以及坐标变换相关话题。此处，每个节点在通信管理模块管理下，可同时发布和订阅多个话题。

所述的可视化子系统是基于UWSim模拟器与Qt工具箱来实现的，包括水下环境配置模块、主相机模块、窗口管理模块以及数据可视化模块，其中：前三者为UWSim模拟器的相关插件，使用DTD文档验证的XML文档对其进行配置。具体地，通过水下环境配置模块配置水下颜色、颜色衰减、最大水深重要参数；通过主相机设置模块对主相机是否跟随目标物移动、视场角度、长宽比、可拍摄最近/最远距离参数进行设定；通过窗口管理模块管理窗口显示属性，包括分辨率、使能开启画面渲染器、世界坐标系、使能AUV实时轨迹显示功能、使能Bullet物理引擎。

如图1所示，所述的数据可视化模块用于在窗口中显示AUV实时轨迹、运动状态变量与传感器实时数据信息，其中：AUV实时轨迹可视化可通过配置窗口管理模块实现；AUV运动状态变量与传感器实时数据可视化可通过Qt工具箱的rqt_plot节点订阅相关话题，然后在绘图窗口中实时显示相关数据随时间变化曲线。

仿真实验过程中，大型AUV关键参数设计如下：AUV长14米，直径2米，速度14节，初始深度100米；海洋环境配置如下：海底地形为10000米×10000米，水下颜色配置为深蓝色，颜色衰减设置为不衰减，最大水深设置为200米；主相机模块配置为：跟踪AUV，视场角度80度，长宽比1.33，可拍摄最近距离1米，可拍摄最远距离200米；窗口管理模块配置如下：分辨率设置为1600像素×1200像素，开启画面渲染器，世界坐标系设置为东-北-地坐标系，开启AUV实时轨迹显示，开启Bullet物理引擎；传感器参数配置中，多波束声呐参数根据实际传感器配置为最大探测距离200米，探测角度±60度，实际仿真效果如图1所示。

上述具体实施可由本领域技术人员在不背离本发明原理和宗旨的前提下以不同的方式对其进行局部调整，本发明的保护范围以权利要求书为准且不由上述具体实施所限，在其范围内的各个实现方案均受本发明之约束。