雙柔性空間機械臂動力學建模與控制

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

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

0 引言

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

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

本文采用自然坐標法對柔性關節進行動力學建模，采用絕對節點坐標法對柔性臂桿進行動力學建模，最終得到了柔性關節柔性臂桿空間機械臂的動力學方程，為指標3 的微分代數方程組。采用5 次多項式對關節軌跡進行規劃，并通過逆動力學得到關節驅動力矩，將其作為前饋輸入;采用PID 控制策略作為反饋輸入，這就形成了前饋-反饋聯合控制策略。在二連桿柔性關節柔性桿機械臂上進行仿真，驗證了該控制策略的有效性。

1 機械臂動力學建模

如圖1 所示，為二連桿柔性關節柔性桿機械臂示意圖，包含2 個關節和2 個臂桿。

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

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

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

剛體上任意一點P 的位置矢量可表示為

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

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

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

采用自然坐標的“兩點兩矢”經典坐標建立柔性關節動力學模型。關節包含2 個殼體和1 個電機轉子，它們共軸;并且將電機轉子和殼體簡化為“扭簧-阻尼”系統。

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

如圖3 所示，在電機轉子和電機殼體上分別建立NCF 坐標，并用qm和qs分別表示電機轉子和電機殼體的NCF 坐標。由qm和qs可得到電機轉子和關節殼體的轉角θm、θs，及其角速度電機通電后驅動，帶動關節殼體轉動，關節殼體受到的力矩為

式中:Kt為扭轉剛度系數;Kd為阻尼系數;Kt、Kd一般通過實驗手段測得。本文暫不考慮電機轉子所受摩擦力等。

電機轉子受到的力矩為

式中:τ 為電機轉子受到的驅動力矩，與電流大小有關。

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

圖3 自然坐標法描述的柔性關節Fig.3 Flexible joint described by NCF

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

式中:ri、rj分別表示i 點、j 點的位置矢量;ri，x、ri，y、ri，z分別表示ri對x、y、z 的偏導;rj，x、rj，y、rj，z分別表示rj對x、y、z 的偏導;x、y、z 分別為單元的局部坐標。

圖4 絕對節點坐標法描述的三維二節點梁單元Fig.4 Three-dimensional beam element of two nodes described by ANCF

單元上任意一點P 的位置矢量為

S 為單元的形函數:

式中: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 為單元初始長度。

采用自然坐標法以及絕對節點坐標法建立的剛柔耦合多體系統動力學方程為指標3 的微分代數方程組:

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

2 前饋－反饋控制策略

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

式中:θ0、θf分別表示關節的初始角度、最終角度;tf表示運動規劃時間。

為滿足上述6 個邊界條件，采用5 次多項式對每個關節軌跡進行規劃:

由(10)式描述的6 個邊界條件，可惟一確定多項式系數ai(i=0，1，…，5);經過整理，最終得到關節軌跡為

對(12)式進行求導，可得到關節速度以及加速度:

在工程實際中，關節的最大速度是受到限制的。因此，在確定了關節的初始角度θ0以及最終角度θf后，應適當地選擇規劃時間tf，以保證關節的最大角速度不超過規定值。

由規劃好的關節軌跡，根據逆動力學，得到各個關節的驅動力矩，作為前饋輸入;采用PID 控制策略作為反饋控制，形成前饋-反饋聯合控制策略。關節控制器設計如圖5 所示。

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

3 數值仿真

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

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

圖6 末端點的X 方向位移Fig.6 Displacement in X-direction of endpoint

圖6、圖7 分別為3 s、4 s、6 s 以及9 s 四種規劃時間情況下，機械臂末端點的X 方向位移、Y 方向位移。由圖可以看出，末端點能在規劃時間內，運動至指定的目標位置，說明該控制策略能很好地實現軌跡跟蹤控制。但是由于關節柔性以及臂桿柔性的客觀存在，末端點在運動過程中產生振動現象，運動至目標位置后，殘余振動將持續。

圖7 末端點的Y 方向位移Fig.7 Displacement in Y-direction of endpoint

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

圖8 末端點的振動幅值Fig.8 Amplitude of vibration at endpoint

4 結論

圖9 末端點的X 方向振動分量Fig.9 Vibration in X-direction of endpoint

圖10 末端點的Y 方向振動分量Fig.10 Vibration in Y-direction of endpoint

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

References)

［1］于登云，潘博，孫京. 空間機械臂關節動力學建模與分析的研究進展［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］戈新生，崔瑋，趙秋玲. 剛柔性耦合機械臂軌跡跟蹤與振動抑制［J］. 工程力學，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］. 兵工學報，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］劉鋮，田強，胡海巖. 基于絕對節點坐標的多柔體系統動力學高效計算方法［J］. 力學學報，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.