BÀI TẬP GIỮA KỲ ROBOT CÔNG NGHIỆP đề tài KHẢO SÁT ROBOT 6 BẬC TỰ DO 5 KHỚP QUAY VÀ 1 KHỚP TỊNH TIẾN

Tieuluanmoi
ĐẠI HỌC BÁCH KHOA HÀ NỘI

TRƯỜNG CƠ KHÍ
----o0o----

BÀI TẬP GIỮA KỲ
ROBOT CÔNG
NGHIỆP

ĐỀ TÀI:
KHẢO SÁT ROBOT 6 BẬC TỰ DO:
5 KHỚP QUAY VÀ 1 KHỚP TỊNH TIẾN

Giảng viên hướng dẫn

: TS. Trần Thị Thanh
Hải

Học viên

: Ninh Ngọc Quý

Mã số học viên

: 20212132M

1


Tieuluanmoi

LỜI NÓI ĐẦU
Hiện nay, khoa học kỹ thuật về robot nói chung và về robot cơng nghiệp nói riêng đang phát
triển rất mạnh mẽ và đặc biệt ở các nước có nền kinh tế phát triển, đã góp phần mang lại hiệu
quả kinh tế cao. Việc nghiên cứu chế tạo robot công nghiệp để ứng dụng trong sản xuất là rất cần
thiết. Với sự ra đời của robot đã thay thế cho con người khi làm việc giúp cho việc thực hiện các
cơng việc địi hỏi độ chính xác cao mà con người khó có thể làm được. Việc sử dụng robot công
nghiệp trong các nhà máy sản xuất đã làm tăng năng suất, nâng cao chất lượng của sản phẩm.
Nhận thấy vai trò và tầm quan trọng của robot như vây, ở nước ta hiện này, robot đang được đầu
tư nghiên cứu rất nhiều, đặc biệt ở các trường đại học trong đó có Trường Đại học Bách khoa Hà
Nội.
“Robot Công Nghiệp” là một học phần bắt buộc trong chương trình Module 3: Robot - Cơ
điện tử, học phần giúp cho sinh viên bước đầu làm quen với việc thiết kế cơ khí của một hệ
thống Robot cơ bản, ứng dụng phần mềm CAD 2D/3D, mô phỏng nguyên lý hoạt động robot,
giúp cho sinh viên nắm vững được các kiến thức cơ bản của các học phần và làm quen với nhiệm
vụ của người kỹ sư. Có thể nói đây là học phần khơng thể thiếu được đối với sinh viên chuyên
ngành Cơ điện tử.
Được sự hướng dẫn và giúp đỡ nhiệt tình của TS. Trần Thị Thanh Hải, em đã hoàn thành đề
tài được giao “Khảo sát robot 6 bậc tự do: 5 khớp quay, 1 khớp tịnh tiến”. Bởi thời gian và kiến
thức có giới hạn, sẽ khơng tránh khỏi những sai sót ngồi ý muốn. Do vậy em rất mong được sự
chỉ bảo và sự đóng góp ý kiến của thầy để đề tài được hoàn thiện hơn. Em xin chân thành cảm ơn
sự hướng dẫn của TS. Trần Thị Thanh Hải đã giúp em hoàn thành đề tài này.
Hà Nội, tháng 6 năm 2022

Ninh Ngọc Quý

2


Tieuluanmoi
Mục lục

LỜI NĨI ĐẦU

2

CHƯƠNG I: PHÂN TÍCH NGUN LÝ VÀ THÔNG SỐ KỸ THUẬT

4

1.1.

Nhiệm vụ thiết kế

4

1.2.

Các thành phần kết cấu của robot

4

CHƯƠNG II: TÍNH TỐN THIẾT KẾ VÀ KHẢO SÁT KHÔNG GIAN LÀM VIỆC

5

2.1. Thiết lập sơ đồ động robot

5

2.2. Giải bài toán động học thuận

6

2.2.1.

Các ma trận biến đổi thuần nhất

6

2.2.2.

Thiết lập ma trận trạng thái khâu thao tác theo tọa độ thao tác

9

2.2.3.

Thiết lập ma trận trạng thái khâu thao tác theo cấu trúc động học

9

2.2.4.

Giải bài tốn động học thuận

CHƯƠNG III: XÁC ĐỊNH KHƠNG GIAN LÀM VIỆC CỦA ROBOT

10
11

3.1. Lập trình trên chương trình Matlab

11

3.2. Kết quả đạt được

15

3


Tieuluanmoi
CHƯƠNG I: PHÂN TÍCH NGUN LÝ VÀ THƠNG SỐ KỸ THUẬT
1.1.

Nhiệm vụ thiết kế
-

Robot 6 bậc tự do: 5 khớp quay và 1 khớp tịnh tiến.

-

Các kích thước khâu và biến khớp phù hợp yêu cầu của đề bài

1.2.

Các thành phần kết cấu của robot
-

Thân robot: Là khâu cố định, đặt thẳng đứng giữ robot cố định khi làm việc, gắn với
khâu động 1 qua khớp quay 1 với trục z01 thẳng đứng

-

Khâu 1: Khâu dẫn động nằm ngang vng góc với trục thẳng đứng trong suốt q trình
làm việc của robot, có khả năng quay xung quanh trục z01 qua khớp quay 1

-

Khâu 2: Khâu động có khả năng quay trong mặt phẳng vuông trục thẳng đứng qua khớp
quay 2 nối với khâu 1

-

Khâu 3: trục vít me - đai ốc bi (trục vít tịnh tiến, đai ốc quay)

-

Khâu 4, 5, 6: Khớp quay cổ tay robot dùng để định hướng điểm thao tác cuối trong không
gian.

4


Tieuluanmoi
CHƯƠNG II: TÍNH TỐN THIẾT KẾ VÀ KHẢO SÁT KHƠNG GIAN LÀM VIỆC
2.1. Thiết lập sơ đồ động robot

Hình 2.1. Sơ đồ động học Robot
Sử dụng phương pháp Denavit-Hartenberg để giải bài toán động học robot. Thiết lập hệ tọa
độ Denavit-Hartenberg:

Bảng D-H:
Link

θ

d

a

α

1

q1

d1

0

п/2

2

q2

d2

0

п/2

3

0

q3

0

0

4

q4

d4

0

п/2

5

q5

0

0

- п/2

6

q6

d6

0

0

5


Tieuluanmoi
Ta chọn: d1 = 3; d2 = 2; d4 = 1; d5 = 0; d6 = 0.5

2.2. Giải bài toán động học thuận
Đối với bài toán động học thuận, vị trí, vận tốc, gia tốc các biến khớp xem như đã biết, cần
tìm vị trí, vận tốc, gia tốc của khâu thao tác đối với hệ tọa độ cố định. Vị trí của khâu thao tác đối
với hệ tọa độ cố định được xác định bởi các tọa độ thao tác gồm các tọa độ định vị điểm tác động
cuối và hướng của khâu thao tác.
Chú ý: Từ nay, để đơn giản các biểu thức, ta quy ước sin = s, cos = c
2.2.1. Các ma trận biến đổi thuần nhất
Ta chọn: d1 = 3; d2 = 2; d4 = 1; d5 = 0; d6 = 0.5
Áp dụng chương trình Matlab ta có các ma trận biến đổi thuần nhất như sau:
A_01 =
[cos(q1), 0,

sin(q1),

0]

[sin(q1),

0,

-cos(q1),

0]

[

0,

1,

0,

3]

[

0,

0,

0,

1]

[cos(q2), 0,

sin(q2),

0]

[sin(q2),

0,

-cos(q2),

0]

[

0,

1,

0,

2]

[

0,

0,

0,

1]

A_12 =

A_23 =
[1, 0, 0,
0]
[0, 1, 0,

0]

[0, 0, 1,

q3]

[0, 0, 0,

1]

6


Tieuluanmoi
A_34 =
[cos(q4), 0,

sin(q4),

0]

[sin(q4),

0,

-cos(q4),

0]

[ 0,

1,

0,

1]

0,

0,

1]

[cos(q5), 0,

-sin(q5),

0]

[sin(q5),

0,

cos(q5),

0]

[

0,

-1,

0,

0]

[

0,

0,

0,

1]

[

0,

A_45 =

A_56 =
[cos(q6),

-sin(q6),

0,

0]

[sin(q6),

cos(q6),

0,

0]

[

0,

0,

1,

1/2]

[

0,

0,

0,

1]

Từ đây ta có:
A_06 =
[ c(q6)*(c(q5)*(s(q1)*s(q4) + c(q1)*c(q2)*c(q4)) + c(q1)*s(q2)*s(q5)) + s(q6)*(c(q4)*s(q1)
- c(q1)*c(q2)*s(q4)), c(q6)*(c(q4)*s(q1) - c(q1)*c(q2)*s(q4)) - s(q6)*(c(q5)*(s(q1)*s(q4) +
c(q1)*c(q2)*c(q4)) + c(q1)*s(q2)*s(q5)),

c(q1)*c(q5)*s(q2) - s(q5)*(s(q1)*s(q4) +

c(q1)*c(q2)*c(q4)), 2*s(q1) + c(q1)*s(q2) - (s(q5)*(s(q1)*s(q4) + c(q1)*c(q2)*c(q4)))/2 +
q3*c(q1)*s(q2) + (c(q1)*c(q5)*s(q2))/2]
[- c(q6)*(c(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)) - s(q1)*s(q2)*s(q5)) - s(q6)*(c(q1)*c(q4)
+ c(q2)*s(q1)*s(q4)), s(q6)*(c(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)) - s(q1)*s(q2)*s(q5)) c(q6)*(c(q1)*c(q4) + c(q2)*s(q1)*s(q4)), s(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)) +

7


Tieuluanmoi
c(q5)*s(q1)*s(q2), s(q1)*s(q2) - 2*c(q1) + (s(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)))/2 +

q3*s(q1)*s(q2) + (c(q5)*s(q1)*s(q2))/2]
[
s(q2)*s(q4)*s(q6),
c(q4)*c(q5)*s(q2)) - c(q6)*s(q2)*s(q4),
c(q4)*s(q2)*s(q5),

- c(q6)*(c(q2)*s(q5) - c(q4)*c(q5)*s(q2)) s(q6)*(c(q2)*s(q5) - c(q2)*c(q5) 3 - (c(q2)*c(q5))/2 - q3*c(q2) -

(c(q4)*s(q2)*s(q5))/2 - c(q2)]
[
0,

0,
0,

1]

8


Tieuluanmoi
2.2.2. Thiết lập ma trận trạng thái khâu thao tác theo tọa độ thao tác
Lấy gốc tọa độ thiết lập trên khâu thao tác O3 trùng với E. Sử dụng góc Cardan xác định
hướng vật rắn. Gọi p là vector tọa độ định vị khâu thao tác.
p=[x

y

z

α

β γ ]T

Trong đó:
yE zE
Tọa độ vị trí điểm tác động cuối: r0 =[x

Hướng của khâu tác động cuối:
−cos β sin γ

cos β cos γ

[

c11 (α , β , γ )

γ)

R0 (α , β , γ )= c
E

c12(α , β ,

][

c13(α , β , γ )

(α , β , γ ) c

(α , β , γ ) c

21

c31 (α , β , γ ) c32 (α , β , γ ) c33 (α , β , γ
)

(α , β , γ ) =
22

23

sin α sin β cos γ
+cos α sin γ
−sin α sin β sin γ
+cos α

−cos α sin β cos γ +sin α sin γ cos α sin β sin γ +sin
α

9


Tieuluanmoi
Khi đó, ma trận biến đổi tọa độ thuần nhất của khâu cuối biểu diễn bởi tọa độ khâu thao tác
là:

0

[

R0 r 0

]

2.2.3. Thiết lập ma trận trạng thái khâu thao tác theo cấu trúc động học
Ma trận biến đổi tọa độ thuần nhất biểu diễn trạng thái khâu thao tác có thể xác định được từ
cấu trúc động học của robot
Ma trận được đưa về dạng như sau;

[

0

A0(q) r 0(q)

0
Trong đó: q=[q

q

q

]

1

]T

[

a11(q )

a12 (q)

10


Tieuluanmoi

a13 (q)

]
A0= a 21(q ) a22

a31(q )

a32 (q)

(q) a23
a33 (q)

(q)

11


Tieuluanmoi
r0=[ x( q)

y (q)

z (q)]T

Phương trình động học robot nhận được trong dạng ma trận như sau:
T 0(q)=T 0( p)
n

[

A0(q)

r0(q)

[

n

R0 ( α , β , γ )

r0 ( x

,y ,z )

]

]

Từ đó có ta có phương trình động học dạng ma trận có dạng khai triển như sau:
a12 (q)

a13 (q)
c12 (α , β , γ
)

[

a11

(q)

x (q)

c13(α , β , γ )

] [

c11 (α , β

,γ)

xE

]

2.2.4. Giải bài toán động học thuận
a. Xác định tọa độ định vị điểm tác động cuối:
Các tọa độ định vị điểm tác động cuối được xác định bằng các so sánh các phần tử ở 2 vế của
hệ phương trình động học dạng ma trận (*), ta có:
xE =x (q)
yE = y (q)

12


Tieuluanmoi
zE =z (q)

⇨ XE = 2*s(q1) + c(q1)*s(q2) - (s(q5)*(s(q1)*s(q4) + c(q1)*c(q2)*c(q4)))/2 +
q3*c(q1)*s(q2) + (c(q1)*c(q5)*s(q2))/2
⇨
⇨ YE = s(q1)*s(q2) - 2*c(q1) + (s(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)))/2 +
q3*s(q1)*s(q2) + (c(q5)*s(q1)*s(q2))/2
⇨ ZE = 3 - (c(q2)*c(q5))/2 - q3*c(q2) - (c(q4)*s(q2)*s(q5))/2 - c(q2)]
b. Xác định hướng của khâu thao tác
Hướng của khâu thao tác có thể xác định bằng cách đối chiếu 3 phần tử không cùng hàng
(cột) giữa hai ma trận quay của phương trình (*):

13


Tieuluanmoi
c11(α , β , γ )=a11(q)
=> c13(α , β , γ )=a13(q )=>
c23 (α , β , γ )=a23 ( q)

c
o
s
β
c

o
s
γ
=
a
1
1

(
q
)
s
i
n
β
=
a
1
3

(
q
)
s
i
n
α
c
o
s

β
=
a
2
3

(
q
)
CHƯƠNG III: XÁC ĐỊNH KHÔNG GIAN LÀM VIỆC CỦA ROBOT
3.1. Lập trình trên chương trình Matlab
Chương trình gồm 2 chương trình con

14


Tieuluanmoi
a) Chương trình 1: Ma_tran_truyen.m

b) Chương trình chính: Vẽ không gian làm việc của robot
Ta nhận thấy tọa độ 3 khâu cuối 4, 5, 6 chỉ xác định hướng của khâu thao tác nên để giảm bớt
tính tốn, ta coi tọa độ 3 khâu này không đổi trong quá trình vẽ.
Ta đặt giới hạn khớp các khâu như sau:
min_q1
min_q2
min_q3
min_q4
min_q5
min_q6

=
=
=
=
=
=

0;
0;
0;
0;
0;
0;

max_q1
max_q2
max_q3
max_q4
max_q5
max_q6

=
=
=
=
=
=

2*pi;
2*pi;

2;
2*pi;
2*pi;
2*pi;

15


Tieuluanmoi
assume(a1,'real');assume(a1>0);
assume(a2,'real');assume(a2>0);
assume(a3,'real');assume(a3>0);
assume(a4,'real');assume(a4>0);
assume(a5,'real');assume(a5>0);
assume(a6,'real');assume(a6>0);
assume(d1,'real');assume(d1>0);
assume(d2,'real');assume(d2>0);
assume(d3,'real');assume(d3>0);
assume(d4,'real');assume(d4>0);
assume(d5,'real');assume(d5>0);
assume(d6,'real');assume(d6>0);
assume(q1,'real');
assume(q2,'real');
assume(q3,'real');
assume(q4,'real');
assume(q5,'real');
assume(q6,'real');
assume(alpha1,'real');
assume(alpha2,'real');
assume(alpha3,'real');

assume(alpha4,'real');
assume(alpha5,'real');
assume(alpha6,'real');
q = [q1;q2;q3;q4;q5;q6];
%% Bang D-H
% theta d
a
alpha
% q1
d1 0
pi/2
% q2
d2 0
pi/2
% 0
q3 0
0
% q4
d4 0
pi/2
% q5
0
0 -pi/2
% q6
d6 0
0
%A = Ma_tran_truyen(theta, a, d, alpha)
A_01 = Ma_tran_truyen(theta1, a1, d1, alpha1)
A_12 = Ma_tran_truyen(theta2, a2, d2, alpha2)
A_23 = Ma_tran_truyen(theta3, a3, d3, alpha3)

A_34 = Ma_tran_truyen(theta4, a4, d4, alpha4)
A_45 = Ma_tran_truyen(theta5, a5, d5, alpha5)
A_56 = Ma_tran_truyen(theta6, a6, d6, alpha6)
A_06 = A_01*A_12*A_23*A_34*A_45*A_56
R_06 = A_06(1:3, 1:3);
%% Thay so
%d1 = 3; d2 = 2; d4 = 1; d5 = 0; d6 = 0.5
A_06 = subs(A_06,{theta1 theta2 theta3 theta4 theta5
a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5
q6 3 2 q3 1 0 0.5 0 0 0 0 0 0 pi/2 pi/2 0 pi/2 -pi/2
T_06 = A_06(1:3,4);
A_01 = subs(A_01,{theta1 theta2 theta3 theta4 theta5
a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5
q6 3 2 q3 1 0 0.5 0 0 0 0 0 0 pi/2 pi/2 0 pi/2 -pi/2

theta6 d1 d2 d3 d4 d5 d6
alpha6},{q1 q2 0 q4 q5
0})
theta6 d1 d2 d3 d4 d5 d6
alpha6},{q1 q2 0 q4 q5
0})

16


Tieuluanmoi
A_12 = subs(A_12,{theta1 theta2 theta3 theta4 theta5
a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5
q6 3 2 q3 1 0 0.5 0 0 0 0 0 0 pi/2 pi/2 0 pi/2 -pi/2
A_23 = subs(A_23,{theta1 theta2 theta3 theta4 theta5

a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5
q6 3 2 q3 1 0 0.5 0 0 0 0 0 0 pi/2 pi/2 0 pi/2 -pi/2
A_34 = subs(A_34,{theta1 theta2 theta3 theta4 theta5
a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5
q6 3 2 q3 1 0 0.5 0 0 0 0 0 0 pi/2 pi/2 0 pi/2 -pi/2
A_45 = subs(A_45,{theta1 theta2 theta3 theta4 theta5
a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5
q6 3 2 q3 1 0 0.5 0 0 0 0 0 0 pi/2 pi/2 0 pi/2 -pi/2
A_56 = subs(A_56,{theta1 theta2 theta3 theta4 theta5
a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5
q6 3 2 q3 1 0 0.5 0 0 0 0 0 0 pi/2 pi/2 0 pi/2 -pi/2

theta6 d1 d2 d3 d4 d5 d6
alpha6},{q1 q2 0 q4 q5
0})
theta6 d1 d2 d3 d4 d5 d6
alpha6},{q1 q2 0 q4 q5
0})
theta6 d1 d2 d3 d4 d5 d6
alpha6},{q1 q2 0 q4 q5
0})
theta6 d1 d2 d3 d4 d5 d6
alpha6},{q1 q2 0 q4 q5
0})
theta6 d1 d2 d3 d4 d5 d6
alpha6},{q1 q2 0 q4 q5
0})

%% Ve khong gian thao tac
min_q1 = 0;

min_q2 = 0;
min_q3 = 0;
min_q4 = 0;
min_q5 = 0;
min_q6 = 0;
max_q1 = 2*pi;
max_q2 = 2*pi;
max_q3 = 2;
max_q4 = 2*pi;
max_q5 = 2*pi;
max_q6 = 2*pi;
step1 = (max_q1
step2 = (max_q2
step3 = (max_q3
step4 = (max_q4
step5 = (max_q5
step6 = (max_q6

-

min_q1)/90;
min_q2)/90;
min_q3)/10;
min_q4)/2;
min_q5)/2;
min_q6)/2;

i = 1;
xE(1,:) = 0;
yE(1,:) = 0;

zE(1,:) = 0;
for i1 = min_q1:step1:max_q1
for i2 = min_q2:step2:max_q2
for i3 = min_q3:step3:max_q3
%for i4 = min_q4:step4:max_q4
%for i5 = min_q5:step5:max_q5
%for i6 = min_q6:step6:max_q6
T = subs(T_06,{q1 q2 q3 q4 q5 q6},{i1 i2 i3 0 0 0});
xE(1,i) = double(T(1,1));
yE(1,i) = double(T(2,1));
zE(1,i) = double(T(3,1));
%plot3(xE(i),yE(i),zE(i),'b.')
%pause(0.01)
i = i+1;
%end
%end

17


Tieuluanmoi

18


Tieuluanmoi
3.2. Kết quả đạt được

Hình 3.1. Khơng gian làm việc trong không gian 3D

19


Tieuluanmoi

Hình 3.2. Mặt phẳng làm việc XY

20


Tieuluanmoi

Hình 3.3. Mặt phẳng làm việc XZ

21