楊 釗，景包睿，張 頻

(中國(guó)船舶集團(tuán)有限公司 第705 研究所昆明分部，云南 昆明 650106)

0 引 言

隨著機(jī)器人技術(shù)的迅猛發(fā)展，機(jī)器人在軍事領(lǐng)域上的應(yīng)用越來(lái)越廣泛。波音公司為美國(guó)海軍研制了以機(jī)械臂為主體、基于潛艇魚(yú)雷發(fā)射管的UUV 回收系統(tǒng)，并于2006 年初完成海試[1]。艦艇通過(guò)發(fā)射管進(jìn)行小型UUV 的布放、回收以及物資輸送，具有很強(qiáng)的隱蔽性，應(yīng)用前景廣闊。目前國(guó)內(nèi)在這方面的研究尚處于空白階段，因此基于發(fā)射管的回收機(jī)械臂具有較好的實(shí)用價(jià)值和研究意義。

機(jī)械臂裝載在發(fā)射管內(nèi)，因此其體積、外形及重量有著嚴(yán)格限制。在機(jī)械臂的設(shè)計(jì)過(guò)程中，動(dòng)力學(xué)求解得到的機(jī)械臂動(dòng)態(tài)過(guò)程中各關(guān)節(jié)的力與力矩可為驅(qū)動(dòng)校核、優(yōu)化機(jī)械臂結(jié)構(gòu)等工作提供參考。Pratheep V G[2]在Solidworks 中建立了機(jī)械臂的三維模型并在Matlab 下進(jìn)行運(yùn)動(dòng)控制仿真。袁凱[3]和鮑俊維[4]分別設(shè)計(jì)了六功能和五功能的中等負(fù)載水下機(jī)械臂，并利用牛頓-歐拉法建立了機(jī)械臂動(dòng)力學(xué)模型。王懿[5]基于迭代Newton-Euler 矢量力學(xué)方法和Morison 方程建立了靜水條件下考慮流體阻力的六自由度機(jī)械臂動(dòng)力學(xué)模型，通過(guò)Matlab 軟件仿真分析了流體阻力對(duì)機(jī)械臂關(guān)節(jié)控制力矩最大影響。

本文針對(duì)艦艇的回收打撈需求，對(duì)機(jī)械臂進(jìn)行結(jié)構(gòu)設(shè)計(jì)，建立虛擬樣機(jī)模型得到相應(yīng)的動(dòng)力學(xué)參數(shù)；規(guī)劃回收軌跡求解機(jī)械臂的關(guān)節(jié)驅(qū)動(dòng)力矩；最后對(duì)比分析浮力、負(fù)載重量、負(fù)載空間尺寸對(duì)機(jī)械臂的動(dòng)力學(xué)特性的影響。

1 機(jī)械臂的結(jié)構(gòu)設(shè)計(jì)與三維模型

1.1 機(jī)械臂的總體結(jié)構(gòu)設(shè)計(jì)

基于發(fā)射管的回收機(jī)械臂在待機(jī)時(shí)能回縮在發(fā)射管內(nèi)，伸展后能完成“抓取-對(duì)齊-回收”任務(wù)。其中，機(jī)械臂待機(jī)時(shí)的回縮功能可通過(guò)改裝機(jī)械臂的安裝平臺(tái)來(lái)實(shí)現(xiàn)，但機(jī)械臂需要具備折疊功能，并保證折疊后的包絡(luò)圓直徑要小于發(fā)射管的管徑?？傮w參數(shù)為：折疊后的包絡(luò)圓直徑不大于300 mm，最大伸展長(zhǎng)度為2 m，負(fù)載為10 kg、長(zhǎng)度800 mm 的圓柱狀負(fù)載。

自由度是衡量機(jī)械臂運(yùn)動(dòng)靈活性的主要技術(shù)指標(biāo)，同時(shí)也衡量著機(jī)械臂結(jié)構(gòu)的復(fù)雜程度[6]。常見(jiàn)的結(jié)構(gòu)緊湊水下機(jī)械臂自由度較少，而自由度較多的水下機(jī)械臂結(jié)構(gòu)尺寸較大，美國(guó)Schilling Robotics 公司的Orion4R 機(jī)械臂（見(jiàn)圖1）和Titan4 機(jī)械臂（見(jiàn)圖2）?；诎l(fā)射管的回收機(jī)械臂對(duì)靈活性要求較高，需要六自由度來(lái)確保能夠以多種姿態(tài)抓取不同位置的物體并完成回收。另外，由于旋轉(zhuǎn)關(guān)節(jié)相比移動(dòng)關(guān)節(jié)靈活性較高、占地空間較小而且工作空間更大，回收機(jī)械臂的6 個(gè)關(guān)節(jié)選擇旋轉(zhuǎn)關(guān)節(jié)。根據(jù)Pieper 法則[7]，機(jī)械臂相鄰的3 個(gè)關(guān)節(jié)軸線平行或交于同一點(diǎn)時(shí)，機(jī)械臂逆運(yùn)動(dòng)學(xué)有封閉解。因此本文設(shè)計(jì)的回收機(jī)械臂第4、第5 和第6 關(guān)節(jié)軸線交于一點(diǎn)。此時(shí)機(jī)械臂的前3 個(gè)關(guān)節(jié)控制末端夾爪的位置；后3 個(gè)關(guān)節(jié)控制末端夾爪的姿態(tài)，可視為由定位機(jī)構(gòu)和定向機(jī)構(gòu)組成。

圖1 Orion4R 機(jī)械臂Fig.1 Orion4R manipulator

圖2 Titan4 機(jī)械臂Fig.2 Titan4 manipulator

為了實(shí)現(xiàn)機(jī)械臂能夠在發(fā)射管內(nèi)伸縮的功能需求，機(jī)械臂采用可折疊方案，以保證機(jī)械臂折疊后具有較小的包絡(luò)圓直徑及足夠大的臂長(zhǎng)。目前主流的折疊結(jié)構(gòu)分為兩折疊式和三折疊式，如圖3 所示。兩折疊式機(jī)械臂的結(jié)構(gòu)方案伸展距離有限，多數(shù)應(yīng)用于工業(yè)、協(xié)作等對(duì)于工作范圍要求不大的場(chǎng)合；三折疊型機(jī)械臂有更長(zhǎng)的臂展，但在同等的包絡(luò)圓直徑下臂截面較小，承載能力不高?？紤]到回收機(jī)械臂空間尺寸的限制和承載能力，回收機(jī)械臂采用兩折疊型，連接臂從常見(jiàn)的圓筒形改為便于折疊的單板結(jié)構(gòu)。

圖3 兩折疊型與三折疊型機(jī)械臂Fig.3 Two fold and three fold manipulators

1.2 機(jī)械臂的三維模型

通過(guò)分配臂長(zhǎng)-驅(qū)動(dòng)校核-改進(jìn)臂長(zhǎng)分配的動(dòng)態(tài)優(yōu)化過(guò)程，確定回收機(jī)械臂結(jié)構(gòu)簡(jiǎn)圖如圖4 所示。選擇合適的電機(jī)、材料后用Solidworks 三維設(shè)計(jì)軟件繪制的三維模型如圖5 所示。

圖4 回收機(jī)械臂的結(jié)構(gòu)簡(jiǎn)圖Fig.4 Structure diagram of recovery manipulator

圖5 機(jī)械臂的虛擬樣機(jī)模型Fig.5 Virtual prototype model of manipulator

建立三維模型后，利用Solidworks 軟件中的質(zhì)量評(píng)估功能[8]獲取機(jī)械臂6 個(gè)連桿的質(zhì)量及動(dòng)力學(xué)參數(shù)。

質(zhì)量：46.5 kg，44.9 kg，14.2 kg，5.3 kg，3.5 kg，2.7 kg。

關(guān)節(jié)坐標(biāo)系下的連桿質(zhì)心：

質(zhì)心坐標(biāo)系下的慣性張量：

2 機(jī)械臂動(dòng)力學(xué)方程的建立

牛頓一歐拉法建立動(dòng)力學(xué)方程原理是在建立連桿間的速度傳遞關(guān)系，得到各旋轉(zhuǎn)關(guān)節(jié)的角度和角速度后，利用牛頓方程與歐拉方程求解各個(gè)連桿的慣性力與慣性力矩（式(1)和式(2)），最后通過(guò)連桿質(zhì)心處的力平衡與力矩平衡方程求解關(guān)節(jié)的力與力矩。

其中：F和N表示連桿質(zhì)心處的合力與合力矩；CI為連桿在質(zhì)心坐標(biāo)系中的慣性張量；為連桿質(zhì)心加速度，關(guān)節(jié)的角速度與角加速度。

為求解連桿質(zhì)心速度與加速度，根據(jù)正運(yùn)動(dòng)學(xué)建立的連桿間的變換關(guān)系，從連桿1 到連桿6 向外遞推：

通過(guò)連桿質(zhì)心上的力和力矩平衡方程可得：

最后，在力和力矩平衡方程式(4) 中令i=6 →1，從連桿6 向連桿1 進(jìn)行力和力矩的迭代計(jì)算，即可求出所有驅(qū)動(dòng)關(guān)節(jié)上的力與力矩。

至此，動(dòng)力學(xué)方程的推導(dǎo)已完成。輸入機(jī)械臂連桿質(zhì)量、動(dòng)力學(xué)參數(shù)（質(zhì)心、慣性張量）和關(guān)節(jié)的運(yùn)動(dòng)軌跡（角度、角速度、角加速度）即可求解關(guān)節(jié)驅(qū)動(dòng)力矩。

3 機(jī)械臂工作過(guò)程的動(dòng)力學(xué)仿真

3.1 回收過(guò)程軌跡規(guī)劃

機(jī)械臂的軌跡規(guī)劃分為笛卡爾空間規(guī)劃與關(guān)節(jié)空間規(guī)劃，規(guī)劃的方法有線性、多項(xiàng)式插值和樣條插值等[9]。回收過(guò)程屬于常規(guī)的點(diǎn)到點(diǎn)運(yùn)動(dòng)，可采用關(guān)節(jié)空間軌跡規(guī)劃；多項(xiàng)式軌跡規(guī)劃能滿足插值結(jié)果連續(xù)、平滑的要求，同時(shí)回收機(jī)械臂起點(diǎn)和終點(diǎn)的速度和加速度及路徑點(diǎn)都可確定，因此采用七次多項(xiàng)式[10]進(jìn)行插值。

在七次多項(xiàng)式下，關(guān)節(jié)角、角速度、角加速度設(shè)置如下：

七次多項(xiàng)式的工作軌跡有8 個(gè)系數(shù)，需要8 組方程來(lái)確定。在確定回收機(jī)械臂起點(diǎn)t0=0 和終點(diǎn)tm時(shí)的角度θ、角速度和角加速度后，選取2個(gè)中間點(diǎn)t1，t2的關(guān)節(jié)角即可求解工作軌跡：

回收機(jī)械臂的工作過(guò)程如圖6 所示。

圖6 機(jī)械臂回收過(guò)程Fig.6 End pose at intermediate point

根據(jù)末端坐標(biāo)系與基坐標(biāo)系的關(guān)系確定各路徑點(diǎn)的位姿矩陣，并由逆運(yùn)動(dòng)學(xué)方程轉(zhuǎn)化關(guān)節(jié)角，選取關(guān)節(jié)角最接近的一組解，結(jié)果如表1 所示。

表1 各路徑點(diǎn)關(guān)節(jié)角Tab.1 Joint angle of each path point

艦艇在回收水面或水下目標(biāo)時(shí)，往往處于超低速或靜止?fàn)顟B(tài)，因此在仿真時(shí)不考慮艦艇運(yùn)動(dòng)的影響。本次仿真設(shè)定目標(biāo)回收物位于機(jī)械臂基坐標(biāo)系右方1 m、下方1.2 m 處；回收管軸線在基坐標(biāo)系下方1 m 處，管口與機(jī)械臂底座對(duì)齊。由于機(jī)械臂在回收過(guò)程的起點(diǎn)、抓取點(diǎn)及終點(diǎn)處均為靜止?fàn)顟B(tài)，分別對(duì)抓取過(guò)程（a）～（c）和回收過(guò)程（d）～（e）的工作軌跡進(jìn)行規(guī)劃。設(shè)定起點(diǎn)和終點(diǎn)角速度、角加速度分別為0 m/s 和0 m/s2，抓取過(guò)程和回收過(guò)程的運(yùn)動(dòng)時(shí)間分別為14 s 和16 s，采用七次多項(xiàng)式插值后得到的各關(guān)節(jié)角度、末端運(yùn)動(dòng)軌跡和關(guān)節(jié)角速度如圖7～圖9 所示。

圖7 回收過(guò)程中各關(guān)節(jié)的角度變化曲線Fig.7 The angle curve of each joint

圖8 回收過(guò)程中末端運(yùn)動(dòng)軌跡Fig.8 The trajectory of the end in the recycling process

圖9 回收過(guò)程中各關(guān)節(jié)的角速度變化曲線Fig.9 Angular velocity curve of each joint

軌跡規(guī)劃的結(jié)果顯示，機(jī)械臂在0 s 時(shí)處于折疊狀態(tài)；在0～14 s 期間伸展并運(yùn)動(dòng)到目標(biāo)物處。在14 s時(shí)，機(jī)械臂的角速度及角加速度都為零，說(shuō)明機(jī)械臂已停穩(wěn)在目標(biāo)物處。14～30 s 期間機(jī)械臂重新啟動(dòng)，開(kāi)始抓取目標(biāo)物、完成目標(biāo)物軸線與回收管道軸線對(duì)齊并移動(dòng)到回收點(diǎn)處的任務(wù)。機(jī)械臂在工作過(guò)程中角度的變化曲線平滑無(wú)斷點(diǎn)，角速度無(wú)突變情況，運(yùn)動(dòng)的穩(wěn)定性基本滿足要求。

3.2 動(dòng)力學(xué)方程的求解

在得到機(jī)械臂的運(yùn)動(dòng)規(guī)律后，結(jié)合獲取的動(dòng)力學(xué)參數(shù)，通過(guò)建立的動(dòng)力學(xué)方程即可求解關(guān)節(jié)實(shí)時(shí)力矩。下面分別將浮力、目標(biāo)負(fù)載的重量和空間尺寸作為變量進(jìn)行動(dòng)力學(xué)仿真，分析其對(duì)回收機(jī)械臂動(dòng)力學(xué)特性的影響。

在考慮重力因素時(shí)，設(shè)定回收機(jī)械臂基坐標(biāo)系{0}處的質(zhì)心加速度g=9.8 m/s2，這等價(jià)于機(jī)械臂以加速度9.8 m/s2向上做加速運(yùn)動(dòng)，這個(gè)假想的加速度與重力作用在連桿上的效果相同。另外，在同時(shí)考慮重力與浮力因素時(shí)，根據(jù)回收機(jī)械臂與水的密度比，設(shè)定基坐標(biāo)系質(zhì)心加速度g1=6.6 m/s2，求解結(jié)果如圖10 所示。

從圖10 可看出，在考慮浮力時(shí)關(guān)節(jié)所需的驅(qū)動(dòng)力矩較小，這是因?yàn)楦×Φ窒嘶厥諜C(jī)械臂部分重量，減小了關(guān)節(jié)處的重力力矩。浮力對(duì)關(guān)節(jié)力矩的影響較大。例如在0 s 時(shí)，浮力可減小關(guān)節(jié)2 約40%驅(qū)動(dòng)力矩。

圖10 考慮浮力下的力矩對(duì)比曲線Fig.10 Driving torque curve considering buoyancy

此外，分別在5 kg，7 kg，10 kg 的負(fù)載重量下按照相同的運(yùn)動(dòng)軌跡進(jìn)行回收機(jī)械臂動(dòng)力學(xué)的求解，結(jié)果對(duì)比如圖11 所示。

圖11 不同負(fù)載重量下的力矩對(duì)比曲線Fig.11 Driving torque curve considering load

從圖11 可看出，回收機(jī)械臂在14 s 時(shí)抓取回收物，因此在14 s 前各個(gè)重量下的力矩曲線相同。負(fù)載的重量與關(guān)節(jié)所需驅(qū)動(dòng)力矩呈正相關(guān)關(guān)系，負(fù)載重量越大，關(guān)節(jié)所需驅(qū)動(dòng)力矩越大，負(fù)載重量不影響關(guān)節(jié)力矩曲線的趨勢(shì)。

為研究不同尺寸的回收物對(duì)驅(qū)動(dòng)力矩的影響，現(xiàn)假設(shè)手爪夾在離質(zhì)心20 cm，此時(shí)產(chǎn)生的額外力矩為2 N/m，繞x軸順時(shí)針轉(zhuǎn)動(dòng)。計(jì)算關(guān)節(jié)力矩時(shí)，需在牛頓-歐拉法中“內(nèi)推”部分末端連桿的力矩平衡中加上額外力矩再進(jìn)行迭代，得到的關(guān)節(jié)力矩對(duì)比曲線如圖12 所示。

圖12 考慮夾持位置下的力矩對(duì)比曲線Fig.12 Driving torque curve considering load size

在14 s 時(shí)回收機(jī)械臂夾取負(fù)載時(shí)姿態(tài)如圖13 所示，此時(shí)額外力矩與關(guān)節(jié)2、關(guān)節(jié)3 和關(guān)節(jié)6 的旋轉(zhuǎn)平面垂直，關(guān)節(jié)1 和關(guān)節(jié)4 的旋轉(zhuǎn)平面與額外力矩不垂直。因此關(guān)節(jié)1 和關(guān)節(jié)4 的力矩曲線在14 s 時(shí)的力矩不同，關(guān)節(jié)2、關(guān)節(jié)3 和關(guān)節(jié)6 的力矩曲線在14 s 時(shí)相同。

圖13 機(jī)械臂在抓取和回收時(shí)的姿態(tài)Fig.13 manipulator attitude in grasping and retrieving

在14 s 后，回收機(jī)械臂夾取負(fù)載并進(jìn)行姿態(tài)變換，將負(fù)載軸線與回收管的軸線對(duì)齊。此時(shí)不同夾持位置產(chǎn)生的額外力矩與關(guān)節(jié)1、關(guān)節(jié)4 和關(guān)節(jié)6 的旋轉(zhuǎn)平面垂直，與關(guān)節(jié)2、關(guān)節(jié)3 和關(guān)節(jié)5 的旋轉(zhuǎn)平面不垂直。因此夾持位置對(duì)于關(guān)節(jié)1 和關(guān)節(jié)4 的影響逐漸減小；對(duì)關(guān)節(jié)2、關(guān)節(jié)3 和關(guān)節(jié)5 的影響逐漸增大。由于6 號(hào)回轉(zhuǎn)關(guān)節(jié)與柱狀負(fù)載的軸線相垂直，因此夾持位置對(duì)6 號(hào)回轉(zhuǎn)關(guān)節(jié)的驅(qū)動(dòng)力矩?zé)o影響。

4 結(jié) 語(yǔ)

本文建立了基于發(fā)射管的回收機(jī)械臂三維模型，完成了回收軌跡規(guī)劃與動(dòng)力學(xué)仿真。軌跡規(guī)劃結(jié)果表明回收機(jī)械臂能靈活完成打撈回收的動(dòng)作；動(dòng)力學(xué)仿真結(jié)果表明回收機(jī)械臂能抓取回收規(guī)定的負(fù)載，且浮力、負(fù)載的重量和空間尺寸均對(duì)關(guān)節(jié)驅(qū)動(dòng)力矩有一定影響，在進(jìn)行動(dòng)力學(xué)分析時(shí)不可忽視。

本文進(jìn)行的動(dòng)力學(xué)仿真為后續(xù)驅(qū)動(dòng)校核與控制系統(tǒng)設(shè)計(jì)等工作提供一定的參考。研究中，回收機(jī)械臂在水中運(yùn)動(dòng)時(shí)水動(dòng)力僅考慮了浮力因素，且動(dòng)力學(xué)參數(shù)由三維模型設(shè)計(jì)軟件計(jì)算得到，沒(méi)有考慮關(guān)節(jié)摩擦和彈性以及實(shí)際加工裝配誤差等因素，因此仿真結(jié)果與真實(shí)應(yīng)用情況存在一定差異。下一步需要進(jìn)行動(dòng)力學(xué)參數(shù)辨識(shí)，并加入水阻力、附加質(zhì)量力等因素，提高動(dòng)力學(xué)仿真的精度。