多質心有限質點法及其在工業機器人動力學建模的應用

劉目珅， 張飛斌， 王天楊， 褚福磊， 程衛東， 劉佑民

(1.清華大學 機械工程系，北京 100084； 2. 北京交通大學 機械與電子控制工程學院，北京 100044； 3. 北京航天發射技術研究所，北京 100084)

工業機器人動力學建模能夠更好地獲取工作過程中的動態特性，是分析機器人末端抖動，提高定位精度的理論基礎，因此建立一個能夠精準滿足實際工況的動力學模型能為機械系統動態性能的評估和優化設計提供強有力的理論和技術支撐[1]。在工業機器人實際生產過程中，工業機器人關節處電流激勵頻率與桿件固有頻率重合導致的工業機器人末端抖動現象時有發生，這就需要考慮工業機器人桿件柔性的精準建模，為有效抑制工業機器人末端抖動提供理論機理。當前工業機器人建模方法主要有Lagrange、Newton-Euler以及Kane法，但是以上方法多基于工業機器人桿件剛性假設，若對機器人模型考慮桿件柔性進行精準建模，常常會面臨建模流程推導困難，求解效率低等問題[2]。

有限質點法[3]是一種基于向量式有限元[4-6]和固體力學，對結構進行數值分析的方法，該方法將模型在物理空間上分為有限個具有質量的質點，在時域上，將時間分解為一系列途徑單元的集合，在每一個途徑單元內結構的分析都是連續和線性的，對于質點采用基于牛頓運動學的控制方程描述運動狀態。有限質點法通過逆向運動的概念分離單元剛體位移和節點純變形，進而計算出虛擬的單元節點內力，再通過正向運動得到真實位置的單元節點內力，因此在本質上有限質點法是將幾何線性和非線性都使用簡便統一的框架進行求解[7]，在求解結構柔性變形上更有優勢。

對工業機器人建模采用實體單元結構能夠更真實的反映結構響應，有著更好的適應性和準確性。然而基于六面體單元的有限質點法[8-9]的逆向運動步驟較為繁瑣，涉及到多次投影、求模以及向量夾角計算等。有限質點法以單元任一節點作為逆向平移參考點、逆向轉動旋轉中心，并且逆向轉動參考平面未能夠充分反映單元節點信息，不能充分將單元剛體位移與單元節點純變形分離，尤其是工業機器人這種剛體大轉動-柔性變形耦合模型，單元剛體位移對求解純變形的影響更為嚴重，使部分節點內力值產生較大偏差，促使單元內其余節點內力與之對應，以達到單元內力平衡狀態，因此在整個收斂過程中，單元節點內力值波動性增加，收斂性變差，甚至會造成節點內力值發散的情況。盡管Hou等[10]已經對基于六面體單元有限質點法的逆向運動步驟進行了有效的簡化，但是未考慮逆向運動參考點和參考平面的選取對于節點內力收斂性的影響。

本文提出的多質心有限質點法與有限質點法相比采用了全新的逆向運動步驟，將平面外、內轉動修改為形式相同、步驟簡單的兩次逆向轉動；將單元平面質心作為逆向運動參考點，讓每一個單元平面質心都參與到逆向轉動所需的參考平面構造中，有效降低單元剛體位移對節點純變形求解的影響，提高了對工業機器人這種剛體大轉動-柔性耦合模型計算的收斂性。多質心有限質點法不同于傳統有限單元法以能量法和連續介質力學作為理論基礎，而是基于向量式結構力學[11]，以牛頓運動定律對質點的軌跡進行描述，對結構實現了真正意義上的離散，在本質上更適合處理工業機器人動力學問題。

基于本文方法對簡單的懸臂梁模型編寫MATLAB程序，將模型結果與理論公式以及ABAQUS軟件計算結果對比，驗證本文理論和算法的正確性。進一步將本文方法結合正運動學引入到工業機器人動力學建模中，基于MATLAB將建模程序與GUI工具箱建立聯系，構建工業機器人動力學建模軟件，將建模結果與實驗數據相對比，驗證本文方法對工業機器人動力學建模的準確性和實用性。

1 多質心有限質點法基本理論

1.1 單元節點位移

多質心有限質點法通過一系列質點和途徑單元的集合，使用基于牛頓運動學的控制方程描述質點運動，是一種真正意義上的離散。對于結構的行為可以使用一組運動方程描述

(1)

結合顯式中心差分公式，將運動方程改寫為

(2)

式中：d為單元節點位移向量；n為迭代步數；h為步長； 節點集中載荷P=F+f。

當迭代步數n=0時，也就是運動時間t=t0=0時，對應第-1步虛擬的位移向量d-1為

(3)

接下來的迭代步(n≥1)都是以式(2)作為單元節點位置計算的控制方程，每一步計算所需要的節點內力f可以根據單元節點純變形計算得出。

1.2 單元逆向運動

接下來在一個途徑單元tn～tn+1時刻內對多質心有限質點法逆向運動步驟詳細描述，對應迭代步數為第n步～第n+1步。

圖1 六面體單元運動和變形示意圖Fig.1 Motion and deformation of a hexahedral element

1.2.1 逆向平移

將tn+1時刻單元以單元質心位置矢量un+1為參考點整體逆向平移至tn時刻單元質心位置矢量un上，如圖2所示，經過逆向平移的tn+1時刻單元節點記為a′-b′-c′-d′-e′-f′-g′-h′，結點i位置矢量d′i為

圖2 逆向平移后的單元位置Fig.2 Element position after reverse translation

(4)

1.2.2 第一次逆向轉動

將經過逆向平移的單元水平方向上的3個平面質心連接，構成三角形參考平面1′-4′-6′，其法向量為Vec′1，同理，tn時刻單元的參考平面1-4-6，其法向量為Vec1。構造的單元參考平面及其法向量，如圖3所示。

圖3 單元參考平面及其法向量示意圖Fig.3 Reference plane and its normal vector of an element

Vec1與Vec′1的夾角θA及其法向量nvecA分別為

(5)

(6)

計算出第一次逆向轉動的轉動矩陣R′(-θA)

R′(-θA)=[1-cos(-θA)]V′2+sin(-θA)V′

(7)

以tn時刻的單元質心位置矢量un為旋轉中心對節點a′-b′-c′-d′-e′-f′-g′-h′進行第一次逆向轉動，如圖4所示。獲得新的單元節點位置為a″-b″-c″-d″-e″-f″-g″-h″，單元結點i位置矢量d″i為

圖4 經第一次逆向旋轉的單元位置Fig.4 Position of the element after the first reverse rotation

d″i=R′(-θA)d′i,i=a,b,c,…,g,h

(8)

1.2.3 第二次逆向轉動

將經過第一次逆向轉動的單元豎直方向上其余3個平面質心連接構造三角形參考平面2′-3′-5′，其法向量為Vec′2，同理，tn時刻單元的參考平面2-3-5，其法向量為Vec2。其余逆向轉動步驟與第一次逆向轉動相同，Vec2與Vec′2的夾角及其法向量分別為：θB和nvecB，第二次逆向轉動的轉動矩陣為：R″(-θB)。經過第二次逆向轉動的單元位置及以tn時刻單元質心為原點的局部坐標系，如圖5所示。單元節點位置為a?-b?-c?-d?-e?-f?-g?-h?，位置矢量d?i為

圖5 經第二次逆向旋轉的單元位置及其局部坐標系Fig.5 Position of the element after the second reverse rotation and its local coordinate

d?i=R″(-θB)d″i,i=a,b,c,…,g,h

(9)

(10)

需要特別說明的是，在整個逆向運動過程中，逆向平移是以單元質心為參考點整體逆向平移，逆向轉動中單元節點都是繞同一旋轉向量旋轉相同的角度，保證了六面體單元的各個節點之間不會發生相對位移。將本文推導過程與有限質點法進行對比，主要差別如表1所示。由表1可知，本文方法有效降低了計算的復雜度，簡化了計算流程。

表1 本文方法與有限質點法復雜度對比Tab.1 The complexity of the proposed method is compared with that of the finite particle method

1.3 單元節點內力

單元節點內力通過單元節點純變形進行計算，計算方法可參考有限單元法[12]。如圖5所示，以tn時刻單元質心作為原點建立局部坐標系O′-x1y1z1，坐標軸方向與全局坐標系保持一致，將全局坐標系下的單元節點位置矢量轉化到局部坐標系中

(11)

對時刻的單元建立形函數，建立方式和表達式形式與傳統有限元方法相同，形函數坐標系記為O″-rst，如圖6所示。

圖6 八節點六面體單元形函數坐標系Fig.6 Eight-node hexahedral element shape function coordinate

八節點六面體單元的形函數Ni表達式為

(12)

式中：r,s,t分別為形函數表達式參數；ri=±1，si=±1，ti=±1為形函數坐標系內節點i的坐標，i=a,b,c,…,g,h。

將局部坐標系內坐標轉換到形函數坐標系中，坐標系的轉換通過雅可比矩陣J來實現

(13)

對雅可比矩陣求逆，得到形函數關于局部坐標的導數為

(14)

(15)

(16)

利用單元變形描述的虛功與應力應變描述的虛功相等這一原理建立等式

(17)

式中：Va為形函數積分參數； δ為變分符號。

(18)

(19)

式中，R′(θA)和R″(θB)分別為第一次和第二次正向運動轉動矩陣。

1.4 步 長

對結合顯示中心差分推導出的控制方程來說，步長的大小尤為重要，一般在問題的分析中，對步長的要求主要有兩方面：①物理行為方面，在結構分析過程中，使用足夠的數據點來滿足計算精度的要求；②數值計算方面，差分公式是函數微分的一種近似計算，必然有一定誤差，所以要避免迭代計算中的誤差累計造成的數據發散。

根據上述對步長的要求得知，步長必須要小于一個步長臨界值hd，才能得到一個收斂、準確的計算結果。然而模型類型不同其步長臨界值也各異，并且精確求解臨界步長也是困難的。丁承先等以簡單的桿單元為對象，提出了一種有效的估算臨界步長值的方法。

1.5 質量和加速度

六面體單元質量分布遵循一個原則：將單元質量均分到各個節點上，再采用集成的方式就可得到單元節點質量，此時的單元節點只考慮質量，忽略節點尺寸[13]。

單元節點的加速度同樣可以通過差分的形式給出

(20)

2 結構算例

2.1 懸臂梁模型

為驗證本文提出的多質心有限質點法的準確性，現以簡單的懸臂梁模型為例，基于本文方法使用MATLAB軟件進行編程，對結構的變形進行分析。

使用六面體單元對懸臂梁模型劃分網格，網格數量為320個，單元節點為525個，如圖7所示。懸臂梁長、寬、高分別為0.5 m，0.1 m和0.1 m。材料類型為彈性，彈性模量為E=206 GPa，密度為Rho=7 800 kg/m3，泊松比為rp=0.29。分析時間TE=0.05 s，步長為h=1×10-6s，末端集中載荷P=-3 N，采用斜坡-平臺方式緩慢加載，加載時間rtime=0.005 s，并使用阻尼系數收斂計算結果，阻尼系數為zeta=100。當前模型變形為小變形，所以依然適用于材料力學理論公式

圖7 懸臂梁六面體單元模型Fig.7 Hexahedral elements of cantilever model

(21)

式中：L為懸臂梁的長；I為截面慣性矩， 對于正方形截面，I=B4/12，B為正方形截面的邊長。

沿懸臂梁中線各節點的豎直方向的位移結果，如圖8所示。橫坐標為中線各節點與固定端的距離，縱坐標為中線各節點豎直方向上產生的位移，可以看出豎直方向上位移與材料力學理論公式及采用C3D8R六面體單元的ABAQUS軟件的計算結果基本保持一致。

圖8 懸臂梁中線各節點豎直方向位移結果Fig.8 Vertical displacement results of each node in the middle line of cantilever beam

本文方法和有限質點法對懸臂梁自由端中心節點豎直方向位移變化過程的收斂性對比，如圖9所示。以理論公式結果為標準值，在相同網格數量前提下，本文方法、有限質點法、理論公式以及ABAQUS軟件的自由端中心節點豎向方向位移的對比結果，如表2所示。

表2 結果對比Tab.2 Results contrast

圖9 本文方法和有限質點法對懸臂梁自由端中心 節點豎直方向位移變化過程的收斂性對比Fig.9 Convergence comparison between the method in this paper and the finite mass method on the vertical displacement change process of the center node of the free end of the cantilever beam

由以上結果證明本文方法有著良好的準確性和收斂性。

2.2 工業機器人動力學模型

首先對建模對象RB08A3機器人進行實驗，發現存在明顯抖動現象，在工業機器人連桿4末端安裝加速度傳感器，測得運動過程中執行末端加速度數據，并記錄J2，J3關節電流變化值，如圖10所示。

圖10 工業機器人及加速度傳感器布置位置Fig.10 Industrial robot and acceleration sensor placement

為分析抖動現象的原因，首先將關節電流數據分別做頻域分析，結果如圖11所示。

圖11 關節電流頻域圖Fig.11 Frequency domain diagram of joint current

對實驗測得的加速度數據進行頻域分析，可知機器人抖動是因為在13.5 Hz和27.5 Hz左右的電流激勵頻率和工業機器人固有頻率重合。下面采用多質心有限質點法對工業機器人進行動力學建模，建模步驟如圖12所示。

圖12 工業機器人動力學建模流程圖Fig.12 Flow chart of industrial robot dynamics modeling

工業機器人結構基本參數，如表3所示。使用六面體單元對機器人劃分網格，建立的物理模型如圖13所示，所需單元個數為960。

表3 RB08A3機器人結構參數Tab.3 RB08A3 robot structure parameters

圖13 工業機器人物理模型Fig.13 Physical model of industrial robot

設定機器人動力學建模基本參數： 運動時間為 2.0 s，迭代步長為1/3 125 s，單元材料密度為7.8×103，彈性模量為206 GPa，泊松比為0.29，根據單元的連接類型，確定單元節點質量，運動狀態為J2和J3關節轉動，其余關節保持固定。

為方便計算，引入固定的整體坐標系和隨機械臂旋轉的坐標系[14]，結合模型的單元與節點信息，以及關節輸出角度變化值，根據正運動學求解出機器人模型剛體運動的位置，然后對模型使用多質心有限質點法進行分析，得到執行末端變形量。進而將變形量疊加到執行末端，重復上述步驟的計算，直至分析時間達到運動時間。使用式(20)可以根據執行末端位置求解出末端加速度變化。

基于MATLAB軟件將工業機器人動力學建模程序與MATLAB軟件中GUI工具箱連接，搭建工業機器人動力學建模軟件，軟件主界面如圖14所示。

圖14 軟件主界面Fig.14 Main interface of software

進而對仿真測得的執行末端z和y方向上的加速度數據頻域分析，并與實驗數據對比，對比結果如圖15和圖16所示。

圖15 建模與實驗在z方向上的加速度頻域圖Fig.15 Modeling and experiments in the z direction acceleration frequency domain diagram

圖16 建模與實驗在y方向上的加速度頻域圖Fig.16 Modeling and experiments in the y direction acceleration frequency domain diagram

在z方向上的建模結果具有13.5 Hz和28.5 Hz，相對實驗結果14.0 Hz和27.5 Hz的誤差分別為-3.6%和3.6%。在y方向上的建模結果具有13.5 Hz，相對實驗結果14.0 Hz的誤差為-3.6%。根據以上結果可以得知，建模與實驗結果在低階固有頻率基本重合，高階能量趨勢基本相同。

3 結 論

本文提出多質心有限質點法，將單元質心不僅作為逆向平移的參考點，也作為逆向轉動的旋轉中心，使每個單元平面都參與到參考平面的構造中，有效減弱了單元剛體位移對節點純變形求解的影響，進而解決了工業機器人這種剛體大轉動-柔性變形耦合模型中因單元節點純變形計算偏差，造成的節點內力值收斂性差的問題。

與傳統有限質點法相比，多質心有限質點法的兩次逆向轉動，除選取的參考平面節點不同以外，都是采用簡單的平面外轉動的旋轉方式，大幅度減少了逆向運動過程中的向量求模、叉乘、投影等步驟次數，有效簡化了計算流程，使得整體框架更為統一。

對簡單懸臂梁模型使用多質心有限質點法自編程序建模分析，并將分析結果與有限質點法、理論公式和ABAQUS軟件分析結果對比，結果表明本文方法有著良好的準確性和收斂性。

將多質心有限質點法結合正運動學對工業機器人動力學建模，通過MATLAB軟件將建模程序與GUI圖形化界面結合，構造基于多質心有限質點法的工業機器人動力學建模軟件，并將建模結果與實驗結果相對比，結果表明使用本文方法的建模結果能夠有效模擬出工業機器人因電流激勵頻率與固有頻率重合產生的抖動頻率。