机器人仿真控制平台、方法及计算机存储介质

技术领域

本发明涉及机器人仿真控制技术领域，尤其涉及机器人仿真控制平台、方法及计算机存储介质。

背景技术

21世纪是“海洋的世纪”。人类对海洋的探索永无止境，随着无人机、无人艇以及水下自主潜器在海上的协同运用，海空潜协同的软件算法开发与调试工作愈发重要。

在开发海空潜协同软件的过程中，通常采用真实的无人机、无人艇以及水下自主潜器在真实环境中进行真实试验，在真实试验时容易出现硬件损坏、危险状况等不可控状况，导致海空潜协同软件的开发成本过高。

上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。

发明内容

本发明的主要目的在于提供一种机器人仿真控制平台、方法及计算机存储介质，旨在降低海空潜协同软件的开发成本。

为实现上述目的，本发明提供一种机器人仿真控制平台，所述机器人仿真控制平台包括：

仿真模型系统，包括至少一个无人机模型、至少一个无人艇模型、至少一个水下自主潜器模型以及环境模型；

机器人操作系统，与所述仿真模型系统通信连接，用于控制至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型执行仿真协同任务，以使至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型与所述环境模型进行交互。

可选地，至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型均设置有传感器插件，所述传感器插件用于在所述传感器插件所在的模型执行所述仿真协同任务时采集传感器数据；

所述机器人操作系统，还用于获取所述传感器数据，并根据所述传感器数据对所述仿真协同任务对应的算法进行性能评估，其中，所述算法包括控制算法、数据融合算法、路径规划算法以及自主导航算法。

可选地，所述仿真模型系统通过通信接口中间件与所述机器人操作系统进行通信连接，所述通信接口中间件用于将所述仿真模型系统发送的数据按照第一数据格式转发至所述机器人操作系统，以及用于将所述机器人操作系统发送的所述仿真协同任务的控制参数按照第二数据格式转发至所述仿真模型系统，其中，第一数据格式与所述机器人操作系统的通信接口对应，第二数据格式与所述仿真模型系统的通信接口对应，所述数据包括传感器数据。

可选地，所述仿真模型系统还包括世界插件，所述世界插件用于在至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型执行所述仿真协同任务，且至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个水下所述自主潜器模型中的任意两个模型接触时生成两个模型之间的连接关系，以及用于在至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型执行所述仿真协同任务，且至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型中的任意两个模型停止接触时取消两个模型之间的连接关系。

可选地，至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型均设置有视觉标识和传感器插件，所述传感器插件包括视觉传感器插件，其中，通过所述视觉传感器插件采集包括所述视觉标识的图像，通过所述图像获取所述视觉传感器插件所在的模型相对于所述视觉标识所在的模型的位置信息和姿态信息，并根据所述位置信息和所述姿态信息对所述仿真协同任务对应的算法进行性能评估。

此外，为实现上述目的，本发明还提供一种机器人仿真控制方法，所述机器人仿真控制方法应用于如上所述中任一项所述的机器人仿真控制平台，包括：

控制所述仿真模型系统中的至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型执行所述仿真协同任务；

在执行所述仿真协同任务时，获取至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型的传感器插件采集的传感器数据；

根据所述传感器数据对所述仿真协同任务对应的算法进行性能评估，其中，所述算法包括控制算法、数据融合算法、路径规划算法以及自主导航算法。

可选地，所述传感器插件包括视觉传感器插件，所述传感器数据包括所述视觉传感器插件采集的图像，所述图像包括视觉标识，所述视觉标识设置于至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型的表面，所述根据所述传感器数据对所述仿真协同任务对应的算法进行性能评估的步骤包括：

根据所述图像获取所述视觉传感器插件所在的模型相对于所述视觉标识所在的模型的位置信息和姿态信息；

根据所述位置信息和所述姿态信息对所述仿真协同任务对应的算法进行性能评估。

可选地，所述控制所述仿真模型系统中的至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型执行所述仿真协同任务的步骤之前，还包括：

在第一预设脚本被触发时，通过第一预设脚本按照预设顺序以及预设时间间隔依次启动所述仿真模型系统、通信接口中间件以及所述机器人操作系统，其中，所述仿真模型系统通过所述通信接口中间件与所述机器人操作系统进行通信连接。

可选地，所述在第一预设脚本被触发时，通过第一预设脚本按照预设顺序以及预设时间间隔依次启动所述仿真模型系统、通信接口中间件以及所述机器人操作系统的步骤之前，还包括：

在接收到模型生成指令时，获取所述模型生成指令对应的模型数量；

获取所述无人机模型、所述无人艇模型以及所述水下自主潜器模型中的至少一个对应的初始模型文件；

通过第二预设脚本按照所述模型数量以及所述初始模型文件生成所述仿真模型系统中的各个模型对应的模型文件。

此外，为实现上述目的，本发明还提供一种计算机存储介质，所述计算机存储介质上存储有机器人仿真控制程序，所述机器人仿真控制程序被处理器执行时实现如上所述中任一项所述的机器人仿真控制方法的步骤。

本发明实施例提出的机器人仿真控制平台、方法及计算机存储介质，机器人仿真控制平台包括：仿真模型系统，包括至少一个无人机模型、至少一个无人艇模型、至少一个水下自主潜器模型以及环境模型；机器人操作系统，与所述仿真模型系统通信连接，用于控制至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型执行仿真协同任务，以使至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型与所述环境模型进行交互。本发明通过建立海空潜三个领域的机器人模型，并控制机器人模型在虚拟环境中进行协同仿真任务，以替代真实试验，从而降低试验成本以及海空潜协同软件的开发成本。

附图说明

图1是机器人仿真控制平台的一种平台架构示意图。

图2是机器人仿真控制平台的另一平台架构示意图；

图3为本发明视觉传感器插件采集图像时模型之间的相对位置的一种示意图；

图4为本发明视觉传感器插件采集的图像的示意图；

图5为实施例方案涉及的硬件运行环境的终端结构示意图；

图6为本发明机器人仿真控制方法的一实施例的流程示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例提供一种解决方案，通过建立海空潜三个领域的机器人模型，并控制机器人模型在虚拟环境中进行协同仿真任务，以替代真实试验，从而降低试验成本以及海空潜协同软件的开发成本。

请参照图1，图1为机器人仿真控制平台的一种架构示意图。

机器人仿真控制平台包括仿真模型系统以及机器人操作系统。

仿真模型系统包括至少一个无人机模型、至少一个无人艇模型、至少一个水下自主潜器模型以及环境模型。

可选地，仿真模型系统为观景亭(Gazebo)平台，通过Gazebo软件进行建模，建立无人机(UAV，Unmanned Aerial Vehicle)模型、无人艇(USV， Unmanned Surface Vessel)模型、水下自主潜器(AUV，Autonomous Underwater Vehicle)模型以及环境模型等虚拟模型，无人机模型、无人艇模型以及水下自主潜器模型用于对真实机器人进行仿真，环境模型用于对真实环境进行仿真。Gazebo是一款3D(3-dimension，三维)动态模拟器，能够在复杂的室内和室外环境中准确有效地模拟机器人群。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。Gazebo包含多个物理引擎、丰富的机器人模型和环境库以及各种各样的传感器，并且程序设计方便，具有简单的图形界面。

可选地，环境模型为海空潜世界模型，包括水面模型、海床模型等，还可包括风、浪、流等海洋物理环境。

机器人操作系统(ROS，Robot Operating System)与仿真模型系统通信连接。ROS提供一系列程序库和工具以帮助软件开发者创建机器人应用软件，并且提供了硬件抽象、设备驱动、库函数、可视化、消息传递和软件包管理等诸多功能。ROS与仿真模型系统的通信是点对点的，采用话题(topic)和/ 或服务(service)的通讯方式实现。

机器人操作系统可按照预先设置好的海空潜协同软件程序，将对应的控制参数发送至至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型，以控制至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型执行对应的仿真协同任务，至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型与环境模型进行交互，从而得知仿真协同任务的完成情况，实现海空潜协同软件程序在虚拟环境下的仿真测试，并根据仿真测试对海空潜协同软件程序进行进一步开发或调试。

可选地，仿真协同任务是指多个机器人模型共同完成指定的任务，例如，由于某一海域内的环境的复杂性，并且非合作目标的智能程度越来越高，非合作目标可能采取大角度机动、通过狭窄航道等方式躲避追踪，单个机器人对该海域内的不明目标或者非合作目标的监测精确度和追踪能力较差，因此，可通过无人机、无人艇以及水下自主潜器协同跟踪以及监测该海域内的水下目标或水面目标。

可选地，至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型均设置有传感器插件，传感器插件用于在传感器插件所在的模型执行仿真协同任务时采集传感器数据，传感器数据即对应的模型(仿真机器人)在执行仿真协同任务时的完成情况。机器人操作系统可通过与仿真模型系统的点对点通信，获取各个模型的传感器数据，并根据传感器数据对仿真协同任务对应的算法(海空潜协同软件程序)进行性能评估，算法可包括控制算法、数据融合算法、路径规划算法以及自主导航算法等机器人相关算法，例如，通过传感器数据计算对机器人的控制精度、定位精度是否合格，传感器数据也可作为提供机器人与环境交互的数据，为基于深度学习的智能协同控制算法的训练提供基础数据，而无需对真实机器人进行大量真实试验来得到试验数据，降低了基于深度学习的智能协同控制算法的开发成本。

可选地，传感器数据包括传感器插件所在的模型的位置、姿态、线速度、线加速度、角速度、角加速度等数据。

可选地，由于ROS的通信接口与真实的ROS通信接口一致，而仿真模型系统是采用Gazebo的通信接口，与ROS的通信接口不匹配，因此，可参照图2，机器人仿真控制平台还包括通信接口中间件(即ROS中间件)，ROS 中间件用于将Gazebo的通信接口转换为ROS的通信接口、将Gazebo的话题接口和ROS的话题接口相互转换、将ROS的服务接口转换为Gazebo中相应的处理等等，ROS中间件可通过设置ROS中间件功能包来实现。例如，ROS 中间件通信接口中间件可将仿真模型系统发送的数据按照第一数据格式转发至机器人操作系统，其中，数据包括但不限于传感器数据、控制指令数据、机器人状态数据等等，以及ROS中间件通信接口中间件可将机器人操作系统发送的仿真协同任务的控制参数按照第二数据格式转发至仿真模型系统，以控制仿真模型系统中的各个模型，其中，第一数据格式与机器人操作系统的 ROS通信接口对应，第二数据格式与仿真模型系统的Gazebo通信接口对应。

可选地，参照图2，仿真模型系统还包括世界插件(world plugin)。世界插件用来处理模型与模型或者机器人与机器人之间的联系，在至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型执行仿真协同任务，且至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型中的任意两个模型接触时，生成两个模型之间的连接关系，以及在少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型执行仿真协同任务，且至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型中的任意两个模型停止接触时，取消两个模型之间的连接关系，例如，当无人机模型降落到无人艇模型上，认为无人机模型与无人艇模型接触，给无人机模型与无人艇模型之间添加固定的连接，以保证模型之间的联系与真实世界一致，又例如，当无人机模型离开无人艇模型去执行任务时，认为无人机模型与无人艇模型停止接触，需要将无人机模型与无人艇模型之间的固定连接取消，以保证模型之间的联系与真实世界一致，多个模型之间的协同仿真更加真实。

可选地，至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型的表面均设置有视觉标识，视觉标识用于视觉仿真，至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型均设置有传感器插件，传感器插件包括视觉传感器插件，因此，可通过视觉传感器插件采集包括视觉标识的图像，根据包括视觉标识的图像进行仿真视觉定位，定位出计算出视觉传感器插件所在的模型相对于视觉标识所在的模型的位置信息和姿态信息，从而根据位置信息和姿态信息对仿真协同任务对应的算法进行性能评估，算法可包括控制算法、数据融合算法、路径规划算法以及自主导航算法等机器人相关算法，例如，在通过仿真协同任务控制多个无人机保持预设队形飞行，可根据仿真视觉定位出的位置信息和姿态信息，判断模型之间的相对位置与仿真协同任务算法中模型之间的理想相对位置是否一致，判断模型之间的相对姿态与仿真协同任务算法中模型之间的理想相对姿态是否一致，根据模型的相对位置和相对姿态，以及仿真协同任务算法中模型之间的理想相对位置和理想相对姿态，计算仿真协同软件程序对于模型的控制精度，又例如，可根据无人机模型与无人艇模型的坐标确定无人机模型与无人艇模型之间的相对位置，结合根据仿真视觉定位出的无人机模型相对于无人艇模型的位置信息即可计算定位精度。根据控制精度、定位精度等信息进行量化估算，得到仿真协同任务对应的控制算法的性能评估结果。

可选地，参照图3以及图4，图3为无人机模型100与无人艇模型300的用户界面图像，无人机模型100位于无人艇模型300上方，无人艇模型300 顶部表面设置有视觉标识200，图4为无人机模型100的视觉传感器插件采集到的包含无人艇模型300顶部表面的视觉标识200的图像，因此可根据包含无人艇模型300顶部表面的视觉标识200的图像进行仿真视觉定位，确定无人机模型100相对于无人艇模型300的位置信息和姿态信息。

如图5所示，图5是本发明实施例方案涉及的硬件运行环境的终端结构示意图。

本发明实施例终端为终端设备。

如图5所示，该终端可以包括：处理器1001，例如CPU(central processing unit，中央处理器)或GPU(Graphics Processing Unit，图形处理器)，通信总线1002，用户接口1003，存储器1004。其中，通信总线1002用于实现这些组件之间的连接通信。用户接口1003可以包括显示屏(Display)、输入单元比如键盘(Keyboard)，可选用户接口1003还可以包括标准的有线接口、无线接口。存储器1004可以是高速RAM(Random Access Memory，随机存取存储器)，也可以是稳定的存储器(non-volatile memory)，例如磁盘存储器。存储器1004可选的还可以是独立于前述处理器1001的存储装置。

本领域技术人员可以理解，图5中示出的终端结构并不构成对终端的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。

如图5所示，作为一种计算机存储介质的存储器1005中可以包括操作系统、用户接口模块以及机器人仿真控制程序。

在图5所示的终端中，用户接口1003主要用于连接客户端(用户端)，与客户端进行数据通信；而处理器1001可以用于调用存储器1004中存储的机器人仿真控制程序，并执行以下操作：

控制所述仿真模型系统中的至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型执行所述仿真协同任务；

在执行所述仿真协同任务时，获取至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型的传感器插件采集的传感器数据；

根据所述传感器数据对所述仿真协同任务对应的算法进行性能评估，其中，所述算法包括控制算法、数据融合算法、路径规划算法以及自主导航算法。

进一步地，处理器1001可以调用存储器1004中存储的机器人仿真控制程序，还执行以下操作：

根据所述图像获取所述视觉传感器插件所在的模型相对于所述视觉标识所在的模型的位置信息和姿态信息；

根据所述位置信息和所述姿态信息对所述仿真协同任务对应的算法进行性能评估。

进一步地，处理器1001可以调用存储器1004中存储的机器人仿真控制程序，还执行以下操作：

在第一预设脚本被触发时，通过第一预设脚本按照预设顺序以及预设时间间隔依次启动所述仿真模型系统、通信接口中间件以及所述机器人操作系统，其中，所述仿真模型系统通过所述通信接口中间件与所述机器人操作系统进行通信连接。

进一步地，处理器1001可以调用存储器1004中存储的机器人仿真控制程序，还执行以下操作：

在接收到模型生成指令时，获取所述模型生成指令对应的模型数量；

获取所述无人机模型、所述无人艇模型以及所述水下自主潜器模型中的至少一个对应的初始模型文件；

通过第二预设脚本按照所述模型数量以及所述初始模型文件生成所述仿真模型系统中的各个模型对应的模型文件。

参照图6，在一实施例中，机器人仿真控制方法基于如上各个实施例所述的机器人仿真控制平台，机器人仿真控制方法包括以下步骤：

步骤S10，控制所述仿真模型系统中的至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型执行所述仿真协同任务；

在本实施例中，机器人仿真控制平台运行于终端设备，机器人仿真控制平台包括仿真模型系统以及机器人操作系统，机器人操作系统可存储有海空潜协同软件程序，并按照海空潜协同软件程序控制仿真模型系统中的至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型执行仿真协同任务。

可选地，仿真模型系统可包括更多的机器人模型，例如，多个无人机模型组成无人机模型集群，多个无人艇模型组成无人艇模型集群，多个无人艇模型组成无人艇模型集群，实现规模更大复杂程度更高的仿真协同任务的控制以及仿真试验。

可选地，由于机器人仿真控制方法的实施需要启动多个终端程序，有多个部分需要启动，有些部分需要等待其他部分初始化完毕后再启动，否则就无法正常工作，需要启动的多个终端程序包括仿真模型系统、通信接口中间件、机器人操作系统、视觉识别程序以及协同程序等。为了避免人为等待后再输入终端指令而浪费宝贵的时间，可预先编写第一预设脚本(即一键启动 Shell(壳)脚本)，在执行机器人仿真控制方法时，用户可触发第一预设脚本，在第一预设脚本被触发时，通过第一预设脚本按照预设顺序以及预设时间间隔依次启动仿真模型系统、通信接口中间件以及机器人操作系统等终端程序，其中，仿真模型系统通过通信接口中间件与机器人操作系统进行通信连接。

可选地，在仿真协同任务需要多个机器人组成的集群共同完成时，通常需要用户逐个编写每一机器人的模型文件(launch(开始)文件)，耗费的时间成本较大，因此，为了减少模型构建成本，可事先编写自动生成launch文件的程序(即第二预设脚本)，这样，用户只需针对无人机模型、无人艇模型以及水下自主潜器模型中的每一类型的模型手动单独编写一个launch文件 (初始模型文件)，再基于自动生成launch文件的程序触发模型生成指令，自动生成launch文件的程序即可根据模型生成指令对应的模型数量以及无人机模型、无人艇模型以及水下自主潜器模型中的至少一个对应的初始模型文件生成与模型数量对应的多个launch文件，以生成指定数量的机器人模型进行仿真，组成无人机模型集群或无人艇模型集群或水下自主潜器模型集群。

步骤S20，在执行所述仿真协同任务时，获取至少一个所述无人机模型、至少一个所述无人艇模型以及至少一个所述水下自主潜器模型的传感器插件采集的传感器数据；

在本实施例中，在仿真模型系统中的各个模型执行仿真协同任务时，获取各个模型的传感器插件采集的传感器数据，以通过传感器数据调整仿真模型系统中的各个模型的后续动作，保证仿真协同任务的顺利完成，例如，在仿真模型系统中的多个模型协同跟踪以及监测某一海域内的水下目标或水面目标，可根据传感器数据调整各个模型的移动路径，以实现对目标更加精确的协同跟踪以及监测。

可选地，在获取各个模型的传感器插件采集的传感器数据时，将传感器数据反馈至机器人操作系统，以供机器人操作系统根据传感器数据对仿真协同任务对应的控制算法进行性能评估。

步骤S30，根据所述传感器数据对所述仿真协同任务对应的算法进行性能评估，其中，所述算法包括控制算法、数据融合算法、路径规划算法以及自主导航算法。

在本实施例中，算法包括但不限于控制算法、数据融合算法、路径规划算法以及自主导航算法等。在仿真协同任务对应的算法进行性能评估时，可对根据传感器数据计算对机器人的控制精度、定位精度等，量化估算出仿真协同任务对应的算法的性能评估结果，实现对海空潜协同软件程序的测试，以便于对海空潜协同软件程序的软件算法进一步开发以及调试。

可选地，传感器插件包括视觉传感器插件，传感器数据包括视觉传感器插件采集的图像，视觉传感器插件采集的图像中包括视觉标识，视觉标识设置于至少一个无人机模型、至少一个无人艇模型以及至少一个水下自主潜器模型的表面。在根据包括视觉标识的图像对仿真协同任务对应的算法进行性能评估时，可通过包括视觉标识的图像获取视觉传感器插件所在的模型相对于视觉标识所在的模型的位置信息和姿态信息，根据位置信息和姿态信息对仿真协同任务对应的算法进行性能评估。

可选地，可通过二维码(Apriltag)定位算法来计算视觉传感器插件所在的模型相对于视觉标识所在的模型的位置信息和姿态信息。二维码(Apriltag) 定位算法包括自适应阈值分割、查找轮廓、查找连通域、对轮廓进行直线拟合、查找候选的凸四边形、对四边形进行解码、识别标签以及坐标变换以转换到世界坐标系等步骤。参照图3以及图4，图3为无人机模型100与无人艇模型300的用户界面图像，无人机模型100位于无人艇模型300上方，无人艇模型300顶部表面设置有视觉标识200，图4为无人机模型100的视觉传感器插件采集到的包含无人艇模型300顶部表面的视觉标识200的图像，因此可根据包含无人艇模型300顶部表面的视觉标识200的图像进行仿真视觉定位，确定无人机模型100相对于无人艇模型300的位置信息和姿态信息。

可选地，根据位置信息和姿态信息对仿真协同任务对应的算法进行性能评估，例如，在通过仿真协同任务控制多个无人机保持预设队形飞行，可根据仿真视觉定位出的位置信息判断模型之间的相对位置与仿真协同任务算法中模型之间的理想相对位置是否一致，可根据仿真视觉定位出的姿态信息判断模型之间的相对姿态与仿真协同任务算法中模型之间的理想相对姿态是否一致，根据模型的相对位置和相对姿态，以及仿真协同任务算法中模型之间的理想相对位置和理想相对姿态，计算仿真协同软件程序对于模型的控制精度，又例如，可根据无人机模型与无人艇模型的坐标确定无人机模型与无人艇模型之间的相对位置，结合根据仿真视觉定位出的无人机模型相对于无人艇模型的位置信息即可计算定位精度。根据控制精度、定位精度等信息进行量化估算，得到仿真协同任务对应的控制算法的性能评估结果。

在本实施例公开的技术方案中，通过建立海空潜三个领域的机器人模型，并控制机器人模型在虚拟环境中进行协同仿真任务，以替代真实试验，从而显著提高海空潜协同软件的开发效率，降低试验成本以及海空潜协同软件的开发成本。

在又一实施例中，基于上述各个实施例所述的机器人仿真控制平台以及方法，对上述各个实施例所述的机器人仿真控制平台以及方法的细节进行详细说明，以更好地支持上述各个实施例。

本申请涉及机器人仿真领域，在Gazebo软件上对多种机器人进行建模，建立一种仿真平台，该仿真平台具有可视化仿真、视觉仿真、协同算法和集群算法的仿真功能。与Matlab(Matrix Laboratory，矩阵实验室)、V-REP(Virtual Robot ExperimentationPlatform，虚拟机器人实验平台)等仿真平台相比， Gazebo平台具有丰富的仿真模型和仿真环境资源，可与ROS快捷双向通信，仿真的程序代码不经修改可以运行在真机系统上，可较为真实地模拟海空潜多机器人系统，包含无人平台本体、各种传感器、风、浪、流等海洋物理环境等优点。本方法包括空中无人机(UAV)的SDF(Simulation Description Format，模拟描述格式)模型、水面无人艇(USV)的URDF(Unified Robot Description Format，统一机器人描述格式)模型和水下自主潜器(AUV)机器人 URDF模型，提出一种中间件方法，实现Gazebo的SDF模型和ROS进行双向数据交互。本方法在机器人模型上添加一个带有视觉标识的长方体模型，并给出计算仿真单目视觉传感器内部参数矩阵方法，进行仿真视觉定位算法功能；提出一种自动生成launch文件的方法，生成指定数量的机器人进行集群仿真；本方法提出一种shell脚本，实现一键启动多个异步的终端程序功能。与现有仿真技术相比，本申请具有海空潜多种机器人协同仿真能力，并设计了提高仿真工作效率的自动化代码方法。

本申请提出的机器人仿真控制平台以及方法，可以同时支持空中、水面、水下多种机器人跨域协同仿真，且通过仿真的算法以及软件代码仅需轻度修改或无需修改即可在真实的机器人场景中使用。

在进行真实实验的时候，由于技术或者实验条件受限，无法获得精度评估和算法性能评估所需相关数据，导致无法评估性能。本申请提出的仿真方法，通过插件拿到机器人相关的位置、速度、加速度和传感器等各种数据，利用这些数据可对算法的性能进行评估，对机器人的控制精度、定位精度等进行量化估算，可为基于学习的智能协同控制算法的训练学习提供大量的智能体与环境交互数据，减少对真实信息物理系统交互数据的依赖，提高相关学习算法网络的训练效率。

本申请的实施步骤主要包括：

第一步，使用三维建模工具设计出无人机，无人艇，水下自主潜器的三维模型部件，然后使用URDF(Unified Robot Description Format)或者 SDF(SimulationDescription Format)语言将这些部件组织成Gazebo可以显示的单体机器人模型，并添加所需的机器人传感器，可以添加视觉，激光，声呐，距离，力矩，射频识别(Radio FrequencyIdentification，RFID)，全球定位系统 (Global Positioning System，GPS)，惯性测量单元(Inertial Measurement Unit， IMU)，根据需要添加噪声相关属性，给数据添加人为噪声模拟真实数据。编写模型插件(model plugin)和自定义传感器插件(sensor plugin)获得机器人的位置、姿态、速度、加速度信息，将插件添加到单体机器人模型文件中。

第二步，用SDF(Simulation Description Format)语言建立世界文件，同时包含空中，水面和水下三种环境，在世界文件中根据需要添加若干个Gazebo 官方库中的模型和机器人模型。编写世界插件(world plugin)，并添加到SDF 世界文件。世界插件用来处理模型与模型或者机器人与机器人之间的联系：当无人机降落到无人艇上，给两个机器人之间添加固定的连接；当无人机离开无人艇去执行任务时，需要将两个机器人之间的固定连接取消。

第三步，编写ROS插件或者ROS中间件功能包，建立Gazebo与ROS 之间的双向数据通信，ROS通信接口和真实机器接口保持一致。ROS插件需要添加到URDF单体机器人模型文件中；ROS中间件功能包可以像普通ROS 软件包一样运行，中间件的职责是将Gazebo的通信接口转换为ROS的通信接口，将Gazebo的话题接口和ROS的话题接口相互转换，将ROS的服务接口转换为Gazebo中相应的处理等等。

第四步，编写launch文件和ROS软件功能包，进行海空潜多机器人协同仿真。编写自动生成launch文件的程序。程序自动生成launch文件，生成指定数量的机器人模型进行仿真。

第五步，编写一键启动Shell脚本。本仿真方法启动程序时需要启动多个终端程序，有多个部分需要启动，有些部分需要等待其他部分初始化完毕后再启动，否则就无法正常工作。为了避免人为等待后再输入终端指令而浪费宝贵的时间，提出一键启动仿真程序的Shell脚本，脚本中先启动一个终端运行Gazebo世界和模型相关程序，在脚本中等待一段时间后，依次启动ROS 中间件程序、视觉识别程序、协同程序。

本申请的具体实施步骤包括：

第一步，编写仿真模型，使用三维建模工具设计出无人机，无人艇，水下自主潜器的三维模型部件，导出.dae或.stl格式的文件。无人机可以是任意类型与尺寸的URDF模型或SDF模型，本方法以SDF的建模过程进行举例， URDF的建模过程和下文无人艇模型的方法一样。使用Gazebo中的模型编辑器工具来编写，导入前述dae或stl格式的文件，添加link(连接)和joint(联合)标签，添加标签相关属性，添加Gazebo官方库中标签，GPS，IMU和单目相机等传感器，最后生成SDF格式的文件。编写Gazebo 模型插件，获得机器人在世界中的真实位置，插件中添加transport::PublisherPtr 对象，通过接口GetWorldPose获得位置姿态数据，然后发布gazebo::msgs::Pose 类型的话题。线速度、线加速度、角速度和角加速度数据分别通过接口 GetWorldLinearVel、GetWorldLinearAccel、GetWorldAngularVel和 GetWorldAngularAccel通过类似的方法获得。获得的数据可用于评估传感器融合类和视觉定位等算法的精度和性能，将此插件添加到SDF文件标签下。

无人艇可以是任何种类尺寸的URDF模型或SDF模型，本专利以无人艇 URDF模型举例，SDF模型建模方法和第一步的方法一样。添加、 标签和标签相应属性，添加Gazebo官方库中的ROS插件，一般是以 libgazebo_ros为前缀，so为后缀的文件。添加这些插件后，机器人模型通过 ROS话题，可以使模型运动，获得一些传感器数据。添加一个带有 AprilTag视觉标识的长方体dae文件，通过修改长方体的贴图图片，可以修改AprilTag视觉标识为其他任意形状和任意类型的视觉标识。添加长方体的 ，类型设置为“fixed”。ROS原生支持URDF，本方法中将URDF中的每个标签，添加属性之后，URDF模型可在Gazebo 中进行仿真。一个关节如果类型属性是"fixed"，则仿真时在Gazebo中不显示此关节。URDF模型文件有两种：urdf后缀的文件和xacro后缀的文件。

如果是xacro文件，在launch文件中添加：

如果是urdf文件，在launch文件中添加：

水下自主潜器模型建模方法与无人机和无人艇一样，可以是各种类型与尺寸的水下自主潜器的URDF模型或者SDF模型，过程不再赘述。

第二步，用三维建模工具建立水面和海床的dae模型，用SDF(SimulationDescription Format)语言建立世界文件。在Gazebo中编辑世界模型，导入dae，添加水面和海床两种环境，空中环境是空旷的环境，仅有无人机在飞行。在世界文件中可以根据需要添加若干个第一步中的模型或者Gazebo官方库中的模型。编写世界插件(world plugin)，当无人机降落到无人艇上之后，ROS客户端服务请求在无人机和无人艇之间添加固定的连接，使用physics::JointPtr 的Attach()接口；当无人机离开无人艇去执行任务时，ROS客户端服务请求取消两个模型的固定连接，使用physics::JointPtr的Detach()接口。

第三步，编写ROS中间件，无人机的图像话题和真实位置姿态通过ROS 中间件功能包转换成ROS话题：中间件中新建gazebo::transport::SubscriberPtr 对象订阅Gazebo中的图像话题，建立ROS中的 image_transport::CameraPublisher对象，发布接收到的图像；中间件中新建 gazebo::transport::SubscriberPtr对象订阅Gazebo中的位置、姿态、线速度、线加速度、角速度、角加速度数据，新建ros::Publiser对象，ROS话题发布接收到的Gazebo数据。

在视觉位置姿态估计算法中，需要用到视觉传感器的的内部参数矩阵K，在真实相机内参标定时一般需要使用棋盘格标定板，采集不同位置和姿态的图片并运行标定算法得到内部参数，标定参数过程繁多复杂，在本方法中不需要像真实相机那样进行复杂的标定过程，仿真相机的内部参数计算公式为：

其中，K是仿真相机的内部参数矩阵，width是图像的宽，height是图像的高，hfov是水平视野参数，是模型文件中属性值，tan是数学中的正切函数，*表示数学中的乘法。相机的畸变系数，仿真中使其为0，认为相机没有畸变。

第四步，编写launch文件，一个机器人添加一个标签，通过“ns”属性的唯一性区分不同的机器人。一键启动多个异步执行程序的Shell脚本示例：

gnome-terminal--tab-e"roslaunch a.launch"--tab-e'bash-c"sleep 5&&python b.py；exec bash"'

可以通过--tab-e添加多个异步的执行程序，休眠时间根据实际情况修改，示例脚本中的文件路径使用绝对的全局路径。

第五步，编写自动生成launch文件的程序。首先手动编写单个无人机的fly.launch，其中标签，无人机的初始位置x,y,z，和编号id留做可修改的参数；单个无人艇的usv.launch，其中标签，初始位置和编号留做可修改的参数；单个水下自主潜器的auv.launch,其中标签，初始位置和编号留做可修改的参数；程序获得机器人数量参数，可以自动生成指定数量的机器人启动文件：flys.launch文件、usvs.launch和auvs.launch。另编写一个启动launch文件，使用标签，添加flys.launch文件、usvs.launch、auvs.launch和世界，以及其他所需的launch组件。

自动生成launch文件的函数代码包括：

参数：文件名fout，ros包名pkgname，机器人数量num，机器人类型type

新建文件fout

fout添加一行文本：""

当前机器人编号i设置为0

共循环num次,

循环体开始

编号i设置为(i+1)

如果类型是无人机,则

fout添加一行文本："\t"

否则，如果类型是无人艇，则

fout添加一行文本："\t"

否则，如果类型是水下自主潜器，则

fout添加一行文本："\t"

fout添加一行文本设置编号："\t\t"

fout添加一行文本设置位置x："\t\t"

fout添加一行文本设置位置y："\t\t"

fout添加一行文本："\t"

循环体结束

fout添加一行文本：""

关闭fout

(1)手动编写单机器人的launch：无人机的fly.launch、无人艇的usv.launch、水下自主潜器的auv.launch。

(2)程序根据机器人数量参数自动生成flys.launch、usvs.launch、auvs.launch，代码自动生成机器人的初始位置、编号等信息。

(3)手动编写总体launch文件:标签包含世界、自动生成的 launch(flys.launch、usvs.launch、auvs.launch)及其他launch组件。

上述的标签名称、程序接口、类名称、变量名称及其他名称由于版本升级及其他原因导致的名称变换或者等价替换均在本方法的保护要求内。文中所述的示例文件名称替换均在本方法的保护要求内。文中所述的在仿真相机内参矩阵计算值基础上的数值微调，在仿真相机畸变系数数值上的微调均在本方法的保护要求内。文中所述自动生成指定数量的仿真集群机器人的launch 文件方法中，示例中仅以线性函数举例，机器人的编号和初始位置可以赋值为任意其他形式的数学上的线性或者非线性函数，均在本方法的保护要求内。

此外，本发明实施例还提出一种计算机存储介质，所述计算机存储介质上存储有机器人仿真控制程序，所述机器人仿真控制程序被处理器执行时实现如上各个实施例所述的机器人仿真控制方法的步骤。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在如上所述的一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。