Giới thiệu chung về robot công nghiệp. Động học vị trí robot. Động lực học robot. Điều khiển chuyển động của robot. Điều khiển tay máy robot có số bậc tự do dư. Mô phỏng robot planar RRR tránh vật cản và điểm cực.Giới thiệu chung về robot công nghiệp. Động học vị trí robot. Động lực học robot. Điều khiển chuyển động của robot. Điều khiển tay máy robot có số bậc tự do dư. Mô phỏng robot planar RRR tránh vật cản và điểm cực.
t=dqd1(in) parasys1; y0=1.39; I2=[1 0;0 0;0 1]; I3=[1 0;0 0;0 1]; q1=in(1); q2=in(2); q3=in(3); q=[q1;q2;q3]; y2=[q1;q2;q3];% chinh la q dq1=in(4); dq2=in(5); dq3=in(6); tt=in(7); dq=[dq1;dq2;dq3]; % Gia tri dat y2d -y2d=[45*pi/180;-70*pi/180;0]; % He so bac tu du H2=35*I2;%40 % Matran gia nghich dao -J11=-l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3); J12=-l2*sin(q1+q2)-l3*sin(q1+q2+q3); J13=-l3*sin(q1+q2+q3); J21=l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3); J22=l2*cos(q1+q2)+l3*cos(q1+q2+q3); J23=l3*cos(q1+q2+q3); J1=[J11 J12 J13;J21 J22 J23]; J1c=pinv(J1); % Tính phan errq va errdq -if tt0 s2=sqrt(1-c2^2); else s2=-sqrt(1-c2^2); end s1=((py-0.3*ny)*(1+c2)-s2*(px-0.3*nx))/((1+c2)^2+s2^2); c1=(px-0.3*nx+s1*s2)/(1+c2); 90 qd1=atan2(s1,c1); qd2=atan2(s2,c2); qd3=phi-qd1-qd2; qd=[qd1;qd2;qd3]; dqd=J1c*[0;-6*(1-t)*t*y0]+(I3-J1c*J1)*H2*(y2d-y2); errq=qd-q; errdq=dqd-dq; outtt=[errq;errdq]; end; if tt>1 t=1; px=1.69; py=y0-(3-2*t)*t^2*y0; phi=q1+q2+q3; nx=cos(phi);ny=sin(phi); c2=((px-0.3*nx)^2+(py-0.3*ny)^2-2)/2; if q2>0 s2=sqrt(1-c2^2); else s2=-sqrt(1-c2^2); end s1=((py-0.3*ny)*(1+c2)-s2*(px-0.3*nx))/((1+c2)^2+s2^2); c1=(px-0.3*nx+s1*s2)/(1+c2); qd1=atan2(s1,c1); qd2=atan2(s2,c2); qd3=phi-qd1-qd2; qd=[qd1;qd2;qd3]; dqd=J1c*[0;-6*(1-t)*t*y0]+(I3-J1c*J1)*H2*(y2d-y2); errq=qd-q; errdq=dqd-dq; outtt=[errq;errdq]; end; out=outtt; Hàm tính H^-1 function out=acce1(in) %da check t1 clc parasys1; tvg(1)=in(1); tvg(2)=in(2); tvg(3)=in(3); tvg=tvg'; q1=in(4); q2=in(5); q3=in(6); H(1,1)=m1*lc1^2+m2*l1^2+m2*lc2^2+m3*l1^2+m3*l2^2+m3*lc3^2+J1+J2+J3+2*(m2*l1*lc2+m3*l1*l2) *cos(q2)+2*m3*l1*lc3*cos(q2+q3)+2*m3*l2*lc3*cos(q3); 91 H(1,2)=m2*lc2^2+m3*l2^2+m3*lc3^2+J2+J3+(m2*l1*lc2+m3*l1*l2)*cos(q2)+m3*l1*lc3*cos(q2+q3)+2* m3*l2*lc3*cos(q3); H(1,3)=m3*lc3^2+J3+m3*l1*lc3*cos(q2+q3)+m3*l2*lc3*cos(q3); H(2,1)=m2*lc2^2+m3*l2^2+m3*lc3^2+J2+J3+(m2*l1*lc2+m3*l1*l2)*cos(q2)+m3*l1*lc3*cos(q2+q3)+2* m3*l2*lc3*cos(q3); H(2,2)=m2*lc2^2+m3*l2^2+m3*lc3^2+J2+J3+2*m3*l2*lc3*cos(q3); H(2,3)=m3*lc3^2+J3+m3*l2*lc3*cos(q3); H(3,1)=m3*lc3^2+J3+m3*l1*lc3*cos(q2+q3)+m3*l2*lc3*cos(q3); H(3,2)=m3*lc3^2+J3+m3*l2*lc3*cos(q3); H(3,3)=m3*lc3^2+J3; out=H\tvg; Hàm tính V(Q,DQ) (ML Function: v_cal1) function out=v_cal1(in) parasys1; dq1=in(1); dq2=in(2); dq3=in(3); q1=in(4); q2=in(5); q3=in(6); h112=-2*(m2*l1*lc2+m3*l1*l2)*sin(q2)-2*m3*l1*lc3*sin(q2+q3); h122=-(m2*l1*lc2+m3*l1*l2)*sin(q2)-m3*l1*lc3*sin(q2+q3); h113=-2*m3*l1*lc3*sin(q2+q3)-2*m3*l2*lc3*sin(q3); h123=-m3*l1*lc3*sin(q2+q3)-2*m3*l2*lc3*sin(q3)-m3*l1*lc3*sin(q2+q3); h133=-m3*l1*lc3*sin(q2+q3)-m3*l2*lc3*sin(q3); h233=-m3*l2*lc3*sin(q3); h212=-(m2*l1*lc2+m3*l1*l2)*sin(q2)-m3*l1*lc3*sin(q2+q3); h213=-m3*l1*lc3*sin(q2+q3)-2*m3*l2*lc3*sin(q3); h223=-2*m3*l2*lc3*sin(q3); h312=-m3*l1*lc3*sin(q2+q3); h313=-m3*l1*lc3*sin(q2+q3)-m3*l2*lc3*sin(q3); h323=-m3*l2*lc3*sin(q3); out(1)=h112*dq1*dq2+h122*dq2^2+h113*dq1*dq3+h123*dq2*dq3+h133*dq3^2; out(2)=h233*dq3^2+h212*dq1*dq2+h213*dq1*dq3+h223*dq2*dq3; out(3)=h312*dq1*dq2+h313*dq1*dq3+h323*dq2*dq3; out=[out(1);out(2);out(3)]; 92 Hàm tính G(Q) (ML Function g_cal1) function out=g_cal1(in) parasys1; q1=in(1); q2=in(2); q3=in(3); out(1)=(m1*g*lc1+m2*g*l1+m3*g*l1)*cos(q1)+(m2*g*lc2+m3*g*l2)*cos(q1+q2)+m3*g*lc3*cos(q1+q2+ q3); out(2)=(m2*g*lc2+m3*g*l2)*cos(q1+q2)+m3*g*lc3*cos(q1+q2+q3); out(3)=m3*g*lc3*cos(q1+q2+q3); out=[out(1);out(2);out(3)]; Hàm tính mơ men điều khiển trường hợp tránh vật cản(ML Function: Kd1) function out =Kd1(in) parasys1; hesodieuchinh1; e1=in(1); e2=in(2); e3=in(3); de1=in(4); de2=in(5); de3=in(6); g1=in(7); g2=in(8); g3=in(9); out=KP*[e1;e2;e3]+KD*[de1;de2;de3]+[g1;g2;g3]; Hàm mô chuyển động Robot (ML Function: ktd) function out=ktd(in) parasys1; q1=in(1); q2=in(2); q3=in(3); xO=0; xA=l1*cos(q1); xB=l1*cos(q1)+l2*cos(q2+q1); xC=l1*cos(q1)+l2*cos(q2+q1)+l3*cos(q1+q2+q3); yO=0; yA=l1*sin(q1); yB=l1*sin(q1)+l2*sin(q2+q1); yC=l1*sin(q1)+l2*sin(q2+q1)+l3*sin(q1+q2+q3); x=[xO;xA;xB;xC];y=[yO;yA;yB;yC]; A=[0.5;0.5;1.2;1.2;0.5]; B=[0;-0.3;-0.3;0;0]; r=plot(x,y,A,B); hold on; text(0.7,-0.15,'\rightarrow Obstacle', 'HorizontalAlignment','left'); out=r; 93 Hàm nhập thông số Robot (ML function: parasys) global m1 m2 m3 l1 l2 g J1 J2 J3 m1=2; m2=; m3=0.7;%kg J1=0.0003; J2=0.00015; J3=0.00015;%kg.m2 l1=1; lc1=0.5; l2=1;%m lc2=0.5; l3=0.3; lc3=0.15; g=9.81; Hàm nhập thông số điều chỉnh (ML function: hesodieuchinh1) global KD KP KP=[100 0 ; 100 ; 0 ]; KD=[50 0 ; 50 ; 0 40 ]; Hàm tính mơ men điều khiển trường hợp tránh điểm cực (ML Function: Kd2) function out=dqd2(in) %1 Thong so robot parasys1; %2 Gia tri dat goc khop(rad) q01=180*pi/180; q02=-170*pi/180; q03=-10*pi/180; %3 Vi tri ban dau: y0=l1*sin(q01)+l2*sin(q01+q02)+l3*sin(q01+q02+q03); x0=l1*cos(q01)+l2*cos(q01+q02)+l3*cos(q01+q02+q03); %4 Ma tran don vi I3=[1 0;0 0;0 1]; %5 Goc khop q1=in(1); q2=in(2); q3=in(3); q=[q1;q2;q3]; %6 Gia tri van toc khop dq1=in(4); dq2=in(5); dq3=in(6); dq=[dq1;dq2;dq3]; %7 Thoi gian tt=in(7); kp=23.5; 94 % Matran gia nghich dao J11=-l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3); J12=-l2*sin(q1+q2)-l3*sin(q1+q2+q3); J13=-l3*sin(q1+q2+q3); J21=l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3); J22=l2*cos(q1+q2)+l3*cos(q1+q2+q3); J23=l3*cos(q1+q2+q3); J1=[J11 J12 J13;J21 J22 J23]; J1c=pinv(J1); % Tinh V(q) -JJ=J1*J1'; JJinv=inv(JJ); V=sqrt(det(JJ)); q11=JJinv(1,1); q12=JJinv(1,2); q21=JJinv(2,1); q22=JJinv(2,2); % Tinh Cxi J11=[-l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3)]; J12=[l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3) l2*cos(q1+q2)+l3*cos(q1+q2+q3) l3*cos(q1+q2+q3)]; J111=[-l1*cos(q1)-l2*cos(q1+q2)-l3*cos(q1+q2+q3) -l2*cos(q1+q2)-l3*cos(q1+q2+q3) l3*cos(q1+q2+q3)]; J121=[-l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3)]; J122=[-l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3)]; J112=[-l2*cos(q1+q2)-l3*cos(q1+q2+q3) -l2*cos(q1+q2)-l3*cos(q1+q2+q3) -l3*cos(q1+q2+q3)]; J113=[-l3*cos(q1+q2+q3) -l3*cos(q1+q2+q3) -l3*cos(q1+q2+q3)]; J123=[-l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3)]; cxi1=0.5*V*(q11*(J111*J11'+J111*J11')+q12*(J111*J12'+J121*J11')+q21*(J121*J11'+J111*J12')+q22*(J 121*J12'+J121*J12')); cxi2=0.5*V*(q11*(J112*J11'+J112*J11')+q12*(J112*J12'+J122*J11')+q21*(J122*J11'+J112*J12')+q22*(J 122*J12'+J122*J12')); cxi3=0.5*V*(q11*(J113*J11'+J113*J11')+q12*(J113*J12'+J123*J11')+q21*(J123*J11'+J113*J12')+q22*(J 123*J12'+J123*J12')); cxi=[cxi1;cxi2;cxi3]; % Tính phan errq va errdq if tt0 s2=sqrt(1-c2^2); else s2=-sqrt(1-c2^2); end s1=((py-0.3*ny)*(1+c2)-s2*(px-0.3*nx))/((1+c2)^2+s2^2); c1=(px-0.3*nx+s1*s2)/(1+c2); qd1=atan2(s1,c1); qd2=atan2(s2,c2); qd3=phi-qd1-qd2; qd=[qd1;qd2;qd3]; dqd=J1c*[0;-(y0+0.1)*6*t*(1-t)]+(I3-J1c*J1)*cxi*kp; errq=qd-q; 95 errdq=dqd-dq; outtt=[errq;errdq]; end; if tt>1 t=1; px=x0; py=y0-(3-2*t)*t^2*(y0+0.1); phi=q1+q2+q3; nx=cos(phi);ny=sin(phi); c2=((px-0.3*nx)^2+(py-0.3*ny)^2-2)/2; if q2>0 s2=sqrt(1-c2^2); else s2=-sqrt(1-c2^2); end s1=((py-0.3*ny)*(1+c2)-s2*(px-0.3*nx))/((1+c2)^2+s2^2); c1=(px-0.3*nx+s1*s2)/(1+c2); qd1=atan2(s1,c1); qd2=atan2(s2,c2); qd3=phi-qd1-qd2; qd=[qd1;qd2;qd3]; dqd=J1c*[0;-(y0+0.1)*6*t*(1-t)]+(I3-J1c*J1)*cxi*kp; errq=qd-q; errdq=dqd-dq; outtt=[errq;errdq]; end; out=outtt; 96 ... lc3=0.15; g=9.81; Hàm nhập thông số điều chỉnh (ML function: hesodieuchinh1) global KD KP KP=[100 0 ; 100 ; 0 ]; KD=[50 0 ; 50 ; 0 40 ]; Hàm tính mơ men điều khiển trường hợp tránh điểm cực (ML... text(0.7,-0.15,' ightarrow Obstacle', 'HorizontalAlignment','left'); out=r; 93 Hàm nhập thông số Robot (ML function: parasys) global m1 m2 m3 l1 l2 g J1 J2 J3 m1=2; m2=; m3=0.7;%kg J1=0.0003;... out(2)=(m2*g*lc2+m3*g*l2)*cos(q1+q2)+m3*g*lc3*cos(q1+q2+q3); out(3)=m3*g*lc3*cos(q1+q2+q3); out=[out(1);out(2);out(3)]; Hàm tính mơ men điều khiển trường hợp tránh vật cản(ML Function: Kd1) function out =Kd1(in) parasys1; hesodieuchinh1;