基于cam矩陣的水下機器人矢量推力分配方法
【專利摘要】本發(fā)明公開了一種基于CAM矩陣的水下機器人矢量推力分配方法，用于解決現(xiàn)有水下機器人矢量推力分配方法方向控制單一的技術問題。技術方案是針對多自由度矢量推進水下機器人，將每個多自由度矢量推進器控制指令的執(zhí)行結果直接與其對水下機器人六個自由度的貢獻聯(lián)系，給予分配貢獻系數，針對每個推進器的每個自由度控制指令形成一個與水下機器人六個自由度相關的貢獻系數向量。當控制器對水下機器人下達各自由度控制指令時，將水下機器人各自由度需求指令組成的向量與每個推進器自由度對應的貢獻系數向量做內積，結果即為推進器在此自由度方向上的控制指令。本發(fā)明方法可以將水下機器人各自由度的控制指令有效地分配于各個多自由度推進器上。
【專利說明】基于CAM矩陣的水下機器人矢量推力分配方法
【技術領域】
[0001] 本發(fā)明涉及一種水下機器人矢量推力分配方法，特別是涉及一種基于CAM矩陣的 水下機器人矢量推力分配方法。
【背景技術】
[0002] 水下機器人，特別是開架式水下機器人，其運動控制是依靠推進器實現(xiàn)的。為了滿 足水下機器人最低的機動性要求，水下機器人最少應該具備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" 公開了 一種水下航行器推力優(yōu)化分配方法，此方法可有效解決固定的單自由度推進器組合下的運 動控制推力分配問題。由于推進器推力方向固定，僅具備正反方向控制的單一自由度矢量 推進，因而可以使用固定的推力分配方式有效配置推力輸出。
[0004] 考慮到如果每個推進器具備兩個及兩個以上的自由度輸出矢量推力的能力，那么 針對特定的運動控制輸入，由多個矢量推進器驅動下的推力分配組合有無窮多種，采用固 定的推力分配方式根本無法適應不同的運動控制要求。如何將水下機器人控制器與多自由 度矢量執(zhí)行機構有效地連接起來，實現(xiàn)特定控制要求下矢量推力的有效分配，是開架式水 下機器人必須研究解決的重要課題。
【發(fā)明內容】
[0005] 為了克服現(xiàn)有水下機器人矢量推力分配方法方向控制單一的不足，本發(fā)明提供一 種基于CAM矩陣的水下機器人矢量推力分配方法。該方法針對多自由度矢量推進開架式水 下機器人，將每個多自由度矢量推進器的控制指令的執(zhí)行結果直接與其對水下機器人六個 自由度的貢獻聯(lián)系起來，給予分配一定的貢獻系數，則針對每個推進器的每個自由度控制 指令都可形成一個與水下機器人六個自由度相關的貢獻系數向量。當控制器對水下機器人 下達各自由度控制指令時，將水下機器人各自由度需求指令組成的向量與每個推進器自由 度對應的貢獻系數向量做內積，結果即為推進器在此自由度方向上的控制指令。本發(fā)明方 法可以將水下機器人各自由度的控制指令有效地分配于各個多自由度推進器上。
[0006] 本發(fā)明解決其技術問題所采用的技術方案是：一種基于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相聯(lián)接，采用動 密封方式插入前矢量推進伺服機構2 ;將左后水平推進器6與右后水平推進器9與推進矢 量后控制軸8相聯(lián)接，采用動密封方式插入后矢量推進伺服機構7。水下機器人根據控制指 令，通過前矢量推進伺服機構2和后矢量推進伺服機構7的聯(lián)動控制實現(xiàn)推力矢量的旋轉 輸出，推進矢量前控制軸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)相聯(lián)接，采用 動密封方式插入前矢量推進伺服機構（2);將左后水平推進器（6)與右后水平推進器（9) 與推進矢量后控制軸（8)相聯(lián)接，采用動密封方式插入后矢量推進伺服機構（7);水下機器 人根據控制指令，通過前矢量推進伺服機構（2)和后矢量推進伺服機構（7)的聯(lián)動控制實 現(xiàn)推力矢量的旋轉輸出，推進矢量前控制軸（3)的旋轉角度用Sf表示，推進矢量后控制軸 (8)的旋轉角度用Sa表示； 按照以上定義，左前水平推進器（1)在三個平移自由度方向的貢獻系數向量表示為 &=^^，枚，役），其中：
右前水平推進器⑷在三個平移自由度方向的貢獻系數向量表示為 ^ ，其中：
左后水平推進器（6)在三個平移自由度方向的貢獻系數向量表示為
【文檔編號】B63H21/00GK104326074SQ201410583348
【公開日】2015年2月4日 申請日期:2014年10月27日 優(yōu)先權日:2014年10月27日
【發(fā)明者】范輝, 趙雄輝, 吝龍艷 申請人:中國船舶重工集團公司第七〇五研究所