雙柔性空間機(jī)械臂動(dòng)力學(xué)建模與控制

楊永泰，榮吉利，李健，劉賓，胡成威

(1.北京理工大學(xué) 宇航學(xué)院，北京100081;2.廣西科技大學(xué) 汽車與交通學(xué)院，廣西 柳州545006;3.中國空間技術(shù)研究院，北京100094)

0 引言

空間機(jī)械臂作為航天器上一種重要的工具，可以完成釋放、回收衛(wèi)星以及空間站的在軌裝配、維修等各種任務(wù)，并可以作為航天員出艙工作的輔助工具，甚至替代航天員的部分工作，從而提高航天員的艙外工作能力及在軌操作的安全性［1］。空間機(jī)械臂的機(jī)械部分主要包括關(guān)節(jié)和臂桿。關(guān)節(jié)主要包括:電機(jī)、傳動(dòng)裝置、運(yùn)動(dòng)軸系、傳感器等;臂桿的制造往往采用輕質(zhì)材料，例如碳纖維等。關(guān)節(jié)柔性以及臂桿柔性，都是不可忽略的。為了研究空間機(jī)械臂的動(dòng)力學(xué)特性并且實(shí)現(xiàn)精確控制，建立包含關(guān)節(jié)柔性以及臂桿柔性的空間機(jī)械臂精確動(dòng)力學(xué)模型，具有十分重要的意義。

在機(jī)械臂柔性關(guān)節(jié)建模方面，Spong 等［2］首次對(duì)其進(jìn)行簡化，提出“轉(zhuǎn)子-扭簧系統(tǒng)”模型。在此基礎(chǔ)上，Bahrami 等［3］將關(guān)節(jié)假設(shè)為“線性扭簧-阻尼”模型。于登云等［1］對(duì)空間機(jī)械臂關(guān)節(jié)動(dòng)力學(xué)建模方法的發(fā)展過程和研究成果進(jìn)行了總結(jié)。在柔性臂桿建模方面，大多數(shù)研究者［4-5］均采用拉格朗日方程以及假設(shè)模態(tài)法進(jìn)行建模。但是，該方法首先需要通過計(jì)算臂桿的模態(tài)，然后進(jìn)行模態(tài)截?cái)?并且，假設(shè)模態(tài)法僅僅適用于臂桿的小變形。將描述柔性構(gòu)件的絕對(duì)節(jié)點(diǎn)坐標(biāo)方法［6］與描述剛體的自然坐標(biāo)方法［7］相結(jié)合，可以形成研究剛-柔多體系統(tǒng)動(dòng)力學(xué)的絕對(duì)坐標(biāo)方法［8-9］。利用絕對(duì)坐標(biāo)方法推導(dǎo)得到的剛-柔多體系統(tǒng)動(dòng)力學(xué)方程的質(zhì)量矩陣為常數(shù)矩陣，不存在科氏力和離心力，并且還可以得到簡潔的系統(tǒng)約束方程表達(dá)式，不僅適用于柔性體的小變形問題，也能得到大變形的精確結(jié)果。

本文采用自然坐標(biāo)法對(duì)柔性關(guān)節(jié)進(jìn)行動(dòng)力學(xué)建模，采用絕對(duì)節(jié)點(diǎn)坐標(biāo)法對(duì)柔性臂桿進(jìn)行動(dòng)力學(xué)建模，最終得到了柔性關(guān)節(jié)柔性臂桿空間機(jī)械臂的動(dòng)力學(xué)方程，為指標(biāo)3 的微分代數(shù)方程組。采用5 次多項(xiàng)式對(duì)關(guān)節(jié)軌跡進(jìn)行規(guī)劃，并通過逆動(dòng)力學(xué)得到關(guān)節(jié)驅(qū)動(dòng)力矩，將其作為前饋輸入;采用PID 控制策略作為反饋輸入，這就形成了前饋-反饋聯(lián)合控制策略。在二連桿柔性關(guān)節(jié)柔性桿機(jī)械臂上進(jìn)行仿真，驗(yàn)證了該控制策略的有效性。

1 機(jī)械臂動(dòng)力學(xué)建模

如圖1 所示，為二連桿柔性關(guān)節(jié)柔性桿機(jī)械臂示意圖，包含2 個(gè)關(guān)節(jié)和2 個(gè)臂桿。

圖1 二連桿柔性機(jī)械臂Fig.1 Two-link flexible manipulator

自然坐標(biāo)法中存在多種不同的廣義坐標(biāo)選取方法［7］。如圖2 所示，選取剛體上2 個(gè)固定點(diǎn)的位置矢量和2 個(gè)單位矢量作為廣義坐標(biāo)，稱之為“兩點(diǎn)兩矢”坐標(biāo)形式，這是最經(jīng)典的自然坐標(biāo)形式，該剛體具有12 個(gè)廣義坐標(biāo):

式中:ri、rj分別表示剛體上i 點(diǎn)、j 點(diǎn)的位置矢量;u、v 分別表示固結(jié)于剛體上的單位矢量。

剛體上任意一點(diǎn)P 的位置矢量可表示為

式中:C=［(1 -c1)I3c1I3c2I3c3I3］，為單元的形函數(shù)，c1、c2、c3為局部坐標(biāo)分量，I3為3 階單位矩陣。

通過虛功原理，可得到自然坐標(biāo)法(NCF)描述的剛體常數(shù)質(zhì)量矩陣:

式中:ρ 表示剛體的密度;V 表示剛體的體積。

采用自然坐標(biāo)的“兩點(diǎn)兩矢”經(jīng)典坐標(biāo)建立柔性關(guān)節(jié)動(dòng)力學(xué)模型。關(guān)節(jié)包含2 個(gè)殼體和1 個(gè)電機(jī)轉(zhuǎn)子，它們共軸;并且將電機(jī)轉(zhuǎn)子和殼體簡化為“扭簧-阻尼”系統(tǒng)。

圖2 自然坐標(biāo)描述的空間剛體Fig.2 Space rigid body described by NCF

如圖3 所示，在電機(jī)轉(zhuǎn)子和電機(jī)殼體上分別建立NCF 坐標(biāo)，并用qm和qs分別表示電機(jī)轉(zhuǎn)子和電機(jī)殼體的NCF 坐標(biāo)。由qm和qs可得到電機(jī)轉(zhuǎn)子和關(guān)節(jié)殼體的轉(zhuǎn)角θm、θs，及其角速度電機(jī)通電后驅(qū)動(dòng)，帶動(dòng)關(guān)節(jié)殼體轉(zhuǎn)動(dòng)，關(guān)節(jié)殼體受到的力矩為

式中:Kt為扭轉(zhuǎn)剛度系數(shù);Kd為阻尼系數(shù);Kt、Kd一般通過實(shí)驗(yàn)手段測得。本文暫不考慮電機(jī)轉(zhuǎn)子所受摩擦力等。

電機(jī)轉(zhuǎn)子受到的力矩為

式中:τ 為電機(jī)轉(zhuǎn)子受到的驅(qū)動(dòng)力矩，與電流大小有關(guān)。

面對(duì)這份要求和重?fù)?dān)，研究院黨委結(jié)合所面臨的工作環(huán)境，由班子成員帶隊(duì)深入各研究所與機(jī)關(guān)科室，開展了“訪基層、講形勢、聚人心、促工作”集中宣講，宣講形勢任務(wù)和上級(jí)政策要求，了解職工思想動(dòng)態(tài)；編發(fā)了形勢任務(wù)教育宣傳材料，開展“跨越重大關(guān)口，決勝扭虧為盈”形勢任務(wù)教育；使大家充分認(rèn)清形勢，明確研究院在分公司的定位，增強(qiáng)榮譽(yù)感，強(qiáng)化擔(dān)當(dāng)意識(shí)，在全院干部員工心目中樹立和固化“公司增效責(zé)任在我，公司發(fā)展我來擔(dān)責(zé)”的責(zé)任心，鼓勵(lì)員工以不服輸、爭口氣的勁頭，把基礎(chǔ)研究工作做得更扎實(shí)、更細(xì)致、更務(wù)實(shí)，努力打好翻身仗。

圖3 自然坐標(biāo)法描述的柔性關(guān)節(jié)Fig.3 Flexible joint described by NCF

柔性臂桿采用Shabana 等［10］與Yakoub 等［11］提出的基于絕對(duì)節(jié)點(diǎn)坐標(biāo)的三維二節(jié)點(diǎn)梁單元進(jìn)行離散，如圖4 所示。該梁單元有2 個(gè)節(jié)點(diǎn)，每個(gè)節(jié)點(diǎn)上有1 個(gè)位置矢量和3 個(gè)斜率矢量，單元共有24 個(gè)廣義坐標(biāo):

式中:ri、rj分別表示i 點(diǎn)、j 點(diǎn)的位置矢量;ri，x、ri，y、ri，z分別表示ri對(duì)x、y、z 的偏導(dǎo);rj，x、rj，y、rj，z分別表示rj對(duì)x、y、z 的偏導(dǎo);x、y、z 分別為單元的局部坐標(biāo)。

圖4 絕對(duì)節(jié)點(diǎn)坐標(biāo)法描述的三維二節(jié)點(diǎn)梁單元Fig.4 Three-dimensional beam element of two nodes described by ANCF

單元上任意一點(diǎn)P 的位置矢量為

S 為單元的形函數(shù):

式中:I3為3 階單位矩陣;s1=1 -3ξ2+2ξ3，s2=ξ -2ξ2+ξ3，s3=η -ξη，s4= ζ - ξζ，s5=3ξ2-2ξ3，s6=-ξ2+ξ3，s7=ξη，s8=ξζ，ξ =x/l，η =y/l，ζ =z/l，x∈［0 l］，l 為單元初始長度。

采用自然坐標(biāo)法以及絕對(duì)節(jié)點(diǎn)坐標(biāo)法建立的剛?cè)狁詈隙囿w系統(tǒng)動(dòng)力學(xué)方程為指標(biāo)3 的微分代數(shù)方程組:

式中:M 為系統(tǒng)質(zhì)量矩陣;q 為系統(tǒng)廣義坐標(biāo);C 為系統(tǒng)約束方程;Cq為系統(tǒng)約束方程對(duì)廣義坐標(biāo)的Jacobi 矩陣;λ 為拉格朗日乘子;Q 為系統(tǒng)廣義外力;F 為系統(tǒng)廣義彈性力。絕對(duì)節(jié)點(diǎn)坐標(biāo)法描述的三維二節(jié)點(diǎn)梁單元彈性力及其Jacobi 的計(jì)算，參見文獻(xiàn)［12 -13］。上述各項(xiàng)按照傳統(tǒng)有限元方法的單元組裝策略進(jìn)行單元組裝。

2 前饋－反饋控制策略

在工程上，往往采用多項(xiàng)式對(duì)關(guān)節(jié)軌跡進(jìn)行插值，包括3 次多項(xiàng)式以及5 次多項(xiàng)式等。若關(guān)節(jié)的位置速度 以及加速度 的初始狀態(tài)(t =0)以及結(jié)束狀態(tài)(t=tf)為

式中:θ0、θf分別表示關(guān)節(jié)的初始角度、最終角度;tf表示運(yùn)動(dòng)規(guī)劃時(shí)間。

為滿足上述6 個(gè)邊界條件，采用5 次多項(xiàng)式對(duì)每個(gè)關(guān)節(jié)軌跡進(jìn)行規(guī)劃:

由(10)式描述的6 個(gè)邊界條件，可惟一確定多項(xiàng)式系數(shù)ai(i=0，1，…，5);經(jīng)過整理，最終得到關(guān)節(jié)軌跡為

對(duì)(12)式進(jìn)行求導(dǎo)，可得到關(guān)節(jié)速度以及加速度:

在工程實(shí)際中，關(guān)節(jié)的最大速度是受到限制的。因此，在確定了關(guān)節(jié)的初始角度θ0以及最終角度θf后，應(yīng)適當(dāng)?shù)剡x擇規(guī)劃時(shí)間tf，以保證關(guān)節(jié)的最大角速度不超過規(guī)定值。

由規(guī)劃好的關(guān)節(jié)軌跡，根據(jù)逆動(dòng)力學(xué)，得到各個(gè)關(guān)節(jié)的驅(qū)動(dòng)力矩，作為前饋輸入;采用PID 控制策略作為反饋控制，形成前饋-反饋聯(lián)合控制策略。關(guān)節(jié)控制器設(shè)計(jì)如圖5 所示。

圖5 前饋-反饋聯(lián)合控制策略Fig.5 Union control scheme of feedforward-feedback

3 數(shù)值仿真

如圖1 所示的二連桿機(jī)械臂，忽略重力影響，并假設(shè)基座的位置和姿態(tài)都是固定不變的。初始時(shí)刻以及結(jié)束時(shí)刻，關(guān)節(jié)的速度以及加速度均為0，關(guān)節(jié)角度分別為Θ0=［0° 0°］T，Θtf=［5° 10°］T.

臂桿均為空心圓柱，長度均為L =3.5 m，內(nèi)徑r=6 cm，外徑R=8 cm;材料均為碳纖維材料，密度ρ=2.0 ×103kg/m3，彈性模量E =80 GPa，泊松比ν=0.3;關(guān)節(jié)剛度系數(shù)均為K=1.2 ×105N·m/(°);關(guān)節(jié)阻尼影響較小，暫不考慮;規(guī)劃時(shí)間tf分別取3 s、4 s、6 s 以及9 s 四種情況，仿真時(shí)間均設(shè)置為T=12 s.

圖6 末端點(diǎn)的X 方向位移Fig.6 Displacement in X-direction of endpoint

圖6、圖7 分別為3 s、4 s、6 s 以及9 s 四種規(guī)劃時(shí)間情況下，機(jī)械臂末端點(diǎn)的X 方向位移、Y 方向位移。由圖可以看出，末端點(diǎn)能在規(guī)劃時(shí)間內(nèi)，運(yùn)動(dòng)至指定的目標(biāo)位置，說明該控制策略能很好地實(shí)現(xiàn)軌跡跟蹤控制。但是由于關(guān)節(jié)柔性以及臂桿柔性的客觀存在，末端點(diǎn)在運(yùn)動(dòng)過程中產(chǎn)生振動(dòng)現(xiàn)象，運(yùn)動(dòng)至目標(biāo)位置后，殘余振動(dòng)將持續(xù)。

圖7 末端點(diǎn)的Y 方向位移Fig.7 Displacement in Y-direction of endpoint

圖8為末端點(diǎn)的振動(dòng)幅值δ，圖9、圖10 分別為振動(dòng)幅值在X 方向、Y 方向的分量δX、δY. 由圖可以看出，規(guī)劃時(shí)間tf=3 s 的情況下，末端點(diǎn)運(yùn)動(dòng)至指定的目標(biāo)位置，但是，運(yùn)動(dòng)過程中出現(xiàn)較大的振動(dòng)，并且運(yùn)動(dòng)結(jié)束后仍有較大的殘余振動(dòng)。這是由于規(guī)劃時(shí)間較短，而規(guī)劃時(shí)間又是前饋輸入的重要因素，一旦引起振動(dòng)，比較難以消除。隨著規(guī)劃時(shí)間的增加，運(yùn)動(dòng)過程以及運(yùn)動(dòng)結(jié)束后，振動(dòng)的幅值越來越小，而且頻率也越來越低。當(dāng)規(guī)劃時(shí)間tf=9 s 時(shí)，末端點(diǎn)的振動(dòng)相比于規(guī)劃時(shí)間tf=3 s 有了非常明顯的減弱，振動(dòng)幅值由20.4 mm 減弱至2.7 mm，而振動(dòng)頻率也有明顯的降低。這說明規(guī)劃時(shí)間作為前饋輸入的一項(xiàng)重要因素，在工程實(shí)際允許的條件下，選擇合適規(guī)劃時(shí)間，能有效地抑制住機(jī)械臂的振動(dòng)。

圖8 末端點(diǎn)的振動(dòng)幅值Fig.8 Amplitude of vibration at endpoint

4 結(jié)論

圖9 末端點(diǎn)的X 方向振動(dòng)分量Fig.9 Vibration in X-direction of endpoint

圖10 末端點(diǎn)的Y 方向振動(dòng)分量Fig.10 Vibration in Y-direction of endpoint

本文采用自然坐標(biāo)法對(duì)柔性關(guān)節(jié)進(jìn)行動(dòng)力學(xué)建模，采用絕對(duì)節(jié)點(diǎn)坐標(biāo)法對(duì)柔性臂桿進(jìn)行動(dòng)力學(xué)建模，最終得到了雙柔性機(jī)械臂的動(dòng)力學(xué)方程，為指標(biāo)3 的微分代數(shù)方程組。既考慮了關(guān)節(jié)柔性，又考慮了臂桿的柔性，動(dòng)力學(xué)模型更精確;相比于僅適用于臂桿小變形的假設(shè)模態(tài)法，該方法不僅適用于小變形問題，也能精確描述大變形問題。采用5 次多項(xiàng)式對(duì)關(guān)節(jié)軌跡進(jìn)行規(guī)劃，并通過逆動(dòng)力學(xué)得到關(guān)節(jié)驅(qū)動(dòng)力矩，將其作為前饋輸入;采用PID 控制策略作為反饋輸入，這就形成了前饋-反饋聯(lián)合控制策略。在二連桿雙柔性機(jī)械臂上進(jìn)行數(shù)值仿真，仿真結(jié)果表明:該前饋-反饋控制策略能很好地實(shí)現(xiàn)軌跡跟蹤控制;軌跡規(guī)劃時(shí)間作為前饋輸入的一項(xiàng)重要因素，對(duì)機(jī)械臂的振動(dòng)影響較大，選擇適當(dāng)?shù)囊?guī)劃時(shí)間，能顯著地減弱機(jī)械臂的振動(dòng)。

References)

［1］于登云，潘博，孫京. 空間機(jī)械臂關(guān)節(jié)動(dòng)力學(xué)建模與分析的研究進(jìn)展［J］. 航天器工程，2010，19(2):1 -10.YU Deng-yun，PAN Bo，SUN Jing. A literature review on dynamic modeling and analysis of the joints in space manipulator［J］.Spacecraft Engineering，2010，19(2):1 -10. (in Chinese)

［2］Spong M W. Modeling and control of elastic joint robots［J］. Journal of Dynamic Systems，Measurement and Control，1987，109(1):310 -319.

［3］Bahrami M，Rahi A. Tip dynamic response of elastic joint manipulators subjected to a stochastic base excitation［J］. JSME International Journal:Series C，2003，46(4):1502 -1508.

［4］戈新生，崔瑋，趙秋玲. 剛?cè)嵝择詈蠙C(jī)械臂軌跡跟蹤與振動(dòng)抑制［J］. 工程力學(xué)，2005，22(6):188 -191.GE Xin-sheng，CUI Wei，ZHAO Qiu-ling. Trajectory tracking control and vibration suppression of rigid flexible manipulators［J］.Engineering Mechanics，2005，22(6):188 -191. (in Chinese)

［5］梁捷，陳力. 柔性空間機(jī)械臂末端運(yùn)動(dòng)及柔性振動(dòng)的模糊自適應(yīng)補(bǔ)償控制［J］. 兵工學(xué)報(bào)，2011，32(1):45 -57.LIANG Jie，CHEN Li. Fuzzy logic adaptive compensation control of end-effect motion and flexible vibration for space-based flexible manipulator［J］. Acta Armamentarii，2011，32(1):45 -57. (in Chinese)

［6］Shabana A A. An absolute nodal coordinates formulation for the large rotation and deformation analysis of flexible bodies，No.MBS96-1-UIC［R］. US:University of Illinois at Chicago，1996.

［7］García De Jalón J，Bayo E. Kinematic and dynamic simulation of multibody systems:the real-time challenge［M］. New York:Springer，1994.

［8］García-Vallejo D，Escalona J L，Mayo J，et al. Describing rigidflexible multibody system using absolute coordinates［J］. Nonlinear Dynamics，2003，34:75 -94.

［9］García-Vallejo D，Mayo J，Escalona J L，et al. Three-dimensional formulation of rigid-flexible multibody systems with flexible beam elements［J］. Multibody System Dynamics，2008，20(1):1 -28.

［10］Shabana A A，Yakoub R Y. Three dimensional absolute nodal coordinate formulation for beam elements:theory［J］. ASME Journal of Mechanical Design，2001，123:606 -613.

［11］Yakoub R Y，Shabana A A. Three dimensional absolute nodal coordinate formulation for beam elements:implementation and applications［J］. ASME Journal of Mechanical Design，2001，123:614 -621.

［12］García-Vallejo D，Mayo J，Escalona J L，et al. Efficient evaluation of the elastic forces and the Jacobian in the absolute nodal coordinate formulation［J］. Nonlinear Dynamics，2004，35:313-329.

［13］劉鋮，田強(qiáng)，胡海巖. 基于絕對(duì)節(jié)點(diǎn)坐標(biāo)的多柔體系統(tǒng)動(dòng)力學(xué)高效計(jì)算方法［J］. 力學(xué)學(xué)報(bào)，2010，42(6):1197 -1205.LIU Cheng，TIAN Qiang，HU Hai-yan. Efficient computational method for dynamics of flexible multibody systems based on absolute nodal coordinate［J］. Chinese Journal of Theoretical and Applied Mechanics，2010，42(6):1197 -1205. (in Chinese)

［14］Arnold M，Brüls O. Convergence of the generalized-a scheme for constrained mechanical systems［J］. Multibody System Dynamics，2007，18:185 -202.

［15］Bottasso C L，Dopico D，Trainelli L. On the optimal scaling of index three DAEs in multibody dynamics［J］. Multibody System Dynamics，2008，19(1):3 -20.