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

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

0 引 言

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

在機(jī)器人的教材中，機(jī)器人動力學(xué)部分均以兩自由度機(jī)器人為研究對象[3]。Pendubot系統(tǒng)是一種典型的欠驅(qū)動兩自由度機(jī)器人[4]，由在垂直平面上運(yùn)動的兩個(gè)關(guān)節(jié)和一個(gè)驅(qū)動電機(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)對控制方法進(jìn)行驗(yàn)證。

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

1 Pendubot系統(tǒng)

1.1 動力學(xué)方程

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

圖1 Pendubot模型

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

(1)

(2)

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

(3)

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

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

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

τ1=K1K2u

(4)

式中：K為電動機(jī)的轉(zhuǎn)矩常數(shù)，取值為0.412 5 N·m/A；K2為驅(qū)動器的增益系數(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)仿真和自動控制領(lǐng)域普遍采用的計(jì)算機(jī)輔助設(shè)計(jì)工具[13-15]。在Simulink中采用普通的模塊搭建復(fù)雜控制系統(tǒng)非常的困難，采用User-Defined Functions庫中的MATLAB Function模塊，可以采用MATLAB語言非常方便靈活的建立復(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)

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

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

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

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

圖4 角度值(LQR)

3 結(jié) 語

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