BÀI TẬP LỚN KT ROBOT : ROBOT YASKAWA EP4000D

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

ĐỒ ÁN MÔN HỌC
BÀI TẬP DÀI : ROBOT YASKAWA EP4000D

Giảng viên hướng dẫn:
Bộ môn:
Viện:

PGS. TS. Nguyễn Phạm Thục Anh
Tự động hóa cơng nghiệp
Điện

Nhóm:
Thành viên:

4
Nguyễn Thế Thiết - 20181767
Quách Cao Hiển - 20181465
Nguyễn Công Minh - 20181650
Bùi Trung Nghĩa - 20181672
Nguyễn Đình Chí - 20181350
Đào Cơng Đồn - 20181393
Trần Minh Hồng - 20181496
Đỗ Tuấn Quang - 20181710
Vũ Mạnh Hùng - 20181513
Nguyễn Sách Dương - 20181439

HÀ NỘI, 1/2022

Lời mở đầu
Trong những năm gần đây cùng sự phát triển mãnh mẽ của các lĩnh vực khoa học, ứng
dụng của chúng trong các ngành công nghiệp, điện tử nhằm nâng cao năng suất và chất
lượng sản phẩm. Đặc biệt, robot công nghiệp là bộ phận cấu thành không thể thiếu trong
việc tạo ra những hệ thống tự động sản xuất có tính linh hoạt cao trong thị trường hàng hóa
cạnh tranh.
Gần nửa thế kỷ có mặt trong sản xuất, robot cơng nghiệp đã có một lịch sử phát triển
hấp dẫn. Ngày nay robot công nghiệp được dùng rộng rãi ở nhiều lĩnh vực sản xuất. Điều
đó xuất phát từ những ưu điểm cơ bản của các loại robot đã được chọn lựa và đúc kết lại
qua bao nhiêu năm ứng dụng ở nhiều nước.
Với đề tài “Tìm hiểu về robot yaskawa ep4000d”, nhóm em sẽ tìm hiểu các vấn đề cấu
trúc cơ cấu động học, động lực học, lập trình quỹ đạo, điều khiển chuyển động,… Các vấn
đề này sẽ lần lượt được đề cập trong các chương :
Chương I: Giới thiệu chung về robot yaskawa ep4000d
Chương II: Động học thuận vị trí robot
Chương III: Ma trận Jacoby
Chương IV: Động học đảo vị trí robot
Chương IV: Thiết kế quỹ đạo chuyển động các khớp
Chương IV: Mơ hình động lực học đối tượng trên toolbox simscape/matlab.
Qua việc thiết kế đồ án mơn học, giúp nhóm em hiểu rõ hơn những gỡ mắc trong q
trình học mơn Kỹ thuật Robot. Hiểu được những ứng dụng thực tế của robot trong công
nghiệp. Tuy nhiên do nội dung mới mẻ, nguồn tài liệu ít ỏi cùng với tầm hiểu biết còn hạn
chế nên khơng tránh khỏi sai sót. Em xin chân thành cảm ơn cô Nguyễn Phạm Thục Anh
đã giảng dạy và giúp đỡ chúng em hồn thành đồ án mơn học này.

1


MỤC LỤC

CHƯƠNG 1. GIỚI THIỆU CHUNG VỀ ROBOT YASKAWA EP4000D ................. 6
1.1

Giới thiệu về Robot Yaskawa ep4000d .................................................................. 6

1.2

Ứng dụng của Robot trong cơng nghiệp ................................................................. 6

1.3

Kết cấu cơ khí, thông số kĩ thuật cơ bản ................................................................. 7

1.4

1.3.1

Kết cấu cơ khí .......................................................................................... 7

1.3.2

Thơng số kĩ thuật cơ bản .......................................................................... 8

1.3.3

Bộ điều khiển Robot ................................................................................ 9

Hình ảnh hoạt động ............................................................................................... 10

CHƯƠNG 2.

ĐỘNG HỌC THUẬN VỊ TRÍ ROBOT ............................................. 11

2.1

Tính tốn động học thuận...................................................................................... 11

2.2

Xây dựng phần mềm giao diện động học thuận vị trí ........................................... 13

CHƯƠNG 3.

MA TRẬN JACOBY ........................................................................... 15

3.1

Tính tốn ma trận Jacoby ...................................................................................... 15

3.2

Các bước tính tốn ma trận Jacoby sử dụng thuật toán ma trận JH ...................... 15

3.3

Chương trình tính tốn trên Matlab ...................................................................... 17

3.4

Xây dựng phần mềm giao diện tính tốn Jacoby .................................................. 18

CHƯƠNG 4.

ĐỘNG HỌC ĐẢO VỊ TRÍ ROBOT................................................... 19

4.1

Cơ sở lý thuyết ...................................................................................................... 19

4.2

Tính động học đảo ................................................................................................. 19

4.3

Xây dựng giao diện nhập xuất dữ liệu và hiển thị kết quả.................................... 20

CHƯƠNG 5.
5.1

THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CÁC KHỚP .................. 21

Các bài toán trong thiết kế quỹ đạo....................................................................... 21
5.1.1

Chuyển động PTP (Point to Point)......................................................... 21

5.1.2

Chuyển động thông qua một số điểm trung gian ................................... 21

5.1.3

Chuyển động có ràng buộc về tốc độ và gia tốc .................................... 22

5.2

Thiết kế quỹ đạo chuyển động PTP ...................................................................... 22

5.3

Tính tốn thiết kế quỹ đạo chuyển động ............................................................... 22

CHƯƠNG 6. XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG
TRÊN TOOLBOX SIMSCAPE/MATLAB .................................................................. 25
6.1

Thiết kế robot trên Solid Work ............................................................................. 25

6.2

Mơ hình đối tượng trên Matlab ............................................................................. 27

KẾT LUẬN ...................................................................................................................... 29
TÀI LIỆU THAM KHẢO .............................................................................................. 30
PHỤ LỤC ......................................................................................................................... 31
2


1.

Chi tiết xây dựng giao diện động học thuận ............................................................. 31

2.

Chi tiết xây dựng giao diện ma trận Jacoby .............................................................. 34

3.

Chi tiết xây dựng động học nghịch ........................................................................... 37

4.

Chi tiết xây dựng quỹ đạo bậc 3 ............................................................................... 38

5.

Chi tiết xây dựng mơ hình đối tượng trên Matlab .................................................... 39

6.

Nhiệm vụ các thành viên trong nhóm ....................................................................... 44

3


DANH MỤC HÌNH VẼ
Hình 1.1. Robot Yaskawa EP4000d.................................................................................... 6
Hình 1.2. Robot nhìn từ phía trước ..................................................................................... 7
Hình 1.3. Robot khi nhìn từ trên xuống .............................................................................. 7

Hình 1.4. Mặt đáy robot ...................................................................................................... 7
Hình 1.5. Mặt cắt cơ cấu cuối ............................................................................................. 7
Hình 1.6. Kích thước và vùng làm việc robot ..................................................................... 8
Hình 1.7. Robot làm việc trong cơng nghiệp .................................................................... 10
Hình 1.8. Hình ảnh robot đang gắp sản phẩm ................................................................... 10
Hình 2.1. Hệ trục tọa độ Robot ......................................................................................... 11
Hình 2.2. Giao diện menu ................................................................................................. 13
Hình 2.3. Giao diện động học thuận ................................................................................. 14
Hình 3.1. Xác định ma trận Tin.......................................................................................... 15
Hình 3.2. Giao diện Jacoby ............................................................................................... 18
Hình 4.1. Giao diện động học đảo .................................................................................... 20
Hình 5.1. Chuyển động PTP ............................................................................................. 21
Hình 5.2. Chuyển động thơng qua một số điểm trung gian .............................................. 21
Hình 5.3. Chuyển động có ràng buộc về tốc độ và gia tốc ............................................... 22
Hình 5.4. Đồ thị các khớp ................................................................................................. 24
Hình 6.1. Phần chân đế robot ............................................................................................ 25
Hình 6.2. Phần thanh nối thứ nhất..................................................................................... 25
Hình 6.3. Phần thanh nối thứ hai....................................................................................... 26
Hình 6.4. Phần thanh nối thứ ba ........................................................................................ 26
Hình 6.5. Cơ cấu tác động ................................................................................................. 27
Hình 6.6. Tổng hợp mơ hình robot ................................................................................... 27
Hình 6.7.Mơ hình điều khiển ............................................................................................ 28

4


DANH MỤC BẢNG BIỂU
Bảng 1.1. Thông số kỹ thuật Robot .................................................................................... 8
Bảng 1.2. Thông số bộ điều khiển ...................................................................................... 9
Bảng 2.1. Bảng thông số Denavit-Hartenberg .................................................................. 11

Bảng 0.1. Nhiệm vụ thành viên......................................................................................... 44

5


CHƯƠNG 1. GIỚI THIỆU CHUNG VỀ ROBOT YASKAWA EP4000D
1.1 Giới thiệu về Robot Yaskawa ep4000d
Robot EP4000D do hãng YASKAWA tại Nhật Bản thiết kế và chế tạo. Hãng được thành
lập năm 1915 với lịch sử trên 100 năm, là tập đoàn hàng đầu thế giới trong sản xuất và
cung cấp các sản phẩm trong lĩnh vực robot công nghiệp, biến tần, truyền động điện...
Các robot EP4000D có trọng tải 200 kg, chiều ngang ngang 3,505 mm và tầm với thẳng
đứng 2,629 mm. Thiết kế gọn nhẹ của robot cho phép làm việc trong không gian chật hẹp
một cách kinh hoạt
Đây là loại robot có sáu trục quay vận hành bằng khí nén kết hợp với động cơ điện, giúp
robot vận hành một cách linh hoạt, di truyển dễ dàng dến từng vị trí đảm bảo độ chính xác
của robot trong cơng việc.

Hình 1.1. Robot Yaskawa EP4000d

1.2 Ứng dụng của Robot trong công nghiệp
Robot EP4000D được phát triển để ứng dụng vào các dây truyền sản xuất công nghiệp với
phù hợp với nhiều công việc khác nhau như:






Robot gắp (Handling)
Robot nâng bốc, đóng gói ( Picking/packing, palletizing)

Robot hàn, hàn điểm ( Arc handling, spot welding)
Robot sơn ( Painting)
Robot lắp ráp ( Assembly/distributing)

Do vậy việc sử dụng sản phẩm robot EP4000D của Yaskawa sẽ đảm bảo tính tương thích,
độ nhất quán và chất lượng dịch vụ cao... trong việc tự động hóa các máy móc, dây chuyền
sản xuất vì Yaskawa có thể cung cấp tát cả các loại robot để giải quyết các bài toán và đáp
ứng các yêu cầu khác nhau của khách hàng.
6


1.3 Kết cấu cơ khí, thơng số kĩ thuật cơ bản
1.3.1

Kết cấu cơ khí

Hình 1.2. Robot nhìn từ phía trước

Hình 1.3. Robot khi nhìn từ trên xuống

Hình 1.4. Mặt đáy robot

Hình 1.5. Mặt cắt cơ cấu cuối

7


Hình 1.6. Kích thước và vùng làm việc robot

Hình ảnh bên dưới cho ta thấy các kích thước của robot, cũng như vùng làm việc của robot

là vùng có màu xanh.
1.3.2

Thông số kĩ thuật cơ bản
Bảng 1.1. Thông số kỹ thuật Robot

THÔNG SỐ KỸ THUẬT CỦA ROBOT

Cấu trúc
Trục điều khiển
Tải trọng
Tầm dọc
Tầm ngang
Độ lặp lại
Phạm vi
chuyển động
tối đa

Trục S
Trục L
Trục U
Trục R
Trục B
Trục T

EP4000D
Standard Press Robot
Kiểu cánh tay nối dọc
6
200 kg (441 lbs.)

2,614 mm (103ꞌꞌ)
3,505 mm(138ꞌꞌ)
±0.5 mm(0.02ꞌꞌ)
±150o
+25o/-122o
+53o/-70o
±360o
±120o
±360o

8


Trục S
Trục L
Trục U
Trục R
Trục B
Trục T

Tốc độ
tối đa

Khối lượng
Phanh
Điện năng tiêu thụ
Momen
cho phép

Trục R

Trục B
Trục T
Trục R
Trục B
Trục T*

Lực quán tính
cho phép
I/O Lines
Air Lines
*Đáng giá trục T song song với sàn

90o/s
90o/s
90o/s
80o/s
80o/s
160o/s
3,100 kg
Tất cả các trục
22 kVA
1,274 N • m
2,156 N • m
0N•m
84.5 kg • m2
330 kg • m2
80 kg • m2
34 wires (0.5 mm2)
2 – 3/8ꞌꞌ air lines

1.3.3 Bộ điều khiển Robot
Robot dùng bộ diều khiển DX100, giúp người vận hành có thể dễ dàng lập trình và điều
khiển robot, cũng như dễ dàng kết nối robot với hệ thống trong dây chuyền sản xuất.
Dưới đây là các thông số kĩ thuật của bộ điều khiển.
Bảng 1.2. Thông số bộ điều khiển
THƠNG SỐ BỘ ĐIỀU KHIỂN DX100**
Kích thước (mm)
1,200 (w) x 1,000 (h) x 650 (d) (47.2ꞌꞌ x 39.4ꞌꞌ x 25.6ꞌꞌ)
Khối lượng
Hệ thống làm mát
Nhiệt độ môi
trường xung
quanh
Độ ẩm tương đối
Yêu cầu
về điện
Digital I/O
NPN - Tiêu chuẩn
PNP - Tuỳ chọn

Tối đa 250 kg (551.3 lbs.)
Làm mát gián tiếp
Trong quá trình hoạt động: 0o đến 45o C (32o đến 113o F)
Trong quá trình vận chuyển và lưu trữ: -10o đến 60o C
( 14o đến 140o F)
Tối đa 90% không ngưng tụ
3 pha
240/480/575 VAC ở mức 50/60 Hz
I/O tiêu chuẩn: 40 đầu vào/40 đầu ra bao gồm 16 đầu vào/16 đầu ra
hệ thống và 24 đầu vào/24 đầu ra người dùng

32 đầu ra Transistor, 8 đầu ra Relay
Tối đa I/O (tuỳ chọn): 2,048 đầu vào/2,048 đầu ra

Phản hồi vị trí
Bộ nhớ
chương trình

Bằng bộ mã hố tuyệt đối
JOB: 200,000 bước, 10,000 lần truy cập
Tiêu chuẩn CIO Ladder: 15,000 bước Mở rộng: 20,000 bước

9


Pendant Dim.(mm)
Pendant Weight
Giao diện
Nút nhấn
Ngơn ngữ lập trình
Chức năng bảo trì
Số lượng robot/trục
Đa tác vụ
Fieldbus
Ethernet
Safety

169(w) x 314.5(h) x 50(d) (6.7ꞌꞌ x 12.4ꞌꞌ x 299ꞌꞌ)
0.998 kg (2.2 lbs)
1 khe cắm flash; 1 cổng USB (1.1)
Teach/Play/Remote Keyswitch selector Servo On, Start, Hold, and

Emergency Stop Buttons
INFORM III, lập trình menu-driven
Hiển thị và khắc phục sự cố cảnh báo, dự đốn độ mịn của bộ giảm
tốc
Lên tới 8 rô bot, 72 trục
Lên tới 16 công việc đồng thời, 4 công việc hệ thống
DeviceNet Master/Slave, AB RIO, Profibus, Interbus-S, M-Net, CC
Link, EtherNer IP/Slave
10 Base T/100 Base TX
Nút dừng khẩn cấp kênh đơi, cơng tắc kích hoạt 3 vị trí,
Nhả phanh bằng tay
Đáp ứng ANSI/RIAR15.06-1999, ANSI/RIA/ISO 10218-2007 và
CSAZ434-03

1.4 Hình ảnh hoạt động

Hình 1.7. Robot làm việc trong cơng nghiệp

Hình 1.8. Hình ảnh robot đang gắp sản phẩm

10


CHƯƠNG 2. ĐỘNG HỌC THUẬN VỊ TRÍ ROBOT
2.1 Tính tốn động học thuận

Xác định các hệ trục tọa độ gắn với Robot:

Hình 2.1. Hệ trục tọa độ Robot

Thành lập bảng thông số Denavit-Hartenberg:
Bảng 2.1. Bảng thông số Denavit-Hartenberg

i
1
2
3
4
5
6

ai
550
1100
250
0
0
0

αi
90
0
90
-90
90
0

θi
θ1
θ2

θ3
θ4
θ5
θ6

di
740
0
2135
0
255
0

11


Xác định ma trận Ai :
Ti i1  Ai  Rot ( Z , i )Trans ( Z , di )Trans ( X i , ai ) Rot ( X , i )
 cos i  sin i 0 0 1


 sin i cos i 0 0  0

 
 0
0
0
1
0



 0
 0
0
0
1

 
 ci ci si si si

 s
ci ci si ci
  i
si
ci
0
0
0
0


0  1

0   0
0 1 d i   0
0 0 1   0
ai ci 

ai si 
d i 

1 
0 0
1 0

0 0 ai  1
0

1 0 0  0 cos i
0 1 0   0 sin i
0 0 1   0
0

0
 sin i
cos i
0

0

0
0
1

Suy ra :

 c1 0 s1 a1c1 
 s 0 c a s 
1
1 1
A1   1

0 1 0
d1  ;


1 
0 0 0
 c3 0 s3 a3c3 
 s 0 c a s 
3
3 3
A3   3
0 1 0
d3  ;


1 
0 0 0
0
 c5 0 s5
 s 0 c

0
5
5

A5  
 0 1 0 d5  ;


1

0 0 0

 c2
s
A2   2
0

0
 c4
s
A4   4
0

0
 c6
s
A6   6
0

0

 s2
c2
0

0 a2 s1 
0 a2 s2 
1
0 


0 0
1 
0  s4 0 
0 c4 0 
1 0 0 

0
0 1
 s6 0 0 
c6 0 0 
0 1 0

0 0 1

Xác định ma trận Ti :

 nx
n
6
0
T6   Ai  A1 A2 A3 A4 A5 A6   y
 nz
i 1

0
Chương trình tính tốn trên matlab:

ox
ox
ox

ax
ay
az

0

0

px 
p y 
pz 

1

syms a1 a2 a3 d1 d3 d5 i1 i2 i3 i4 i5 i6
A1=[cos(i1),0,sin(i1),a1*cos(i1);sin(i1),0,-cos(i1),a1*sin(i1);0,1,0,d1;0,0,0,1];
A2=[cos(i2),-sin(i2),0,a2*sin(i2);sin(i2),cos(i2),0,a2*sin(i2);0,0,1,0;0,0,0,1];
A3=[cos(i3),0,sin(i3),a3*cos(i3);sin(i3),0,-cos(i3),a3*sin(i3);0,1,0,d3;0,0,0,1];
A4=[cos(i4),0,-sin(i4),0;sin(i4),0,cos(i4),0;0,-1,0,0;0,0,0,1];
A5=[cos(i5),0,sin(i5),0;sin(i5),0,-cos(i5),0;0,1,0,d5;0,0,0,1];
A6=[cos(i6),-sin(i6),0,0;sin(i6),cos(i6),0,0;0,0,1,0;0,0,0,1];
T60=simplify(A1*A2*A3*A4*A5*A6);

12


Ta thu được kết quả
nx = s6(c4s1-s4c1c23) - c6[s5c1s23-c5(s1s4+c4c1c23)]
ny = -c6[s5s1s23+c5(c1s4-c4s1c23)] - s6(s4s1c23+c1c4)

nz = c6(c23s5+s23c4c5) – s23s4s6,
ox = s6[s5c1s23-c5(s1s4+c4c1c23)] + c6(c4s1-s4c1c23)
oy = s6[s5s1s23+c5(c1s4-c4s1c23)] - c6(s4s1c23+c1c4)
oz = -s6(c23s5+s23c4c5) – s23c6s4
ax = c5c1s23 + s5(s1s4+c4c1c23)
ay = c5c1s23 - s5(c1s4-c4s1c23)
az = s23c4s5 - c23c5
px = d5(c4s1-s4c1c23) + a1c1 + d3s1 + a2c1s2 + a3c1c23
py = a1s1 - d3c1 - d5(s4s1c23+c1c4) + a2s1s2 + a3s1c23
pz = d1+a3s23 + a2s2 - d5s23s4
2.2 Xây dựng phần mềm giao diện động học thuận vị trí

Sử dụng matlab guide, ta xây dựng được giao diện menu :

Hình 2.2. Giao diện menu

13


Xây dựng giao diện tính tốn động học thuận :

Hình 2.3. Giao diện động học thuận

14


CHƯƠNG 3. MA TRẬN JACOBY
3.1 Tính tốn ma trận Jacoby
Ta có:
n o a

0 0 0
⎡n o a
0 0 0⎤
⎢
⎥
n
o
a
0
0
0
⎥ . HJ
J=⎢
0
0
0
n
o
a
⎢
⎥
0
0
0
n
o
a
⎢
⎥
⎣0 0 0 n o a ⎦

n o a
0 0 0
JH
⎡n o a
0 0 0 ⎤ ⎡JH
⎢
⎥ ⎢
n
o
a
0
0
0
⎥ . ⎢JH
=⎢
⎢ 0 0 0 n o a ⎥ ⎢JH
⎢ 0 0 0 n o a ⎥ ⎢JH
⎣ 0 0 0 n o a ⎦ ⎣JH

JH
JH
JH
JH
JH
JH

JH
JH
JH
JH

JH
JH

JH
JH
JH
JH
JH
JH

JH
JH
JH
JH
JH
JH

JH
JH
JH
JH
JH
JH

⎤
⎥
⎥
⎥
⎥
⎦

3.2 Các bước tính tốn ma trận Jacoby sử dụng thuật toán ma trận JH
Bước 1: Xác định ma trận 𝑇 (𝑖 = 0 → 𝑛 − 1)

Hình 3.1. Xác định ma trận Tin

Bước 2: Xác định ma trận 𝐽

15


J

H
6n

 H px

 q1
 H p
y

 q1
 H
 pz
H
 X  q1

H
Q

  x
 q
1

H
  y

 q1
 H 
z

 q1

-

px
q2

H

H

p y

...

q2
pz
q2

...

 x
q2

...

H

H

H

px 

qn 
H
p y 

qn 

H
pz 
qn 

H
 x 
qn 

H

 y 

qn 
H
 z 

qn 
H

...

 y

...

q2
 z
q2

H

...

Khi (i+1) là khớp trượt, biến khớp 𝑟

-

Sử dụng ma trận 𝑇

 y H  y

 x


0
ri 1
ri 1
ri 1
H

H

px i
 nz
ri 1

H

H

p y

ri 1

 i oz

pz i
 az
ri 1

H

H

px

i 1

Khi (i+1) là khớp quay, biến khớp 𝜃
H

 i ny i px  i nx i p y

 x i
 nz
i 1
,

H

p y

, i 1
H

 i o y i px  i ox i p y

 y

i 1

 i oz

H

pz

, i 1

 i a y i px  i ax i p y

,

 z i
 az
i 1

H

,

Bước 3: Tính J
 R0
J  n
0

0 H
 J
Rn0 

16

3.3 Chương trình tính tốn trên Matlab

17


3.4 Xây dựng phần mềm giao diện tính tốn Jacoby
Sử dụng công cụ guide trong matlab, ta xây dựng được giao diện:

Hình 3.2. Giao diện Jacoby

18


CHƯƠNG 4. ĐỘNG HỌC ĐẢO VỊ TRÍ ROBOT
4.1 Cơ sở lý thuyết
Đã biết Tn0   n o a

p

 Tìm 𝜃 1
Bước 1:
VT1(𝜃 1) ---->

A11 (1 )Tn0  A2 ( 2 )... An ( n )

<------- VF1(𝜃 2, 𝜃 3, 𝜃 4, 𝜃 5, 𝜃 6)

Bước 2: Tìm phần tử O hoặc hằng số trong VF1

Bước 3: Cân bằng với phần tử tương ứng trong VF1
Áp dụng các trường hợp trong động học đảo giải tích
Bước 4: Tìm 𝜃 1
 Tìm 𝜃 2

A21 ( 2 )VT 1 (1 )  A21 ( 2 ) A11 (1 )Tn0  A3 (3 )... An ( n ) <-----

VT2(𝜃 2) ---->

-----VF2(𝜃 3, 𝜃 4, 𝜃 5, 𝜃 6)
Tương tự các bước 2,3,4 Tìm 𝜃 2
Quay lại từ các bước 1 để xác định tiếp các khớp cịn lại.
4.2 Tính động học đảo
Ta chia robot Yaskawa EP4000d thành 2 phần để thuận lợi cho việc tính tốn động học
ngược. Do robot có sáu bậc tự do dẫn đến việc khó khăn trong tính tốn.
Ma trận định vị tổng quát có dạng:

 nx
n
y
T 
 nz

0

ox a x
oy a y
oz

az

0

0

px 

py 
pz 

1 

Chương trình matlab:
syms c1 s1 d1 c2 s2 c3 s3 d3 a1 a2 a3 nx ny nz ox oz oy ax ay az px py pz
A1 = [c1, 0, s1, a1*c1; s1, 0, -c1, a1*s1; 0, 1, 0, d1; 0, 0, 0, 1];
A2 = [c2, -s2, 0, a2*c2; s2, c2, 0, a2*s2; 0, 0, 1, 0; 0, 0, 0, 1];
A3 = [c3, 0, s3, a3*c3; s3, 0, -c3, a3*s3;0, 1, 0, d3; 0, 0, 0, 1];
T13=simplify(A2*A3);
T03=simplify(A1*A2*A3);
T = [nx, ox, ax, px; ny, oy, ay, py; nz, oz, az, pz; 0 ,0 , 0, 1]
T3=A1^-1*T;
A2^-1*T3

19


Dùng matlab tính ta được :
 nx
n
1

1  y
T3  A1
 nz

 0

ox ax px   nxC1  ny S1 , oxC1  o y S1 , axC1  a y S1 , pxC1  p y S1  a1  p y S1 

 
,
oz
,
az
,
p z  d1
o y a y p y   nz



oz az pz   nx S1  ny C1 , ox S1  o yC1 , ax S1  a y C1 , px S1  p yC1

 
0 0 1  0

,
0
,
0
,
1

C2C1nx  ny S1  nz S2 , C2C1ox  o y S1  oz S2 , C2  axC1  a y S1   az S2 , C2  pxC1  p y S1  a1  pz S 2  a2  S 2  d1  pz   


 C2 nz  S 2C1nz  ny S1 , C2oz  S2 (oxC1  oy S1 ), az C2  S2  axC1  a y S1  ,  S 2  pxC1  py S1  a1  pz S2  C2  d1  p z   
1
A3  A2 T3  

, ox S1  oyC1
, ax S1  a y C1
, px S1  p y C1

 nx S1  ny C1

 0
,
0
,
0
,
1



Từ đó ta tính được:
theta1=atan2(ox,-oy);
theta2=atan2(((pz-d1-a3*(ax*cos(theta1)+ay*sin(theta1)))/a2),(px*cos(theta1)a1+py*sin(theta1)-a3*(nx*cos(theta1)+ny*sin(theta1)))/a2);
theta3=atan2((cos(theta2)*nzsin(theta2)*(nx*cos(theta1)+ny*sin(theta1))),cos(the
ta2)*(nx*cos(theta1)+ny*sin(theta1)
)+sin(theta2)*nz);

4.3 Xây dựng giao diện nhập xuất dữ liệu và hiển thị kết quả
Sử dụng công cụ guide trong matlab, ta xây dựng được giao diện:

Hình 4.1. Giao diện động học đảo

20


CHƯƠNG 5. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CÁC KHỚP
5.1 Các bài toán trong thiết kế quỹ đạo
5.1.1

Chuyển động PTP (Point to Point)

Hình 5.1. Chuyển động PTP

 Tay máy robot được yêu cầu chuyển động từ một vị trí này đến vị trí khác trong
khoảng thời gian tf.
 Khơng u cầu theo quỹ đạo chính xác.
 Áp dụng trong mơi trường khơng có chướng ngại vật.
5.1.2

Chuyển động thơng qua một số điểm trung gian

Hình 5.2. Chuyển động thơng qua một số điểm trung gian

 Vẫn yêu cầu tay máy chuyển động từ vị trí này sang vị trí khác trong khoảng thời
gian tf, nhưng có vơ số chướng ngại vật trên đường đi của robot.
 Ta thiết lập các điểm trung gian nằm giữa các chướng ngại vật và có khoảng cách
tương đối an toàn với chúng để tạo quỹ đạo tránh chướng ngại vật.

 Quỹ đạo có thể khơng cần chính xác đi qua điểm trung gian. Nhưng bắt buộc phải
dừng tại điểm cuối trong thời gian yêu cầu.
21


5.1.3

Chuyển động có ràng buộc về tốc độ và gia tốc

Hình 5.3. Chuyển động có ràng buộc về tốc độ và gia tốc

Trên thực tế bao giờ chúng ta cũng mong muốn chuyển động Robot làm sao tận dụng được
tối ưu năng lượng của hệ thống, và đảm bảo chính xác về điều khiển vị trí. Để tiết kiệm
năng lượng thì khi bắt đầu chúng ta cần tăng tốc tối đa, sau đó giữ tốc độ ổn định rồi giảm
tốc để đưa vật thể về đúng vị trí. Từ đó sinh ra bài tốn chuyển động có ràng buộc về tốc
độ và gia tốc.
5.2 Thiết kế quỹ đạo chuyển động PTP
Thiết kế quỹ đạo chuyển động của Robot có liên quan đến bài toán điều khiển robot di
chuyển từ vị trí này đến vị trí khác trong khơng gian làm việc. Đường đi và quỹ đạo được
thiết kế là đại lượng đặc trưng cho hệ thống điều khiển vị trí của Robot. Do đó độ chính
xác của quỹ đạo sẽ ảnh hưởng đến chất lượng di chuyển của Robot.
Yêu cầu thiết kế quỹ đạo chuyển động của Robot:
- Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian làm việc
hoặc di chuyển theo một quỹ đạo xác định.
- Quỹ đạo của robot phải là đường liên tục về vị trí trong một khoảng thời gian nhất
định.
- Khơng có bước nhảy về vận tốc, gia tốc. Quỹ đạo là các đường cong có dạng:
Đa thức bậc 2 : x(t)= a + bt + 𝑐𝑡2
Đa thức bậc 3 : x(t)= a + bt + 𝑐𝑡2 + 𝑑𝑡3
…

Đa thực bậc n : x(t)= a + bt + 𝑐𝑡2 + … + m 𝑑𝑡𝑛
5.3 Tính tốn thiết kế quỹ đạo chuyển động
- Chọn quỹ đạo thiết kế là hàm đa thức bậc 3 theo thời gian như sau :

i (t )  ai  bit  ci t 2  di t 3

(1)

Với i = 1,2,3,4,5,6 tương ứng với 6 khớp của robot.
Suy ra, quỹ đạo tốc độ và gia tốc của khớp thứ i có cơng thức :

i (t )  bi  2cit  3di t 2

(2)

và i ( t )  2 c i  6 d i t

(3)
22


- Nếu robot đi từ A đến B là trong khoảng thời gian từ 0 s đến t f s và vận tốc tại 2
điểm A và B của bằng 0. Ta có hệ phương trình :

i (0)  ai
 (0)  b
i

i
2

i (t f )  ai  bi t f  ci t f  di t f 3
 (t )  b  2c t  3d t 2
i

-

f

i

i f

i f

Từ hệ phương trình trên, ta rút ra được 4 ẩn a , b , c , d như sau:

ai  i (0)
b   (0)
i

ci 

di 
-

i

3
tf

2

( i (t f )   i (0)) 

2 
1
 i (t f )  i (0)
tf
tf

2
1 
(

(
t
)


(0))

( i (t f )  i (0))
i
f
i
3
2
tf
tf

Thay các giá trị a , b , c , d vào phương trình (1), (2) và (3). Từ đó thu được quỹ
đạo chuyển động, vận tốc và gia tốc của mỗi khớp.

Ví dụ: Giả sử robot cần di chuyển từ điểm A (ứng với góc quay của các khớp là ma trận
𝑄 ) tới điểm B (ứng với góc quay của các khớp là ma trận 𝑄 ) trong 5s
𝑄 = [0, 10,45,75,120,20]
𝑄 = [45,80,15,90,50,140]
Trong chuyển động ta cần các ràng buộc tốc độ 𝜃̇ và 𝜃̈ đều bằng 0 nên ta có:
𝑄̇ = 𝜃̇ , 𝜃̇ , 𝜃̇ , 𝜃̇ , 𝜃̇ , 𝜃̇
𝑄̇ = 𝜃̇ , 𝜃̇ , 𝜃̇ , 𝜃̇ , 𝜃̇ , 𝜃̇

= [0,0,0,0,0,0]
= [0,0,0,0,0,0]

Sử dụng công cụ matlab, ta thu được kết quả:

23


a

b

c

d

e

f
Hình 5.4. Đồ thị các khớp

a. Khớp 0
d. Khớp 3

b. Khớp 1
e. Khớp 4

c. Khớp 2
f. Khớp 5

24