量布置矩阵，可以表示为：
[0040] 式中，β ;--第i ( τ i、τ 2、τ 3、τ 4)个推进器与本体坐标系OX轴的夹角；β i-- 第i ( τ 5、τ 6、τ 7、τ s)个推进器与本体坐标系OZ轴的夹角；xh、yh、zh--分别为四个水平 推进器（τ η τ 2、τ 3、τ 4)距离ROV本体坐标系的OX、0Y、OZ轴的距离；xv、yv、Z v--分别 为四个垂直推进器（τ 5、τ 6、τ 7、τ s)距离ROV本体坐标系的OX、0Y、OZ轴的距离；符号 c--余弦函数cos ( ·);符号s--正弦函数sin ( ·)。
[0041] 八个推进器的期望推力可按照下式计算：
[0043] 最终作用于ROV本体上的合力/合力矩可按照下式计算：
[0045] 式中：
一一分别为八个推进器实际输出的推力合 成六个自由度的推力/推力矩；
一一分别为机械手作业时的 六个自由度的力/力矩；
--分别为ROV作业时脐带缆的六个 自由度的力/力矩；Tx、TY、Tz、TK、T M、Tn-一分别为作用于ROV本体上的六个自由度的合力 /合力矩；
[0046] 八个推进器的液压控制环节的放大倍数可按照下式计算：
[0048] 式中：Pl-一液压马达油液两端的压力差J1 一一第i个推进器的期望推力； K1一一第i个推进器所对应的放大倍数。
[0049] 液压马达的输出力矩可按下式计算：
[0051] 式中：Tg--液压马达的输出扭矩，即驱动力矩；Dm--液压马达的排量。
[0052] 液压马达和负载力矩的平衡方程为：
[0054] 式中：Tg-一液压马达产生的驱动力矩；Bm-一负载和液压马达的粘性阻尼系数； G一一负载的扭转弹簧刚度，对于液压推进器来说，负载的扭转弹簧刚度为零；J一一液压 马达和负载的总惯性量；?Υ-一作用在液压马达的任意外负载力矩，对于液压推进器来说， 可以认为外负载力矩为螺旋桨的扭矩，即IY= Q。
[0055] 螺旋桨扭矩可用下式计算：
[0057] 式中：Q-一螺旋桨的扭矩；KQ-一螺旋桨转矩系数；P -一水的密度；η-一螺旋 桨的转速；D-一螺旋桨的直径；
[0058] 螺旋桨推力可用下式计算：
[0060] 式中：Kt--螺旋桨推力系数。
[0061] 本发明属于无人水下机器人运动控制技术领域，具体涉及一种过驱动作业型遥控 水下机器人（R0V -Remotely Operated Vehicle)训练模拟器的六自由度运动仿真控制系 统设计方法，包括ROV本体水动力学系统模型、海底流场干扰速度模型、机械手作业干扰力 /力矩模型、脐带缆干扰力/力矩模型、ROV六自由度PID控制器模块、过驱动推力分配模 块、液压螺旋桨推进器仿真控制模块、8个推进器推力合成6个自由度的推力/力矩、6个自 由度的推力/力矩和外部干扰力/力矩合成模块、ROV自动控制功能模块和四自由度ROV操 纵手柄模块等。本发明可实现以下功能：可实时模拟通过操纵手柄或自动控制功能按钮来 控制作业型ROV六自由度的运动；实时模拟六自由度控制器的期望推力/推力矩的动态变 化过程；实时模拟过驱动推力分配算法功能，将六个方向的期望推力/推力矩按照一定优 化分配方法分解成八个推进器的期望推力；通过引入液压螺旋桨推进器仿真控制模块，实 时模拟液压控制推进器的扭矩、推力、转速的动态变化过程；实时模拟机械手作业干扰力、 脐带缆干扰力对ROV运动控制性能的影响；实时模拟海流速度干扰对ROV运动控制性能的 影响。本发明中的作业型ROV运动控制仿真系统具有结构简单、使用方便，可以比较真实的 描述作业型ROV的内部控制体系结构组成，可以逼真模拟作业型ROV六自由度运动及控制 参数的动态变化过程等优点。
【主权项】
1. 一种作业型ROV训练模拟器的运动控制仿真系统，其特征在于：包括ROV本体水动 力学系统模型、海底流场干扰速度模型、机械手作业干扰力与力矩模型、脐带缆干扰力与力 矩模型、R0V六自由度PID控制器模块、过驱动推力分配模块、液压螺旋桨推进器仿真控制 模块、8个推进器推力合成6个自由度的推力与力矩模块、6个自由度的推力与力矩和外部 干扰力与力矩合成模块、R0V自动控制功能模块和四自由度R0V操纵手柄模块，R0V本体水 动力学系统模型接收海底流场干扰速度模型输出的干扰速度，并输出六自由度运动状态， 包括位移、角度、速度、角速度、加速度和角加速度信息； R0V六自由度PID控制器模块实时采集六自由度运动状态，并接收R0V自动控制功能模 块和四自由度R0V操纵手柄模块输出的控制指令，经过六个方向的PID控制器的控制，实时 输出六自由度的期望推力与推力矩给过驱动推力分配模块； 过驱动推力分配模块根据接收的信息，采用推力分配伪逆算法，实时输出四个水平推 进器和四个垂直推进器的期望推力给液压螺旋桨推进器仿真控制模块； 液压螺旋桨推进器仿真控制模块根据接收到的信息，输出八个推进器的实际推力和转 速，将推进器实际推力和转速输入到8个推进器推力合成6个自由度的推力与力矩模块，得 到六个自由度的实际推力与推力矩，传送给6个自由度的推力/力矩和外部干扰力与力矩 合成模块； 6个自由度的推力与力矩和外部干扰力与力矩合成模块还接收脐带缆干扰力与力矩模 型输出的脐带缆干扰力和机械手作业干扰力与力矩模型输出的机械手作业干扰力，将接收 的实际推力与推力矩与脐带缆干扰力和机械手作业干扰力进行合成计算，得到作用到R0V 本体上的推力与推力矩传送给R0V本体水动力学系统模型。2. 根据权利要求1所述的一种作业型R0V训练模拟器的运动控制仿真系统，其特征在 于：所述的四自由度R0V操纵手柄模块输出控制指令为R0V四个自由度的运动，包括前进或 后退、左移或右移、上浮或下沉和左或右转艏运动；R0V自动控制功能模块输出的控制指令 为自动保持航向、自动保持高程、自动保持深度和动力定位功能； R0V六自由度PID控制器模块还包括逻辑判断模块，逻辑判断模块根据R0V自动控制功 能模块和四自由度R0V操纵手柄模块的控制指令预先设置优先级，六个方向的PID控制器 根据当前接收的控制指令和预先设置的优先级进行控制响应。3. 根据权利要求1所述的一种作业型R0V训练模拟器的运动控制仿真系统，其特征在 于：所述的液压螺旋桨推进器仿真控制模块包括放大器、液压马达排量模块、螺旋桨动力学 方程模块，放大器接收四个水平推进器和四个垂直推进器的期望推力，转换成期望的液压 马达两端油液压力，输出给液压马达排量模块，转化为液压马达的扭矩输出给螺旋桨动力 学方程模块，输出八个推进器的实际推力和转速。
【专利摘要】本发明公开了一种作业型ROV训练模拟器的运动控制仿真系统。包括ROV本体水动力学系统模型、海底流场干扰速度模型、机械手作业干扰力与力矩模型、脐带缆干扰力与力矩模型、ROV六自由度PID控制器模块、过驱动推力分配模块、液压螺旋桨推进器仿真控制模块、8个推进器推力合成6个自由度的推力与力矩模块、6个自由度的推力与力矩和外部干扰力与力矩合成模块、ROV自动控制功能模块和四自由度ROV操纵手柄模块。本发明具有结构简单、层次清晰，可以真实的描述作业型ROV的内部控制体系结构组成，能够准确的模拟实际作业型ROV水下运动控制功能及过程，还具有能够逼真的输出作业型ROV的各种运动及控制参数变化过程的优点。
【IPC分类】G05B17/02
【公开号】CN105319987
【申请号】CN201510756848
【发明人】李新飞, 袁利毫, 韩端锋, 祝海涛, 姜迈, 宋磊
【申请人】哈尔滨工程大学
【公开日】2016年2月10日
【申请日】2015年11月9日