基于Matlab/Simulink的欠驅(qū)動機械臂仿真實驗

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

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

Pendubot是在垂直平面上運動的欠驅(qū)動兩自由度機械臂[1-3]。Pendubot系統(tǒng)的驅(qū)動電機安裝在第一個關(guān)節(jié),為主動臂；第二個關(guān)節(jié)可以自由運動,為欠驅(qū)動臂。Pendubot系統(tǒng)具有非線性和強耦合等特性,并作為欠驅(qū)動的基準(zhǔn)系統(tǒng),用于驗證復(fù)雜的非線性控制算法。Pendubot系統(tǒng)的控制方式包括擺起控制和平衡控制,在擺起控制和平衡控制之間通過切換控制器進行切換。

Pendubot系統(tǒng)的擺起控制方法主要有部分反饋線性化方法[4]、能量控制方法[5-6]、混雜控制方法[7]、模糊控制方法[8]和滑膜控制方法[9-10]等；平衡控制方法常用線性二次型調(diào)節(jié)器(LQR)。本文在分析Pendubot系統(tǒng)的動力學(xué)方程的基礎(chǔ)上,利用Matlab/Simulink建立Pendubot系統(tǒng)模型,然后采用部分反饋線性化方法進行擺起控制,采用LQR進行平衡控制,并設(shè)計了切換控制器。通過對典型的Pendubot系統(tǒng)進行仿真驗證,向?qū)W生完整地展示系統(tǒng)的控制過程,加深學(xué)生對Pendubot系統(tǒng)的理解,增強學(xué)生的實際動手能力和學(xué)習(xí)興趣,增強欠驅(qū)動系統(tǒng)的理論和實驗教學(xué)效果。

1 Pendubot系統(tǒng)

欠驅(qū)動Pendubot系統(tǒng)由2個關(guān)節(jié)組成。第一個關(guān)節(jié)是主動臂,第二個關(guān)節(jié)是欠驅(qū)動臂,如圖1所示。q1和q2分別表示主動臂和欠驅(qū)動臂角度,m1和m2分別為主動臂和欠驅(qū)動臂的質(zhì)量,l1和l2分別為主動臂和欠驅(qū)動臂長度,lc1和lc2分別表示主動臂和欠驅(qū)動臂相對于質(zhì)心的距離,I1和I2分別表示主動臂和欠驅(qū)動臂的轉(zhuǎn)動慣量。

圖1 欠驅(qū)動兩桿機械臂Pendubot

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

(1)

τ=[τ1，0],為驅(qū)動力矩向量；

為慣性矩陣；

為重力勢能矩陣；

為科里奧利和離心力的結(jié)合矩陣。

(2)

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

(3)

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

2 控制器

2.1 擺起控制器

在Pendubot系統(tǒng)的擺起控制中,部分反饋線性化方法具有算法簡單、驅(qū)動力矩小和擺起時間短等優(yōu)點,因此本文采用部分反饋線性化方法。將Pendubot系統(tǒng)的動力學(xué)方程寫成如下形式：

(4)

(5)

(6)

式中

采用部分反饋線性化進行擺起控制[4],主動臂的驅(qū)動力矩為

(7)

(8)

其中,參數(shù)kp>0和kd>0。當(dāng)Pendubot位于豎直向上的靜止?fàn)顟B(tài)時,主動臂的角度為π/2,角速度為0,控制器的輸入u也為0。

2.2 平衡控制器

在豎直向上的位置x2進行線性化,可以得到

(9)

其中參數(shù)A和B為常數(shù)矩陣。平衡控制器采用LQR,LQR具有很好的魯棒性,目的是在一定的性能指標(biāo)下,使系統(tǒng)的控制效果最佳,即利用最少的控制量來達到最小的狀態(tài)誤差。LQR控制器的力矩為

τ1=-Kx2

(10)

參數(shù)K為最優(yōu)狀態(tài)反饋矩陣,在Matlab軟件中采用函數(shù)lgr()進行計算。函數(shù)lgr()的調(diào)用格式為K=lqr(A,B,Q,R),其中參數(shù)Q為單位矩陣,R為1。通過計算,Pendubot系統(tǒng)在位置x2處的參數(shù)K為

2.3 切換控制器

在擺起控制和平衡控制之間需要進行控制器的切換。主動臂和欠驅(qū)動臂位于豎直向上的位置附近時,進行控制器的切換,控制器切換的條件為

(11)

其中參數(shù)t1為1.4,參數(shù)t2為0.8。式(11)表示主動臂和欠驅(qū)動臂位于豎直向上位置x2附近,進行控制器的切換。當(dāng)由擺起控制器切換為平衡控制器時,一直采用平衡控制器,不能再切換為擺起控制器。

3 仿真實驗平臺

Matlab功能強大、使用簡單方便,并且對問題的描述和求解符合人們的思維習(xí)慣和數(shù)學(xué)表達習(xí)慣,已經(jīng)廣泛應(yīng)用于動態(tài)系統(tǒng)仿真[13]。Simulink是Matlab軟件最重要的組件之一,能夠進行動態(tài)系統(tǒng)建模、仿真和綜合分析。根據(jù)Pendubot系統(tǒng)的動力學(xué)方程,采用Matlab/Simulink軟件建立仿真模型和控制器,控制器包括擺起控制器、平衡控制器和切換控制器。采用Simulink中用戶自定義功能模塊庫中的Matlab Function模塊、積分模塊和使能子系統(tǒng)來建立整個系統(tǒng),如圖2所示。

圖2 Pendubot系統(tǒng)仿真實驗平臺

functionddq = fcn(q, dq, tau1)

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

t1=0.0308; t2=0.0106; t3=0.0095;

t4=0.2086; t5=0.0630;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];

ddq=D([tau1; 0]-C*[dq1; dq2]-G); //公式(2)

根據(jù)式(7)和式(8)可以建立擺起控制器。在式(8)中,參數(shù)kp為44.2,參數(shù)kd為8.8。在圖2中,擺起控制器模塊Swing up內(nèi)的代碼如下：

function tau = fcn(q, dq)

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

t1=0.0308; t2=0.0106; t3=0.0095;

t4=0.2086; t5=0.0630; 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);

fd11=d11-d12*d21/d22;

fc11=c11-d12*c21/d22;

fc12=c12;

fg1=g1-d12*g2/d22;

kp=44.2; kd=8.8;

v1=kp*(pi/2-q1)-kd*dq1; //公式(8)

tau=fd11*v1+fc11*dq1+fc12*dq2+fg1;//式(7)

Pendubot系統(tǒng)的平衡控制器采用LQR,即式(10)。在圖2中,平衡控制器模塊Balance control內(nèi)的代碼如下：

function tau = fcn(q,dq)

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

K=-[40.67, 40.23, 7.54, 5.29];

tau=-K*[q1-pi/2; q2; dq1; dq2]; //公式(10)

切換控制器采用式(11),在圖2中,切換控制器模塊switch control內(nèi)的代碼如下：

function M = fcn(q,dq)

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

t=abs(q1-pi/2)+abs(q2)+0.1*abs(dq1)+...

0.1*abs(dq2);

if q1>1.4 //公式(11)

if t<0.8 M=1; else M=0; end

elseM=0;end

通過切換控制器,將擺起控制器切換為平衡控制器,并通過使能子系統(tǒng),使系統(tǒng)一直采用平衡控制器。在圖2中,主動臂的驅(qū)動力矩通過Switch模塊具體進行切換操作,積分模塊的初始值設(shè)置為初始狀態(tài)x1,通過To Workspace模塊將運行結(jié)果保存在工作空間中。

4 仿真設(shè)計實例

在仿真過程中,采用變步長的ode45算法,最大步長為0.001 s,仿真時間為3 s。采用的Pendubot系統(tǒng)參數(shù)[6]為θ1=0.0308vs2,θ2=0.0106vs2,θ3=0.0095vs2,θ4=0.2086vs2/m,θ5=0.0630vs2/m。

Pendubot系統(tǒng)仿真實驗的運行結(jié)果如圖3和圖4所示。在圖3中,主動臂的最大力矩為3.09 N·m,最小力矩為-0.90 N·m,主動臂的力矩最終趨近于0。在圖4中,Pendubot系統(tǒng)的初始狀態(tài)為豎直向下的位置,q1=-π/2和q2=0。最終穩(wěn)定的狀態(tài)為豎直向上的位置,q1和q2分別趨近于π/2和0。

圖3 主動臂的力矩

圖4 主動臂和欠驅(qū)動臂的角度

當(dāng)采用擺起控制器時,切換信號M為0,當(dāng)切換為平衡控制器時,切換信號M為1。在Pendubot系統(tǒng)控制中,切換信號如圖5所示。圖5中,在0.7168 s控制器由擺起控制器切換為平衡控制器,然后一直采用平衡控制器進行控制。

圖5 切換信號

5 結(jié)語

通過對欠驅(qū)動兩桿機械臂Pendubot的動力學(xué)方程進行分析,采用Matlab/Simulink建立了系統(tǒng)的仿真和控制實驗平臺,擺起控制采用部分反饋線性化方法,平衡控制采用LQR。利用該仿真平臺,可以進行Pendubot系統(tǒng)的控制,有助于學(xué)生對欠驅(qū)動系統(tǒng)理論的理解,能夠提高學(xué)生的學(xué)習(xí)興趣。學(xué)生也可以在該仿真平臺上自己編寫控制算法,提高實際動手能力和創(chuàng)新能力。

[1] 高丙團,陳宏鈞,張曉華.欠驅(qū)動機械系統(tǒng)控制設(shè)計綜述[J].電機與控制學(xué)報,2006,10(5):541-546.

[2] Liu Y, Yu H. A survey of underactuated mechanical systems[J].IET Control Theory and Applications,2013,7(7):1-15.

[3] Spong M W. The swing up control problem for the acrobat[J].IEEE Control Systems Magazine,1995,15(1):49-55.

[4] Spong M W, Block D J. The Pendubot: a mechatronic system for control research and education[C]//Proceedings of the 34th Conference on Decision & Control. LA, USA,1995:555-556.

[5] Fantoni I, Loazno R, Spong M W. Energy based control of the pendubot[J].IEEE Transactions on Automatic Control,2000,45(4):725-729.

[6] Xin X, Tanaka S, She J, et al. New analytical results of energy-based swing-up control for the Pendubot[J].International Journal of Non-linear Mechanics,2013,52(3):110-118.

[7] Zhang M, Tarn T J. Hybrid control of the pendubot[J].IEEE/ASME Transactions on Mechatronics,2002,7(1):79-86.

[8] Li W, Tanaka K, Wang H O. Acrobatic control of a Pendubot[J].IEEE Transactions on Fuzzy Systems,2004,12(4):549-552.

[9] 王偉,易建強,趙冬斌,等.Pendubot的一種分層滑膜控制方法[J].控制理論與應(yīng)用,2005,22(3):417-422.

[10] 牛瑞燕,許午嘯,劉金琨.欠驅(qū)動機械臂滑膜控制與實驗研究[J].儀器儀表學(xué)報,2016,37(2):348-355.

[11] Lai X Z, She J H, Yang S X, et al. Comprehensive unified control strategy for underactuated two-link manipulators[J].IEEE Transactions on Systems: Man & Cybernetics Part B: Cybernetics,2009,39(2):389-398.

[12] 蔡自興.機器人學(xué)[M].2版.北京:清華大學(xué)出版社,2009.

[13] 薛定宇,陳陽泉.基于MATLAB/Simulink的系統(tǒng)仿真技術(shù)與應(yīng)用[M].2版.北京:清華大學(xué)出版社,2011.