[發(fā)明專利]基于CAM矩陣的水下機(jī)器人矢量推力分配方法

有效

• 申請?zhí)枺?/td>: 201410583348.8
• 申請日: 2014-10-27
• 公開（公告）號: CN104326074A
• 公開（公告）日: 2015-02-04
• 發(fā)明（設(shè)計）人: 范輝;趙雄輝;吝龍艷
• 申請（專利權(quán)）人: 中國船舶重工集團(tuán)公司第七〇五研究所
• 主分類號: B63H5/00
• 分類號: B63H5/00;B63H21/00;B63C11/52
• 代理公司: 西北工業(yè)大學(xué)專利中心 61204
• 代理人: 王鮮凱

1.一種基于CAM矩陣的水下機(jī)器人矢量推力分配方法，其特征在于包括以下步驟：

步驟一、選擇水下機(jī)器人外輪廓的六個面形成長方體，以此長方體的幾何中心作為隨體坐標(biāo)系的坐標(biāo)原點O，OX軸在長方體縱對稱面內(nèi)，垂直于機(jī)器人前端面，指向前進(jìn)方向；OZ軸同在長方體縱對稱面內(nèi)，與OX軸垂直指向上方；OY軸垂直于ZOX平面，與OX軸和OZ軸構(gòu)成右手直角坐標(biāo)系；

步驟二、水下機(jī)器人在三個平移自由度需求分別用Γ_x、Γ_y和Γ_z表示，在三個旋轉(zhuǎn)自由度需求分別用Ω_x、Ω_y和Ω_z表示，則自由度需求指令向量表示為：

D＝[Γ_x?Γ_y?Γ_z?Ω_x?Ω_y?Ω_z]^T；

步驟三、水下機(jī)器人有四個水平推進(jìn)器和兩個垂直推進(jìn)器，四個水平推進(jìn)器軸線在同一平面內(nèi)，且與XOY平面平行，左前水平推進(jìn)器(1)、右前水平推進(jìn)器(4)、左后水平推進(jìn)器(6)和右后水平推進(jìn)器(9)的軸線與ZOX平面的夾角分別為γ_FL、γ_FR、γ_AL和γ_AR，左前水平推進(jìn)器(1)、右前水平推進(jìn)器(4)、左后水平推進(jìn)器(6)和右后水平推進(jìn)器(9)的安裝固定點P_FL、P_FR、P_AL和P_AR在隨體坐標(biāo)系內(nèi)的坐標(biāo)分別為(X_FL,Y_FL,Z_FL)、(X_FR,Y_FR,Z_FR)、(X_AL,Y_AL,Z_AL)和(X_AR,Y_AR,Z_AR)；兩個垂直推進(jìn)器5軸線在同一平面內(nèi)，且與YOZ平面平行，垂直左(CL)推進(jìn)器和垂直右(CR)推進(jìn)器與ZOX平面的夾角分別為γ_CL、γ_CR，兩各垂直推進(jìn)器5的安裝固定點P_CL和P_CR在隨體坐標(biāo)系內(nèi)的坐標(biāo)為(X_CL,Y_CL,Z_CL)和(X_CR,Y_CR,Z_CR)；

將左前水平推進(jìn)器(1)與右前水平推進(jìn)器(4)與推進(jìn)矢量前控制軸(3)相聯(lián)接，采用動密封方式插入前矢量推進(jìn)伺服機(jī)構(gòu)(2)；將左后水平推進(jìn)器(6)與右后水平推進(jìn)器(9)與推進(jìn)矢量后控制軸(8)相聯(lián)接，采用動密封方式插入后矢量推進(jìn)伺服機(jī)構(gòu)(7)；水下機(jī)器人根據(jù)控制指令，通過前矢量推進(jìn)伺服機(jī)構(gòu)(2)和后矢量推進(jìn)伺服機(jī)構(gòu)(7)的聯(lián)動控制實現(xiàn)推力矢量的旋轉(zhuǎn)輸出，推進(jìn)矢量前控制軸(3)的旋轉(zhuǎn)角度用δ_f表示，推進(jìn)矢量后控制軸(8)的旋轉(zhuǎn)角度用δ_a表示；

按照以上定義，左前水平推進(jìn)器(1)在三個平移自由度方向的貢獻(xiàn)系數(shù)向量表示為其中：

FFLx=cosγFL·cosδfFFLy=-cosγFL·sinδfFFLz=sinγFL]]>

右前水平推進(jìn)器(4)在三個平移自由度方向的貢獻(xiàn)系數(shù)向量表示為其中：

FFRx=cosγFR·cosδfFFRy=-cosγFR·sinδfFFRz=-sinγFR]]>

左后水平推進(jìn)器(6)在三個平移自由度方向的貢獻(xiàn)系數(shù)向量表示為其中：

FALx=-cosγAL·cosδaFALy=-cosγAL·sinδaFALz=sinγAL]]>

右后水平推進(jìn)器(9)在三個平移自由度方向的貢獻(xiàn)系數(shù)向量表示為其中：

FARx=-cosγAR·cosδaFARy=-cosγAR·sinδaFARz=-sinγAR]]>

兩個垂直推進(jìn)器5在三個平移自由度方向的貢獻(xiàn)系數(shù)向量分別表示為和其中：

FCLx=cosγCLFCLy=-cosγCLFCLz=sinγCL]]>

FCRx=cosγCRFCRy=-cosγCRFCRz=-sinγCR]]>

左前水平推進(jìn)器(1)在三個旋轉(zhuǎn)自由度方向的貢獻(xiàn)系數(shù)向量按下式計算：

右前水平推進(jìn)器(4)在三個旋轉(zhuǎn)自由度方向的貢獻(xiàn)系數(shù)向量按下式計算：

左后水平推進(jìn)器(6)在三個旋轉(zhuǎn)自由度方向的貢獻(xiàn)系數(shù)向量按下式計算：

右后水平推進(jìn)器(9)在三個旋轉(zhuǎn)自由度方向的貢獻(xiàn)系數(shù)向量按下式計算：

兩垂直推進(jìn)器5在三個旋轉(zhuǎn)自由度方向的貢獻(xiàn)系數(shù)向量分別表示為和按下式計算：

最終由各推進(jìn)器的貢獻(xiàn)系數(shù)向量得到的控制分配機(jī)矩陣L為：

L=FFLxFFLyFFLzMFLxMFLyMFLzFFRxFFRxFFRzMFRxMFRyMFRzFALxFALyFALzMALxMALyMALzFARxFARyFARzMARxMARyMARzFCLxFCLyFCLzMCLxMCLyMCLzFCRxFCRyFCRzMCRxMCRyMCRz]]>

步驟四、由自由度需求指令向量D與控制分配機(jī)矩陣L的內(nèi)積L·D的結(jié)果得到m×1的m個推進(jìn)器的推力指令向量T＝[T_FL?T_FR?T_AL?T_AR?T_CL?T_CR]^T，計算如下式，由此完成推力分配：

TFLTFRTALTARTCLTCR=FFLxFFLyFFLzMFLxMFLyMFLzFFRxFFRyFFRzMFRxMFRyMFRzFALxFALyFALzMALxMALyMALzFARxFARyFARzMARxMARyMARzFCLxFCLyFCLzMCLxMCLyMCLzFCRxFCRyFCRzMCRxMCRyMCRzΓxΓyΓzΩxΩyΩz,]]>

其中，m是推進(jìn)器的個數(shù)。