BÀI TẬP LON ROBOT.docx

Bộ môn: Điều khiển robot Đỗ Trung Hiếu

BÀI TẬP ROBOT

ĐỀ SỐ 5

1. Cho Robot có cấu hình như hình vẽ: a1 = 0,3 m; a2= 0,1 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= 300; θ2 = 300; d3 = 0,1m.

2. Cho Robot θ – r có r1 = 0,6 m; m1 = m2 = 2 Kg. Khớp tịnh tiến chuyển động với tốc độ

r = 0,1 m/s từ r1 tới rmax = 1,2 m. Khớp quay quay với tốc độ θ = π/36 rad/s. Giá trị góc

ban đầu là 0o.

a. Xác định góc θ 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 “Bù trọng lượng”

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

1/22


BÀI LÀM

1.

Đề bài:

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

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



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

m.

Bài làm:

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.

2/22


- 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

.


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, x1 và x2.

3/22


Ta thiết lập được bảng D-H:

Khung tọa độ ai αi θi di

1 a1 0 θ1 0

2 a2 π θ2 0

3 0 0 0 d3

Thành lập các ma trận:

Ma trận T biểu diễn hệ tọa độ tay Robot:

T = 0A1 . 1A2 . 2A3

4/22



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

0iT = [C1 . C2−S1. S2 C1 . S2+C2 . S1 0 0 ]T

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

0 jT = [C1 . S2+C2 . S1 S1 . S2−C1 .C2 0 0 ]T

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

0kT = [ 0 0 −1 0 ]T

Cột 4: Tọa độ điểm OT gốc khung tọa độ tay Robot T trong khung tọa độ gốc

0OT = [a1 .C1+a2 .C1 .C2−a2 . S1 . S2 a1 . S1+a2 .C1 . S2+a2 . C2 . S1 −d3 1 ]T

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

m.

Thay các giá trị θ1 = 300; θ2 = 300; 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.310 0.237 −0.1 1 ]T

2.

Đề bài:

Cho Robot θ – r có r1 = 0,6 m; m1 = m2 = 2 Kg. Khớp tịnh tiến chuyển động với

tốc độ

r = 0,1 m/s từ r1 tới rmax = 1,2 m. Khớp quay quay với tốc độ θ = π/36 rad/s. Giá

trị góc ban đầu là 0o.

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

f. 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.

5/22


g. Thiết kế bộ điều khiển “Bù trọng lượng”

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

Bài làm:

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

chúng ta giả thuyết:

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

- Khối lượng 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 .

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

Thời gian khớp tịnh tiến chuyển động với tốc độ r = 0,1 m/s từ r1 tới rmax =

1,2 m:

6/22


Góc θ của Robot ở cuối hành trình chuyển động:

b. 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.

Hàm Lagrange là hiệu của tổng động năng và tổng thế năng của hệ:

11\* MERGEFORMAT ()

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

22\* MERGEFORMAT ()

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

Xác định tổng thế năng P của hệ :

33\* MERGEFORMAT ()

- Xác định thế năng thanh số 1:

44\* MERGEFORMAT ()

- Xác định thế năng thanh số 2:

55\* MERGEFORMAT ()

Thay (4) và (5) vào (3) ta được tổng thế năng P:

66\* MERGEFORMAT ()

Xác định tổng động năng K của hệ:

77\* MERGEFORMAT ()

- Xác định động năng thanh nối 1 (J1=0):

7/22


88\* MERGEFORMAT ()

Trong đó, là tốc độ chuyển động tịnh tiến của điểm A (tâm thanh nối 1)

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

99\* MERGEFORMAT ()

1010\*

MERGEFORMAT ()

Thay (10) vào (8) ta được:

1111\* MERGEFORMAT ()

- Xác định động năng thanh nối 2 (J2=0):


Trong đó, là tốc độ chuyển động tịnh tiến của điểm B (tâm thanh nối 2)

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

1313\*

MERGEFORMAT ()




8/22


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

1616\*

MERGEFORMAT ()

Hàm Lagrange

Từ các biểu thức (6) và (16) thay vào (1) ta được:

1717\*

MERGEFORMAT ()

Tính momen khớp quay ( ):

Áp dụng (2) với ta được:


Từ (18) ta có:



2020\*

MERGEFORMAT ()

Tính lực khớp tịnh tiến ( ):

Áp dụng (2) với ta được:

9/22



Từ (21) ta có:




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:

, , , (do khớp tịnh tiến chuyển động với

vận tốc không đổi)

(do khớp quay chuyển động với vận tốc không đổi)

vào (20) và (23) ta được:

10/22


c. Thiết kế bộ điều khiển PD “Bù trong lượng” :

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ại

như sau:

Viết lại dưới dạng phương trình ma trận:

Trong đó,

: Ma trận biến khớp

: Ma trận mô-men hoặc lực của khớp,

: Ma trận thành phần tỉ lệ với gia tốc

: Ma trận thành phần tỉ lệ với tốc độ

: Ma trận mô-men và lực của trọng lực.

Mục đích bộ điều khiển: khử ảnh hưởng của tới độ chính xác tĩnh (

không phụ thuộc G).

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


Trong đó:

11/22


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

Robot di chuyển từ vị trí khớp tới vị trí khớp trong thời gian theo 3 giai

đoạn:

Giai đoạn khởi động trong khoảng thời gian : quỹ đạo khớp có dạng

parabol bậc 2, gia tốc của khớp là hằng số.


Giai đoạn chuyển động đều trong khoảng thời gian , quỹ đạo khớp

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:


Tốc độ không đổi trong giai đoạn này bằng tốc độ cuối giai đoạn khởi

động:


Giai đoạn hãm trong khoảng thời gian , quỹ đạo khớp có dạng

parabol bậc 2.

12/22


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


Giải (28), xác định thời gian :


Biểu thức (29) có nghĩa khi và chỉ khi:


13/22


Giới hạn của gia tốc phải nhỏ hơn gia tốc cho phép từ độ bền cơ khí của Robot.

Kết hợp với (29), gia tốc chuyển động khớp được chọn phải thỏa mãn bất đẳng

thức:


Với các thông số được xác định bởi (31) và được xác định bởi (29),

phương trình quỹ đạo chuyển động được thiết kế là:

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

Chương trình mô phỏng được viết bằng ngôn ngử MATLAB được trình bày

ở phần phụ lục.Sau đây là kết quả mô phỏng ứng với bộ điều khiển PD với

các ma trận thông số như sau:

14/22


15/22


Phụ lục

Chương trình Matlab cần cho cùng vào 1 thư mục làm việc và chỉ cần chạy file chương trình

mô phỏng. Kết quả được ghi lại trong file Robot.txt cùng thư mục.

1. File chương trình mô phỏng

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

deal([]);

% ------------------------------------------

Kp = 60;

Kd = 10;

m1 = 2; %Khoi luong thanh 1

m2 = 2; %Khoi luong thanh 2

r1 = 0.6; %Chieu dai thanh noi 1

r0 = r1; %Vi tri dau khop tinh tien

rc = 1.2; %Vi tri cuoi khop tinh tien

theta0 = 0; %Vi tri dau khop quay

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

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

% ------------------------------------------

% Vi tri cac khop thoi diem dau va cuoi

q0 = [theta0;r0];

qc = [thetac;rc];

%q0 = [0;0.55];

%qc = [pi/6;1.2];

%tc = 6;

%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

16/22


%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 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; %Chu ky lay mau cua mo hinh robot

%Dieu kien ban dau (So kien)

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

%Mo phong

file1 = fopen('Robot_data.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

[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] =

17/22


QuiDaoKhop(q0(2),qc(2),ddq2,t12,t22,tc,t);

qd = [qd1; qd2];

dqd = [dqd1; dqd2];

% Tinh toan trong luong

G1 = (m1*r1 + m2*q(2))*9.81*cos(q(1));

G2 = m2*9.81*sin(q(1));

G = [G1;G2];

%Tinh momen can thiet de thuc hien chuyen dong

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

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

M

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

%Lay cac thong so de ve do thi

%------------------------------------------------------

qdd1(i) = qd(1);

qdd2(i) = qd(2);

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

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

%--------------------------------------------------------

%Luu du lieu vao file

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

18/22


n',t,qd(1),q(1),qd(2),q(2),M(1),M(2));

end

fclose(file1);

%--------------------------------------------------------

% Ve do thi ket qua mo phong

%--------------------------------------------------------

figure

% Mo men khop

subplot(2,2,1,'FontSize',14)

plot1 = plot(At1,AM1,At1,AM2);

title('Momen khop quay va luc khop tinh tien');

set(plot1(1),'LineWidth',2.5,'DisplayName','Momen khop

1');

set(plot1(2)

,'LineWidth',2.5,'LineStyle','--','Color','r',...

'DisplayName','Luc khop 2');

xlabel('Thoi gian (s)');

ylabel('');

legend('show');

grid on;

% Sai so goc quay va tinh tien tren cung do thi


plot1 = plot(At1,AeTheta,At1,Aer);

title('Sai so cac bien khop');

set(plot1(1),'LineWidth',2.5,'DisplayName','e(theta)');

set(plot1(2)


'DisplayName','e(r)');


ylabel('');

19/22


legend('show');

grid on;

% Goc quay khop 1


plot1 = plot(At1,Aq1,At1,qdd1);

title('Bien khop quay');

set(plot1(1),'LineWidth',2.5,'DisplayName','theta

thuc');

set(plot1(2)


'DisplayName','theta dat');


ylabel('');

legend('show','Location','NorthWest');

grid on;

% Do dich chuyen cua khop 2 tinh tien


plot1 = plot(At1,Aq2,At1,qdd2);

title('Bien khop tinh tien');

set(plot1(1),'LineWidth',2.5,'DisplayName','r thuc');

set(plot1(2)


'DisplayName','r dat');


ylabel('');

legend('show','Location','NorthWest');

grid on;

% Phong to hinh

drawnow

jFig = get(handle(gcf), 'JavaFrame');

20/22


jFig.setMaximized(true);

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

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

% Quy dao chuyen dong 2-1-2

if t<=t1 %Tang toc bac 2

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

dq = ddq * t;

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

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

end

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

function[M,errorTheta, errordTheta] =

Controller(sd,dsd,theta,dtheta,Kp,Kd,G)

% Bo dieu khien bu trong luc

%Tham so bo dieu khien vi tri phan hoi PD

Kpp = diag([Kp Kp]);

Kdd = diag([Kd Kd]);

%Tinh sai lech goc quay va dao ham goc quay

errorTheta = sd - theta;

errordTheta = dsd - dtheta;

%Momen dieu khien bu trong luc

M = Kpp * errorTheta - Kdd * dtheta + G;

21/22


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

%-------------------------------------------

C1 = cos(q(1));

S1 = sin(q(1));

%---------------------------------------------

%Ma tran quan tinh

H11 = m1*r1*r1 + m2*q(2)*q(2); %r = q(2)

H12 = 0;

H21 = 0;

H22 = m2;

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

%Momen nhot va momen huong tam

v1 = 2*m2*q(2)*dq(2)*dq(1);

v2 = -m2*q(2)*dq(1)*dq(1);

V = [v1;v2];

%Momen trong luc

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

G2 = m2*9.81*S1;

G = [G1;G2];

%Nghich dao ma tran H

22/22


Hinv = inv(H);

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

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

%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

q(1) = q(1) + Tk * dq1;

q(2) = q(2) + Tk * dq2;

dq(1) = dq(1) + Tk * ddq(1);

dq(2) = dq(2) + Tk * ddq(2);

23/22

Documents

BÀI TẬP LON ROBOT.docx