基于ADAMS與MATLAB的6R機器人運動學(xué)分析與軌跡規(guī)劃聯(lián)合仿真

顧海華 ，胡興柳 ，毛軍輝

1.鹽城工學(xué)院 電氣工程學(xué)院，江蘇 鹽城 224002；2.金陵科技學(xué)院 智能科學(xué)與控制工程學(xué)院，江蘇 南京 211169；3.南京郵電大學(xué) 自動化學(xué)院、人工智能學(xué)院，江蘇 南京 210003

隨著社會與科學(xué)技術(shù)的不斷發(fā)展，工業(yè)機器人已經(jīng)成為生產(chǎn)制造和科學(xué)研究領(lǐng)域的熱點。研究機器人首先要對機器人進行運動學(xué)分析，而運動學(xué)分析通常分為正運動學(xué)分析與逆運動學(xué)分析。機器人的正運動學(xué)分析是已知關(guān)節(jié)驅(qū)動力或者力矩，求解機器人末端執(zhí)行器的位姿，如標(biāo)準(zhǔn)D-H參數(shù)法與改進的D-H參數(shù)法等；逆運動學(xué)分析為已知末端執(zhí)行器位姿，求解對應(yīng)關(guān)節(jié)產(chǎn)生的位移，如Newton-Euler 法、Lagrange 法、Kane法等。這些方法被廣泛運用到眾多場景中，目前，在MATLAB 與ADAMS 中進行聯(lián)合仿真是有效分析機器人運動特性的一種方式［1］，其不但可以保證軌跡規(guī)劃的精確性，也可測試系統(tǒng)的各項特性［2］。

1 6R機器人改進D-H參數(shù)法建模

一般工業(yè)機器人均由轉(zhuǎn)動副和移動副組成，6R 機器人作為目前最為廣泛使用的工業(yè)機器人，其6 個關(guān)節(jié)均是由旋轉(zhuǎn)副組成。圖1 為6 自由度機器人的KUKA KR16 操作臂，可實現(xiàn)3 個空間位置和3個控件姿態(tài)的任意運動。

圖1 KUKA KR16操作臂Fig. 1 KUKA KR16 manipulator

在針對KUKA KR16 進行運動學(xué)分析前，需要建立其數(shù)學(xué)模型。為便于分析，建立機器人的連桿坐標(biāo)系，如圖2 所示，即以機器人的第i- 1個關(guān)節(jié)的軸線Zi-1為Z軸，以Zi-1和Zi的公垂線Xi-1為X軸建立坐標(biāo)系，其中X軸的方向為關(guān)節(jié)i- 1 指向關(guān)節(jié)i，而Y軸的Yi-1則按右手法則確定，即Yi-1=Zi-1?Xi-1。因此，坐標(biāo)系中的每個連桿均可通過ai-1、αi-1、di和θi這4個參數(shù)進行描述，其中ai-1表示連桿長度，為Zi-1和Zi兩關(guān)節(jié)軸線之間的公垂線長度，αi-1表示連桿扭角，為Zi-1和Zi兩關(guān)節(jié)軸線之間的夾角，di表示相鄰兩關(guān)節(jié)之間的線位移，為Xi-1和Xi之間在Zi方向上的長度，θi表示相鄰兩關(guān)節(jié)之間的角位移，為Xi-1到Xi繞Zi轉(zhuǎn)過的角度。

圖2 連桿坐標(biāo)系Fig. 2 l ink-pole coordinate system

根據(jù)機器人的各項機械特性參數(shù)，采用目前使用最為廣泛的改進D-H 參數(shù)法，建立改進的D-H參數(shù)表［3］，如表1所示。

表1 改進的D-H參數(shù)表Table 1 Improved D-H parameters table

通過MATLAB中的Link和SerialLink函數(shù)建立KUKA KR16機械臂模型［4］。由于采用改進的D-H參數(shù)表，MATLAB中Link函數(shù)的一般表達(dá)式為L= Link([θi,d i,ai-1,αi-1,offset],′modified′)。

根據(jù)表1，可以得到Link 函數(shù)建立機械臂的命令為：

在MATLAB 中運行以上程序后，即可得到圖3所示的機器人模型。

圖3 機器人MATLAB模型Fig. 3 Robot MATLAB model

2 6R機器人正、逆運動學(xué)分析

根據(jù)表1 和圖2、圖3 機器人各參數(shù)之間的關(guān)系［5］，可推導(dǎo)出兩個連桿坐標(biāo)系之間的齊次變換矩陣，如式（1）所示。

將表1 數(shù)據(jù)帶入式（1），可以得出各個關(guān)節(jié)的變換矩陣，分別為：

根據(jù)機器人正運動學(xué)原理，機器人的關(guān)節(jié)變換矩陣可寫為：

式中：

根據(jù)機器人逆運動學(xué)中的反代數(shù)法，將機器人各關(guān)節(jié)變量分離出來，再逐次、逐個地求解出各個關(guān)節(jié)的關(guān)節(jié)變量(θ1，θ2，θ3，θ4，θ5，θ6)。具體求解方式如下：

設(shè)機器人初始位姿（機器人第1～6 個關(guān)節(jié)的角度）為θstart=[0，- pi/2， 0， pi， pi， 0]，通過正運動學(xué)方程求解，結(jié)果如式（10）所示；通過MATLAB/Robotics Toolbox 工具箱求解，結(jié)果如式（11）所示。

比較式（10）、式（11）可以看出，式（10）的二行二列為-1，式（11）的二行二列為1，其余各行各列的數(shù)值都相同。二行二列兩個數(shù)值的不同是因為機器人關(guān)節(jié)正反轉(zhuǎn)的不同，不影響最終位姿呈現(xiàn)的結(jié)果。故正運動學(xué)方程推導(dǎo)出的結(jié)果和MATLAB 工具箱求解的結(jié)果是一致的，證明了DH參數(shù)選取和齊次變換矩陣的正確性。

采用MATLAB/Robotics Toolbox 工具箱中的ikine 函數(shù)對機器人進行逆運動學(xué)計算，得到各關(guān)節(jié)的初始角度，如式（12）所示。由于運動學(xué)逆解存在不唯一解，在比較式（12）與初始位姿θstart=[0，- pi/2， 0， pi， pi， 0]時，可以發(fā)現(xiàn)式（12）與初始位姿θstart設(shè)定的結(jié)果基本一致。

3 6R機器人軌跡規(guī)劃

機器人軌跡規(guī)劃分為關(guān)節(jié)空間軌跡規(guī)劃與笛卡爾空間軌跡規(guī)劃。機器人運動軌跡確定方法很多，為獲得更高的平滑性，采用關(guān)節(jié)空間軌跡規(guī)劃中的五次多項式插值的方法進行軌跡規(guī)劃［6］。

設(shè)機器人各關(guān)節(jié)角位移函數(shù)為：

式中：s0、s1、s2、s3、s4、s5分別為五次多項式的系數(shù)。

對式（13）位移函數(shù)求一階導(dǎo)數(shù)，即可得到機器人各關(guān)節(jié)運動角的速度函數(shù)：對位移函數(shù)求二階導(dǎo)數(shù)即可得到機器人各關(guān)節(jié)運動角加速度函數(shù)：

為得到唯一的關(guān)節(jié)運動軌跡曲線，必須對其起始時刻的關(guān)節(jié)角度、關(guān)節(jié)角速度、角加速度共計6參數(shù)進行約束。

式中：θ0i為第i個關(guān)節(jié)的初始角度，（°）；為第i個關(guān)節(jié)的初始角速度，（°）/s；為第i個關(guān)節(jié)的初始角加速度，(°)/s2；θfi為第i個關(guān)節(jié)的終止角度，（°）為第i個關(guān)節(jié)的終止角速度，（°)/s為第i個關(guān)節(jié)的終止角加速度，（°)/s2；t0i為第i個關(guān)節(jié)運動的起始時刻，s；tfi為第i個關(guān)節(jié)從初始角度運動至終止角度的時間，s。

將式（16）帶入式（13）～式（15），得：

為得到機器人運動軌跡，在MATLAB 仿真前需先確定機器人各關(guān)節(jié)的初始角度θintial= [0，0，0，0，0，0]、終 止 角 度θtarget= [pi/2，pi/4，-pi/4，pi/3， pi/6，pi]、仿真時間10 s、采樣50 次，再根據(jù)式（17）及式（13）～式（15），在MATLAB 中仿真即可得出各關(guān)節(jié)對應(yīng)的角度、角速度、角加速度的變化曲線，如圖4～圖6所示。

由圖4 可知，圖形準(zhǔn)確地顯示了各關(guān)節(jié)的起始角度和終止角度；由圖5 可知，圖中曲線平滑，無突兀的激增和陡降，說明各關(guān)節(jié)角速度合適；由圖6可知，圖中曲線平穩(wěn)變化。顯然，采用五次多項式插值的方法進行軌跡規(guī)劃是比較成功的［7］。

圖4 各關(guān)節(jié)角度軌跡規(guī)劃Fig. 4 Angle trajectory planning of each joint

圖5 各關(guān)節(jié)角速度軌跡規(guī)劃Fig. 5 Angular velocity trajectory planning of each joint

圖6 各關(guān)節(jié)角加速度軌跡規(guī)劃Fig. 6 Angular acceleration trajectory planning of each joint

4 ADAMS與MATLAB聯(lián)合仿真

ADAMS 是一款集建模、求解、可視化等技術(shù)于一體，在虛擬樣機領(lǐng)域被廣泛使用的軟件。利用ADAMS 軟件，用戶可以直接建立機械系統(tǒng)的幾何模型，或者從其他三維軟件（SolidWorks、Pro/E 等）中導(dǎo)入更為精細(xì)的模型［8］。本文利用Solid?Works 軟件建立模型并保存為Parasolid 格式，然后將其導(dǎo)入至ADAMS。需要注意的是，為使得仿真結(jié)果更貼近實際，同時還能減小計算量，通常在仿真之前先將細(xì)小的、零碎的、不影響仿真結(jié)果的部件刪除，然后將相關(guān)零件通過布爾和合為一體。另外，在模型導(dǎo)入至ADAMS 后，各零部件的材質(zhì)、屬性、質(zhì)量等參數(shù)都會丟失。因此，在模型導(dǎo)入至ADAMS 后，需要為每個零部件重新定義材質(zhì)，并添加合適的約束。本文將所有材料屬性定義為steel，各關(guān)節(jié)之間均取旋轉(zhuǎn)約束（本文為6R機器人）。

利用MATLAB 軟件將五次多項式插值法軌跡規(guī)劃的數(shù)值計算出來，并將其保存為txt 格式，再導(dǎo)入至ADAMS 中；在ADAMS 中為各約束添加合適的驅(qū)動力后，利用ADAMS 中的spline 函數(shù)將各關(guān)節(jié)角度作用至對應(yīng)的關(guān)節(jié)即可完成驅(qū)動，再利用軌跡工具畫出機器人運動軌跡線，如圖7～圖9 所示。機器人各關(guān)節(jié)之間的約束關(guān)系及驅(qū)動關(guān)系分別如表2～表3所示。

表2 機器人各關(guān)節(jié)約束關(guān)系Table 2 Constraint relation of each joint of robot

表3 機器人各關(guān)節(jié)驅(qū)動關(guān)系Table 3 Driving relationship of each joint of robot

對比圖7～圖9，可知各曲線的數(shù)值大小、弧度等都近似，說明聯(lián)合仿真比較成功。

圖8 機器人各個關(guān)節(jié)在ADAMS中的角度曲線Fig. 8 Angles of each joint in ADAMS

圖9 機器人各關(guān)節(jié)在MATLAB中的角度曲線Fig. 9 Angles of robot joints in MATLAB

在ADAMS 軌跡規(guī)劃中，也可使用“一般點驅(qū)動”功能對機器人進行軌跡規(guī)劃［9-10］，操作步驟為：在模型的末端執(zhí)行器上設(shè)置一個標(biāo)記點，在該標(biāo)記點上添加“一般點驅(qū)動”，在驅(qū)動編輯里設(shè)置相應(yīng)的驅(qū)動方程。

利用“一般點驅(qū)動”功能對常見且較為復(fù)雜的圓錐螺旋曲線進行仿真，步驟如下：

（1）確定圓錐螺旋曲線的數(shù)學(xué)表達(dá)式

式中：S為螺旋線導(dǎo)程，mm；T為運動周期，s；a為螺旋曲線的螺距，mm；t為仿真時間，s。

（2）設(shè)置相關(guān)參數(shù)：螺旋線導(dǎo)程S= 30 mm，運動周期T= 2s，螺旋曲線的螺距a= 15 mm，仿真時間t=6 s，步數(shù)300步。

（3）選擇交互仿真。

仿真結(jié)束后在末端執(zhí)行器上通過軌跡工具即可得到末端執(zhí)行器的運動軌跡曲線，如圖10所示。

圖10 機器人末端執(zhí)行器軌跡Fig. 10 Trajectory diagram of robot end-effector

在ADAMS 中，也可以使用spline 函數(shù)進行仿真，方法如下：清除之前設(shè)置的點驅(qū)動，導(dǎo)入6 個關(guān)節(jié)的角度spline 函數(shù)，給每個關(guān)節(jié)添加相對應(yīng)的驅(qū)動（驅(qū)動函數(shù)由spline 函數(shù)完成），并設(shè)置仿真時間6 s，步數(shù)300 步［11］。通過DX、DY、DZ 函數(shù)得到末端執(zhí)行器在X、Y、Z軸方向上的運動曲線，如圖11 所示；通過Ax/Ay/Az Projected Rotation 得到機器人各關(guān)節(jié)角度變化曲線，如圖12 所示；通過Relative Angular Velocity 得到機器人各關(guān)節(jié)角加速度變化曲線，如圖13所示。

圖11 機器人末端執(zhí)行器運行曲線Fig. 11 Running curve of robot end-effector

圖12 機器人各關(guān)節(jié)角度變化曲線Fig. 12 Angle variation curve of each joint of robot

圖13 機器人各關(guān)節(jié)角速度變化曲線Fig. 13 Angular velocity curve of each joint of robot

將圖11～圖13 與圖10 進行對比，可知仿真結(jié)果正確。

在ADAMS 中，通過“一般點驅(qū)動”與spline 函數(shù)的2次仿真，可以發(fā)現(xiàn)仿真目標(biāo)均為空間形狀，仿真后最終所呈現(xiàn)的末端執(zhí)行器的運行軌跡與機器人運動學(xué)分析相符，說明ADAMS 下運動學(xué)仿真分析是正確的，驗證了從SW 導(dǎo)入至ADAMS的KUKA KR16機器人模型的可靠性。

5 結(jié)論

針對6 自由度工業(yè)機器人的運動學(xué)與軌跡規(guī)劃問題，在改進的D-H 參數(shù)法基礎(chǔ)上，通過矩陣的齊次變換推導(dǎo)出機器人的運動學(xué)方程，再通過正逆運動學(xué)進行求解；采用MATLAB/Robotics Toolbox 工具箱對機器人進行正逆運動學(xué)求解，驗證了運動學(xué)方程的正確性，再通過五次多項式插值法進行軌跡規(guī)劃；最后在ADAMS 中導(dǎo)入KUKA KR16 機器人的3D 模型，利用ADAMS 與MATLAB 聯(lián)合仿真技術(shù)驗證了運動學(xué)模型的正確性。