BÀO CÁO BÀI TẬP DÀI KỸ THUẬT ROBOT ĐỀ 13

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘIVIỆN ĐIỆN BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG BÁO CÁO BÀI TẬP DÀI KỸ THUẬT ROBOT Sinh viên thực hiện: Trần Đình Thiêm Hà nội, ngày 5 tháng 12 năm 2013... Robot quay

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

VIỆN ĐIỆN

BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

BÁO CÁO

BÀI TẬP DÀI KỸ THUẬT ROBOT

Sinh viên thực hiện: Trần Đình Thiêm

Hà nội, ngày 5 tháng 12 năm 2013

BÀI TẬP LỚN KỸ THUẬT ROBOT ĐỀ 13

Câu 1: Cho Robot có cơ cấu có 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 tay robot trong hệ tọa độ gốc khi θ1=60o, d2=0.1m, d3=0.1m

Câu 2:Cho Robot θ−r có r1=0.6m, m1=m2=1kg Khớp tịnh tiến chuyển động với tốc độr=0.1m/s từ r1 tới rmax =1.2m Robot quay từ góc ban đầu 0o tới góc 60o

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ànhtrì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

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ớp 1

- 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ọa độ 3 có gốc O3 được gắn vào khâu tác động cuối

 Chiều của các 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 được chọ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ọn vuô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 được chọ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 vuông góc với mặt giấy

- Trục Y0, Y1, Y2, Y3 được xác định theo quay tắc bàn tay phải

Từ đó ta xây dựng được hệ trục tọa độ như sau:

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

Từ hình vẽ trên ta lập được bảng D-H các tham số 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-1Ai.

Ma trận 0A1 mô tả quan hệ giữa khâu 1 và khâu 0 (khâu cố gắn với thân Robot), 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

Dạng tổng quát của ma trận i-1Ai 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:

- Vector n,o,a xác định hướng của khung tọa độ tay so với khung tọa độ gốc cố định

- Vector pbiểu diễn vị trí của khung tọa độ tay so với khung tọa độ gốc

Qua ma trận T người ta có thể phân tích sự hoạt động và lập trình điều khiển cho Robot.

Ma trận T có ý nghĩa rất lớn trong bài toán động học thuận và bài toán động học ngược:

- Động học thuận: khi biết giá trị của các biến khớp thay đổi theo thời gian thì vị trí vàhướng của tay Robot sẽ hoàn toàn xác định tại mọi thời điểm

- Động học ngược: khi biết hướng và vị trí của điểm tác động cuố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 giải hệ phương trình động học T trên

d) Xác định vị trí tay Robot trong hệ tọa độ gốc khi θ1=60 o , d 2 =0.1m, d 3 =0.1m

Khi robot quay một góc θ1=60o và chuyển động tịnh tiến các đoạn d2=0.1m, d3=0.1mthì ta có ma trận T như sau:

Câu 2:

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

Ta có mô hình của Robot θ−r như sau:

Để xây dựng được các phương trình động lực học của Robot θ-rr đơn giản chúng ta giả

thuyết:

- Khối lượng m1 của xilanh tập trung tại điểm cuối của xilanh tức là điểm A

- Khối lượng m2 của pittong tập trung ở bàn tay Robot tức là tập trung tại điểm B

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

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

d L

m r

dt r L

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ậntốc r = 0.1(m/s) là:

max 1 1.2 0.6

6( ) 0.1

- Mômen của khớp quay:

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

Phương trình động lực học của cơ cấu Robot θ-r đã trình bày ở trên được viết lạinhư sau:

m r m r H

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

K diag(K d1,K d2)- ma trận đường chéo các kệ số đạo hàm

Quỹ đạo đặt cho hai khớp có dang 2-1-2 được tính toán đảm bảo tay robot di chuyển từ vị tríban đầu đến vị trí cuối cùng trong thời gian t c 6s

Ma trận các hệ số khuếch đại và đạo hàm được lựa chọn

c) Mô phỏng hệ thống

 Thiết kế quỹ đạo chuyển động cho hai khớp dạng 2 – 1 – 2

Để đảm bảo tay Robot di chuyển từ vị trí ban đầu A(x0 , y0 ) đến vị trí cuối cùng là B(xc , yc) trong khoảng thời gian tc (s) ta có thể tính toán quỹ đạo như sau:

Khi nó Robot di chuyển từ A tới B xảy ra 3 quá trình :

- Đầu tiên là quá trình khởi động có dạng bậc 2

- Quá trình chuyển động đều có dạng là đường thẳng bậc 1

- Quá trình hãm có dạng đường cong bậc 2

2 1

1 2

2

0 1

- Chọn q nằm trong khoảng theo biểu thức (2-8).1

- Biết q , tc, qc, q0 ta tính được t1 theo (2-7).1

Chương trình mô phỏng

%======================================

% CHUONG TRINH CHINH

%=======================================

% khai bao gia tri bo dieu khien

% goi ham RobotThetaR

[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR()

% ========================================

% do thi tin hieu thu duoc

%=========================================

% ve mo men va luc cua robot tren cung do thi

plot(At1,AM1, 'r' ) % mo mem khop quay (khop 1)

hold on

plot(At1,AM2) % luc cua khop tinh tien (khop 2)

grid

% ve sai so goc quay va tinh tien tren cung do thi

plot(At1,AeTheta, 'r' ) % mo mem khop quay (khop 1)

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

%===Vi tri cua tay ban dau` va` cuoi ==

%Vi tri ban dau va cuoi cua cac khop

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 dong tang toc, deu va giam toc 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( 'RobotThetaR.txt' , 'w' );

i = 0; %Bien dung de dem

for t = 0:0.001:tc;

i = i+1;

At1(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

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

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

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

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

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

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

Aer(i) = qd(2) - q(2); %Sai lech chuyen dong 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 = 1 ; %Khoi luong thanh 1

m2 = 1 ; %Khoi luong thanh 2

r1 = 0.5; %Chieu dai thanh noi 1

%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

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

Kd = [kd1,0; 0,kd2];

%M = Kp * errorTheta - Kd * dtheta + G;

U = dsd+Kp*errorTheta+Kd*errordTheta; M=H*U+V+G;

Tài liệu tham khảo:

[1] TS Nguyễn Mạnh Tiến Điều Khiển Robot Công Nghiệp Nhà xuất bản Khoa Học và

Kỹ Thuật Hà Nội 2006

[2] TS Nguyễn Mạnh Tiến Bài Giảng Robot Công Nghiệp

[3] GS.TSKH Nguyễn Thiện Phúc Robot Công Nghiệp Nhà xuất bản Khoa Học và Kỹ

Thuật Hà Nội 2006