基于MATLAB/Simulink的欠驅(qū)動(dòng)機(jī)器人仿真實(shí)驗(yàn)

趙海濱， 劉 沖， 陸志國(guó)， 于清文， 顏世玉

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

0 引 言

機(jī)器人是一門(mén)多學(xué)科高度交叉的前沿學(xué)科，包括機(jī)械工程、電氣與電子工程、計(jì)算機(jī)科學(xué)、認(rèn)知科學(xué)、生物學(xué)、控制論與控制工程等學(xué)科[1]，無(wú)論在基礎(chǔ)理論方面還是在實(shí)踐應(yīng)用方面發(fā)展都非常快。機(jī)器人動(dòng)力學(xué)在機(jī)器人的教學(xué)中非常重要，可以用于機(jī)器人的實(shí)時(shí)控制研究、動(dòng)態(tài)設(shè)計(jì)和仿真等[2]。機(jī)器人的動(dòng)力學(xué)方程非常抽象和復(fù)雜，需要扎實(shí)的數(shù)學(xué)基礎(chǔ)，不容易理解。

在機(jī)器人的教材中，機(jī)器人動(dòng)力學(xué)部分均以兩自由度機(jī)器人為研究對(duì)象[3]。Pendubot系統(tǒng)是一種典型的欠驅(qū)動(dòng)兩自由度機(jī)器人[4]，由在垂直平面上運(yùn)動(dòng)的兩個(gè)關(guān)節(jié)和一個(gè)驅(qū)動(dòng)電機(jī)組成。Pendubot系統(tǒng)具有非線性和強(qiáng)耦合等特性[5-6]?？刂品绞桨〝[起控制和平衡控制，擺起控制常用的算法有能量法[7-9]和滑膜控制方法[10-11]等，平衡控制常用線性二次型調(diào)節(jié)器(Linear quandratic regulator,LQR)。Pendubot系統(tǒng)作為一種典型的機(jī)電裝置，可以用于非線性控制和機(jī)器人領(lǐng)域的教學(xué)與科研，也可以作為一個(gè)基準(zhǔn)系統(tǒng)對(duì)控制方法進(jìn)行驗(yàn)證。

本文首先對(duì)Pendubot系統(tǒng)的動(dòng)力學(xué)方程和平衡控制進(jìn)行理論分析，采用Matlab/Simulink建立動(dòng)力學(xué)模型，進(jìn)行Pendubot系統(tǒng)仿真和控制實(shí)驗(yàn)。學(xué)生可以修改系統(tǒng)的參數(shù)，然后進(jìn)行系統(tǒng)的動(dòng)態(tài)仿真。該仿真系統(tǒng)能夠加深學(xué)生對(duì)欠驅(qū)動(dòng)機(jī)器人仿真和控制的理解，能夠增強(qiáng)學(xué)生的學(xué)習(xí)興趣和熱情，并有助于培養(yǎng)學(xué)生的動(dòng)手能力和創(chuàng)新意識(shí)。

1 Pendubot系統(tǒng)

1.1 動(dòng)力學(xué)方程

欠驅(qū)動(dòng)Pendubot系統(tǒng)有兩個(gè)關(guān)節(jié)組成，第一個(gè)關(guān)節(jié)是主動(dòng)臂，第二個(gè)關(guān)節(jié)是欠驅(qū)動(dòng)臂，如圖1所示。

圖1 Pendubot模型

通過(guò)拉格朗日方程建立Pendubot系統(tǒng)的動(dòng)力學(xué)方程[1]為

(1)

(2)

Pendubot系統(tǒng)動(dòng)力學(xué)方程中的參數(shù)可以用下面的5個(gè)參數(shù)表示

(3)

式中：l1和l2分別為主動(dòng)臂和欠驅(qū)動(dòng)臂長(zhǎng)度；lc1和lc2分別為主動(dòng)臂和欠驅(qū)動(dòng)臂質(zhì)心長(zhǎng)度；m1和m2分別為主動(dòng)臂和欠驅(qū)動(dòng)臂的質(zhì)量；q1和q2分別表示主動(dòng)臂和欠驅(qū)動(dòng)臂角度；I1和I2分別表示主動(dòng)臂和欠驅(qū)動(dòng)臂質(zhì)心轉(zhuǎn)動(dòng)慣量。

將這些參數(shù)代入到Pendubot系統(tǒng)的動(dòng)力學(xué)方程，可得:

Pendubot系統(tǒng)的主動(dòng)臂由直流電動(dòng)機(jī)驅(qū)動(dòng)[12]。主動(dòng)臂的驅(qū)動(dòng)轉(zhuǎn)矩τ1和輸入電壓u的關(guān)系為

τ1=K1K2u

(4)

式中：K為電動(dòng)機(jī)的轉(zhuǎn)矩常數(shù)，取值為0.412 5 N·m/A；K2為驅(qū)動(dòng)器的增益系數(shù)，取值為2.68 A/V。

1.2 平衡控制

(5)

式中:參數(shù)A和B為常數(shù)矩陣，分別為

平衡控制采用線性二次型調(diào)節(jié)器(LQR)。LQR屬于現(xiàn)代控制理論，具有很好的魯棒性。LQR控制器是在一定的性能指標(biāo)下，使系統(tǒng)的控制效果最佳。LQR控制器的力矩為

u=-Kx2

(6)

2 MATLAB/Simulink仿真實(shí)驗(yàn)

MATLAB/Simulink具有強(qiáng)大的數(shù)學(xué)運(yùn)算能力，以及方便實(shí)用的繪圖功能，已經(jīng)成為系統(tǒng)仿真和自動(dòng)控制領(lǐng)域普遍采用的計(jì)算機(jī)輔助設(shè)計(jì)工具[13-15]。在Simulink中采用普通的模塊搭建復(fù)雜控制系統(tǒng)非常的困難，采用User-Defined Functions庫(kù)中的MATLAB Function模塊，可以采用MATLAB語(yǔ)言非常方便靈活的建立復(fù)雜的系統(tǒng)[16]。

采用MATLAB/Simulink建立Pendubot系統(tǒng)如圖2所示。在圖2中，采用MATLAB Function模塊，根據(jù)式(2)建立Pendubot系統(tǒng)，然后采用兩個(gè)積分器，分別得到角速度和角度值，并將其反饋給Pendubot模塊。角度和角速度的初始值由外部設(shè)置。在圖2中，Pendubot模塊內(nèi)的代碼為:

functionddq=fcn(q, dq, u)

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

t1=0.010 6; t2=0.005 97; t3=0.005 09;

t4=0.075 1; t5=0.036 7;

g=9.81; q2=mod(q2, 2*pi);

d11=t1+t2+2*t3*cos(q2); d12=t2+t3*cos(q2);

d21=d12; d22=t2;

c11=-t3*sin(q2)*dq2;

c12=-t3*sin(q2)*(dq2+dq1);

c21=t3*sin(q2)*dq1;

g1=t4*g*cos(q1)+t5*g*cos(q1+q2);

g2=t5*g*cos(q1+q2);

D=[d11, d12; d21, d22]; C=[c11, c12; c21, 0];

G=[g1; g2]; k1=0.412 5; k2=2.68;

ddq=D([k1*k2*u; 0] - C*[dq1; dq2] - G);

圖2 Pendubot系統(tǒng)

通過(guò)式(6)，采用LQR進(jìn)行平衡控制。在圖2中，LQR模塊內(nèi)的代碼為:

function u=fcn(q,dq)

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

K=-[37.6473, 36.7494, 6.7068, 4.8000];

u=-K*[q1-pi/2; q2; dq1; dq2];

圖2中，主動(dòng)臂和欠驅(qū)動(dòng)臂的初始角速度均為0。通過(guò)手動(dòng)選擇開(kāi)關(guān)S1選擇控制輸入為L(zhǎng)QR或0，通過(guò)S2選擇初始的角度值。系統(tǒng)仿真采用變步長(zhǎng)的ode45算法，最大仿真步長(zhǎng)為1 ms，仿真時(shí)間為5 s。通過(guò)To Workspace模塊，可以將變量輸出到工作空間，然后再進(jìn)一步的處理。

通過(guò)開(kāi)關(guān)S1選擇輸入u為0，通過(guò)開(kāi)關(guān)S2選擇初始狀態(tài)為[-π/2-0.1;0.3]T。在輸入為0的情況下，主動(dòng)臂和欠驅(qū)動(dòng)臂在豎直向下的穩(wěn)定平衡位置附近自由擺動(dòng)。在沒(méi)有輸入的情況下，主動(dòng)臂角度q1在-π/2附近，欠驅(qū)動(dòng)臂角度q2在0附近做周期性的運(yùn)動(dòng)，主動(dòng)臂和欠驅(qū)動(dòng)臂的角度值如圖3所示。

圖3 角度值q(輸入為0)

下面通過(guò)修改參數(shù)，采用LQR在豎直向上的不穩(wěn)定平衡位置附近進(jìn)行平衡控制。通過(guò)開(kāi)關(guān)S1選擇輸入u為L(zhǎng)QR的輸出，通過(guò)開(kāi)關(guān)S2選擇初始狀態(tài)為[π/2-0.2;0.1]T。在LQR控制器下，角度值如圖4所示，主動(dòng)臂的角度q1趨近于π/2，欠驅(qū)動(dòng)臂的角度q2趨近于0，能進(jìn)行平衡控制。

圖4 角度值(LQR)

3 結(jié) 語(yǔ)

本文以欠驅(qū)動(dòng)兩自由度機(jī)器人Pendubot系統(tǒng)為研究對(duì)象，針對(duì)機(jī)器人動(dòng)力學(xué)方程非常抽象復(fù)雜和不容易理解等特點(diǎn)，采用Matlab/Simulink軟件建立了仿真和控制實(shí)驗(yàn)。根據(jù)動(dòng)力學(xué)方程，采用MATLAB Function模塊建立了系統(tǒng)的仿真模型，并采用LQR進(jìn)行平衡控制。仿真模型結(jié)構(gòu)清晰，容易理解。學(xué)生可以修改系統(tǒng)的參數(shù)，然后進(jìn)行動(dòng)態(tài)仿真和觀察實(shí)驗(yàn)結(jié)果，讓學(xué)生對(duì)機(jī)器人仿真和控制有一個(gè)非常直觀的了解，加深對(duì)理論知識(shí)的理解，增強(qiáng)學(xué)習(xí)的興趣和動(dòng)力，并有助于培養(yǎng)學(xué)生的編程能力和創(chuàng)新意識(shí)。