[发明专利]基于CAM矩阵的水下机器人矢量推力分配方法

摘要

本发明公开了一种基于CAM矩阵的水下机器人矢量推力分配方法，用于解决现有水下机器人矢量推力分配方法方向控制单一的技术问题。技术方案是针对多自由度矢量推进水下机器人，将每个多自由度矢量推进器控制指令的执行结果直接与其对水下机器人六个自由度的贡献联系，给予分配贡献系数，针对每个推进器的每个自由度控制指令形成一个与水下机器人六个自由度相关的贡献系数向量。当控制器对水下机器人下达各自由度控制指令时，将水下机器人各自由度需求指令组成的向量与每个推进器自由度对应的贡献系数向量做内积，结果即为推进器在此自由度方向上的控制指令。本发明方法可以将水下机器人各自由度的控制指令有效地分配于各个多自由度推进器上。

主权项

一种基于CAM矩阵的水下机器人矢量推力分配方法，其特征在于包括以下步骤：步骤一、选择水下机器人外轮廓的六个面形成长方体，以此长方体的几何中心作为随体坐标系的坐标原点O，OX轴在长方体纵对称面内，垂直于机器人前端面，指向前进方向；OZ轴同在长方体纵对称面内，与OX轴垂直指向上方；OY轴垂直于ZOX平面，与OX轴和OZ轴构成右手直角坐标系；步骤二、水下机器人在三个平移自由度需求分别用Γ_x、Γ_y和Γ_z表示，在三个旋转自由度需求分别用Ω_x、Ω_y和Ω_z表示，则自由度需求指令向量表示为：D＝[Γ_x Γ_y Γ_z Ω_x Ω_y Ω_z]^T；步骤三、水下机器人有四个水平推进器和两个垂直推进器，四个水平推进器轴线在同一平面内，且与XOY平面平行，左前水平推进器(1)、右前水平推进器(4)、左后水平推进器(6)和右后水平推进器(9)的轴线与ZOX平面的夹角分别为γ_FL、γ_FR、γ_AL和γ_AR，左前水平推进器(1)、右前水平推进器(4)、左后水平推进器(6)和右后水平推进器(9)的安装固定点P_FL、P_FR、P_AL和P_AR在随体坐标系内的坐标分别为(X_FL,Y_FL,Z_FL)、(X_FR,Y_FR,Z_FR)、(X_AL,Y_AL,Z_AL)和(X_AR,Y_AR,Z_AR)；两个垂直推进器5轴线在同一平面内，且与YOZ平面平行，垂直左(CL)推进器和垂直右(CR)推进器与ZOX平面的夹角分别为γ_CL、γ_CR，两各垂直推进器5的安装固定点P_CL和P_CR在随体坐标系内的坐标为(X_CL,Y_CL,Z_CL)和(X_CR,Y_CR,Z_CR)；将左前水平推进器(1)与右前水平推进器(4)与推进矢量前控制轴(3)相联接，采用动密封方式插入前矢量推进伺服机构(2)；将左后水平推进器(6)与右后水平推进器(9)与推进矢量后控制轴(8)相联接，采用动密封方式插入后矢量推进伺服机构(7)；水下机器人根据控制指令，通过前矢量推进伺服机构(2)和后矢量推进伺服机构(7)的联动控制实现推力矢量的旋转输出，推进矢量前控制轴(3)的旋转角度用δ_f表示，推进矢量后控制轴(8)的旋转角度用δ_a表示；按照以上定义，左前水平推进器(1)在三个平移自由度方向的贡献系数向量表示为其中：$\left\{\begin{array}{c}{F}_{\mathrm{FL}}^x={\cos \mathrm{\γ}}_{\mathrm{FL}}\·{\cos \mathrm{\δ}}_f\\ F_{\mathrm{FL}}^y=-{\cos \mathrm{\γ}}_{\mathrm{FL}}\·{\sin \mathrm{\δ}}_f\\ F_{\mathrm{FL}}^z={\sin \mathrm{\γ}}_{\mathrm{FL}}\end{array}\right.$右前水平推进器(4)在三个平移自由度方向的贡献系数向量表示为其中：$\left\{\begin{array}{c}{F}_{\mathrm{FR}}^x={\cos \mathrm{\γ}}_{\mathrm{FR}}\·{\cos \mathrm{\δ}}_f\\ F_{\mathrm{FR}}^y=-{\cos \mathrm{\γ}}_{\mathrm{FR}}\·{\sin \mathrm{\δ}}_f\\ F_{\mathrm{FR}}^z={-\sin \mathrm{\γ}}_{\mathrm{FR}}\end{array}\right.$左后水平推进器(6)在三个平移自由度方向的贡献系数向量表示为其中：$\left\{\begin{array}{c}{F}_{\mathrm{AL}}^x={-\cos \mathrm{\γ}}_{\mathrm{AL}}\·{\cos \mathrm{\δ}}_a\\ F_{\mathrm{AL}}^y=-{\cos \mathrm{\γ}}_{\mathrm{AL}}\·{\sin \mathrm{\δ}}_a\\ F_{\mathrm{AL}}^z={\sin \mathrm{\γ}}_{\mathrm{AL}}\end{array}\right.$右后水平推进器(9)在三个平移自由度方向的贡献系数向量表示为其中：$\left\{\begin{array}{c}{F}_{\mathrm{AR}}^x=-{\cos \mathrm{\γ}}_{\mathrm{AR}}\·{\cos \mathrm{\δ}}_a\\ F_{\mathrm{AR}}^y=-{\cos \mathrm{\γ}}_{\mathrm{AR}}\·{\sin \mathrm{\δ}}_a\\ F_{\mathrm{AR}}^z={-\sin \mathrm{\γ}}_{\mathrm{AR}}\end{array}\right.$两个垂直推进器5在三个平移自由度方向的贡献系数向量分别表示为和其中：$\left\{\begin{array}{c}{F}_{\mathrm{CL}}^x={\cos \mathrm{\γ}}_{\mathrm{CL}}\\ F_{\mathrm{CL}}^y={-\cos \mathrm{\γ}}_{\mathrm{CL}}\\ F_{\mathrm{CL}}^z={\sin \mathrm{\γ}}_{\mathrm{CL}}\end{array}\right.$$\left\{\begin{array}{c}{F}_{\mathrm{CR}}^x={\cos \mathrm{\γ}}_{\mathrm{CR}}\\ F_{\mathrm{CR}}^y={-\cos \mathrm{\γ}}_{\mathrm{CR}}\\ F_{\mathrm{CR}}^z={-\sin \mathrm{\γ}}_{\mathrm{CR}}\end{array}\right.$左前水平推进器(1)在三个旋转自由度方向的贡献系数向量按下式计算：右前水平推进器(4)在三个旋转自由度方向的贡献系数向量按下式计算：左后水平推进器(6)在三个旋转自由度方向的贡献系数向量按下式计算：右后水平推进器(9)在三个旋转自由度方向的贡献系数向量按下式计算：两垂直推进器5在三个旋转自由度方向的贡献系数向量分别表示为和按下式计算：最终由各推进器的贡献系数向量得到的控制分配机矩阵L为：$L=\left[\begin{array}{cccccc}{F}_{\mathrm{FL}}^x& F_{\mathrm{FL}}^y& F_{\mathrm{FL}}^z& M_{\mathrm{FL}}^x& M_{\mathrm{FL}}^y& M_{\mathrm{FL}}^z\\ F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^z& M_{\mathrm{FR}}^x& M_{\mathrm{FR}}^y& M_{\mathrm{FR}}^z\\ F_{\mathrm{AL}}^x& F_{\mathrm{AL}}^y& F_{\mathrm{AL}}^z& M_{\mathrm{AL}}^x& M_{\mathrm{AL}}^y& M_{\mathrm{AL}}^z\\ F_{\mathrm{AR}}^x& F_{\mathrm{AR}}^y& F_{\mathrm{AR}}^z& M_{\mathrm{AR}}^x& M_{\mathrm{AR}}^y& M_{\mathrm{AR}}^z\\ F_{\mathrm{CL}}^x& F_{\mathrm{CL}}^y& F_{\mathrm{CL}}^z& M_{\mathrm{CL}}^x& M_{\mathrm{CL}}^y& M_{\mathrm{CL}}^z\\ F_{\mathrm{CR}}^x& F_{\mathrm{CR}}^y& F_{\mathrm{CR}}^z& M_{\mathrm{CR}}^x& M_{\mathrm{CR}}^y& M_{\mathrm{CR}}^z\end{array}\right]$步骤四、由自由度需求指令向量D与控制分配机矩阵L的内积L·D的结果得到m×1的m个推进器的推力指令向量T＝[T_FL T_FR T_AL T_AR T_CL T_CR]^T，计算如下式，由此完成推力分配：$\left[\begin{array}{c}{T}_{\mathrm{FL}}\\ T_{\mathrm{FR}}\\ T_{\mathrm{AL}}\\ T_{\mathrm{AR}}\\ T_{\mathrm{CL}}\\ T_{\mathrm{CR}}\end{array}\right]=\left[\begin{array}{cccccc}{F}_{\mathrm{FL}}^x& F_{\mathrm{FL}}^y& F_{\mathrm{FL}}^z& M_{\mathrm{FL}}^x& M_{\mathrm{FL}}^y& M_{\mathrm{FL}}^z\\ F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^y& F_{\mathrm{FR}}^z& M_{\mathrm{FR}}^x& M_{\mathrm{FR}}^y& M_{\mathrm{FR}}^z\\ F_{\mathrm{AL}}^x& F_{\mathrm{AL}}^y& F_{\mathrm{AL}}^z& M_{\mathrm{AL}}^x& M_{\mathrm{AL}}^y& M_{\mathrm{AL}}^z\\ F_{\mathrm{AR}}^x& F_{\mathrm{AR}}^y& F_{\mathrm{AR}}^z& M_{\mathrm{AR}}^x& M_{\mathrm{AR}}^y& M_{\mathrm{AR}}^z\\ F_{\mathrm{CL}}^x& F_{\mathrm{CL}}^y& F_{\mathrm{CL}}^z& M_{\mathrm{CL}}^x& M_{\mathrm{CL}}^y& M_{\mathrm{CL}}^z\\ F_{\mathrm{CR}}^x& F_{\mathrm{CR}}^y& F_{\mathrm{CR}}^z& M_{\mathrm{CR}}^x& M_{\mathrm{CR}}^y& M_{\mathrm{CR}}^z\end{array}\right]\left[\begin{array}{c}{\mathrm{\Γ}}_x\\ {\mathrm{\Γ}}_y\\ {\mathrm{\Γ}}_z\\ {\mathrm{\Ω}}_x\\ {\mathrm{\Ω}}_y\\ {\mathrm{\Ω}}_z\end{array}\right],$其中，m是推进器的个数。