黃玉平，賈龍飛，郭亞星，陶云飛，鄭繼貴

（1.北京精密機(jī)電設(shè)備研究所，北京，100076；2.航天伺服驅(qū)動(dòng)與傳動(dòng)技術(shù)實(shí)驗(yàn)室，北京，100076）

0 引 言

在諸多領(lǐng)域中，定期的監(jiān)測(cè)和維護(hù)是確保設(shè)備安全工作的重要方式，但由于此類設(shè)備具有復(fù)雜的結(jié)構(gòu)，使得可供監(jiān)測(cè)及維護(hù)作業(yè)的空間非常狹小，維修人員不易進(jìn)行操作。為了順利完成狹小空間中設(shè)備的監(jiān)測(cè)和維護(hù)，機(jī)械臂被用來代替人類進(jìn)行作業(yè)。此類機(jī)械臂需要具備穿越狹窄環(huán)境、回避障礙物的能力，并具有足夠大的靈巧操作空間。

準(zhǔn)連續(xù)型機(jī)械臂具有多個(gè)自由度，可呈現(xiàn)多種運(yùn)動(dòng)模式，可在狹小、復(fù)雜及非結(jié)構(gòu)化的環(huán)境中執(zhí)行任務(wù)[1]，且具有多目標(biāo)優(yōu)化能力，廣泛應(yīng)用于航天[2]、航空[3]、特種加工、核電站監(jiān)測(cè)[4]、醫(yī)療[5]和災(zāi)害救援[6]等領(lǐng)域。由于準(zhǔn)連續(xù)型機(jī)械臂采用繩索驅(qū)動(dòng)的方式，具有多個(gè)自由度，而且驅(qū)動(dòng)繩和臂桿之間存在摩擦力，為實(shí)現(xiàn)準(zhǔn)連續(xù)型機(jī)械臂的精確控制，故需建立準(zhǔn)確的動(dòng)力學(xué)模型。

常用的動(dòng)力學(xué)建模方法包括牛頓歐拉法[7]、Schiehlen 法[8]、Kine 法、拉格朗日法[9]等，其中拉格朗日法借助廣義坐標(biāo)，可極大簡(jiǎn)化建模過程，因此在實(shí)際工程中應(yīng)用廣泛。但如果模型中的自由度超過6 個(gè)或更多，則會(huì)大大增加拉格朗日方法的運(yùn)算量，在實(shí)際應(yīng)用中無法滿足實(shí)時(shí)控制的要求。遞歸牛頓歐拉法在多個(gè)自由度的問題上，具有較小的運(yùn)算量，故廣泛用于建立準(zhǔn)連續(xù)型機(jī)械臂的動(dòng)力學(xué)建模。

針對(duì)準(zhǔn)連續(xù)型機(jī)械臂的動(dòng)力學(xué)研究，Yuan[10]建立了一種連續(xù)型機(jī)械臂的靜力學(xué)模型，驗(yàn)證了在機(jī)械臂偏轉(zhuǎn)角度較大的情況下，摩擦力對(duì)機(jī)械臂的運(yùn)動(dòng)精度影響較大。Rone[11]利用虛功率原理建立了繩驅(qū)連續(xù)型機(jī)械臂的靜態(tài)動(dòng)力學(xué)摩擦模型，在不考慮慣性效應(yīng)的情況下推導(dǎo)了靜力平衡模型。馬曙光[12,13]利用空間算子代數(shù)方法推導(dǎo)了機(jī)械臂的動(dòng)力學(xué)方程，對(duì)動(dòng)力學(xué)模型中的繩索模型進(jìn)行改進(jìn)，通過增大仿真時(shí)間步長(zhǎng)臨界值來提高動(dòng)力學(xué)模型的數(shù)值仿真效率，可實(shí)現(xiàn)實(shí)時(shí)仿真，但未給出具體的關(guān)節(jié)力矩與驅(qū)動(dòng)繩拉力的轉(zhuǎn)換關(guān)系，并未對(duì)最優(yōu)化問題展開分析。劉天亮[14]針對(duì)繩驅(qū)動(dòng)超冗余機(jī)械臂建立了動(dòng)力學(xué)模型，并給出力矩與驅(qū)動(dòng)繩拉力的轉(zhuǎn)換關(guān)系，但建立的動(dòng)力學(xué)模型較為復(fù)雜，且利用該方法得到的驅(qū)動(dòng)繩拉力變化曲線不光滑。

準(zhǔn)連續(xù)型機(jī)械臂具有多個(gè)自由度，因此本文采用遞歸牛頓歐拉法進(jìn)行動(dòng)力學(xué)建模分析。通過確定機(jī)械臂運(yùn)動(dòng)過程中，各變量之間的轉(zhuǎn)換關(guān)系，在考慮摩擦力的前提下，建立了完整的動(dòng)力學(xué)模型。并針對(duì)關(guān)節(jié)力矩與驅(qū)動(dòng)繩拉力之間的轉(zhuǎn)換，優(yōu)化了最優(yōu)化模型中的目標(biāo)函數(shù)，采用置信域算法解決了24 根驅(qū)動(dòng)繩拉力值曲線的不光滑問題，并通過降元進(jìn)一步縮減了計(jì)算時(shí)間。

1 動(dòng)力學(xué)建模

1.1 機(jī)械結(jié)構(gòu)

本文所研究的準(zhǔn)連續(xù)型機(jī)械臂如圖1a 所示，主要包括機(jī)械臂本體結(jié)構(gòu)、驅(qū)動(dòng)機(jī)構(gòu)、導(dǎo)軌。機(jī)械臂本體結(jié)構(gòu)由8 個(gè)剛性臂桿串聯(lián)而成，臂桿之間通過十字軸萬向節(jié)連接，驅(qū)動(dòng)機(jī)構(gòu)由伺服電機(jī)、聯(lián)軸器、滾珠絲杠、滑塊組成。伺服電機(jī)帶動(dòng)絲杠轉(zhuǎn)動(dòng)來控制滑塊移動(dòng)，從而拉動(dòng)驅(qū)動(dòng)繩，進(jìn)一步可通過控制驅(qū)動(dòng)繩產(chǎn)生的位移及拉力來調(diào)整機(jī)械臂的姿態(tài)。

每一組驅(qū)動(dòng)機(jī)構(gòu)控制一根驅(qū)動(dòng)繩，與驅(qū)動(dòng)繩數(shù)量相同的24組驅(qū)動(dòng)機(jī)構(gòu)以圓周陣列的形式均布于驅(qū)動(dòng)機(jī)構(gòu)中的基座上。穿過孔ija、孔ijb、孔ijc 的3 根驅(qū)動(dòng)繩驅(qū)動(dòng)第j（j=1,2,…,8 且j≥i）個(gè)臂桿，在第i個(gè)臂桿的圓形截面上取驅(qū)動(dòng)繩穿過孔ija、孔ijb、孔ijc 的圓心與該圓形截面圓心連成的直線與Zi軸之間的夾角為該點(diǎn)的位置角度φija、φijb、φijc。如圖1c 中的孔41a、孔47a、孔43c 的位置角度分別為0、90、270 。

圖1 準(zhǔn)連續(xù)型機(jī)械臂結(jié)構(gòu)示意Fig.1 Schematic Diagram of Quais-continuous Manipulator

續(xù)圖1

1.2 運(yùn)動(dòng)學(xué)分析

準(zhǔn)連續(xù)型機(jī)械臂模型基于以下假設(shè)：

a）每節(jié)臂桿為剛體；

b）驅(qū)動(dòng)繩不可伸長(zhǎng)，且質(zhì)量和慣量很小。

圖2 為基座的本體坐標(biāo)系{O0}以及8 個(gè)臂桿的本體坐標(biāo)系{O1}～{O8}。本文建立相對(duì)于地面固定不變的世界坐標(biāo)系{Oxyz}，當(dāng)8 個(gè)臂桿的軸線在同一水平方向時(shí)，以8 個(gè)臂桿的軸線方向?yàn)閄軸方向，以重力的反方向?yàn)閆軸方向，Y軸方向可根據(jù)右手定則確定。

圖2 機(jī)械臂的D-H 坐標(biāo)系Fig.2 The D-H Coordinate System of Manipulator

第i個(gè)臂桿具有2 個(gè)自由度，分別是相對(duì)于第i-1個(gè)臂桿的偏航角αi和俯仰角θi，坐標(biāo)系{Oi-1}先沿著Xi-1軸平移L，再繞Zi-1軸旋轉(zhuǎn)αi，最后繞Yi軸旋轉(zhuǎn)θi轉(zhuǎn)換為坐標(biāo)系{Oi}?？傻玫睫D(zhuǎn)換矩陣為

在轉(zhuǎn)換矩陣的基礎(chǔ)上可推導(dǎo)出各驅(qū)動(dòng)繩長(zhǎng)度、關(guān)節(jié)角度、臂桿末端點(diǎn)等數(shù)據(jù)之間的轉(zhuǎn)換關(guān)系，即可建立準(zhǔn)連續(xù)型機(jī)械臂完整的運(yùn)動(dòng)學(xué)模型。

1.3 動(dòng)力學(xué)分析

1.3.1 單臂桿受力分析

下面在第i個(gè)臂桿的本體坐標(biāo)系{Oi}中，對(duì)其進(jìn)行受力分析，得到如圖3 所示的單臂桿受力分析圖。

圖3 單臂桿受力分析Fig.3 Force Analysis of the Single Arm

圖3 中的力和力矩都為第i個(gè)臂桿本體坐標(biāo)系{Oi}中的力和力矩，根據(jù)力平衡及力矩平衡，可得：

圖4 單臂桿簡(jiǎn)化受力分析示意及局部放大圖Fig.4 Simplified Force Analysis of the Single Arm and Local Enlarged View

合并式（2）和式（3），可得式（4）。

基座運(yùn)動(dòng)的力由直線導(dǎo)軌提供，用符號(hào)F0表示。由于有末端外力Fe及自身重量Gi的影響，需要24 根驅(qū)動(dòng)繩提供驅(qū)動(dòng)力，進(jìn)而轉(zhuǎn)換為驅(qū)動(dòng)力矩，來維持整個(gè)系統(tǒng)的平衡。fiia、fiib、fiic為第i個(gè)臂桿的3 根驅(qū)動(dòng)繩的拉力，合力為fii。fija、fijb、fijc為第i個(gè)關(guān)節(jié)處驅(qū)動(dòng)第j個(gè)臂桿的3 根驅(qū)動(dòng)繩的拉力，合力為fij。為驅(qū)動(dòng)第i+1 至第8 個(gè)臂桿的繩子在第i個(gè)關(guān)節(jié)處的合力。通過第i個(gè)關(guān)節(jié)的3×(9-i)根驅(qū)動(dòng)繩在第i個(gè)關(guān)節(jié)處的合力為fi。以上各力可通過式（8）進(jìn)行轉(zhuǎn)換。niiy、niiz為驅(qū)動(dòng)繩拉力fii等效至Yi軸、Zi軸的力矩，為等效至Yi軸、Zi軸的力矩。niix、niiy、niiz的合力矩為nii，的合力矩為，通過第i個(gè)關(guān)節(jié)的所有驅(qū)動(dòng)繩在第i個(gè)關(guān)節(jié)處的合力fi平移至坐標(biāo)系{Oi}原點(diǎn)產(chǎn)生的合力矩。以上各力矩可通過式（9）進(jìn)行轉(zhuǎn)換。

1.3.2 數(shù)學(xué)模型建立

牛頓歐拉法是一種基于牛頓方程及歐拉方程分析矢量力學(xué)的方法，可采用遞歸方法計(jì)算準(zhǔn)連續(xù)型機(jī)械臂的動(dòng)力學(xué)參數(shù)。牛頓歐拉法的遞推由兩部分組成：第1 部分為正向遞推，從基座到第1 個(gè)臂桿，然后到第2 個(gè)臂桿，依次往后，直至第8 個(gè)臂桿，由式（10）可計(jì)算每個(gè)臂桿的角速度、角加速度及質(zhì)心加速度；第2 部分為反向遞推，從第8 個(gè)臂桿到基座，由式（11）反向計(jì)算作用力和力矩及關(guān)節(jié)驅(qū)動(dòng)力矩。本文需要將關(guān)節(jié)驅(qū)動(dòng)力矩等效轉(zhuǎn)換為驅(qū)動(dòng)繩的驅(qū)動(dòng)力矩。

式中mi和Ici分別為第i個(gè)臂桿的的質(zhì)量和質(zhì)心的慣性張量。

綜上可得準(zhǔn)連續(xù)型機(jī)械臂各參數(shù)轉(zhuǎn)化示意，如圖5所示。圖5 中包括運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)2 大模塊，運(yùn)動(dòng)學(xué)模塊主要分為3 個(gè)小模塊，分別為求解旋轉(zhuǎn)矩陣及平移矩陣、求解驅(qū)動(dòng)繩在每個(gè)位置的方向以及求解驅(qū)動(dòng)繩與臂桿軸線的夾角及驅(qū)動(dòng)繩相對(duì)于臂桿的速度。動(dòng)力學(xué)模塊主要分為4 個(gè)小模塊：利用牛頓方程求解質(zhì)心加速度等參數(shù)、利用歐拉方程求解合力矩等參數(shù)、利用最優(yōu)化算法將二力矩轉(zhuǎn)換為三繩拉力、利用摩擦力公式計(jì)算繩子在不同位置處的拉力大小。

圖5 準(zhǔn)連續(xù)型機(jī)械臂各參數(shù)轉(zhuǎn)化示意Fig.5 Schematic of Conversion of Parameters

2 動(dòng)力學(xué)仿真

在建立動(dòng)力學(xué)模型的基礎(chǔ)上，借助Matlab 進(jìn)行準(zhǔn)連續(xù)型機(jī)械臂的動(dòng)力學(xué)仿真分析，得出準(zhǔn)連續(xù)型機(jī)械臂中24 根驅(qū)動(dòng)繩的拉力大小與其運(yùn)動(dòng)狀態(tài)的關(guān)系。圖6 為準(zhǔn)連續(xù)型機(jī)械臂的運(yùn)動(dòng)軌跡。為保證對(duì)應(yīng)的軌跡規(guī)劃下各臂桿的位置、速度及加速度變化曲線光滑且連續(xù)，令機(jī)械臂由水平狀態(tài)開始，圍繞x0軸線做圓周運(yùn)動(dòng)，0～3 s 由水平位置過渡到“畫圓”的位置，3～15 s 每個(gè)臂桿的末端點(diǎn)圍繞一個(gè)圓轉(zhuǎn)動(dòng)，15～18 s 由“畫圓”的位置過渡到水平位置。

圖6 準(zhǔn)連續(xù)機(jī)械臂的運(yùn)動(dòng)軌跡Fig.6 The Trajectory of Manipulator

規(guī)定第i個(gè)臂桿的起始點(diǎn)為(xi，yi，zi)，坐標(biāo)的變化規(guī)律如式（13）所示。其中T=8 s 為臂桿末端繞圓轉(zhuǎn)一周的時(shí)間，采用七次多項(xiàng)式求解g(t)，得出式（14）。

根據(jù)圖6 對(duì)應(yīng)的運(yùn)動(dòng)軌跡及前面建立的動(dòng)力學(xué)模型，仿真可得8 個(gè)關(guān)節(jié)處的所有驅(qū)動(dòng)繩的等效力矩在Yi軸、Zi軸方向上的分量大小niy、niz，如圖7 所示。初始狀態(tài)下等效力矩在Yi軸上的分量為niy(0)，從第1～8個(gè)關(guān)節(jié)分別為-64mgl、-49mgl、-36mgl、-25mgl、-16mgl、-9mgl、-4mgl、-1mgl（其中，m為臂桿的質(zhì)量；g為重力加速度；l為臂桿的長(zhǎng)度）。從圖7 中可以看出當(dāng)臂桿末端繞圓形軌跡轉(zhuǎn)動(dòng)一周時(shí)，等效力矩變化了 3個(gè)周期，等效力矩曲線光滑且連續(xù)。

圖7 各個(gè)關(guān)節(jié)處的等效力矩變化Fig.7 Equivalent Torque Variation of Each Joint

3 非光滑時(shí)變最優(yōu)化問題研究

通過動(dòng)力學(xué)模型可推導(dǎo)出各個(gè)關(guān)節(jié)處所需的等效力矩，初始狀態(tài)下在第i個(gè)關(guān)節(jié)處的所有驅(qū)動(dòng)繩的等效力矩在Yi軸上的分量niy(0)=-(9-i)2mgl，進(jìn)而可推出在第i個(gè)關(guān)節(jié)處的驅(qū)動(dòng)第i個(gè)臂桿的驅(qū)動(dòng)繩的等效力矩在Yi軸上的分量niiy(0)=(2i-17)mgl。如何將關(guān)節(jié)兩個(gè)方向的力矩niiy、niiz轉(zhuǎn)換為3 根驅(qū)動(dòng)繩上的拉力尤為重要。

由2 個(gè)方程求解3 個(gè)未知量可有無窮解，首先建立對(duì)應(yīng)的最優(yōu)化問題的數(shù)學(xué)模型，如式（15）所示。

上述問題的目標(biāo)函數(shù)J1(f)為變量fiia、fiib、fiic的線性函數(shù)，約束條件也為變量fiia、fiib、fiic的線性函數(shù)，可利用Matlab優(yōu)化工具箱中的linprog函數(shù)求解該線性規(guī)劃問題，通過仿真可得到24 根驅(qū)動(dòng)繩拉力隨時(shí)間的變化曲線，以驅(qū)動(dòng)第3 個(gè)臂桿的繩子為例，3 根驅(qū)動(dòng)繩拉力隨時(shí)間的變化曲線如圖8 所示。

圖8 線性規(guī)劃下的驅(qū)動(dòng)繩拉力隨時(shí)間的變化曲線Fig.8 The Variation Curve of the Tension of Driving Cables with Time under Linear Programming

從圖8 中可以看出驅(qū)動(dòng)繩拉力變化曲線連續(xù)但有些曲線不光滑、導(dǎo)數(shù)不連續(xù)，始終有1 根驅(qū)動(dòng)繩的拉力約為零。若按照?qǐng)D8 中的變化規(guī)律控制驅(qū)動(dòng)繩拉力，則容易拉斷驅(qū)動(dòng)繩，影響整個(gè)系統(tǒng)的運(yùn)動(dòng)平穩(wěn)性。針對(duì)上述問題，將線性目標(biāo)函數(shù)J1(f)改為如式（16）所示的非線性目標(biāo)函數(shù)J2(f)，并設(shè)定了如式（17）所示的的fiia，fiib，fiic的下限。

非線性規(guī)劃問題的求解方法有許多種，本文分別采用遺傳算法、模擬退火算法、粒子群算法、置信域算法進(jìn)行非線性規(guī)劃問題的求解，圖9 為4 種算法下的驅(qū)動(dòng)第3 個(gè)臂桿的3 根驅(qū)動(dòng)繩拉力在9～10 s 的局部放大圖。

圖9 4 種算法下的驅(qū)動(dòng)第3 個(gè)臂桿繩拉力的局部放大示意Fig.9 Local Enlarged View of the Tension of Cables Driving the Third Link under Four Algorithms

從圖9 中可以看出，若利用前3 種算法進(jìn)行最優(yōu)化求解得到的曲線相對(duì)不光滑且計(jì)算時(shí)間較長(zhǎng)，在某些地方容易陷入局部最小值，而且前3 種算法計(jì)算速度較慢且易出錯(cuò)，故不適用于計(jì)算24 根驅(qū)動(dòng)繩拉力的實(shí)時(shí)期望數(shù)值。

故本文采用置信域算法進(jìn)行求解非線性規(guī)劃問題，可將計(jì)算時(shí)間大大縮短，通過仿真可得出24 根驅(qū)動(dòng)繩拉力隨時(shí)間的變化曲線，以驅(qū)動(dòng)第3 個(gè)臂桿的3根驅(qū)動(dòng)繩為例，其拉力變化曲線如圖10 所示。

圖10 非線性規(guī)劃下的驅(qū)動(dòng)繩拉力隨時(shí)間的變化Fig.10 The Variation Curve of the Tension of Driving Cables with Time under Nonlinear Programming

分析仿真結(jié)果發(fā)現(xiàn)，任何時(shí)刻在第i個(gè)關(guān)節(jié)處驅(qū)動(dòng)第i個(gè)臂桿的3 根驅(qū)動(dòng)繩的拉力的等效力矩nii都等于本關(guān)節(jié)所有驅(qū)動(dòng)繩的拉力的等效力矩ni減去驅(qū)動(dòng)第i+1至第8 個(gè)臂桿的繩子在第i個(gè)關(guān)節(jié)處的等效合力矩，從而驗(yàn)證了動(dòng)力學(xué)模型的正確性。

對(duì)比圖8、圖10，可以看出雖然線性規(guī)劃下的驅(qū)動(dòng)繩拉力的最大值（例如圖8 中max(f33)=21.14mgl/r）小于非線性規(guī)劃下的驅(qū)動(dòng)繩拉力的最大值（例如圖10 中max(f33)=38.93mgl/r），但非線性規(guī)劃下的24 根驅(qū)動(dòng)繩的拉力變化曲線連續(xù)且光滑，在機(jī)械臂的實(shí)時(shí)運(yùn)動(dòng)過程中，可提高整個(gè)系統(tǒng)的平穩(wěn)性，有效地降低抖動(dòng)。

該優(yōu)化問題中含有3 個(gè)變量，為進(jìn)一步降低計(jì)算量、減少計(jì)算時(shí)間，可利用式（7）中的兩力矩niiy、niiz與3 根驅(qū)動(dòng)繩拉力fiia、fiib、fiic的等式關(guān)系，將3 個(gè)變量轉(zhuǎn)化為1 個(gè)變量，再采用置信域算法針對(duì)非線性目標(biāo)函數(shù)求取最優(yōu)解。最終得到的仿真結(jié)果與圖10相同，但仿真時(shí)間從108 s 縮短至12 s，大大提高了兩力矩與3 繩拉力之間的最優(yōu)化轉(zhuǎn)換速度。

4 結(jié)束語

本文利用牛頓歐拉法建立了準(zhǔn)連續(xù)型機(jī)械臂的動(dòng)力學(xué)模型，精準(zhǔn)分析了運(yùn)動(dòng)過程中24 根驅(qū)動(dòng)繩在不同位置的方向、速度，以及摩擦力對(duì)繩子拉力大小的影響，將每個(gè)關(guān)節(jié)處的驅(qū)動(dòng)繩拉力及支持力兩個(gè)未知量合并為一個(gè)未知量進(jìn)行求解，從而實(shí)現(xiàn)了機(jī)械臂各參數(shù)之間的轉(zhuǎn)化，并通過仿真驗(yàn)證了動(dòng)力學(xué)模型的正確性。

同時(shí)針對(duì)關(guān)節(jié)力矩與驅(qū)動(dòng)繩拉力之間的轉(zhuǎn)換關(guān)系，提出了一種非線性目標(biāo)函數(shù)，采用置信域算法消除了驅(qū)動(dòng)繩拉力曲線的不光滑問題，提高了整個(gè)系統(tǒng)的平穩(wěn)性，有效降低了抖動(dòng)，并通過降元進(jìn)一步縮減了計(jì)算時(shí)間，可滿足準(zhǔn)連續(xù)型機(jī)械臂的實(shí)時(shí)控制需求。

后續(xù)工作中將在本文建立的動(dòng)力學(xué)模型的基礎(chǔ)上，在準(zhǔn)連續(xù)性機(jī)械臂的控制模型中增加驅(qū)動(dòng)繩拉力的反饋信號(hào)，采用PID 或其他方法實(shí)時(shí)調(diào)節(jié)24 根驅(qū)動(dòng)繩拉力，從而提高機(jī)械臂運(yùn)動(dòng)的平穩(wěn)性及精確性。