bài tập lớn kỹ thuật robot

bài tập lớn kỹ thuật robot

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

ĐỀ 12

Bài 1: Cho Robot có cơ cấu như hình vẽ

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 tahy Robot trong hệ tọa độ gốc khi θ1=600 , d2 = 0,1m ,

Bài 1:

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

Ta có sơ đồ cánh tay của Robot như sau:

Hình 1.1: Sơ đồ minh họa cơ cấu Robot

Từ sơ đồ cánh tay, ta gắn các hệ trục tọa độ như sau:

 Vị trí gốc tọa độ:

- Khâu số 0 thân Robot là khâu cố định có gốc tọa độ O được đặt tại khớp1

- Khung tọa độ 1 có gốc O1 được gắn vào khớp thứ 2

- Khung tọa độ 2 có gốc O2 được gắn vào khớp thứ 3

- Khung tạo độ 3 có gốc O3 được gắn vào khâu tác động đuôi

 Chiều của khung tọa độ:

- Khung tọa độ số 0 có trục Z0 trùng với trục của khớp 1, trục X0 đượcchọn vuông góc với mặt giấy

- Khung tọa độ số 1 có trục Z1 trùng với trục của khớp số 2 và trục X1 chọnvuông góc với mặt giấy

- Khung tọa độ số 2 có trục Z2 trùng với trục của khớp số 3 và trục X2 đượcchọn vuông góc với mặt giấy

- Khung tọa độ số 3 có trục Z3 trùng với phương của trục Z2 và trục X3

Vậy ta có hệ trục tọa độ như sau:

Hình 1.2: Hệ tọa độ cho các thanh nối

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

Theo hình 1.2 ta lập được bảng D-H các tham số của khâu như sau:

Vị trí của hai khâu liền kề nhau được mô tả bởi một ma trận biến đổi đồng nhất

i-i-1Ai Ma trận 0A1 mô tả quan hệ giữa khâu 1 và khâu 0 (khâu cố định gắn với thânRobot ), Ma trận 1A2 mô tả quan hệ giữa khâu 2 và khâu 1, Ma trận 2A3 mô tả quan

hệ giữa khâu 3 và khâu 2 Cấu hình của khâu mộtt bất kì nào đó của Robot có thểthu được bằng cách nhân một số thích hợp các ma trận i-1Ai lại với nhau

Dạng tổng quát của ma trận i-1Ai như sau:

Áp dụng (1.2) để tính toán các ma trân i-1Ai với tham số cho trong bảng

D-H như sau:

Từ đó thu được ma trận T như sau:

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

Ma trận T có dạng tổng quát như sau:

- Động học ngược: Khi biết vị trí và hướng của các điểm tác độngcuối ta hoàn toàn có thể xác định được giá trị của các biến khớp từviệc gải hệ phương trình động học T trên.

d. Xác định vị trí của tahy Robot trong hệ tọa độ gốc khi θ 1 =60 0 , d 2 = 0,1m ,

d 3 = 0,1m.

Khi robot quay một góc θ 1 = 60 0 và chuyển động tịnh tiến các đoạn d 2 = 0,1m và

d 3 = 0,1 m thì ta có ma trận T có giá trị như sau:

Vậy vị trí của gốc tọa độ gắn lên tay có vị trí như sau:

Trình tự xây dựng phương trình động lực học như sau:

Với khớp quay ta có phương trình động lực học như sau:

Với khớp tịnh tiến ta có phương trình động lực học như sau:

Vận tốc điểm A:

Động năng của khớp 1:

Động năng của khớp 2:

Vị trí điểm B trong hệ tọa độ như sau:

Vận tốc của điểm B theo các trục như sau:

 Tính Mô men khớp quay (khớp 1)

Mô men quán tính được tính theo công thức :

Robot θ-r có hai bậc tự do gồm khớp 1 là khớp quay, khớp thứ hai là khớptịnh tiến Giả thuyết chuyển động của các khớp này là chuyển động đều Đồngthời thời gian để khớp quay quay hết quỹ đạo yêu cầu bằng thời gian mà khớptịnh tiến thực hiện hết hành trình làm việc của mình.

Thời gian để cơ cấu tịnh tiến chuyển động từ r1 = 0,6 [m] tới vị trí rmax = 1.2 [m]với vận tốc r = 0,1 [m/s] là:

Mặt khác, cơ cấu Robot lại quay từ 00 đến 600 vậy ở cuối hành trình thì khớp đãquay một góc θ = 600 = π/3 [rad] và thời gian khớp quay quay đúng bằng thời giankhớp tịnh tiến thực hiện hết hành trình của mình nên ta có vận tốc góc của khớpquay tính như sau:

Tiếp theo ta có khớp quay và khớp tịnh tiến chuyển động đều với vận tốc khôngđổi nên:

Mặt khác ở cuối hành trình chuyển động thì :

r = rmax = 1.2 [m] và θ = π/3

Thay vào các công thức đã xây dựng ở trên được

- Mô mem của khơp quay:

- Lực tổng của khớ tịnh tiến:

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

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ạngnhư sau :

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

 Ta có các ma trận thành phần của phương trình động lực học Robot :

Để đơn giản ta sẽ xây dựng quỹ đạo đặt cho hai khớp là quĩ đạo kép dạng 2-1-2

Ta tính toán để đảm bảo tay Robot di chuyển từ vị trí ban đầu (x0 y0) đến vị trícuối cùng là (xc yc) trong thời gian là tc(s) Cách tính toán được trình bày như sau:

Hình 2.2 : Dạng quỹ đạo 2-1-2 Quá trình khởi động bậc 2, quá trình chuyển động đều bậc 1, và quá trình hãm dừng cũng là bậc 2.

Ta giả thiết Mặt khác ta có:

Chọn nằm trong khoảng theo biểu thức (**).

Biết , tc, qc, q0 ta tính được t1 theo (*)

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

Ma trận hệ số khuyếch đại tỷ lệ và đạo hàm được chọn dựa theo các yêu cầu về chất lượng truyền động như 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ươngtrình thông qua đó xem xét các đáp ứng, từ đó tìm ra bộ điều khiển manglạ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ềukhiể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.6 0)

 Vị trí cuối (0 1.2)

 Thời gian đáp ứng của Robot t = 6(s)

Code các file chương trình mô phỏng.

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 = 6; %Dat thoi gian di chuyen cua tay Robot

m1 = 1 ; %Khoi luong thanh 1

m2 = 1 ; %Khoi luong thanh 2

r1 = 0.6; %Chieu dai thanh noi 1

Kp=8000; % chon he so khuyech dai

Kd=100; %chon he so dao ham

%Vi tri cua tay ban dau va cuoi

%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

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

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

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

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

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

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

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

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

% -%Cac thong so cua Robot

m1 = 2 ; %Khoi luong thanh 1

m2 = 2 ; %Khoi luong thanh 2

r1 = 0.55; %Chieu dai thanh noi 1

%Momen trong luc

%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

5. Controller.m: thực hiện luật điều khiển PD kinh điển.

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

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