Môn học Kỹ thuật Robot Bài tập robot đề số 34

Môn học Kỹ thuật Robot Bài tập robot đề số 34

BÀI TẬP ROBOT

ĐỀ SỐ 34

1 Cho Robot có cấu hình như hình vẽ: a2= 0,2 m

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 ma trận T.

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

2 Cho Robot θ – r có r1 = 0,5 m; m1 = m2 = 2,5 Kg Khớp tịnh tiến chuyển động với tốc độ r = 0,05 m/s Khớp quay quay với tốc độ θ = 90/s Thời gian chuyển động là 5s Giá trị góc ban đầu là 0o

a Xác định góc θ và r của Robot ở cuối hành trình chuyển động.

b Hãy xác định momen ở khớp quay và lực tổng hợp ở khớp tịnh tiến khi Robot

ở vị trí cuối hành trình chuyển động.

c Thiết kế bộ điều khiển PID

d Mô phỏng hệ thống.

Bài làm Bài 1

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

 Nguyên tắc :

- Gốc khung tọa độ thanh I đặt trùng với chân pháp tuyến chung của trục i và i+1 và nằm trên trục của khớp i+1.

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

- Trục xi đặt theo phương pháp tuyến chung của trục I và i+1 theo hướng từ trục i tới i+1.

 Một vài trường hợp đặc biệt :

- Hai trục song song, sẽ có nhiều pháp tuyến chung Khi đó sẽ chọn được pháp tuyến chung trùng với pháp tuyến chung của khớp trước Gốc khung tọa độ được chọn sao cho di là nhỏ nhất.

- Đối với khớp tịnh tiến: khoảng cách di là biến khớp Hướng của trục khớp tùng với hướng di chuyển của khớp Chiều dài ai = 0 Gốc tọa độ đặt trùng với gốc thanh nối tiếp theo

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

Chọn khung tọa độ gốc O0x0y0z0 gắn với khớp 1 như hình vẽ Bỏ qua khoảng cách giữa 2 trục x1 và x0.

1 0

1

1

0 0

0

0 0 A

1 0

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

Ma trận T biểu diễn khung tọa độ tay Robot trong khung tọa độ gốc Khung tọa độ T có:

- Gốc tọa độ OT.

- Các vecto đơn vị ´iT, ´jT, ´kT.

Cột 1: Tọa độ vecto đơn vị iT của khung tọa độ tay Robot T trong khung tọa độ gốc

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

Thay các giá trị θ1 = 450; θ2 = 450; d3 = 0,1 m vào ma trận 0OT ta được tọa độ vị trí tay Robot trong khung tọa độ gốc:

0OT = [ 0.05 0.15 −0.212 1 ]T

Bài 2

Bài làm:

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

thuyết:

-r Khối lượng m1 của thanh nối 1 tập trung tại điểm cuối A

-r Khối lượng m2 của thanh nối 1 tập trung tại điểm cuối B.

- Mô mem quán tính ở các khớp J1 J2  0 .

a Xác định góc θ và r của Robot ở cuối hành trình chuyển động.

Góc θ được xác định theo công thức:  0 t 0 9 5 45  0 0



Coi giá trị ban đầu của r = r1 = 0.5m và giá trị cuối được xác định theo công thức:

 0

Momen (lực) theo phương trình chuyển động Lagrange:

 

i i

i i

Trong đó qi là biến khớp: - Góc quay θ nếu là khớp quay

- Độ dịch chuyển d nếu là khớp tịnh tiến

Vị trí điểm A so với khung tọa độ gốc:

cos

X Y

1 2

Vị trí điểm B so với khung tọa độ gốc:

 2 2 2

1 2

K  m r    r 

1515\* MERGEFORMAT () Thay (11) và (15) vào (7) ta được tổng động năng K của hệ của hệ:

L

m r m r

d L

m r m r r r dt

Thay (19) vào (18) ta được:

d L

m r

dt r L

2

2323\* MERGEFORMAT ()

Tính toán mô men khớp quay M và lực ở khớp tịnh tiến khi Robot ở cuối hành trình chuyển động:

Thay các số liệu:

c Thiết kế bộ điều khiển PID

Cấu trúc của hệ thống điều khiển phản hồi kinh điển PID

Phương trình động lực học của Robot θ – r ở trên được viết lại như sau:

Viết lại dưới dạng ma trận: M  H q q V q q ( )   ( , )   G q ( )

m r m r H

( ) cos

sin

m r m r g G

sẽ làm cho sai lệch tĩnh rất nhỏ và tiến tới 0.

Luật điều khiển PID:

K  diag KI  diag (K , K )i1 i2 KD  diag (K , K )d1 d2

 Thiết kế quỹ đạo đặt hai khớp dạng 2 – 1 – 2

Robot di chuyển từ vị trí khớp q0 tới vị trí khớp qc trong thời gian tc theo 3 giai đoạn:

 Giai đoạn khởi động trong khoảng thời gian 0 t  1: quỹ đạo khớp q0 q1 có dạng parabol bậc 2, gia tốc q1của khớp là hằng số.

2

1 2

q  q  q t 

2424\* MERGEFORMAT ()

 Giai đoạn chuyển động đều trong khoảng thời gian t1 t2, quỹ đạo khớp q1 q2 có dạng đường thẳng bậc 1, để đảm bảo tính đối xứng của quỹ đạo, điểm trung bình được chọn:

c m

1

1 11

m m

Thay (26) vào(27), kết hợp với (25) ta thu được phương trình:

01

0 4

( )

2 1

( ) 2

-m1 = 2.5; %Khoi luong thanh 1

m2 = 2.5; %Khoi luong thanh 2

r1 = 0.5; %Chieu dai thanh noi 1

r0 = r1; %Vi tri dau khop tinh tien

rc = 0.75; %Vi tri cuoi khop tinh tien

theta0 = 0; %Vi tri dau khop quay

thetac = pi/4; %Vi tri cuoi khop quay

tc = 5; %Thoi gian di chuyen cua tay Robot

%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 khoi dong, dac diem la chuyen dong nhanh dan deu tang toc, deu va giam toc cho

Tk = 0.01; %Chu ky lay mau cua mo hinh robot

%Dieu kien ban dau (So kien)

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

%Tinh toan gia tri dat cho cac khop

[qd1, dqd1] = QuiDaoKhop(q0(1),qc(1),ddq1,t11,t21,tc,t); %dqd1 la van %toc cua khop 1, ddq1 la gia toc khop 1

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

qd = [qd1; qd2];

dqd = [dqd1; dqd2];

%Tinh momen can thiet de thuc hien chuyen dong

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

% Quy dao chuyen dong 2-1-2

if t<=t1 %Tang toc bac 2

3) Hàm tính luật điều khiển

function[M,saisovitri,saisotocdo,tichphanmoi] =

Controller(sd,dsd,qthuc,dqthuc,tichphancu)

% Bo dieu khien bu trong luc

%Tham so bo dieu khien vi tri phan hoi PD

%Momen dieu khien bu trong luc

M = Kpp * saisovitri + Kii * tichphanmoi + Kdd * saisotocdo;

end

4) Hàm tính biến khớp từ mô hình Robot

function[q,dq] = RobotModel(M,q,dq,Tk)

%Mo hinh mo phong robot

%Dau vao: momen M, bien trang thai q,dq khi bat dau chu ky lay mau,

%thoi gian lay mau Tk,

%Dau ra: bien trang thai q,dq khi ket thuc chu ky lay mau

% -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 = evalin('base','m1'); %Khoi luong thanh 1

m2 = evalin('base','m2'); %Khoi luong thanh 2

r1 = evalin('base','r1'); %Chieu dai thanh noi 1

%Bien nho trang thai

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

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

%Tinh gan dung phuong trinh vi phan