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

馮小庭，王航

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

0 引言

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

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

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

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

連桿的變換矩陣為：

(1)

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

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

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

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

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

(2)

(3)

(4)

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

2 動(dòng)力學(xué)建模

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

(5)

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

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

(6)

第i的偽慣量矩陣為：

(7)

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

(8)

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

(9)

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

(10)

2.2 程序設(shè)計(jì)

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

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

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

主程序調(diào)用的子函數(shù)中最重要的輸入變量是連桿之間的附體坐標(biāo)變換矩陣。為了簡(jiǎn)化程序的實(shí)現(xiàn)過程，將連桿變換矩陣存儲(chǔ)在元胞數(shù)組中。本計(jì)算方法的實(shí)現(xiàn)完全依照式(3)、式(4)，難點(diǎn)在于附體坐標(biāo)系相對(duì)于基坐標(biāo)系的變換矩陣以及中間變換矩陣的獲得。設(shè)需要建立的機(jī)器人的連桿數(shù)是num，預(yù)先建立num+1階的元胞數(shù)組T。元胞數(shù)組T{i,i}儲(chǔ)存的是單位矩陣，T{i,i+1}儲(chǔ)存的是第i個(gè)連桿的變換矩陣。為獲得坐標(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ù)的計(jì)算過程主要圍繞該數(shù)組的讀取及應(yīng)用。

2.3 動(dòng)力學(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ī)器人的動(dòng)力學(xué)公式是極其復(fù)雜的非線性公式。

3 數(shù)值仿真

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

圖2 角位移

圖3 角速度

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

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

4 結(jié)語

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