matlab仿真--二自由度機械臂動態(tài)仿真

機電系統(tǒng)的動力與運動的計算機仿真基于二自由度兩連桿平面機器人系統(tǒng)仿真馬國鋒梁應海周凱（武漢理工大學機電工程學院機械工程及門動化系）摘要：平面兩連桿機器人（機械臂）是-種簡單的兩自山度的機械裝冒，其具有一定的復雜動力特 性，對其的簡單研究能夠對機電系統(tǒng)和機器人冇更好的學習了解和認識。利用matlab仿真的快捷，簡 潔，以及可視化操作可以使其研究更方便，以及利用PID調節(jié)，使系統(tǒng)具有更好的時間響應性能。關鍵詞：matlab仿真 PID控制調節(jié)平面機器人伺服直流電動機Abstract: The Planar two-link robot （Robot Arm） is a simple mechanical device of two degrees of freedom, it has complex dynamic characteristics. We can gain better learning and understanding for Mechanical and Electrical systems and Robots only through studying 讓 simply! Using the superior performance of MATLAB zwe can make the research more convenient,besides ,we also can make the system have better performance in Time Response through the PID correction.0、引言隨著科學技術的發(fā)展，利用計算機對控制系統(tǒng)進行仿真和分析，是研究控制系統(tǒng)的重要方 法。對控制系統(tǒng)進行仿真，首先應該建立系統(tǒng)模型，然后根據(jù)系統(tǒng)模型進行仿真,并充分的利用 計算機作為工具進行數(shù)值求解。Mat lab是目前應用最為廣泛的仿真語言之一。該軟件具有以 下特點：數(shù)值計算功能強大：編程環(huán)簡單：數(shù)據(jù)可視化功能強：豐富的程序工具箱；可擴展 性能強等。Simulink是MATLAB下用于建立系統(tǒng)框圖和仿真的環(huán)境。Simulink環(huán)境仿真的優(yōu) 點是：框圖搭建方便、仿真參數(shù)可以隨時修改、可實現(xiàn)完全可視化編程。并且可以再仿真過 程中進行系統(tǒng)的相關調節(jié)，利用PID校正或相位滯后校正使系統(tǒng)具有更優(yōu)的性能。本文就從系統(tǒng)仿真和調節(jié)以及運動過程的可視化進行研究說明1、二自由度兩連桿平面機器人系統(tǒng)模型介紹下圖為一個兩連桿平面機器人的三維示意圖以及簡單的平而示意圖這個例子在機器人學文獻中經(jīng)常能夠遇到，它為平面機器人最為簡單的形式，由兩根連桿和 兩個由電機(伺服直流電機)驅動的兩個轉動狡所組成，其具有一定的復雜動力特性，本文 就將對這個系統(tǒng)的伺服電機進行仿真與調節(jié)，并且導出該機構的動力方程，其動力學仿真就 要求再有兩個輸入?yún)?shù)下的運動問題。2直流伺服電機系統(tǒng)仿真2.1直流電機的物理模型由于直流電動機具有良好的啟動性能 和調速性能，而機械臂由于調速要求高，正反 轉和啟制動頻繁，所以仍選用直流伺服電機來 驅動。右圖為直流伺服電機線路示意圖。圖中, Ra、La分別為電樞繞組的電阻和電感，ia為電 樞電流，RfLf分別為勵磁電路的電阻和電感,if為勵磁電流，ef為磁場勵磁電HG &為加到電樞上的電床，而eb為電樞中的反電動勢；9。為電動機的軸的角位移，Tm為電動機產(chǎn)生的轉矩，Tn為負載；J、B分別為電動機和負載折 算到電動機軸上的等效轉動慣量和粘性阻尼系數(shù)。2. 2直流電機的數(shù)學模型由控制輸入電床6旳開始，系統(tǒng)的因果方程式為:電樞電壓方程：R 2匚+叩亠(0 = ex (t) - q (t)電動機轉矩方程:mW)轉矩平衡方程：+ctt"dt電動機的反電動勢正比于速度：q(t) = %2%(t)dt其中反電動勢常數(shù)根據(jù)本系統(tǒng)，查找相關資料以及計算，設置其中電機的參數(shù)如卜：J=0. 016kg m B=0. lN/m s1; K=KbM(t=0. 04N m/A;R=Ra=lQ ; L=La=O. 01H;現(xiàn)利用兩種方法來進行模型的建立:(1)拉普拉斯變化和simulink相結合進行建立 對上面四個方程進行變換如下：(Las + Rj Ia(s) = E1(s)-Eb(s TmCXiqiaG)(Js2 + Bs) )o(s)二R(s)A(s)Eb(s) = Kbs0(s)則建立系統(tǒng)方框圖如下：利用simulink進行建模當負載轉矩為0時，由上式消去中間變量，可以得到電樞控制伺服電機以e(t)為輸入量、 久(t)為輸出量的傳遞函數(shù)，即：G麗色血=咚 =冷 Ei(s) s(-s+&)(Js+B)+KTKbs(s?+2紐+血)若得到輸出為角速度的傳遞函數(shù)時，由角速度為角度的微分，即需在上述傳遞函數(shù)中乘 以S即可。將設置的參數(shù)帶入方程屮既可以得到系統(tǒng)傳遞函數(shù)方程式:當輸出為角度時:250當輸出為角度時:250當輸出為角速度時:S (sA2 +106.3 s+ 635)250sA2 + 106.3 s + 635(2)、可以利用空間狀態(tài)方程來進行系統(tǒng)的建模 由系統(tǒng)的數(shù)學模型可以得到下面的矩陣關系式:1B7K0KTRLe(i)0 = (0 1 0) 0Imatlab編程,并利用sys=ss(A,B,C,D)指令進行編程,程序如下:當輸出為角度時:250J=0.016;B=0. 1;K=O. 04;R=1.L=O. 01;A=0, 1?0;0? -B/J, K/J;0, -K/-R/L; 氐血 0,1/L5 ;mcu；D=0;sys=ss(Aj Bj CjD)SYSl=tf (sys)輸出為角速度模型程序得到傳遞函數(shù)如下：Transfer function:250J=0. 016;B=0. l;K=0. 04;R=.;L=0. 01;A二 Q 1, U; 0, -B/L K/J ; 0, -K/L, -R/L;B= D 0 J/L;0=11. 0,0;D=0;sys=ss (Aj C, D)SYS2=tf (sys)輸出為角度模型程序Transfer function:250當輸出為角度時:250當輸出為角度時:250sA2 + 106.3 s + 635s3 + 106. 32 + 035 s3直流伺服電機系統(tǒng)PID控制調節(jié)校正3.1 PID簡介PID控制是將設定値r(t)與輸出反饋值C(t)的偏差e(t)=r(t)-C(t),按比例、積分、微分 運算后，并通過線性組合構成控制量u(t),對控制對象進行控制，如圖所示，所以簡稱為P(比 例)、1(積分)、D(微分)控制器。de(t)"ITPID控制器的結構方程u(t) = KpC(t) + KJ e(t)dt + Kd式中，e是偏差量，即輸出量與設定值之間的差值：u是控制量，作用于被控制對象并 引起輸出量的變化;Kp是比例增益系數(shù)，其控制效果是減少響應曲線的上升時間及靜態(tài)誤差， 但不能消除豫態(tài)誤差：Ki是積分部分的增益系數(shù)，控制效果是消除穩(wěn)態(tài)誤差：Kd是微分部分 的增益系數(shù)，其控制效果是增強系統(tǒng)的穩(wěn)定性，減少過渡時間過程，降低超調量。在應用過 程屮，可能會用PI、PD或PID控制，本系統(tǒng)只利用PI控制器進行校正優(yōu)化。3. 2 simulink動態(tài)仿真以及系統(tǒng)校正本文所述電機系統(tǒng)要求在電圧輸入端輸入單位階躍電床(1V)后，直流伺服電機的轉 軸應能輸出穩(wěn)泄的轉角，且應同時滿足下列要求：系統(tǒng)調整時間ts<o.is,最大超調量Mp< 15%,系統(tǒng)穩(wěn)態(tài)誤差ess=l%o(1)系統(tǒng)階躍輸入響應仿真在Simulink中建立模型如下，設置仿真環(huán)境的參數(shù)。運行后顯示如圖所示閉環(huán)系統(tǒng)對單 位階躍輸入的響應，即直流伺服電機輸入單位階躍電壓時，電機的輸岀轉角速度曲線。并觀 察其頻率響應。山得出的系統(tǒng)方程將仿真系統(tǒng)進行簡化得從而得到的單位階躍響應為:0350.3由但為階躍響應可知，其不能滿足系統(tǒng)的要求?，F(xiàn)觀察頻域響應:S9S7-9035180Jo30Bode Diagram -200-040-100-1200通過觀察，系統(tǒng)的時間響應明顯不滿足要求，而其頻率性能良好，故需要通過PID調節(jié) 進行控制，使其具有良好的階躍響應性能。(2) PID調節(jié)1建立PID調節(jié)模型，如下圖n并利用程序進行系統(tǒng)階躍響應：a/bzc/d=linmod2(,all,);% all是你繪制的框圖名，可任意命名 sys=ss(a,b,c,d);SYS=tf(sys)step(sys)ffl 首先，針對其穩(wěn)態(tài)誤差進行積分增益系數(shù)的確定，系統(tǒng)對階躍響應的穩(wěn)態(tài)誤差為: 利用matlab編程如下：J=0.016;B=0.1;K=0.04;R=1;L=O.O1;A=O,1,O;O,-B/J,K/J;O,“L,R/L; B=OzO/l/Ll，；C=l/0,0;D=0;sys=ss(AzB,C,D)SYS=tf(sys) ess=0.01 ki=l/dcgain(SYS)/ess 則 ki=254Transfer funct xon:250e A 2 + 1 063 e + 635ess =0 OLOOkx =254 0000S kp=l, ki=254, kd=O時其仿真如下2|System: sysPeak amplitude: 1.82 Overshoot (%): 82 4 At time (sec): 0 13Step Response1.61.42 18 1 oSystem: sysSettling Time (sec): 3 450.60.40.2051.522.533.5Time (sec)IV現(xiàn)對kp進行調節(jié)對kp取值如下并進行觀察單位階躍響應曲線:Overshoot (%) 14 6Peak ampMude 1 15Kp=20> ki=253, kd=O發(fā)現(xiàn)在隨著kd的增加其穩(wěn)定時間在減小，超調最也在減小，故仍需要增加kd比例 增益系數(shù)值Kp=30, ki=253, kd=OKp=40t ki=253, kd=O通過觀察，當大于40時，隨著kp增加，穩(wěn)定時間在減少，但超調量在增加，故需要將 kp值設置在30到40之間，通過以上分析可知，隨著kp值的增加，其穩(wěn)定時間在減小，但超 調量先減小，后增加故對30和40之間進行試驗，通過實驗和調節(jié)，得到kp最佳值為33,這 是的響應曲線為：00.050.10.15Time (sec)系統(tǒng)的上升時間為0.02s,超調量為14.4%,穩(wěn)定時間為0.0703,滿足系統(tǒng)時域要求。V分析其頻率特性，觀察其頻域響應,其bode圖如下:Q Figure 1File Edit Vievk Insert Tools Desktop 空ndow HelpD旨口俸fe釵QOS）眶 0 口口(Eop) OOELCfipepnuuBes101 102 103Froquoncy (rad/eoc)通過分析知其幅值裕度為無窮大，相位穿越頻率為無窮大。相位裕度為S4.5,幅值穿越 頻率為68.3rad/s,系統(tǒng)是穩(wěn)定。對系統(tǒng)不再進行微分校正，通過PD校正，可知系統(tǒng)的時域和頻域響應性能都比較好4、對利用matlab對機械臂進行動力學分析(1)模型的簡介建立的模型如下：為方便對其進行分析，做一下假設：連桿的密度是均勻的， rl=O. 8, rcl=O. 4, r2=0. 6, rc2=0. 3,連桿質星ml二4kg, m2=2. 8kg,并計算得到兩連桿各自質心的 轉動慣量分別為：11=0. 16kg -m2 , 12=0.063 kg m2 ,其所承受重量為mpl=2kgo(2)運動學方程的建立其中以第一較鏈點為坐標原點，則其手部點的矢量方程為：Rpi=Ri+R2需要注意的是下面所給的角度中，都是相對于前一連桿的方位，而不是相對于整體坐標 系x軸的，這種習慣在機器人學中很普遍，其源于安裝在及其人手臂的傳感器所測得的是連桿 的相對轉角，而不是絕對轉角。對應的標量方程為：xpi = r cos(?i + r2cos(0i -b 62)yPi = ri sinft +%)對上式進行求導得:工訊=rtwjsin r2（ttj）+?）sin（!?i + 仇）yPi = 門oncost + 廠2（仙"Taj：>cos（ +%）轉化為矩陣如下：并有G? = cos（久十念）,Sn =sin（仿 +）dpi r 廠】$ r?S2 尸？Si2 -yPi J L 門 G + r2 C12 r2 Ci2 .通過查找相關資料可以得到以下方程：Xpi 4-（H Si + 廠2S12）tfi + FSjai = （尸 1C + "C2+ eG2< + 2卩23gGd ypl （八 G 十 Ciz ）a】一=1 廠1 S + 廠2 Si2 ）淤十廠2 S2血 + 22I1 CM2 $2 ad+cSg rd Cj"Gai = rSi a>i+（H Sj + rc2 S12 ）ai +rkSi2G =（廠iC+nG2o/? + eG?応+2&（ui 妙 Cj4,y + "1G rclC2 ）ai rC3（Z2 =一 （口 Si +&Si2）tt 4" Ta+223蝕 Sj（3人動力學方程第一個連桿的受力分析和相關方程F 01 “Fiji+ F?1,J =Few + Em rnxg = mxacX,yM M2 F21/1S1 +F2i./G mgrci Ci = IiQi 第二個連桿的受力分析及運動方程：Fg Fzj.z =F3川一 Eg 一叫g = m<2.y閥-Fz廠&S12 + 尸2】八2G2 月2"（七一 fQSk+Fj2，（廠2 PC】2 = hat所受負載的的動力學分析和方程：、J叫S =一 幾2“ 嘰叫g叫g = F32r> mplg綜上所分析，共有六個運動學方程，有八個動力學方程，聯(lián)立這十四個方程 （4）建立約束矩陣聯(lián)立這14個方程如下：小"2殆Z200001000000o 1i000001000000 a2汕0100000000000cU001000000000lrrl5l *112Gia001000000000%22000100000000心y0000000101000A/iU000-眄0000010100A000000000貞"口000000加、A00000-101000000處200000T01hx0h00000000V2vn。:1C|?“-巧2lly000000mpt0（JD0e10Slx.0000000葉00000132y 一 qq +勺qjto?十Ge? + 2勺叫c】21-（rjSj + r2sxl十2曲七234弭門-乜G斫-詁研TSG + G2G2冏 + rc2Cl2i + 2力3|5久 J亠+力殆）十一22囲十2:22250叫T| 一乃一叫jG0叫20-咻根據(jù)方程建立simulink框圖，并進行編程仍真:其中建立的simulink模型如卜:編寫的仿真程序為： 文件名robot. mfunct ion out=robot(u)%u(l)=wl%u(2)=sl%u(3)二 w2%u(4)=s2%u(5) =torl%u(6) =tor2 g=9. 8067;rl=0. 8;rcl=0. 4;r2=0. 6; rc2=0. 3; ml=4;m2=2. 8;11=0. 16; 12=0. 063;mpl=2;sl=sin(u(2);sl2=sin(u(2)+u(4); cl=cos(u(2);cl2=cos(u(2)+u(4); a=zeros(14, 14);b=zeros (14, 1);a(l, I)=rl*sl+r2*sl2; a(l, 2)=r2*sl2; a(l, 7)=1;a(2, l)=-rl*cl-r2*cl2; a(2, 2)=-r2*cl2; a(2, 8)=1;a(3, l)=rcl*sl; a(3, 3)=1;a(4, l)=-rcl*cl; a(4, 4)=1;a(5, I)=rl*sl+rc2*sl2; a(5, 2)=rc2*sl2; a(5, 5) =1a(6, l)=-rl*cl-rc2*cl2; a(6, 2)=-rc2*cl2; a(6, 6) =1;a(7, 3)二一ml;a(7, 9) =1;a(7, 11) =1;a(8, 4) =-ml; a(8, 10)=1; a(8, 12)=1;a(9, l)=Il;a(9, U)=rl*sl;a(9, 12)=-rl*cl;a(10, 5)=-m2;a(10, ll)=-l;a(10, 13)=1;a(ll, 6)=-m2;a(U, 12)=-l;a(ll, 14)=1;a(12, 2) =12;a(12, 11) =rc2*sl2;a(12, 12)=-rc2*cl2.a(12, 13) = (r2-rc2)*sl2;a(12, 14)=-(r2-rc2)*d2;a(13, 7) =mpl; a(13, 13) =1;a(14, 8)=mpl;a(14, 14)=1;%b(1) =-(r 1 *c 1+r2*c 12)*u(l)*2+r2*c 12*u(3)*2+2*r2*u(l)*u(3)*cl2); b(2) =-(rl*sl+r2*sl2)*u(l) 2+r2*sl2*u(3) 2+2*r2*u(l)*u(3)*sl2); b(3)=-rcl*cl*u(l) 2;b(4)=-rcl*sl*u(l) 2;b(5)=-(rl*cl+rc2*cl2)*u(l) 2+rc2*cl2*u(3) 2+2*rc2*u(1)*u(3)*c12);b(6)=-(rl*sl+rc2*sl2)*u(l) 2+rc2*sl2*u(3)"2+2*rc2*u(l)*u(3)*sl2); b(8)二 ml*g;b(9)=u(5)u(6)-ml*g*rcl*cl;b仃1)=m2*g;b(12)=u(6);b(14)=-mpl*g;%out=inv(a)*bMATLAB Fen兩數(shù)的設置如F：0 Bloek Parameters! MATLAB Fen仿真結果當系統(tǒng)輸入轉矩都為0.且處于下圖所示位宣時（8 1二tt/2, 6 2=0）,在重力作用及軸 承等縻擦粘滯阻尼而產(chǎn)生的數(shù)度損失系數(shù)為2這時利用matlab進行系統(tǒng)仿真，輸出四個數(shù)據(jù)，分別為連桿一的角度，連桿二的角度。 端點P的X方向，Y方向點的變化坐標這時示波器顯示如卜：Q $copl1 o T尋23MOQ e A在這樣的條件下，端點p的坐標軌跡的求法：利用to workspace模塊對仿真程序的數(shù)據(jù)進行保存。保存數(shù)組名稱為simout,然后讓輸岀 的P點X方向坐標為橫坐標，丫方向坐標為從坐標畫圖，并修飾，程序如下：plot(simout(1:60,3),simout(1:60,4),b);hold on;plot(simout(60:115,3),simout(60:115,4),r);hold on;plot(simout(115:171,3),simout(115:171,4),/);hold on;plot(simout(171:201,3),simout(171:201,4),g);grid on;box off;title(卩點運動軌跡)；xlabel(卩點橫坐標);ylabel(卩點縱坐標)；legendf第一次順時針運動第一次逆時針運動第二次順時針運動T第二次逆時針運 動)；得到如下圖： . - h 第一次頗時針運動 第一次逆時針運動 第二次妙時針運動 第二次逆時苕運動Vn 1r1P點運動軌進2 4 6 8-O-O4-O.:+-.12槪坐標當系統(tǒng)輸入轉矩T1=T2=0,且處于卜圖所示位置時(8 1=0, 0 2=0)驅動及巫力作用及軸承等摩擦粘滯阻尼而產(chǎn)生的數(shù)度損失系數(shù)為2利用to workspace模塊對仿真程序的數(shù)據(jù)進行保存。保存數(shù)組名稱為simout,然后讓輸出的P 點X方向坐標為橫坐標，丫方向坐標為從坐標畫圖，并修飾得到如下圖：Q5005P點構坐標0 2 40 06 80 0歸斛S取d第一次時針運動 第一次謹時針運動 箱二次顫時針運動 第二次逆時竹運動P點運動91進當系統(tǒng)輸入轉矩T1=5. T2=0,且處于位置時(6 1=0, 0 2=0),在驅動及更力作用及軸承等 摩擦粘滯阻尼而產(chǎn)生的數(shù)度損失系數(shù)為2利用to workspace模塊對仿真程序的數(shù)據(jù)進行保存。保存數(shù)組名稱為simout,然后讓輸出的P 點X方向坐標為橫坐標，丫方向坐標為從坐標畫圖，并修飾，得到如下圖：0204-06-08薦一次談時針運動第一次逆時針運動第二次議時針運動第次 ifritiain-05(6 1=0? 0 2=0)，在驅動及重力作用當系統(tǒng)輸入轉矩T1=5. T2=0,且處于下圖所示位置時 及軸承等摩擦粘滯阻尼而產(chǎn)生的數(shù)度損失系數(shù)為20P點椅生標0.50P點椅生標0.51.5P點運動軌跡0.5005-1怎部袞旺d0P點椅生標0.50P點椅生標0.5由此，可以得到此次仿真是完全成功的.0P點椅生標0.5