趙海濱， 于清文， 劉 沖， 陸志國， 顏世玉

(東北大學(xué) 機(jī)械工程與自動(dòng)化學(xué)院， 沈陽 110819)

0 引 言

機(jī)器人是一門跨專業(yè)，高度綜合的新興學(xué)科[1]，無論在基礎(chǔ)理論方面還是在實(shí)踐應(yīng)用方面發(fā)展速度都非?？臁Ｔ絹碓蕉嗟母叩葘W(xué)校面向高年級(jí)本科生和低年級(jí)研究生開設(shè)機(jī)器人學(xué)等機(jī)器人領(lǐng)域的相關(guān)課程，并對(duì)課程的改革進(jìn)行了研究[2-4]。機(jī)器人課程的教學(xué)包括理論教學(xué)和實(shí)驗(yàn)教學(xué)[5-7]。由于資金和設(shè)備等原因，各個(gè)高校往往以理論教學(xué)為主，對(duì)實(shí)驗(yàn)教學(xué)不夠重視。在機(jī)器人課程的理論教學(xué)中存在大量的公式推導(dǎo)和微分方程等，對(duì)于學(xué)生比較抽象、復(fù)雜和難以理解。因此，采用Matlab/Simulink軟件進(jìn)行機(jī)器人的仿真和控制實(shí)驗(yàn)非常必要。

在機(jī)器人的教材中，機(jī)器人動(dòng)力學(xué)部分的公式推導(dǎo)均以兩自由度機(jī)械臂為例，本文也以兩自由度機(jī)械臂為研究對(duì)象。根據(jù)機(jī)械臂的動(dòng)力學(xué)方程，采用Matlab/Simulink軟件建立機(jī)械臂仿真模型，采用五階多項(xiàng)式進(jìn)行軌跡規(guī)劃，并采用逆動(dòng)力學(xué)進(jìn)行機(jī)械臂的控制。通過機(jī)械臂的運(yùn)動(dòng)學(xué)方程獲取關(guān)節(jié)的位置坐標(biāo)，并動(dòng)態(tài)的顯示機(jī)械臂的運(yùn)動(dòng)過程。最后，對(duì)機(jī)械臂控制系統(tǒng)進(jìn)行動(dòng)態(tài)仿真，并對(duì)仿真結(jié)果進(jìn)行了分析。學(xué)生可以修改參數(shù)，然后進(jìn)行動(dòng)態(tài)仿真。通過仿真實(shí)驗(yàn)，向?qū)W生完整的展示機(jī)械臂的控制過程，能夠加深學(xué)生對(duì)機(jī)器人動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)和控制等理論的理解，增強(qiáng)學(xué)生的編程能力和學(xué)習(xí)興趣，有助于機(jī)器人課程的理論和實(shí)驗(yàn)教學(xué)。

1 機(jī)械臂

本文以兩自由度機(jī)械臂為研究對(duì)象，通過拉格朗日方程建立動(dòng)力學(xué)方程[1]為

(1)

(2)

在動(dòng)力學(xué)方程中，機(jī)械臂的參數(shù)[8-9]如下：

c12=2c11

c21=0

g1=(m1+m2)gr1cos(q2)+m2gr2cos(q1+q2)

g2=m2gr2cos(q1+q2)

式中：m1和m2分別為兩個(gè)連桿的質(zhì)量；r1和r2分別為兩個(gè)連桿的長(zhǎng)度；J1和J2分別為兩個(gè)連桿的轉(zhuǎn)動(dòng)慣量；g為重力加速度。

2 控制系統(tǒng)

本文以兩自由度機(jī)械臂為研究對(duì)象，采用Matlab/Simulink軟件建立綜合性仿真實(shí)驗(yàn)平臺(tái)。根據(jù)動(dòng)力學(xué)方程建立機(jī)械臂的模型，然后采用軌跡規(guī)劃和逆動(dòng)力學(xué)控制進(jìn)行機(jī)械臂的控制，最后通過運(yùn)動(dòng)學(xué)方程將機(jī)械臂在關(guān)節(jié)空間的坐標(biāo)轉(zhuǎn)換為工作空間下的位置坐標(biāo)，并將機(jī)械臂的運(yùn)動(dòng)通過動(dòng)畫的形式進(jìn)行顯示。機(jī)械臂控制仿真系統(tǒng)如圖1所示。

機(jī)械臂的控制方法有PD控制、帶重力補(bǔ)償?shù)腜D控制、逆動(dòng)力學(xué)控制、魯棒控制和自適應(yīng)控制等。本文采用常用的逆動(dòng)力學(xué)控制方法。機(jī)械臂的軌跡規(guī)劃采用點(diǎn)到點(diǎn)運(yùn)動(dòng)方法，常用的有3階多項(xiàng)式軌跡規(guī)劃、5階多項(xiàng)式軌跡規(guī)劃、帶有拋物線的直線段方法和開關(guān)(bang-bang)方法等。本文采用5階多項(xiàng)式軌跡規(guī)劃方法。

圖1 機(jī)械臂控制系統(tǒng)原理圖

2.1 軌跡規(guī)劃

軌跡規(guī)劃采用點(diǎn)到點(diǎn)運(yùn)動(dòng)， 即規(guī)劃一條從初始位置q(t0)到最終位置q(tf)的軌跡。本文采用5階多項(xiàng)式軌跡規(guī)劃方法[10]。在t0時(shí)刻的角度、角速度和角加速度分別滿足：

(3)

在tf時(shí)刻的角度、角速度和角加速度分別滿足：

(4)

采用如下的5階多項(xiàng)式軌跡，

q(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(5)

根據(jù)起始位置和終點(diǎn)位置的角度、角速度和角加速度，可以得到下面的方程組：

(6)

通過求解該方程組可以得到參數(shù)ai(i=0,1,…,5)。然后，可以得到期望角度、角速度和角加速度分別為：

(7)

2.2 逆動(dòng)力學(xué)控制

根據(jù)機(jī)械臂的動(dòng)力學(xué)方程(1)，選擇控制輸入為

(8)

通過比較式(1)和式(8)可以得到

(9)

穩(wěn)定的控制律y可以選擇為

(10)

期望軌跡為qd(t)，為了跟蹤該軌跡，選擇r為

(11)

將式(11)代入到式(10)，然后再代入到式(9)，可以得到二階微分方程

(12)

2.3 運(yùn)動(dòng)學(xué)方程

正運(yùn)動(dòng)學(xué)是根據(jù)機(jī)械臂各個(gè)關(guān)節(jié)的角度值，來確定工作空間下的位置坐標(biāo)。對(duì)于具有兩個(gè)自由度的機(jī)械臂，q1和q2分別為兩個(gè)關(guān)節(jié)的角度，r1和r2分別為兩個(gè)關(guān)節(jié)的長(zhǎng)度，則第1個(gè)關(guān)節(jié)末端的位置坐標(biāo)x1,y1為

(13)

第2個(gè)關(guān)節(jié)末端的位置坐標(biāo)x2,y2為

(14)

3 仿真實(shí)驗(yàn)

Matlab功能強(qiáng)大、使用簡(jiǎn)單方便，并且對(duì)問題的描述和求解符合人們的思維習(xí)慣和數(shù)學(xué)表達(dá)習(xí)慣，已經(jīng)廣泛應(yīng)用于動(dòng)態(tài)系統(tǒng)仿真[11]。Simulink是Matlab軟件最重要的組件之一，能夠進(jìn)行系統(tǒng)建模、仿真和綜合分析等[12]。Simulink和外界硬件的接口還可以建立半實(shí)物仿真和實(shí)時(shí)控制實(shí)驗(yàn)[13]。在Simulink系統(tǒng)中用戶自定義函數(shù)庫中的Matlab Function模塊，可以采用Matlab語言非常方便靈活的建立復(fù)雜系統(tǒng)[14]。Matlab/Simulink軟件非常適合進(jìn)行機(jī)器人的控制和仿真實(shí)驗(yàn)[15-16]，因此本文采用Matlab/Simulink軟件建立仿真實(shí)驗(yàn)系統(tǒng)。

兩自由度機(jī)械臂的參數(shù)選擇為：m1=0.5 kg，m2=1.5 kg，r1=1 m，r2=0.8 m，J1=5 kg·m，J2=5 kg·m，g=9.81 m/s2。在逆動(dòng)力學(xué)控制中參數(shù)Kp=diag(9,9)，Kd=diag(6,6)。

采用采用Matlab/Simulink軟件建立機(jī)械臂控制系統(tǒng)仿真實(shí)驗(yàn)平臺(tái)，如圖2所示。系統(tǒng)采用變步長(zhǎng)的ode45算法，最大仿真步長(zhǎng)為1 ms，仿真時(shí)間為4 s。在圖2中主要采用了Simulink中用戶自定義功能模塊庫中的Matlab Function模塊和積分模塊等。通過常量模塊Constant設(shè)置機(jī)械臂的初始角度和角速度。通過To Workspace模塊將運(yùn)行結(jié)果保存在工作空間中。 機(jī)械臂兩個(gè)關(guān)節(jié)的期望角度分別采用5階多項(xiàng)式進(jìn)行軌跡規(guī)劃。關(guān)節(jié)1和關(guān)節(jié)2的初始角度q1和q2分別為π/10和π/8，目標(biāo)位置的角度分別為3π/4和π/2。關(guān)節(jié)1和關(guān)節(jié)2的初始角速度和目標(biāo)位置的角速度均為0，初始角加速度和目標(biāo)位置的角加速度也均為0。

通過軌跡規(guī)劃得到的期望角度、角速度和角加速度，以及機(jī)械臂的角度和角速度，采用式(8)進(jìn)行逆動(dòng)力學(xué)控制。在圖2中，逆動(dòng)力學(xué)控制模塊內(nèi)的代碼為

function tau = fcn(dq,q,qd,dqd,ddqd)

q1=q(1);q2=q(2);dq1=dq(1);dq2=dq(2);

m1=0.5;m2=1.5;r1=1;r2=0.8;g=9.81;J1=5;

J2=5;

d11=(m1+m2)*r1^2+m2*r2^2+2*m2*r1*r2*

cos(q2)+J1;

d12=m2*r2^2 +m2*r1*r2*cos(q2);

d21=d12;d22=m2*r2^2 +J2;

D=[d11,d12;d21,d22];

c11=m2*r1*r2*sin(q2);

圖2 機(jī)械臂控制系統(tǒng)仿真實(shí)驗(yàn)平臺(tái)

C=[-c11*dq1,-2*c11*dq1; 0,c11*dq2];

g1=(m1+m2)*g*r1*cos(q2)+m2*g*r2*cos(q1+q2);

g2=m2*g*r2*cos(q1+q2);

G=[g1;g2];

w=3;e=q-qd;de=dq-dqd;

Kp=diag([w^2,w^2]);Kd=diag([2*w, 2*w]);

y=ddqd-Kd*de-Kp*e;

tau=D*y+C*dq+G;

機(jī)械臂控制系統(tǒng)運(yùn)行后，機(jī)械臂的角度如圖3所示。關(guān)節(jié)1的角度q1從初始位置π/10運(yùn)動(dòng)到目標(biāo)位置3π/4，關(guān)節(jié)2的角度q2從初始位置π/8運(yùn)動(dòng)到目標(biāo)位置π/2。

圖3 機(jī)械臂的角度

機(jī)械臂的角速度如圖4所示。在圖4中，關(guān)節(jié)1的角速度初始值為0，然后速度逐漸增大，在第2 s達(dá)到最大值為0.957 rad/s，最后逐漸減小到0。關(guān)節(jié)2的角速度初始值為0，然后逐漸增大，并在第2 s達(dá)到最大值0.552 rad/s，最后逐漸減小到0。

圖4 機(jī)械臂的角速度

機(jī)械臂的驅(qū)動(dòng)力矩如圖5所示。在圖5中，關(guān)節(jié)1的驅(qū)動(dòng)力矩在0.605 s達(dá)到最大值33.83 Nm，在0.306 s達(dá)到最小值-15.36 Nm。關(guān)節(jié)2的驅(qū)動(dòng)力矩在0.563 s達(dá)到最大值12.04 Nm，在0.275 s達(dá)到最小值-14.20 Nm。

通過機(jī)械臂的運(yùn)動(dòng)學(xué)方程，可以得到兩個(gè)關(guān)節(jié)的位置坐標(biāo)。在關(guān)節(jié)空間中，通過逆動(dòng)力學(xué)控制將機(jī)械臂從初始位置移動(dòng)到目標(biāo)位置。通過運(yùn)動(dòng)學(xué)方程，可以得到機(jī)械臂第一個(gè)關(guān)節(jié)初始位置的坐標(biāo)為cos(π/10),sin(π/10)=0.951,0.309。機(jī)械臂第2個(gè)關(guān)節(jié)初始位置的橫坐標(biāo)為cos(π/10)+0.8cos(π/10+π/8)=1.56

圖5 機(jī)械臂的驅(qū)動(dòng)力矩

縱坐標(biāo)為

sin(π/10)+0.8sin(π/10+π/8)=0.829

在工作空間中機(jī)械臂的運(yùn)動(dòng)軌跡如圖6所示。在圖6中，每間隔0.2 s繪制機(jī)械臂的位置，動(dòng)態(tài)的顯示機(jī)械臂的運(yùn)動(dòng)過程。

圖6 機(jī)械臂的運(yùn)動(dòng)軌跡

4 結(jié) 語

通過Matlab/Simulink軟件建立了兩自由度機(jī)械臂控制系統(tǒng)仿真實(shí)驗(yàn)平臺(tái)，能夠進(jìn)行機(jī)械臂的點(diǎn)到點(diǎn)控制，并動(dòng)態(tài)的顯示機(jī)械臂的運(yùn)動(dòng)過程。通過仿真實(shí)驗(yàn)，學(xué)生可以對(duì)機(jī)械臂的整個(gè)控制系統(tǒng)有一個(gè)完整的了解，還可以自己編寫控制算法。通過機(jī)械臂控制系統(tǒng)的仿真實(shí)驗(yàn)，有助于學(xué)生對(duì)機(jī)器人的基本理論的理解，能夠增強(qiáng)學(xué)生的實(shí)際編程能力，激發(fā)學(xué)生的學(xué)習(xí)興趣，有助于機(jī)器人課程的理論和實(shí)驗(yàn)教學(xué)。