基于cam矩阵的水下机器人矢量推力分配方法
【专利摘要】本发明公开了一种基于CAM矩阵的水下机器人矢量推力分配方法，用于解决现有水下机器人矢量推力分配方法方向控制单一的技术问题。技术方案是针对多自由度矢量推进水下机器人，将每个多自由度矢量推进器控制指令的执行结果直接与其对水下机器人六个自由度的贡献联系，给予分配贡献系数，针对每个推进器的每个自由度控制指令形成一个与水下机器人六个自由度相关的贡献系数向量。当控制器对水下机器人下达各自由度控制指令时，将水下机器人各自由度需求指令组成的向量与每个推进器自由度对应的贡献系数向量做内积，结果即为推进器在此自由度方向上的控制指令。本发明方法可以将水下机器人各自由度的控制指令有效地分配于各个多自由度推进器上。
【专利说明】基于CAM矩阵的水下机器人矢量推力分配方法
【技术领域】
[0001] 本发明涉及一种水下机器人矢量推力分配方法，特别是涉及一种基于CAM矩阵的 水下机器人矢量推力分配方法。
【背景技术】
[0002] 水下机器人，特别是开架式水下机器人，其运动控制是依靠推进器实现的。为了满 足水下机器人最低的机动性要求，水下机器人最少应该具备3个自由度的运动，及进退、潜 浮和转艏。绝大多数水下机器人的运动自由度不少于4个，除上面3个自由度以外还包括 侧移运动。
[0003] 文献"Optimization of Thrust Allocation in the Propulsion System of an Underwater Vehicle. Int. J. Appl. Math. Comput. Sci2004, Vol. 14, No. 4:461 - 467" 公开了 一种水下航行器推力优化分配方法，此方法可有效解决固定的单自由度推进器组合下的运 动控制推力分配问题。由于推进器推力方向固定，仅具备正反方向控制的单一自由度矢量 推进，因而可以使用固定的推力分配方式有效配置推力输出。
[0004] 考虑到如果每个推进器具备两个及两个以上的自由度输出矢量推力的能力，那么 针对特定的运动控制输入，由多个矢量推进器驱动下的推力分配组合有无穷多种，采用固 定的推力分配方式根本无法适应不同的运动控制要求。如何将水下机器人控制器与多自由 度矢量执行机构有效地连接起来，实现特定控制要求下矢量推力的有效分配，是开架式水 下机器人必须研究解决的重要课题。
【发明内容】
[0005] 为了克服现有水下机器人矢量推力分配方法方向控制单一的不足，本发明提供一 种基于CAM矩阵的水下机器人矢量推力分配方法。该方法针对多自由度矢量推进开架式水 下机器人，将每个多自由度矢量推进器的控制指令的执行结果直接与其对水下机器人六个 自由度的贡献联系起来，给予分配一定的贡献系数，则针对每个推进器的每个自由度控制 指令都可形成一个与水下机器人六个自由度相关的贡献系数向量。当控制器对水下机器人 下达各自由度控制指令时，将水下机器人各自由度需求指令组成的向量与每个推进器自由 度对应的贡献系数向量做内积，结果即为推进器在此自由度方向上的控制指令。本发明方 法可以将水下机器人各自由度的控制指令有效地分配于各个多自由度推进器上。
[0006] 本发明解决其技术问题所采用的技术方案是：一种基于CAM矩阵的水下机器人矢 量推力分配方法，其特点是采用以下步骤：
[0007] 步骤一、选择水下机器人外轮廓的六个面形成长方体，以此长方体的几何中心作 为随体坐标系的坐标原点〇, OX轴在长方体纵对称面内，垂直于机器人前端面，指向前进方 向；OZ轴同在长方体纵对称面内，与OX轴垂直指向上方；OY轴垂直于ZOX平面，与OX轴和 OZ轴构成右手直角坐标系。
[0008] 步骤二、水下机器人在三个平移自由度需求分别用rx、1\和rz表示，在三个旋 转自由度需求分别用Qx、Qy和Qz表示，则自由度需求指令向量表示为：D= [rx ry rz
[0009] 步骤三、水下机器人有四个水平推进器和两个垂直推进器，四个水平推进器轴线 在同一平面内，且与XOY平面平行，左前水平推进器1、右前水平推进器4、左后水平推进器6 和右后水平推进器9的轴线与ZOX平面的夹角分别为Y%、YFK、和Yak，左前水平推进器 1、右前水平推进器4、左后水平推进器6和右后水平推进器9的安装固定点P%、PFK、Pi和Pak在随体坐标系内的坐标分别为（Xfu Yfu ZfJ、（Xfk，Yfk，Zfk)、（X auYauZaJ 和（Xak，Yak，Zak);两 个垂直推进器5轴线在同一平面内，且与YOZ平面平行，垂直左（CL)推进器和垂直右（CR) 推进器与ZOX平面的夹角分别为Y a、Y Oi，两各垂直推进器5的安装固定点Pa和Pai在随 体坐标系内的坐标为（Xa，Ya，Za)和（XCK，Y ra，Zra)。
[0010] 将左前水平推进器1与右前水平推进器4与推进矢量前控制轴3相联接，采用动 密封方式插入前矢量推进伺服机构2 ;将左后水平推进器6与右后水平推进器9与推进矢 量后控制轴8相联接，采用动密封方式插入后矢量推进伺服机构7。水下机器人根据控制指 令，通过前矢量推进伺服机构2和后矢量推进伺服机构7的联动控制实现推力矢量的旋转 输出，推进矢量前控制轴3的旋转角度用Sf表示，推进矢量后控制轴8的旋转角度用Sa表不。
[0011] 按照以上定义，左前水平推进器1在三个平移自由度方向的贡献系数向量表示为 其中：
[0012]
【权利要求】
1. 一种基于CAM矩阵的水下机器人矢量推力分配方法，其特征在于包括以下步骤： 步骤一、选择水下机器人外轮廓的六个面形成长方体，以此长方体的几何中心作为随 体坐标系的坐标原点0, OX轴在长方体纵对称面内，垂直于机器人前端面，指向前进方向； OZ轴同在长方体纵对称面内，与OX轴垂直指向上方；OY轴垂直于ZOX平面，与OX轴和OZ 轴构成右手直角坐标系； 步骤二、水下机器人在三个平移自由度需求分别用rx、「7和rz表示，在三个旋转自 由度需求分别用Qx、Qy和Qz表示，则自由度需求指令向量表示为： D= [rx ry rz Qx Qy qjt； 步骤三、水下机器人有四个水平推进器和两个垂直推进器，四个水平推进器轴线在同 一平面内，且与XOY平面平行，左前水平推进器（1)、右前水平推进器（4)、左后水平推进器 (6)和右后水平推进器（9)的轴线与ZOX平面的夹角分别为Yfl、YFK、和Yak，左前水平 推进器（1)、右前水平推进器（4)、左后水平推进器（6)和右后水平推进器（9)的安装固定 点P%、PFK、Pa和Pak在随体坐标系内的坐标分别为ZfJ、（X FK，YFK，ZFK)、（XAU Yau ZiJ 和（XAK，YAK，ZAK);两个垂直推进器5轴线在同一平面内，且与YOZ平面平行，垂直左（CL)推 进器和垂直右（CR)推进器与ZOX平面的夹角分别为两各垂直推进器5的安装固 定点Pa和P?在随体坐标系内的坐标为（UeuZa)和U^Zai); 将左前水平推进器（1)与右前水平推进器（4)与推进矢量前控制轴（3)相联接，采用 动密封方式插入前矢量推进伺服机构（2);将左后水平推进器（6)与右后水平推进器（9) 与推进矢量后控制轴（8)相联接，采用动密封方式插入后矢量推进伺服机构（7);水下机器 人根据控制指令，通过前矢量推进伺服机构（2)和后矢量推进伺服机构（7)的联动控制实 现推力矢量的旋转输出，推进矢量前控制轴（3)的旋转角度用Sf表示，推进矢量后控制轴 (8)的旋转角度用Sa表示； 按照以上定义，左前水平推进器（1)在三个平移自由度方向的贡献系数向量表示为 &=^^，枚，役），其中：
右前水平推进器⑷在三个平移自由度方向的贡献系数向量表示为 ^ ，其中：
左后水平推进器（6)在三个平移自由度方向的贡献系数向量表示为
【文档编号】B63H21/00GK104326074SQ201410583348
【公开日】2015年2月4日 申请日期:2014年10月27日 优先权日:2014年10月27日
【发明者】范辉, 赵雄辉, 吝龙艳 申请人:中国船舶重工集团公司第七〇五研究所