的期望推力/推力矩。将这六个自由度的推力/推力矩输入到过驱 动推力分配算法模块，根据推力分配的伪逆算法，可实时输出四个水平和四个垂直推进器 的期望推力。接着，将八个推进器的期望推力输入到液压螺旋桨推进器仿真控制模块，如图 2所示，分别经过放大器、液压马达排量模块、螺旋桨动力学方程模块，输出八个推进器的实 时推力和转速。如图1所示，将推进器实际推力输入到8个推进器推力合成6个自由度的推 力/力矩模块，来输出六个自由度的实际推力/推力矩。将以上得到的推力/推力矩输入 到6个自由度的推力/力矩和外部干扰力/力矩合成模块，与外部干扰力/力矩（机械手 作业干扰力、脐带缆干扰力）进行合成计算，即可得到作用到ROV本体上的推力/推力矩。
[0025] 作业型ROV训练模拟器的六自由度运动控制器是这样实现的，如图3所示：ROV操 纵手柄可以控制ROV四个自由度的运动，如：前进/后退、左移/右移、上浮/下沉、左/右 转艏运动。ROV自动控制功能包含自动保持航向、自动保持高程、自动保持深度和动力定位 (DP)功能。逻辑判断模块的功能是根据以上控制命令的预先设置优先级，来自动判断执行 哪个命令。每个自由度的控制器都分为内/外环控制器（位置环和速度环），R〇V操纵手柄 的控制命令从速度环换直接输入。优先级最高的为DP功能；次之为自动保持航向、自动保 持高程/深度命令；最后ROV操纵手柄命令。其中，自动保持深度和自动保持高程命令具有 互斥特点，只能有一个命令有效，当激活其中一个命令时，另一个命令自动失效。作业型ROV 具有自动保持横倾角和纵倾角稳定的功能。
[0026] 本发明所设计的ROV训练模拟器运动控制系统具有结构简单、层次清晰、使用方 便，可以比较真实的描述作业型ROV的内部控制体系结构组成，具有能够准确的模拟实际 作业型ROV水下运动控制功能及过程的特点，还具有能够逼真的输出作业型ROV的各种运 动及控制参数变化过程的优点。
[0027] -种作业型ROV训练模拟器的运动控制仿真系统，基于某作业型ROV水动力学模 型，设计了作业型ROV六自由度运动控制仿真系统，包含：R〇V本体水动力学系统模型、海底 流场干扰速度模型、机械手作业干扰力/力矩模型、脐带缆干扰力/力矩模型、ROV六自由 度PID控制器模块、过驱动推力分配模块、液压螺旋桨推进器仿真控制模块、8个推进器推 力合成6个自由度的推力/力矩、6个自由度的推力/力矩和外部干扰力/力矩合成模块、 ROV自动控制功能模块和四自由度ROV操纵手柄模块。本发明可实现以下功能：可实时模拟 通过操纵手柄或自动控制功能按钮来控制作业型ROV六自由度的运动，实时模拟六自由度 控制器的推力/推力矩的动态变化过程，实时模拟液压控制推进器的推力变化过程，实时 模拟机械手作业干扰力、海流速度干扰、脐带缆干扰力对作业型ROV运动控制性能的影响。
[0028] 六个方向的运动控制器都是双闭环（速度环和位置环）控制。ROV操纵手柄通过 对应的四个速度环可控制四个自由度（前进/后退、左横移/右横移、升沉、左/右转艏） 的运动；而纵倾和横倾控制器具有自动保持功能。ROV六自由度PID控制器还具有逻辑判 断功能，可根据预先设定的优先级，自动判断执行ROV控制手柄操纵命令还是自动控制功 能命令。
[0029] 根据运动控制器输出的六个方向的期望推力/推力矩，按照过驱动伪逆推力分配 算法，将其分配成八个推进器（四个水平推进器和四个垂直推进器）的期望推力。
[0030] 将推进器的阀控液压环节视为一个比例放大环节，将输入的每个推进器的期望推 力转换成期望的液压马达两端油液压力，进而转化为液压马达的扭矩输出，用这个扭矩来 控制液压螺旋桨的转速及推力。在控制系统仿真中加入液压螺旋桨推进器控制模块，可使 ROV六自由度运动控制仿真更接近实际作业型ROV运动控制的动态过程。
[0031] 下面给出本发明专利的实施方式，并结合附图1-3加以说明。首先根据图1中ROV 训练模拟器的运动控制仿真系统原理示意图，在matlab中建立ROV本体水动力学系统模 型、海底流场干扰速度模型、机械手作业干扰力/力矩模型、脐带缆干扰力/力矩模型、ROV 六自由度PID控制器模块、过驱动推力分配模块、液压螺旋桨推进器仿真控制模块、8个推 进器推力合成6个自由度的推力/力矩、6个自由度的推力/力矩和外部干扰力/力矩合成 模块等。
[0032] 如图1所示，作业型ROV本体水动力学模型输出六个自由度运动状态，ROV六自由 度PID控制器实时采集水下机器人六自由度的运动状态，并根据控制指令，如：ROV操纵手 柄命令、自动保持航向、自动保持高程、自动保持深度和DP命令，经过六个方向的PID控制 器，实时输出六个自由度的期望推力/推力矩。公式（1)-(2)所示的ROV六自由度的推力 /推力矩和八个推进器的期望推力之间的关系；将这六个自由度的推力/推力矩输入到过 驱动推力分配算法模块，根据公式（3)的伪逆算法，可实时输出四个水平和四个垂直推进 器的期望推力。
[0033] 如图2所示，将八个推进器的期望推力输入到液压螺旋桨推进器仿真控制模块， 将八个推进器的期望推力输入到液压螺旋桨推进器仿真控制模块，分别经过放大器、液压 马达排量模块、螺旋桨动力学方程模块，输出八个推进器的实时推力和转速。其中：放大器 可按照下式（5)计算，具体参数值可参考液压螺旋桨推进器的输出特性曲线。液压马达排 量模块，即为液压马达的排量，液压螺旋桨推进器的输出扭矩可按照下式（6)计算。液压马 达输出的扭矩输入到螺旋桨动力学方程模块，可参考下公式（7)-(9)，即可得到螺旋桨推进 器的实时转速及推力。
[0034] 如图1所示，接着，将推进器实际推力输入到8个推进器推力合成6个自由度的推 力/力矩模块，按照公式（1)输出六个自由度的实际推力/推力矩。将以上得到的推力/ 推力矩输入到6个自由度的推力/力矩和外部干扰力/力矩合成模块，与外部干扰力/力 矩（机械手作业干扰力、脐带缆干扰力）按照公式（4)进行合成计算，即可得到作用到ROV 本体上的推力/推力矩。
[0035] 在调试以上模块功能及互相联调无误后，建立ROV自动控制功能模块和四自由度 ROV操纵手柄模块输入旋钮。在Matlab中运行仿真程序后，操纵人员可通过使用ROV操纵 手柄或自动控制功能，给ROV期望的控制命令。这些操纵命令被输入给ROV六自由度运动 PID控制器，通过控制器中逻辑判断功能模块，输出优先级最高的控制命令。这些控制指令 将控制ROV的运动，如前进/后退、横向运动、升/沉、转艏、保持航向、保持高程、保持深度、 动力定位功能（DP功能）等。同时，仿真控制程序还可根据操作人员选择实现以下功能： 实时输出水下机器人的运动和控制状态变化过程，如：ROV本体坐标系下和大地坐标系下 ROV的三个方向线性运动的加速度、速度、位移等、三个旋转运动的角加速度、加速度、角位 移等；六个自由度的期望输出推力/推力矩、六个自由度的实际输出推力/推力矩；八个推 进器的期望推力、八个推进器的实际推力；八个推进器的转速与消耗功率；八个推进器的 液压阀压力等。通过设置不同海底流场流速，并根据ROV的运动速度，进而实时输出八个推 进器的转速及推力的动态变化过程。通过设置ROV作业时的不同机械手干扰力和脐带缆干 扰力状态，可实时仿真模拟机械手作业和脐带缆力对ROV水下运动的影响。
[0036] ROV控制器输出的期望推力/推力矩和八个推进器期望推力之间的关系如下式所 示：
[0038] 式中，7I1--控制器输出期望纵向推力；玎--控制器输出期望横向推力；C1 一一控制器输出期望垂向推力；&一一控制器输出期望纵倾力矩；％ -一控制器输出 期望横倾力矩；仏一一控制器输出期望偏航力矩；τ i一一第i个推进器的期望推力； B ( β ) -一推进器的矢