Điều khiển Robot

Luận Văn: Điều khiển Robot

từ vị trí [0,4: 0,0 m] đến [0,0: 0,4 m] theo một đờng thẳng Đồ thị tốc độ đặt trớc của tay Robot dọc theo quỹ đạo cho ở hình 2.

4 Thiết kế “bộ điều khiển bù trọng lực” cho Robot.

5 Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1 thay đổi nhảy cấp từ 0 đến 1 rad.

Hình 2 Đồ thị tốc độ.

Bảng thông số của Robot:

5 Momen quán tính khớp 1 quay quanh tâm khối (J1) 0,528 Kgm2

6 Momen quán tính khớp 2 quay quanh tâm khối (J2) 0,19 Kgm2

7 Khoảng cách từ khớp 1 đến tâm khối 1 (lg1) 0,25 m8 Khoảng cách từ khớp 2 đến tâm khối 2 (lg2) 0,15 m9 Hằng số momen của động cơ khớp 1, 2 (KM) 0,5 Nm/A

11 Momen lớn nhất của động cơ khớp 1 (M1max) 1,2 Nm12 Momen lớn nhất của động cơ khớp 2 (M2max) 0,7 Nm13 Tốc độ lớn nhất của động cơ khớp 1, 2 (ωmax) 90 rad/s14 Momen quán tính của động cơ (JĐ) 0,0004 Kgm2

Bài giải

0 0.25 0.75 1.0 t(s)V (m/s)

Câu 1 Xây dựng ph ơng trình động học thuận và ng ợc biểu diễn mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints)

T : biểu diễn hệ toạ độ thanh thứ n so với hệ toạ độ gốc.

nT : biểu diễn hệ toạ độ tay (điểm kẹp) so với thanh n.

E0

Theo đó ta xác định đợc các ma trận biến đổi A của robot nh sau:

(cθ =cosθ;sθ =sinθ)

Thay vào (1) ta đợc:

paon

Theo (1) ta cã:

2110E

( ) 21E011

tơng đơng với:

Cân bằng vế trái và phải của (5), ta có:

(6)

Với

Bình phơng hai vế của hai phơng trình trong (6) và cộng vế ta có:

Đặt :

ta có:

Thay (7) vào (6) ta đợc:

Giải ra ta có:

trong đó:

Vậy phơng trình động học ngợc của hệ đã cho đợc biểu diễn dới dạng

Với :

7

Câu 2: Xây dựng quan hệ giữa tốc độ các khớp và tốc độ của tay Robot

Đạo hàm hai vế phơng trình động học thuận theo thời gian ta có:

Đặt:

(J: ma trận Jacobien)

Phơng trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của tay Robot đợc biểu diễn dới dạng:

(12)

Với:

12

Câu 3 Viết hàm MATLAB thực hiện các ph ơng trình ở câu 1 và 2 và vẽ đ ờng biểu diễn vị trí và tốc độ khớp khi ta y Robot di chuyển từ vị trí [0,4:0,0] đến [0,0:0,4] theo một đ ờng thẳng Đồ thị tốc độ của tay Robot đã cho nh hình 2

d1=atan2(y,x)-atan2(k1,k3); %Goc theta 1 tinh theo radian.d2=atan2(k1,k2);%Goc theta 2 tinh theo radian.d(1)=d1*360/(2*pi)%Goc theta 1 tinh theo do.d(2)=d2*360/(2*pi)%Goc theta 2 tinh theo do.

9

l1*cos(d1)+l2*cos(d1+d2),l2*cos(d1+d2)]% Ma tran Jacobien

v=[vx;vy]; % Vecto van toc theo truc x va y.

vd=inv(J)*v; % Van toc goc cua khop 1 va 2 tinh theo radian/s

d Vẽ đờng biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0 m] đến [0,0:0,4 m] theo một đờng thẳng

Dựa vào dạng đồ thị tốc độ ở hình 2 ta có quỹ đạo chuyển động của tay robot là quỹ đạo 2-1-2 Mặt khác tay Robot di chuyển từ vị trí [0,4: 0,0 m] đến [0,0: 0,4 m] theo một đờng thẳng thực chất là chuyển động tịnh tiến theo cả hai trục x và y

Xét khoảng thời gian từ 0ữ0,25s, tay Robot di chuyển theo quỹ đạo bậc 2:

Các điều kiện biên gồm:

Xét khoảng thời gian từ 0,25ữ0,75s, robot di chuyển theo quỹ đạo bậc 1:

Từ điều kiện biên suy ra:

Xét khoảng thời gian từ 0,75ữ1s, robot di chuyển theo quỹ đạo bậc 2:

Dựa vào các điều kiện biên có:

⇒

Xét khoảng thời gian sau 1 giây ta có:

⇒

Mặt khác để đảm bảo tính liên tục của quỹ đạo ta có:

Vậy ta có:

(0,40,125V0,5V ) 00,25

(0,125V0,5V ) 0,40,25

sm5333,0Vxmax =−

sm5333,0Vymax =

Thay vào các hệ số của phơng trình quỹ đạo chuyển động ở trên ta đợc phơng trình quỹ đạo chuyển động của robot nh sau:

11

( )()

t=i/N; time(i+1)=t; if t<=0.25

x(i+1)=-1.0666*t*t+0.4; vx(i+1)=-2.1332*t; y(i+1)=1.0666*t*t; vy(i+1)=2.1332*t;

elseif (t>0.25)&(t<=0.75)

x(i+1)=-0.5333*(t-0.25)+0.3333; vx(i+1)=-0.5333;

y(i+1)=0.5333*(t-0.25)+0.0667; vy(i+1)=0.5333;

elseif(t>0.75)&(t<=1)

x(i+1)=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666; vx(i+1)=2.1332*(t-0.75)-0.5333;

y(i+1)=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333; vy(i+1)=-2.1332*(t-0.75)+0.5333;

else

x(i+1)=0; y(i+1)=0.4; end %if

Hình 3 Quỹ đạo chuyển động của tay robot

Để xác định tốc độ và vị trí các khớp, cần tính đợc các giá trị x,y rời rạc của tay Robot sau đó thông qua phơng trình động học ngợc xác định góc quay θ1 và θ2 Biểu diễn θ1 và θ2 trên đồ thị thời gian, ta đợc đồ thị biểu diễn vị trí của khớp Đạo hàm chuyển động của từng khớp theo thời gian ta sẽ đợc dạng đờng vận tốc của khớp.

Quỹ đạo chuyển động khớp:

function cdkhop=cdkhop(N)%N so diem quy dao can vel1=0.4;

l2=0.3;for i=0:N t=i/N; time(i+1)=t; if t<=0.25

x=-1.0666*t*t+0.4; vx=-2.1332*t; y=1.0666*t*t; vy=2.1332*t;

elseif (t>0.25)&(t<=0.75) x=-0.5333*(t-0.25)+0.3333; vx=-0.5333;

y=0.5333*(t-0.25)+0.0667; vy(i+1)=0.5333;

elseif(t>0.75)&(t<=1)

x=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666; vx=2.1332*(t-0.75)-0.5333;

y=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333; vy=-2.1332*(t-0.75)+0.5333;

else x=0; y=0.4;

end %if

d=Nguoc(x,y,l1,l2); theta1(i+1)=d(1); theta2(i+1)=d(2);end %for

Chạy chơng trình với số điểm là 150 ta đợc:

>> cdkhop(150)

Hình 4 Quỹ đạo chuyển động của các khớp

Chơng trình tính toán và vẽ đồ thị vận tốc chuyển động của khớp:

function vkhop=vkhop(N)%N so diem quy dao can vel1=0.4;

l2=0.3;for i=0:N

15

t=i/N; time(i+1)=t; if t<=0.25

x=-1.0666*t*t+0.4; vx=-2.1332*t; y=1.0666*t*t; vy=2.1332*t;

elseif (t>0.25)&(t<=0.75) x=-0.5333*(t-0.25)+0.3333; vx=-0.5333;

y=0.5333*(t-0.25)+0.0667; vy(i+1)=0.5333;

elseif(t>0.75)&(t<=1)

x=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666; vx=2.1332*(t-0.75)-0.5333;

y=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333; vy=-2.1332*(t-0.75)+0.5333;

else x=0; y=0.4; end %if

d=Nguoc(x,y,l1,l2); theta1(i+1)=d(1); theta2(i+1)=d(2); end %for

% Lay dao ham chuyen dong khop theo goc quay cua tung khop.for i=1:N-1

omega1(i)=(theta1(i+1)-theta1(i))*N; % dw1/dt omega2(i)=(theta2(i+1)-theta2(i))*N; % dw2/dtend;

ylabel('Van toc goc(do/s)');

Chạy chơng trình với số điểm vẽ N=150 ta đợc:

>>vkhop(150);

Hình 5 Đồ thị vận tốc chuyển động của khớp

17

Câu 4 Thiết kế bộ điều khiển bù trọng lực cho robot

a Xây dựng phơng trình động lực học

Phơng trình động lực học của hệ thống:

( )qqC( )q,qqFqg( )qMH +   +  + =

Ta cần xác định đợc các hệ số của phơng trình trên Đối với robot Planar ta có (bỏ qua thành phần F):

cos6,02555,2

15,0.4,0.215,04,0.5528,025,0.10

cos3,03025,0

.15,0.4,015,0.5

cos3,03025,0

3025,0

( 11212)

12222mglcg =

b Xây dựng luật điều khiển

Các hệ thống điều khiển có phản hồi nhằm tạo ra khả năng chống nhiễu tốt Nhng khi cần bám sát quỹ đạo với tốc độ và gia tốc lớn thì chúng không đáp ứng đợc Cơ chế điều khiển có bù cho phép giảm sai số quỹ đạo.

Việc xác định tín hiệu điều khiển sao cho hệ thống ổn định xung quanh điểm cân bằng dựa trên phơng pháp ổn định Lyapunov.

Gọi vector trạng thái của hệ thống là [ TT]T

V  =  +    + 

Từ phơng trình động lực học của hệ thống:

( )qqC( )q,qqFqg( )qMH +   +  + =

Vậy ta có:

21

Câu 5: Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1 thay đổi nhảy cấp từ 0 radian đến 1 radian

Mô phỏng bằng SIMULINK nh hình vẽ

Hình 10 Mô phỏng hệ bằng Simulink

Hình 11 Kết quả mô phỏng - đáp ứng đầu

H×nh 12 KÕt qu¶ m« pháng, sai lÖch tÜnh cña hÖ

21

Tµi liÖu tham kh¶o

[1] NguyÔn M¹nh TiÕn “Bµi gi¶ng m«n Robot cao häc n¨m 2002”.[2] Lorenzo Sciavicco, Bruno Sciliano “Modeling and Control of Robot Manipulators”.