七自由度冗余機械臂運動學和動力學分析及仿真

張澤璽，熊根良，張華，許寶陽，包俊陽

（201620 上海市 上海工程技術大學 上海市大型構件智能制造機器人技術協同創新中心）

0 引言

串聯機械臂在工業領域逐漸大放異彩，特別是以六自由度機械臂為代表的非冗余機械臂技術已漸趨成熟。隨著工業技術的高速發展，機械臂的工作場景變得愈加復雜，人們對機械臂的工作性能也有了更高的要求。傳統的低自由度非冗余機械臂漸漸難以勝任，因此需要加強對以七自由度機械臂為代表的冗余機械臂的研究。冗余機械臂最大的優勢在于其具有多余的自由度，因而相較于非冗余機械臂有著更高的靈活性、容錯性與可靠性，且利用其自運動的特性可以實現避障[1]、避奇異點[2]、避免關節限位、優化關節力矩[3]、適應人類動作[4]等性能指標，但隨著機械臂的自由度數目的增加，機械臂的運動學與動力學求解的難度也隨之提高[5]，因此對冗余機械臂進行深入的研究是非常必要的。

國內外學者對七自由度冗余機械臂的運動學及動力學研究取得許多進展。Shimizu 等[6]對七自由度機械臂進行逆運動學研究，證明將臂形角作為冗余參數可完全反應機械臂的冗余性，該方法的幾何意義明顯，缺點在于其普適性低，僅適用于機械臂肩部和腕部均為等效球關節的構型；趙京等[7]基于臂形角參數化方法，以最小勢能指標為理論依據可以確定機械臂唯一的肘部位置，繼而完成了七自由度機械臂的逆運動學求解；徐俊虎等[8]對“骨科手術輔助機器人系統”中的無偏置七自由度冗余機械臂進行運動學逆解時，提出了一種以減小最大關節位移為控制優化目標的優化方法，利用該方法可顯著提升機械臂的作業效率；祖迪等[9]將梯度投影法與關節角參數化方法相結合求解冗余機械臂的逆運動學問題，在梯度投影法得到一個優化近似解的基礎上，再利用關節角參數化方法得到一個更為精確的解；Fei 等[10]提出將加權最小范數法與梯度投影法相結合的思路，與僅使用加權最小范數法相比，關節速度變化更加平穩，求解更加可靠；馮玉倩等[11]基于拉格朗日方法對七軸機械臂進行動力學分析與仿真，驗證了其建立模型的合理性，但其不足在于仿真沒有建立在實際工況環境中；劉壯壯[12]采用牛頓-歐拉方法對冗余機械臂進行了動力學求解及仿真，并基于慣性橢球原理對機械臂的動力學性能進行了分析；陳罡等[13]基于ADAMS 與MATLAB 對七自由度機械臂進行聯合仿真，分析了各關節之間控制參數的耦合性，仿真結果驗證了機械臂末端執行器的動態響應可以滿足實際工況的需求。

本文針對七自由度機械臂Diana V2 的運動學及動力學建模的正確性問題分別進行分析與驗證。針對運動學問題，首先基于D-H 方法建立機械臂的正運動學模型，再采用梯度投影法對機械臂的逆運動學問題進行求解，利用MATLAB 機器人工具箱驗證結果；針對動力學問題，在SolidWorks中建立該冗余機械臂的三維模型，采用拉格朗日方法對其進行動力學理論建模，然后將模型導入ADAMS 中，將機械臂按照規劃路徑進行運動，得出機械臂七個關節力矩隨時間變化的仿真曲線，并將仿真得到的輸出信息作為MATLAB 中動力學方程的輸入，MATLAB 數值分析后繪制出的力矩曲線與ADAMS 仿真曲線進行對比，以驗證動力學模型的可靠性。

1 七自由度機械臂構型分析

冗余機械臂最顯著的構型特點在于其“自運動”特性，也就是在末端位姿固定的情況下可以有無數組關節角度與之對應。如今已經有很多學者對七自由度機械臂的構型問題進行了大量的研究與分析，本文所使用的Diana V2 機械臂是思靈公司生產的一款輕量化協作機械臂，其構型為帶有偏置的SRS 型，具備反應快、自適應、靈敏度高等優點，適合精密裝配與在狹窄空間中作業。在建立機械臂坐標系的各種方法中，D-H 方法的應用是最為廣泛的，本文采用改進后的D-H 方法描述機械臂的結構參數，并建立各個連桿的坐標系，該機械手臂的模型及DH 坐標系如圖1 所示，機械臂D-H 參數值如表1 所示。

表1 冗余機械臂D-H 參數值Tab.1 D-H parameter value of manipulator

圖1 機械臂模型及連桿坐標系Fig.1 Manipulator model and linkage coordinate system

2 機械臂運動學分析

2.1 正運動學

2.2 逆運動學

機械臂逆運動學與正運動學的求解過程相反，相對更為困難，是已知末端位姿求解機械臂各個關節的角度值。但是由于冗余機械臂自身的冗余特性，使得同一個末端位姿可以反解出無數組解，因此需要補充相應的約束條件求得唯一解。本文采用梯度投影法求運動學逆解，該方法求得的解由特解和齊次解2 部分組成，由于特解與齊次解是正交關系，因此梯度投影法在不影響完成首要任務的同時，能對次要任務進行優化。梯度投影法的公式為

式中：——逆運動學方程=的解，可以保證機械臂末端的運動規律；k(I-J+J)——=0的齊次解，是H? 在零空間的投影，該部分的作用是在不影響機械臂末端位姿的前提下，通過對目標函數的優化實現關節變量的最優化；k——梯度系數，其大小與優化速度呈正相關；H?——優化目標H的梯度。

在實際應用場景中，梯度系數k的取值是難點，k取值過大易引發震蕩，k取值較小則對機械臂關節運動的約束作用不明顯，可能造成關節變量超出關節極限范圍。

利用MATLAB 機器人工具箱對機械臂逆運動學算法進行驗證，圖2 為機械臂各關節角度分別為pi/2，-pi/4，-pi/2，pi/3，pi/2，3/4pi，-3/4pi 時 的仿真模型，將關節角度代入正運動學方程計算出對應的末端位姿矩陣，并將該位姿矩陣作為逆運動學算法的輸入，計算得到各關節角度與初始值誤差很小，且由圖3 可知逆運動學解算出的關節角連續且平滑，而且角度變化范圍也在各關節的運動范圍之內，因此驗證了逆運動學算法的正確性。

圖2 機械臂仿真模型Fig.2 Simulation model of manipulator

圖3 各關節角度變化曲線Fig.3 Joint angle change curve

2.3 工作空間求解

蒙特卡洛法是基于概率統計論對隨機性問題進行仿真，機械臂末端的理論工作位置并不屬于隨機性問題，因此先將機械臂工作空間的解設計為統計參量，再利用該方法可在MATLAB 中求解得到該七自由度機械臂的工作空間。此次定義隨機采樣點數為5 000。隨機采樣點數越大，呈現出的工作空間范圍則越接近真實的工作空間，可得到該機械臂的工作空間范圍接近一個橢球體，機械臂工作空間云圖如圖4 所示。

圖4 機械臂工作空間云圖Fig.4 Cloud map of manipulator workspace

3 機械臂動力學分析

3.1 動力學建模

動力學主要研究的是物體運動與力之間的關系，該機械臂所有關節均為旋轉關節，故對其進行運動和關節力矩分析時，需要考慮各個連桿的慣性張量I。將機械臂各個連桿的質心C作為原點建立坐標系，則相對此坐標系慣性張量I 為

其中對角線元素為剛體繞坐標軸的質量慣性矩，其余元素為慣性積。機械臂各連桿的質量、質心位置、轉動慣量等參數如表2 所示。

表2 機械臂屬性參數Tab.2 Mechanical arm attribute parameters

研究機械臂動力學的方法有牛頓-歐拉法、拉格朗日法、旋量方法及凱恩方法等，鑒于Diana七自由度機械臂構型的復雜性，采用拉格朗日方法對其進行動力學分析。拉格朗日方法本質上是基于系統能量對時間采用微分的動力學方法，該方法基于能量平衡方程，更適合于分析同一系統中的多個連桿運動。

定義拉格朗日函數為L=K-P，其中：L——拉格朗日函數，K——機械臂系統總動能，P——系統總勢能。對于Diana 七自由度機械臂而言，系統動能和勢能分別為

連桿i的質心在連桿i局部坐標系中的位置為

機械臂各個關節力矩的推導方程為

式中：τi——機械臂廣義力或力矩；θi——廣義坐標；——廣義坐標的1 階導數。計算上述拉格朗日方程中的導數項可得新的各關節力矩表達式為

式（9）中的第1 部分代表機械臂廣義加速度產生的慣性力對動力學的影響，第2 部分為科氏力和離心力項，第3 部分表示重力對關節轉矩的影響。

本文以Diana 七自由度機械臂為實驗平臺進行動力學分析，將其動力學方程展開為

在七自由度冗余機械臂的運動過程中，科氏力與向心力對動力學的影響與轉速呈正相關。通常大臂關節轉速較慢，故可以忽略對動力學影響很小的科氏力與向心力，但同時小臂及腕部關節的轉速相對較高，因此在求解動力學的過程中不可忽略。另外，角加速度與重力對機械臂運動的穩定性與精度有著較大的影響。故應當綜合分析對機械臂動力學產生影響的因素并相應地做出簡化，以得到各個關節的力矩推導公式。

3.2 動力學仿真

采用ADAMS 作為仿真平臺對七自由度冗余機械臂進行動力學分析。首先基于SolidWorks 建立了冗余機械臂的三維模型，并將模型保存為Parasolid格式并導入ADAMS，需注意建立的模型應盡量簡化，滿足虛擬樣機仿真性能的同時，構件數量盡可能少。在SolidWorks 中建立的簡化模型如圖5 所示。

圖5 機械臂簡化模型Fig.5 Simplified model of robotic arm

首先需要根據機械臂的實際需要設置仿真參數，從而預測機械臂的力矩值。將上述建立的機械臂模型導入到ADAMS 中，導入后對機械臂的屬性進行定義，包括顏色、位置、名稱和材料等，特別是質量信息，以確保仿真模型的質心與轉動慣量與三維模型保持一致，然后再對機械臂添加約束，由于Diana 機械臂有7 個轉動自由度，因此在機械臂各個關節處添加一個轉動副，并在基座與大地之間添加一個固定副。設置完成后的模型如圖6 所示。

圖6 機械臂約束與驅動Fig.6 Constraint and drive of manipulator

為了使機械臂按照預定軌跡運動，需要對每個轉動副設置相應的驅動，設置完成后的各關節初始關節角度均為0，目標關節角度依次為60，30，45，30，90，45，120。規劃仿真時間為5 s，仿真步長設置為300。此時便可以求解系統中的反約束力，根據運動求作用力就是逆動力學問題。通過ADAMS 完成對機械臂的動態仿真后，從后處理模塊中得到各關節仿真力矩隨時間變化曲線。

另外，機械臂動力學求解包含大量的矩陣運算，因此需要在MATLAB 中對動力學理論模型進行相應的簡化，將仿真得到的機械臂各關節運動物理量（角度、角速度、角加速度）作為動力學方程的輸入，求出各個時間點對應的關節力矩值并擬合成相應的曲線，最后與上述仿真得到的力矩曲線進行對比。圖7 為機械臂各關節理論力矩曲線和仿真力矩曲線的對比。

圖7 各關節理論力矩曲線與仿真力矩曲線對比圖Fig.7 Comparison diagram of theoretical torque curve and simulation torque curve of each joint

由圖7 可知，相較其他關節，關節7 的驅動力矩最小，這是因為關節7 運動過程中僅有很小的負載，且負載集中在其關節軸線附近，因此即使有較大的角加速度，對驅動力矩的影響也很??；關節2的驅動力矩逐漸增大且幅值遠大于其他關節，這是因為機械臂按照預定路徑運動時，關節2 處所受的負載逐漸變大，且需要承受上部關節的重力和運動時產生的向心力及科氏力。通過觀察各關節的力矩變化，總體可以看出各關節力矩受其余關節的耦合影響。通過機械臂各關節的力矩仿真曲線，分析各關節的動力學特性，為進一步研究機械臂控制及設備性能可靠性提供參考。

綜上，七自由度機械臂按照規劃路徑運行時的理論與仿真力矩數據基本吻合，驗證了動力學建模的準確性，因此上述建立的動力學方程可應用于后續的算法研究中，也為機械臂后續控制提供了試驗基礎。

4 結論

（1）采用改進D-H 法建立了冗余機械臂的連桿坐標系，推導出相應的運動學方程，基于梯度投影法求解其逆運動學問題，并利用MATLAB 機器人工具箱驗證了算法的可靠性；（2）以拉格朗日方法為理論基礎，建立了冗余機械臂的動力學模型，當機械臂運動時，可通過動力學方程計算出機械臂各個關節的理論力矩；（3）在SolidWorks 中建立了冗余機械臂的三維模型，將模型導入ADAMS 進行動力學仿真并繪制出仿真力矩曲線，將ADAMS仿真得到的運動信息作為MATLAB 動力學方程的輸入，繪制理論力矩曲線。將兩者進行對比，判斷擬合程度，結果證明了動力學建模的可靠性。

本文對冗余機械臂運動學與動力學的分析研究為后續控制研究和運動優化提供了試驗依據和理論基礎。