的期望推力/推力矩。將這六個(gè)自由度的推力/推力矩輸入到過驅(qū) 動(dòng)推力分配算法模塊，根據(jù)推力分配的偽逆算法，可實(shí)時(shí)輸出四個(gè)水平和四個(gè)垂直推進(jìn)器 的期望推力。接著，將八個(gè)推進(jìn)器的期望推力輸入到液壓螺旋槳推進(jìn)器仿真控制模塊，如圖 2所示，分別經(jīng)過放大器、液壓馬達(dá)排量模塊、螺旋槳?jiǎng)恿W(xué)方程模塊，輸出八個(gè)推進(jìn)器的實(shí) 時(shí)推力和轉(zhuǎn)速。如圖1所示，將推進(jìn)器實(shí)際推力輸入到8個(gè)推進(jìn)器推力合成6個(gè)自由度的推 力/力矩模塊，來輸出六個(gè)自由度的實(shí)際推力/推力矩。將以上得到的推力/推力矩輸入 到6個(gè)自由度的推力/力矩和外部干擾力/力矩合成模塊，與外部干擾力/力矩（機(jī)械手 作業(yè)干擾力、臍帶纜干擾力）進(jìn)行合成計(jì)算，即可得到作用到ROV本體上的推力/推力矩。
[0025] 作業(yè)型ROV訓(xùn)練模擬器的六自由度運(yùn)動(dòng)控制器是這樣實(shí)現(xiàn)的，如圖3所示：ROV操 縱手柄可以控制ROV四個(gè)自由度的運(yùn)動(dòng)，如：前進(jìn)/后退、左移/右移、上浮/下沉、左/右 轉(zhuǎn)艏運(yùn)動(dòng)。ROV自動(dòng)控制功能包含自動(dòng)保持航向、自動(dòng)保持高程、自動(dòng)保持深度和動(dòng)力定位 (DP)功能。邏輯判斷模塊的功能是根據(jù)以上控制命令的預(yù)先設(shè)置優(yōu)先級，來自動(dòng)判斷執(zhí)行 哪個(gè)命令。每個(gè)自由度的控制器都分為內(nèi)/外環(huán)控制器（位置環(huán)和速度環(huán)），R〇V操縱手柄 的控制命令從速度環(huán)換直接輸入。優(yōu)先級最高的為DP功能；次之為自動(dòng)保持航向、自動(dòng)保 持高程/深度命令；最后ROV操縱手柄命令。其中，自動(dòng)保持深度和自動(dòng)保持高程命令具有 互斥特點(diǎn)，只能有一個(gè)命令有效，當(dāng)激活其中一個(gè)命令時(shí)，另一個(gè)命令自動(dòng)失效。作業(yè)型ROV 具有自動(dòng)保持橫傾角和縱傾角穩(wěn)定的功能。
[0026] 本發(fā)明所設(shè)計(jì)的ROV訓(xùn)練模擬器運(yùn)動(dòng)控制系統(tǒng)具有結(jié)構(gòu)簡單、層次清晰、使用方 便，可以比較真實(shí)的描述作業(yè)型ROV的內(nèi)部控制體系結(jié)構(gòu)組成，具有能夠準(zhǔn)確的模擬實(shí)際 作業(yè)型ROV水下運(yùn)動(dòng)控制功能及過程的特點(diǎn)，還具有能夠逼真的輸出作業(yè)型ROV的各種運(yùn) 動(dòng)及控制參數(shù)變化過程的優(yōu)點(diǎn)。
[0027] -種作業(yè)型ROV訓(xùn)練模擬器的運(yùn)動(dòng)控制仿真系統(tǒng)，基于某作業(yè)型ROV水動(dòng)力學(xué)模 型，設(shè)計(jì)了作業(yè)型ROV六自由度運(yùn)動(dòng)控制仿真系統(tǒng)，包含：R〇V本體水動(dòng)力學(xué)系統(tǒng)模型、海底 流場干擾速度模型、機(jī)械手作業(yè)干擾力/力矩模型、臍帶纜干擾力/力矩模型、ROV六自由 度PID控制器模塊、過驅(qū)動(dòng)推力分配模塊、液壓螺旋槳推進(jìn)器仿真控制模塊、8個(gè)推進(jìn)器推 力合成6個(gè)自由度的推力/力矩、6個(gè)自由度的推力/力矩和外部干擾力/力矩合成模塊、 ROV自動(dòng)控制功能模塊和四自由度ROV操縱手柄模塊。本發(fā)明可實(shí)現(xiàn)以下功能：可實(shí)時(shí)模擬 通過操縱手柄或自動(dòng)控制功能按鈕來控制作業(yè)型ROV六自由度的運(yùn)動(dòng)，實(shí)時(shí)模擬六自由度 控制器的推力/推力矩的動(dòng)態(tài)變化過程，實(shí)時(shí)模擬液壓控制推進(jìn)器的推力變化過程，實(shí)時(shí) 模擬機(jī)械手作業(yè)干擾力、海流速度干擾、臍帶纜干擾力對作業(yè)型ROV運(yùn)動(dòng)控制性能的影響。
[0028] 六個(gè)方向的運(yùn)動(dòng)控制器都是雙閉環(huán)（速度環(huán)和位置環(huán)）控制。ROV操縱手柄通過 對應(yīng)的四個(gè)速度環(huán)可控制四個(gè)自由度（前進(jìn)/后退、左橫移/右橫移、升沉、左/右轉(zhuǎn)艏） 的運(yùn)動(dòng)；而縱傾和橫傾控制器具有自動(dòng)保持功能。ROV六自由度PID控制器還具有邏輯判 斷功能，可根據(jù)預(yù)先設(shè)定的優(yōu)先級，自動(dòng)判斷執(zhí)行ROV控制手柄操縱命令還是自動(dòng)控制功 能命令。
[0029] 根據(jù)運(yùn)動(dòng)控制器輸出的六個(gè)方向的期望推力/推力矩，按照過驅(qū)動(dòng)偽逆推力分配 算法，將其分配成八個(gè)推進(jìn)器（四個(gè)水平推進(jìn)器和四個(gè)垂直推進(jìn)器）的期望推力。
[0030] 將推進(jìn)器的閥控液壓環(huán)節(jié)視為一個(gè)比例放大環(huán)節(jié)，將輸入的每個(gè)推進(jìn)器的期望推 力轉(zhuǎn)換成期望的液壓馬達(dá)兩端油液壓力，進(jìn)而轉(zhuǎn)化為液壓馬達(dá)的扭矩輸出，用這個(gè)扭矩來 控制液壓螺旋槳的轉(zhuǎn)速及推力。在控制系統(tǒng)仿真中加入液壓螺旋槳推進(jìn)器控制模塊，可使 ROV六自由度運(yùn)動(dòng)控制仿真更接近實(shí)際作業(yè)型ROV運(yùn)動(dòng)控制的動(dòng)態(tài)過程。
[0031] 下面給出本發(fā)明專利的實(shí)施方式，并結(jié)合附圖1-3加以說明。首先根據(jù)圖1中ROV 訓(xùn)練模擬器的運(yùn)動(dòng)控制仿真系統(tǒng)原理示意圖，在matlab中建立ROV本體水動(dòng)力學(xué)系統(tǒng)模 型、海底流場干擾速度模型、機(jī)械手作業(yè)干擾力/力矩模型、臍帶纜干擾力/力矩模型、ROV 六自由度PID控制器模塊、過驅(qū)動(dòng)推力分配模塊、液壓螺旋槳推進(jìn)器仿真控制模塊、8個(gè)推 進(jìn)器推力合成6個(gè)自由度的推力/力矩、6個(gè)自由度的推力/力矩和外部干擾力/力矩合成 模塊等。
[0032] 如圖1所示，作業(yè)型ROV本體水動(dòng)力學(xué)模型輸出六個(gè)自由度運(yùn)動(dòng)狀態(tài)，ROV六自由 度PID控制器實(shí)時(shí)采集水下機(jī)器人六自由度的運(yùn)動(dòng)狀態(tài)，并根據(jù)控制指令，如：ROV操縱手 柄命令、自動(dòng)保持航向、自動(dòng)保持高程、自動(dòng)保持深度和DP命令，經(jīng)過六個(gè)方向的PID控制 器，實(shí)時(shí)輸出六個(gè)自由度的期望推力/推力矩。公式（1)-(2)所示的ROV六自由度的推力 /推力矩和八個(gè)推進(jìn)器的期望推力之間的關(guān)系；將這六個(gè)自由度的推力/推力矩輸入到過 驅(qū)動(dòng)推力分配算法模塊，根據(jù)公式（3)的偽逆算法，可實(shí)時(shí)輸出四個(gè)水平和四個(gè)垂直推進(jìn) 器的期望推力。
[0033] 如圖2所示，將八個(gè)推進(jìn)器的期望推力輸入到液壓螺旋槳推進(jìn)器仿真控制模塊， 將八個(gè)推進(jìn)器的期望推力輸入到液壓螺旋槳推進(jìn)器仿真控制模塊，分別經(jīng)過放大器、液壓 馬達(dá)排量模塊、螺旋槳?jiǎng)恿W(xué)方程模塊，輸出八個(gè)推進(jìn)器的實(shí)時(shí)推力和轉(zhuǎn)速。其中：放大器 可按照下式（5)計(jì)算，具體參數(shù)值可參考液壓螺旋槳推進(jìn)器的輸出特性曲線。液壓馬達(dá)排 量模塊，即為液壓馬達(dá)的排量，液壓螺旋槳推進(jìn)器的輸出扭矩可按照下式（6)計(jì)算。液壓馬 達(dá)輸出的扭矩輸入到螺旋槳?jiǎng)恿W(xué)方程模塊，可參考下公式（7)-(9)，即可得到螺旋槳推進(jìn) 器的實(shí)時(shí)轉(zhuǎn)速及推力。
[0034] 如圖1所示，接著，將推進(jìn)器實(shí)際推力輸入到8個(gè)推進(jìn)器推力合成6個(gè)自由度的推 力/力矩模塊，按照公式（1)輸出六個(gè)自由度的實(shí)際推力/推力矩。將以上得到的推力/ 推力矩輸入到6個(gè)自由度的推力/力矩和外部干擾力/力矩合成模塊，與外部干擾力/力 矩（機(jī)械手作業(yè)干擾力、臍帶纜干擾力）按照公式（4)進(jìn)行合成計(jì)算，即可得到作用到ROV 本體上的推力/推力矩。
[0035] 在調(diào)試以上模塊功能及互相聯(lián)調(diào)無誤后，建立ROV自動(dòng)控制功能模塊和四自由度 ROV操縱手柄模塊輸入旋鈕。在Matlab中運(yùn)行仿真程序后，操縱人員可通過使用ROV操縱 手柄或自動(dòng)控制功能，給ROV期望的控制命令。這些操縱命令被輸入給ROV六自由度運(yùn)動(dòng) PID控制器，通過控制器中邏輯判斷功能模塊，輸出優(yōu)先級最高的控制命令。這些控制指令 將控制ROV的運(yùn)動(dòng)，如前進(jìn)/后退、橫向運(yùn)動(dòng)、升/沉、轉(zhuǎn)艏、保持航向、保持高程、保持深度、 動(dòng)力定位功能（DP功能）等。同時(shí)，仿真控制程序還可根據(jù)操作人員選擇實(shí)現(xiàn)以下功能： 實(shí)時(shí)輸出水下機(jī)器人的運(yùn)動(dòng)和控制狀態(tài)變化過程，如：ROV本體坐標(biāo)系下和大地坐標(biāo)系下 ROV的三個(gè)方向線性運(yùn)動(dòng)的加速度、速度、位移等、三個(gè)旋轉(zhuǎn)運(yùn)動(dòng)的角加速度、加速度、角位 移等；六個(gè)自由度的期望輸出推力/推力矩、六個(gè)自由度的實(shí)際輸出推力/推力矩；八個(gè)推 進(jìn)器的期望推力、八個(gè)推進(jìn)器的實(shí)際推力；八個(gè)推進(jìn)器的轉(zhuǎn)速與消耗功率；八個(gè)推進(jìn)器的 液壓閥壓力等。通過設(shè)置不同海底流場流速，并根據(jù)ROV的運(yùn)動(dòng)速度，進(jìn)而實(shí)時(shí)輸出八個(gè)推 進(jìn)器的轉(zhuǎn)速及推力的動(dòng)態(tài)變化過程。通過設(shè)置ROV作業(yè)時(shí)的不同機(jī)械手干擾力和臍帶纜干 擾力狀態(tài)，可實(shí)時(shí)仿真模擬機(jī)械手作業(yè)和臍帶纜力對ROV水下運(yùn)動(dòng)的影響。
[0036] ROV控制器輸出的期望推力/推力矩和八個(gè)推進(jìn)器期望推力之間的關(guān)系如下式所 示：
[0038] 式中，7I1--控制器輸出期望縱向推力；玎--控制器輸出期望橫向推力；C1 一一控制器輸出期望垂向推力；&一一控制器輸出期望縱傾力矩；％ -一控制器輸出 期望橫傾力矩；仏一一控制器輸出期望偏航力矩；τ i一一第i個(gè)推進(jìn)器的期望推力； B ( β ) -一推進(jìn)器的矢