楊 釗，景包睿，張 頻

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

0 引 言

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

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

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

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

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

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

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

圖1 Orion4R 機械臂Fig.1 Orion4R manipulator

圖2 Titan4 機械臂Fig.2 Titan4 manipulator

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

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

1.2 機械臂的三維模型

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

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

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

建立三維模型后，利用Solidworks 軟件中的質(zhì)量評估功能[8]獲取機械臂6 個連桿的質(zhì)量及動力學(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 機械臂動力學(xué)方程的建立

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

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

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

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

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

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

3 機械臂工作過程的動力學(xué)仿真

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

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

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

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

回收機械臂的工作過程如圖6 所示。

圖6 機械臂回收過程Fig.6 End pose at intermediate point

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

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

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

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

圖8 回收過程中末端運動軌跡Fig.8 The trajectory of the end in the recycling process

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

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

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

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

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

從圖10 可看出，在考慮浮力時關(guān)節(jié)所需的驅(qū)動力矩較小，這是因為浮力抵消了回收機械臂部分重量，減小了關(guān)節(jié)處的重力力矩。浮力對關(guān)節(jié)力矩的影響較大。例如在0 s 時，浮力可減小關(guān)節(jié)2 約40%驅(qū)動力矩。

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

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

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

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

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

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

在14 s 時回收機械臂夾取負(fù)載時姿態(tài)如圖13 所示，此時額外力矩與關(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 時的力矩不同，關(guān)節(jié)2、關(guān)節(jié)3 和關(guān)節(jié)6 的力矩曲線在14 s 時相同。

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

在14 s 后，回收機械臂夾取負(fù)載并進行姿態(tài)變換，將負(fù)載軸線與回收管的軸線對齊。此時不同夾持位置產(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)平面不垂直。因此夾持位置對于關(guān)節(jié)1 和關(guān)節(jié)4 的影響逐漸減??；對關(guān)節(jié)2、關(guān)節(jié)3 和關(guān)節(jié)5 的影響逐漸增大。由于6 號回轉(zhuǎn)關(guān)節(jié)與柱狀負(fù)載的軸線相垂直，因此夾持位置對6 號回轉(zhuǎn)關(guān)節(jié)的驅(qū)動力矩?zé)o影響。

4 結(jié) 語

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

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