Bài tập lớn Kĩ thuật Robot HUST

Xác định ma trận T biểu diễn hệ tọa độ tay Robot c.. Xây dựng hệ tọa độ cho các thanh nối Robot có cơ cấu tay dạng cầu với 2 khớp quay và 1 khớp tịnh tiến.. Chọn khung tọa độ góc O và c

MỤC LỤC

Đề bài 2

Câu 1: 3

a Xây dựng hệ tọa độ cho các thanh nối 3

b Xác định ma trận T biểu diễn hệ tọa độ tay Robot 3

c Giải thích ý nghĩa của ma trận T 5

d Xác định vị trí của tay Robot trong hệ tọa độ gốc 5

Câu 2: 7

a Momen ở khớp quay và lực tổng ở khớp tịnh tiến khi Robot ở cuối hành trình chuyển động 7

b Thiết kế bộ điều khiển phản hồi PD cho từng khớp 10

c Mô phỏng hệ thống bằng Matlab-Simulink 13

Phụ lục : Chương trình mô phỏng trên Matlab 15

Đề 15

Câu 1: Cho Robot có cơ cấu như hình vẽ : a2 = 0.2m;

a Xây dựng hệ tọa độ cho các thanh nối

b Xác định ma trận T biểu diễn hệ tọa độ tay Robot

c Giải thích ý nghĩa của ma trận T

d Xác định vị trí của tay Robot trong hệ tọa độ gốc khi = 450 ;

450 ; d3 = 0.1m

Câu 2: Cho Robot θ – r có r1=0.5m ; m1=m2 = 2Kg Khớp tịnh tiến dịch chuyển

từ vị trí ban đầu r1 đến vị trí cuối cùng ứng vơi rmax =1 m Khớp quay quay với tốc độ θ = 18 độ/s từ góc ban đầu là 00 đến góc 900

a Hãy xác định momen ở khớp quay và lực tổng ở khớp tịnh tiến khi Robot ở cuối hành trình chuyển động

b Thiết kế bộ điều khiển phản hồi PD cho từng khớp

c Mô phỏng hệ thống

Câu 1:

a Xây dựng hệ tọa độ cho các thanh nối

Robot có cơ cấu tay dạng cầu với 2 khớp quay và 1 khớp tịnh tiến Trục của khớp quay θ1 vuông góc với trục của khớp quay θ2 , đồng thời trục của khớp quay θ2 cũng vuông góc với trục của khớp tịnh tiến

Chọn khung tọa độ góc (O) và các khung tọa độ thanh nối như hình vẽ :

Hình 1: Các khung tọa độ thanh nối Robot

Trong đó :

+ Gốc khung tọa độ thanh i đặt ở trục thanh i+1 (cuối thanh i)

+ Trục zi đặt theo phương của trục khớp i+1

+ Đặt trục xi sao cho phép quay xung quanh trục xi thì zi-1 tiến tới zi

b Xác định ma trận T biểu diễn hệ tọa độ tay Robot

Các khung tọa độ các thanh nối của Robot đã được thiết kế ở Hình 1 từ

đó ta xác định được các tham số trong bảng D – H như sau :

Bảng 1: Bảng D – H của Robot

Ta có ma trận thành phần biểu diễn quan hệ giữa hai khung tọa độ hai khớp i-1,i

là :

Trong đó

Ta có ma trận T biểu diễn hệ trục tọa độ tay Robot :

=

=

c Giải thích ý nghĩa của ma trận T

- Ma trận T có 4 hàng 4 cột

- Biểu diễn một khung tọa độ tay Robot trong khung tọa độ chuẩn

- Biểu diễn vị trí và hướng tay Robot

- 3 cột đầu tiên biểu diễn hướng của tay Robot, cột thứ 4 biểu diễn vị trí của tay Robot

+ Cột thứ nhất biểu diễn vecto pháp tuyến n (normal)

+ Cột thứ 2 biểu diễn vectơ có hướng mà theo đó ngón tay của bàn tay nắm

vào nhau khi cần nắm đối tượng, gọi là vectơ o (orientation)

+ Cột thứ 3 biểu diễn vecto có hướng mà theo đó bàn tay tiếp cận với đối

tượng gọi là vectơ a (approach)

d Xác định vị trí của tay Robot trong hệ tọa độ gốc

Ta có ma trận T =

=

=

Vậy vị trí tay Robot trong khung tọa độ gốc là :

Câu 2:

a Momen ở khớp quay và lực tổng ở khớp tịnh tiến khi Robot ở cuối hành trình chuyển động

Hình 2.1: Cấu trúc Robot θ – r có hai thanh nối

Dạng tổng quát của phương trình động lực học :

Trong đó :

+ lần lượt là momen của khớp thứ i và lực tác dụng lên thanh nối thứ i

+ q là biến của khớp quay hoặc khớp tịnh tiến

+ L là hàm Lagrange

+ lần lượt là động năng và thế năng của thanh nối thứ i

Giả thiết = 0

Suy ra

Ta có :

→

=

→

=

Suy ra

Ta có :

Suy ra : P = =

L = K - P =

 Xác định momen ở khớp quay

Đối với khớp quay ta có :

M =

Do đó :

M = + (1)

Theo đề bài ta có : ;, ;

θ = , g = 9.81

Momen khi robot ở cuối hành trình ứng với r = 1 m

Do khớp quay với tốc độ không đổi nên

Tương tự ra có

Thời gian chuyển động của khớp quay là : t = = 5 (s)

Tốc độ dịch chuyển của khớp tịnh tiến là : = = (m/s)

Thay số ta có: M = 2*2*1*0.1* + (2*0.5+2*1)*9.81*cos =0.126 (N.m)

 Xác định lực tổng ở khớp tịnh tiến

Đối với khớp tịnh tiến ta có :

F =

Ta có : =

Suy ra F =

-Thay số vào ta có F = 2*0 – 2*1*+2*9.81*sin = 19.82 (N)

Vậy momen ở khớp quay và lực tổng ở khớp tịnh tiến khi robot ở cuối hành trình chuyển động là :

M = 0.126 M.m

F = 19.82 N

b Thiết kế bộ điều khiển phản hồi PD cho từng khớp

Ta có :

Phương trình động lực học Robot :

Suy ra :

H= ; V= ; G=

Sơ đồ cấu trúc hệ thống điều khiển robot với bộ điều khiển phản hồi PD có dạng như sau :

Hình 2.2: Cấu trúc điều khiển PD

Phương trình luật điều khiển PD có dạng :

với :

= – sai số vị trí khớp

tốc độ khớp robot

=diag() – ma trận đường chéo hệ số khuếch đại

=diag() – ma trận đường chéo hệ số đạo hàm

Chọn ; =

là momen điều khiển

Ta có bộ điều khiển PD với robot θ – r :

Khớp quay : = +

Khớp tịnh tiến :

Quá trình khởi động bậc 2 ,quá trình chuyển động đều bậc 1 , quá trình hãm dừng cũng là bậc 2

Ta tính toán đảm bảo tay robot di chuyển từ vị trí đầu (0.5 0)đến vị trí cuối

(0 1) trong thời gian 5s

Cách tính được trình bày như sau :

Hình 2.3 : Dạng quỹ đạo 2-1-2

Giả thiết :

Mặt khác ta có :

→

Từ đồ thị ta thấy t1< do đó ta có :

(*)

Biểu thức chỉ có nghĩa khi hàm dưới dấu căn lớn hơn không

Vậy >0 → (**)

Giới hạn của gia tốc phụ thuộc vào độ bền cơ khí của robot.Như vậy có thể thấy cách thiết kế quỹ đạo như sau :

Chọn nằm trong khoảng theo công thức (**) Biết ta tính được theo (*)

Khi đó quỹ đạo trong các khoảng thời gian như sau :

Ma trận hệ số khuếch đại tỷ lệ và đạo hàm được chọn dựa theo các yêu cầu

về truyền động đó là thời gian quá độ và độ quá điều chỉnh

c Mô phỏng hệ thống bằng Matlab-Simulink

Chương trình mô phỏng được xây dựng bằng phần mềm Matlab-Simulink Chương trình có các tính năng chính sau :

+ Có thể điều chỉnh các tham số của bộ điều khiển phản hồi PD ở chương trình thông qua đó xem xét các đáp ứng từ đó tìm ra bộ điều khiển mang lại chất lượng cao nhất

+ Có thể xem ngay đáp ứng của hệ thống sau khi đã lựa chọn tham số điều khiển cũng như các tham số về vị trí đầu , cuối và thời gian mô phỏng Khi chạy chương trình ta có chế độ mặc định như sau :

+ Vị trí ban đầu (0.5 0)

+ Vị trí cuối (0 1)

+ Thời gian đáp ứng của robot t = 5s

; =

Kết quả đồ thị mô phỏng:

Phụ lục : Chương trình mô phỏng trên Matlab

%1.RobotThetaR.m;

%chuong trinh mo phong he thong dieu khien vi tri robot hai khop thetaR

%controller: Dieu khien vi tri voi bo dieu khien phan hoi PD

function [At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] =

RobotThetaR(Kp,Kd)

tc = 5; %Dat thoi gian di chuyen cua tay Robot

m1 = 2 ; %Khoi luong thanh 1

m2 = 2 ; %Khoi luong thanh 2

r1 = 0.5; %Chieu dai thanh noi 1

Kp=5000; % chon he so khuyech dai

Kd=100; %chon he so dao ham

%Vi tri cua tay ban dau va cuoi

S0 = [0.5 0];

Sc = [0 1];

%Chuyen vi tri ban dau cua tay Robot sang vi tri cac khop

q0 = DHN(S0);

qc = DHN(Sc);

%Khac phuc truong hop chia 0

if (qc(2)==q0(2));

qc(2) = qc(2)-0.001;

end

if (qc(1)==q0(1));

qc(1) = qc(1)-0.001;

end

%Tinh toan gia toc hai khop tinh tien va quay

ddq1 = 1.3*4*abs(qc(1)-q0(1))/tc^2; %Gia toc khop quay

ddq2 = 1.3*4*abs(qc(2)-q0(2))/tc^2; %Gia toc khop tinh tien

%Xac dinh cac khoang thoi gian chuyen dongkhoi dong, deu va ham cho cac khop

t11 = tc/2 - sqrt((tc^2*ddq1-4*(qc(1)-q0(1)))/ddq1)/2; %Thoi gian tang toc

t21 = tc - t11; %t21 - t11/2 se la thoi gian chuyen dong deu, tc-t21 se la

%thoi gian giam toc ve 0 Tuc la thoi gian tang va giam toc

%deu bang t11/2

t12 = tc/2 - sqrt((tc^2*ddq2-4*(qc(2)-q0(2)))/ddq2)/2; %Thoi gian tang toc

t22 = tc - t12;

Tk = 0.01; %Sau Tk(s) ta se tinh toan cac tham so cua robot

%Dieu kien ban dau (So kien)

q = q0; dq = [0;0];

X0 = [q0(1);dq(1);q0(2);dq(2)]; %So kien bien trang thai X

file1 = fopen('text.txt','w');

i = 0; %Bien dung de dem

for t = 0:0.001:tc; i = i+1;

j(i)=t; %Lay thoi gian de ve do thi

%Tinh toan gia tri dat cho cac khop trong tung khoang thoi gian chuyen

%dong

[qd1, dqd1] = quiDaoKhopThetaR(q0(1),qc(1),ddq1,t11,t21,tc,t); %dqd1 la van

%toc cua khop 1, ddq1 la gia toc khop 1

[qd2, dqd2] = quiDaoKhopThetaR(q0(2),qc(2),ddq2,t12,t22,tc,t); qd = [qd1; qd2];

dqd = [dqd1; dqd2];

%Tinh momen can thiet de thuc hien chuyen dong

[M,xe,fe] = Controller(qd,dqd,q,dq,Kp,Kd);

%Lay thong so qui dao thuc robot chuyen dong duoc qua M

[q, dq] = Robot(M,X0,Tk); %Lay cac thong so de ve do thi

% -qdd1(i)= qd(1);

qdd2(i) = qd(2);

q1(i) = q(1); %Goc quay khop quay

q2(i) = q(2); %r

dq1(i) = dq(1); %Toc do khop quay

dq2(i) = dq(2); %Toc do khop tinh tien

M1(i) = M(1); %Mo men khop quay

M2(i) = M(2); %Luc truyen dong cho khop tinh tien

eTheta(i) = qd(1) - q(1); %Sai lech goc quay

er(i) = qd(2) - q(2); %Sai lech chuyen dong tinh tien

% -X0 = [q(1); dq(1); q(2); dq(2)]; %Dat lai so kien cho lan tinh sau %q(1)-> Theta; q(2)->r

%Luu du lieu vao file

fprintf(file1,'%3.4f %3.4f %3.4f %3.4f %2.4f %3.4f

%3.4f\n',t,qd(1),q(1),qd(2),q(2),M(1),M(2));

end

%ve do thi mo phong

subplot(3,2,1)

plot(j,qdd1);hold on; %ve do thi gia tri dat khop quay

plot(j,q1,'r '); %ve do thi gia tri thuc khop quay

grid on;

title('Hinh1.goc quay cua khop 1(rad)')

xlabel('t(s)')

% -subplot(3,2,2)

plot(j,qdd2);hold on; % ve do thi gia tri dat khop tinh tien

plot(j,q2,'r '); %ve do thi gia tri thuc khop tinh tien

grid on;

title('Hinh2.quang duong cua khop tinh tien 2(m)')

ylabel('r(m)')

% -subplot(3,2,3)

plot(j,eTheta);hold on; % do thi bieu dien sai lech giua tin hieu thuc %va tin hieu dat khop quay

plot(j,er,'r '); % do thi bieu dien sai lech giua tin hieu thuc %va tin hieu dat khop tinh tien

grid on;

title(' Hinh3.sailechgiuatinhieuthucvadat')

xlabel('t(s)')

ylabel('e')

% -subplot(3,2,4)

plot(j,M1);hold on; %ve di thi momen khop quay

plot(j,M2,'r '); %ve di thi luc khop tinh tien

grid on;

title(' Hinh4.momen(N.m)va luc(N)')

xlabel('t(s)')

ylabel('M')

% -subplot(3,2,5)

plot(j,dq1); %do thi bieu dien toc do khop quay

grid on;

title('Hinh5.toc do khop quay(rad/s)')

xlabel('t(s)')

plot(j,dq2); %do thi bieu dien toc do khop quay

grid on;

title(' Hinh6.toc do khoptinh tien (m/s)') xlabel('t(s)')

fclose(file1);

% 2.Hàm tính mô hình Robot

function [q,dq] = Robot(M,X0,Tk)

% -M01 = M(1); %Mo men dieu khien cho khop quay

F02 = M(2); %Luc dieu khien cho khop tinh tien

x11 = X0(1); %Goc Theta1

x12 = X0(2); %Toc do goc khop quay

x21 = X0(3); %Gia tri r

x22 = X0(4); %Toc do khop tinh tien

% -%Cac thong so cua Robot

m1 = 2 ; %Khoi luong thanh 1

m2 = 2 ; %Khoi luong thanh 2

r1 = 0.5; %Chieu dai thanh noi 1

% -C1 = cos(x11);

S1 = sin(x11);

% -%Ma tran quan tinh

H11 = m1*r1^2 + m2*x21^2; %r = x21

H12 = 0;

H21 = 0;

H22 = m2;

H = [H11, H12; H21, H22];

%Momen nhot va momen huong tam

v1 = 2*m2*x21*x22*x12;

v2 = -m2*x22*x12*x12;

V = [v1;v2];

%Momen trong luc

G1 = (m1*r1 + m2*x21)*9.81*C1;

G2 = m2*9.81*S1;

G = [G1;G2];

%Nghich dao ma tran H

Hinv = inv(H);

%Tinh toan gia toc khop tu phuong trinh dong luc hoc dang nguoc

dX = -Hinv * (V+G) + Hinv*[M01;F02];

%Phuong trinh trang thai

x11p = x12; %Toc do khop quay

x21p = x22; %Toc do khop tinh tien

x12p = dX(1); %Gia toc khop quay

x22p = dX(2); %Gia toc khop tinh tien

%Tinh gan dung phuong trinh vi phan

x11 = x11 + Tk * x11p;

x12 = x12 + Tk * x12p;

x22 = x22 + Tk * x22p;

X = [x11;x12;x21;x22];

%Tinh vi tri tay Robot

s = [x21; 0];

q = [x11; x21];

dq = [x12; x22];

% 3.Hàm động học ngược

function [q] = DHN(S)

%Px = S(1); Py = S(2)

%Theta = q(1); r = q(2)

Px = S(1);

Py = S(2);

Theta = atan2(Py,Px);

r = Px*cos(Theta) + Py*sin(Theta);

q = [Theta; r];

% 4.Hàm tính lượng đặt

function [q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t)

if t<=t1 %Khoi dong

q = q0 + ddq*t^2/2;

dq = ddq * t;

elseif ((t>t1) & (t<=t2)) %Chuyen dong deu

q = q0 + ddq*t1*(t-t1/2);

dq = ddq * t1;

elseif ((t>t2)&(t<=tc)) %Giam toc do ve khong

q = qc - ddq*(t-tc)^2/2;

dq = ddq*t1 - ddq*(t-t2);

end

% 5.Hàm tính luật điều khiển

function [M,errorTheta, errordTheta] = Controller(sd,dsd,theta,dtheta,Kp,Kd)

%Cai dat tham so bo dieu khien vi tri phan hoi PD

kp1 = Kp; kp2 = Kp; kd1 = Kd; kd2 = Kd;

%Tinh sai lech goc quay va dao ham

errorTheta = sd - theta;

errordTheta = dsd - dtheta;

%Momen dieu khien

Kp = [kp1,0; 0,kp2];

Kd = [kd1,0; 0,kd2]

M = Kp * errorTheta - Kd * dtheta;