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

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

0 引 言

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

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

1 機械臂

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

(1)

(2)

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

c12=2c11

c21=0

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

g2=m2gr2cos(q1+q2)

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

2 控制系統(tǒng)

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

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

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

2.1 軌跡規(guī)劃

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

(3)

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

(4)

采用如下的5階多項式軌跡，

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

(5)

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

(6)

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

(7)

2.2 逆動力學(xué)控制

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

(8)

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

(9)

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

(10)

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

(11)

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

(12)

2.3 運動學(xué)方程

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

(13)

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

(14)

3 仿真實驗

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

兩自由度機械臂的參數(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。在逆動力學(xué)控制中參數(shù)Kp=diag(9,9)，Kd=diag(6,6)。

采用采用Matlab/Simulink軟件建立機械臂控制系統(tǒng)仿真實驗平臺，如圖2所示。系統(tǒng)采用變步長的ode45算法，最大仿真步長為1 ms，仿真時間為4 s。在圖2中主要采用了Simulink中用戶自定義功能模塊庫中的Matlab Function模塊和積分模塊等。通過常量模塊Constant設(shè)置機械臂的初始角度和角速度。通過To Workspace模塊將運行結(jié)果保存在工作空間中。 機械臂兩個關(guān)節(jié)的期望角度分別采用5階多項式進行軌跡規(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ī)劃得到的期望角度、角速度和角加速度，以及機械臂的角度和角速度，采用式(8)進行逆動力學(xué)控制。在圖2中，逆動力學(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 機械臂控制系統(tǒng)仿真實驗平臺

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;

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

圖3 機械臂的角度

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

圖4 機械臂的角速度

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

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

圖5 機械臂的驅(qū)動力矩

縱坐標(biāo)為

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

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

圖6 機械臂的運動軌跡

4 結(jié) 語

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