基于MATLAB的多連桿機(jī)械人動力學(xué)方程建立

馮小庭，王航

(西安鐵路職業(yè)技術(shù)學(xué)院，陜西 西安 710026)

0 引言

機(jī)器人廣泛應(yīng)用于醫(yī)療器械、汽車工業(yè)、建筑工程以及石油工業(yè)。工業(yè)機(jī)器人是一個復(fù)雜的動力學(xué)系統(tǒng)，它由多個關(guān)節(jié)和多個連桿組成，具有多個輸入和多個輸出，連桿之間存在著非線性耦合關(guān)系。研究機(jī)器人動力學(xué)是為了實現(xiàn)高精度的實時控制，以完成高質(zhì)量的生產(chǎn)過程。因此，是否可以準(zhǔn)確高效地獲得機(jī)器人的動力學(xué)方程是非常必要的。而在一些領(lǐng)域?qū)ζ矫娑噙B桿機(jī)器人有著特殊需求。平面連桿機(jī)器人連桿數(shù)量的增加可以提高使用的靈活性。當(dāng)連桿的數(shù)量增加時，機(jī)器人的動力學(xué)建模將是非常繁瑣的過程。眾多文獻(xiàn)對機(jī)器人連桿或通過函數(shù)求解運動方程，或三維軟件建模以及獲得動力學(xué)方程，但是隨著連桿數(shù)量的增加，求解耗時多，且容易出錯；運動仿真的精度有待驗證。

本文使用MATLAB使得多連桿機(jī)器人的動力學(xué)建模得到簡化，可以實現(xiàn)任意多連桿平面機(jī)器人的建模。以七連桿機(jī)器人的建模過程驗證本程序的正確性。本程序亦可用于關(guān)節(jié)有驅(qū)動力時，連桿機(jī)器人末端軌跡的仿真。

1 多連桿機(jī)器人的建模

本文使用拉格朗日法對連桿機(jī)器人進(jìn)行動力學(xué)建模。為了獲得各連桿的位置，需要首先知道D-H附體坐標(biāo)系的矩陣表示方法。

連桿的變換矩陣為：

(1)

其中i=1,2,…,n。

式中：αi為連桿扭轉(zhuǎn)角；ai為連桿長度；di為連桿偏移量；θi為關(guān)節(jié)轉(zhuǎn)角。

由式(1)可以知道任意連桿相對于基礎(chǔ)坐標(biāo)的位置。

連桿機(jī)器人簡圖如圖1所示。有n個連桿的機(jī)器人有n個自由度，每個連桿有1個自由度。機(jī)器人動力學(xué)方程的一般公式表示如下。

圖1 連桿機(jī)器人簡圖

(2)

(3)

(4)

在編寫相關(guān)的程序時，直接引用了上述的推導(dǎo)結(jié)果。上式中，第一部分是角加速度慣量項，第二部分是驅(qū)動器慣量項，第三部分是科里奧利力和向心力，最后是重力項。慣量項和重力項對于機(jī)器人系統(tǒng)的穩(wěn)定性和定位精度至關(guān)重要。向心力和科力奧利力在機(jī)器人低速運動時可以忽略，但在機(jī)器人高速運動時，其作用非常重要。

2 動力學(xué)建模

2.1 七連桿機(jī)器人的動力學(xué)模型

(5)

其中i=1,2,…,n。

由于，本文針對的是平面連桿機(jī)器人，故式(1)中αi=0，di=0，變換矩陣寫為

(6)

第i的偽慣量矩陣為：

(7)

式中：INi是第i個連桿相對于坐標(biāo)系Oxiyizi的轉(zhuǎn)動慣量。這里假設(shè)機(jī)器人的連桿是均勻的細(xì)直桿，則

(8)

一般連桿機(jī)器人的動力學(xué)公式寫為如下的矩陣形式[9]：

(9)

七連桿機(jī)器人的動力學(xué)公式可以寫為下式：

(10)

2.2 程序設(shè)計

在這里需要做出補(bǔ)充說明，本文的連桿機(jī)器人動力學(xué)建模程序只針對基座固定的機(jī)器人而言。而且本程序目前只適用于平面串聯(lián)圓柱關(guān)節(jié)機(jī)器人。

MATLAB強(qiáng)大的符號工具箱是本文所述內(nèi)容得以實現(xiàn)的根本原因。程序的編寫主要參照了上述連桿機(jī)器人普遍動力學(xué)公式的推導(dǎo)過程。下文將會圍繞程序的核心部分進(jìn)行說明。程序中較多的使用元胞數(shù)組，這是MATLAB特有的數(shù)據(jù)結(jié)構(gòu)。得益于計算機(jī)技術(shù)的提升，這種數(shù)據(jù)結(jié)構(gòu)使得計算的過程簡化，代碼的邏輯易讀，盡管元胞數(shù)組會額外增加計算機(jī)內(nèi)存的使用。

在系統(tǒng)動力學(xué)的推導(dǎo)過程中，并沒有考慮關(guān)節(jié)處的驅(qū)動機(jī)構(gòu)的轉(zhuǎn)動慣量，且沒有考慮關(guān)節(jié)處的摩擦力。當(dāng)然，程序中添加計算驅(qū)動裝置的轉(zhuǎn)動慣量和摩擦力是非常易于實現(xiàn)的。其次，計算出的動力學(xué)公式，僅需極小的改動即可應(yīng)用于其他語言的仿真程序中。例如可以直接將計算結(jié)果粘貼到C語言中，進(jìn)行運動的仿真。

主程序調(diào)用的子函數(shù)中最重要的輸入變量是連桿之間的附體坐標(biāo)變換矩陣。為了簡化程序的實現(xiàn)過程，將連桿變換矩陣存儲在元胞數(shù)組中。本計算方法的實現(xiàn)完全依照式(3)、式(4)，難點在于附體坐標(biāo)系相對于基坐標(biāo)系的變換矩陣以及中間變換矩陣的獲得。設(shè)需要建立的機(jī)器人的連桿數(shù)是num，預(yù)先建立num+1階的元胞數(shù)組T。元胞數(shù)組T{i,i}儲存的是單位矩陣，T{i,i+1}儲存的是第i個連桿的變換矩陣。為獲得坐標(biāo)系之間的變換矩陣，引入如下的算法。限于篇幅，這里只摘錄部分關(guān)鍵代碼。

for ii=1:num+1

for jj=1:num+1

if((ii+1)==jj)||(ii>=jj)

continue;

else

T_temp=sym(eye(4));

n_temp=jj-ii;

for kk=1:n_temp

T_temp=T_temp*T{ii+kk-1,ii+kk};

end

T{ii,jj}=T_temp;

end

end

end

其中的元胞數(shù)組T即是所求，后續(xù)的計算過程主要圍繞該數(shù)組的讀取及應(yīng)用。

2.3 動力學(xué)方程

通過調(diào)用相關(guān)的程序，即可得出相關(guān)的結(jié)果。由于系數(shù)矩陣的結(jié)構(gòu)非常復(fù)雜，限于篇幅，只將其中的一部分摘錄如下：

m5a2a4cos(q3+q4)+m6a2a4cos(q3+q4)+m7a2a4cos(q3+q4)+

m6a4a6cos(q5+q6)+2m7a4a6cos(q5+q6)+m7a5a7cos(q5+q6)+

m5a2a4cos(q3+q4)+m6a2a4cos(q3+q4)+m7a2a4cos(q3+q4)+

m7a3a4cos(q4)+m5a4a5cos(q5)+2m6a4a5cos(q5)+

2m7a4a5cos(q5)+m6a5a6cos(q6)+2m7a5a6cos(q6)+

m7a3a4cos(q4)+m5a4a5cos(q5)+2m6a4a5cos(q5)+

m7a3a6cos(q4+q5+q6)+m7a4a7cos(q5+q6+q7);

m6a4a5cos(q5)+m7a4a5cos(q5)+m6a5a6cos(q6)+2m7a5a6cos(q6)+m7a6a7cos(q7)+

由以上公式可知，七連桿機(jī)器人的動力學(xué)公式是極其復(fù)雜的非線性公式。

3 數(shù)值仿真

動力學(xué)仿真主要用于驗證計算方法的正確性，若動力學(xué)模型有誤，則仿真結(jié)果也將產(chǎn)生誤差，一般而言會使計算的結(jié)果發(fā)散。以2.1節(jié)中七連桿機(jī)器人為例，在各關(guān)節(jié)處施加正弦力矩信號，得到了七連桿機(jī)器人的運動軌跡圖，如圖2-圖3所示(本刊黑白印刷，相關(guān)疑問咨詢作者)。為了使從動力學(xué)模型到仿真的計算過程更加便利，本文依然借助于MATLAB來實現(xiàn)建模和仿真的無縫銜接。具體而言，就是將得到的符號函數(shù)轉(zhuǎn)換為函數(shù)句柄，這個過程是通過MATLAB自帶的函數(shù)完成的，即使用MATLAB Function(symstring)函數(shù),其中函數(shù)的輸入為symstring是符號函數(shù)表達(dá)式。這樣做會增加仿真過程的耗時，但是考慮到計算的總時長較短，而且這種方法不會發(fā)生謄錄公式時易發(fā)生的錯誤。常微分方程組的數(shù)值解使用的是定步長四階龍格庫塔法。設(shè)機(jī)器人各連桿的長度都為1.0 m，質(zhì)量為2 kg，各關(guān)節(jié)處的轉(zhuǎn)矩幅值均為20 N·m。計算的步長為0.001 s，仿真的時間為1s。

圖2 角位移

圖3 角速度

由于七連桿機(jī)器人的動力學(xué)模型非常復(fù)雜，因此仿真的時間較短。從圖2、圖3中可以看出，七連桿機(jī)器人的角位移、角速度的變化均較為劇烈，但都呈現(xiàn)出連續(xù)的曲線形式。圖4是機(jī)器人是動作姿態(tài)。在整個時間歷程中，等時間間隔地選取其中5個時間節(jié)點的動作姿態(tài)。可知末端連桿的位形改變得較大，機(jī)器人整體呈現(xiàn)豎直的趨勢，這是因為關(guān)節(jié)力矩不足以抵消重力的影響所導(dǎo)致的，因而整體有向下運動的趨勢。

圖4 連桿的動作姿態(tài)

4 結(jié)語

由七連桿機(jī)器人的動力學(xué)仿真可知，在2連桿、3連桿情況下推導(dǎo)出的結(jié)果與一般方法的動力學(xué)方程完全一致，可知在多連桿的情況下本方法也是正確的。本方法未考慮連桿之間的內(nèi)力，簡化了推導(dǎo)的過程,提高了推導(dǎo)的效率和正確性。本方法對后續(xù)、復(fù)雜的、有平移關(guān)節(jié)和球關(guān)節(jié)組成的空間型機(jī)器人的動力學(xué)推導(dǎo)有一定的借鑒意義；也對平面開鏈多連桿柔性機(jī)器人的動力學(xué)建模有借鑒意義。