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
VIỆN ĐIỆN
==============
BỘ MÔN TỰ ĐỘNG HÓA

BÀI TẬP LỚN
KỸ THUẬTROBOT

Sinh viên:
Họ và tên:
Lớp
:
SHSV :

1|Page

ĐK&TĐH 4-K54
20090793


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

ĐỀ 12
Bài 1: Cho Robot có cơ cấu như hình vẽ
a.
b.
c.
d.

Xây dựng hệ tọa độ cho các thanh nối.
Xác định ma trận T biểu diễn hệ tọa độ tay Robot
Giải thích ý nghĩa của ma trận T.
Xác định vị trí của tahy Robot trong hệ tọa độ gốc khi θ1=600 , d2 = 0,1m ,
d3 = 0,1m.

Bài 2: Cho Robot θ - r có r1=0.6 m; m1 = m2 = 1Kg . Khớp tịnh tiến chuyển động
với tốc độ r = 0,1 m/s từ r 1 đến rmax = 1.2 m. Robot quay từ góc ban đầu 0 0 đến góc
600
a.
b.
c.

Hãy xác định mô mem ở khớp quay và lực tổng hợp ở khớp tịnh tiến khi
Robot ở cuối hành trình chuyển động.
Thiết kế bộ điều khiển phản hồi PD cho từng khớp.
Mô phỏng hệ thống

2|Page


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

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ớ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ạ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 Z 0 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 Z 1 trùng với trục của khớp số 2 và trục X 1 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.

3|Page


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

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:
KTĐ
K1
K2
K3

θi
θ1
0
0

di
0
d2
d3

αi
0
- 900
0

ai
0
0
0

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
Ai. 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â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. 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.
i-i-1

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

4|Page


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

Áp dụng (1.2) để tính toán các ma trân i-1Ai với tham số cho trong bảng DH 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:

5|Page


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




d.

Trong đó:
- Véc tơ n , o, a xác định hướng của khung tọa độ tay so với khung
tọa độ cố định.
- Véc tơ p biể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 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 vị trí và hướng của các đ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 gải hệ phương trình động học T trên.
Xác định vị trí của tahy Robot trong hệ tọa độ gốc khi θ1=600 , d2 = 0,1m ,
d3 = 0,1m.

Khi robot quay một góc θ1 = 600 và chuyển động tịnh tiến các đoạn d 2= 0,1m và
d3 = 0,1 m thì ta có ma trận T có giá trị như sau:

6|Page


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

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

7|Page


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

Bài 2

a. Hãy xác định momen ở khớp quay và lực tổng hợp ở 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 :

θ

Y

X

Y2

r

B

Y1

A

r1

X1

X2

Hình 2.1. Mô hình robot θ-r
Để 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 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.
- Mô mem quán tính ở các khớp Ji = 0.
8|Page


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

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

Xác định hàm lagrange

Phương trình lagrange của một cơ cấu được xác định như sau:
L=K–P
Trong đó :
- K : Tổng động năng của hệ thống
- P : Tổng thế năng của hệ thống

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:

Xác định động năng của hệ thống
Động năng của hệ thống :


K = K1 + K2
Trong đó
-

K1 : động năng của khớp 1.
K2 : động năng của khớp 2.

Theo hình 2.1 ta có:
Động năng của khớp 1:
Vị trí của điểm A trong hệ tọa độ như sau:

Đạo hàm hai vế theo thời gian thu được thành phần vận tốc theo các trục:

9|Page


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

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:

Vận tốc của B :




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

Xác định thế năng của hệ thống
- Thế năng của khớp 1:

-



Thế năng của khớp 2:

Hàm lagrange

Từ các tính toán trên ta có hàm lagrange như sau:

10 | P a g e


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



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

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

Và




Từ đó mô men khớp quay như sau:

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

Lực tác động lên khớp tịnh tiến được tính như sau:
Trong đó:





Vậy lực cần tìm là :

Tím toán với tham số cụ thể

11 | P a g e


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

Robot θ-r có hai bậc tự do gồm khớp 1 là khớp quay, khớp thứ hai là khớp
tịnh tiến. Giả thuyết chuyển động của các khớp này là chuyển động đều. Đồng
thời thời gian để khớp quay quay hết quỹ đạo yêu cầu bằng thời gian mà khớp
tị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ừ r 1 = 0,6 [m] tới vị trí r max = 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ừ 0 0 đế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 gian
khớ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ớp
quay 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ạng
như sau :

12 | P a g e

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

Phương trình luật điều khiển có dạng :
Trong đó :
: sai số vị trí của khớp Robot .
là tốc độ của khớp Robot .
là biến khớp .
- ma trận đường chéo các hệ số khuếch đại .
Ta chọn :

-

Với robot

-

- ma trận đường chéo các hệ số đạo hàm.
Ta chọn :

-

là momen điều khiển .
thì ta có:

Phương trình động lực học dạng ma trận của Rob

Mà :

13 | P a g e


BÀI TẬP LỚN KỸ THUẬT ROBOT


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

Bộ điều khiển phản hồi PD cho từng khớp của robot

:

Khớp quay:
Khớp tịnh tiến:

Với :
Để đơ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:

14 | P a g e


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

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

15 | P a g e

. Mặt khác ta có:


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


Từ đồ thị trên ta nhận thấy t1 < tc /2 (do ta mong muốn chọn như vậy). 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 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:
16 | P a g e


BÀI TẬP LỚN KỸ THUẬT ROBOT




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.6 0)
Vị trí cuối (0 1.2)
Thời gian đáp ứng của Robot t = 6(s)

Đồ thị mô phỏng thu được.

17 | P a g e


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

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

18 | P a g e


BÀI TẬP LỚN KỸ THUẬT ROBOT
%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
S0 = [0.6 0];
Sc = [0 1.2];
%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];

19 | P a g e


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

%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)')
xlabel('t(s)')
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')
%---------------

20 | P a g e


BÀI TẬP LỚN KỸ THUẬT ROBOT
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)')
%-------------------subplot(3,2,6)
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.

Robot.m: Mô hình động lực học của Robot, được xây dựng dựa trên mô hình
động lực học của Robot Theta-R đã trình bày ở trên.
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.55; %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];

21 | P a g e


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

%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
x11 =
x21 =
x12 =
x22 =

gan

x11
x21
x12
x22

dung
+ Tk
+ Tk
+ Tk
+ Tk

phuong trinh vi phan
* x11p;
* x21p;
* x12p;
* x22p;

X = [x11;x12;x21;x22];
%Tinh vi tri tay Robot
s = [x21; 0];
q = [x11; x21];
dq = [x12; x22];

3.

DHN.m: hàm động học ngược của Robot, cho phép tính toán từ vị trí của tay ra
vị trí của các khớp.
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.

quiDaoKhop.m: tính toán lượng đặt vị trí khớp và tốc độ các khớp sao cho đảm
bảo đúng quĩ đạo 2-1-2 mong muốn trong khoảng thời gian tc.
function[q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t)
if t<=t1

22 | P a g e

%Khoi dong


BÀI TẬP LỚN KỸ THUẬT ROBOT
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

end

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
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;

23 | P a g e