Đồ án Robot công nghiệp

KHOA ĐIỆN TỬ
Bộ môn Cơ Điện Tử

Cộng hòa xã hội chủ nghĩa Việt Nam
Độc Lập – Tự Do – Hạnh Phúc
------------o0o------------

ĐỒ ÁN ROBOT CÔNG NGHIỆP
Thiết kế:

1.

Nghiêm Đức Bình

2.

Nguyễn Mạnh Công

Giáo viên hướng dẫn:

Lớp:

K49CĐT01

Lê Thị Thu Thuỷ

Đề tài: Thiết kế, mô phỏng robot công nghiệp dạng Puma 560

Nội dung:

1. Tổng quan về robot
2. Tính toán động học
3. Cấu tạo và vai trò từng bộ phận của Robot
4. Mô phỏng Robot
5. Kết luận

Ngày giao đề:

…………………………………………

Ngày hoàn thành:

…………………………………………

Trưởng Bộ môn

Giáo viên hướng dẫn

TS. Phạm Thành Long

1

Th.s Lê Thị Thu Thủy

1
1


Đồ án Robot công nghiệp


NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………

2


2
2


Đồ án Robot công nghiệp
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
……………………………………………………………………………………….
NHẬN XÉT CỦA HỘ ĐỒNG CHẤM ĐỒ ÁN
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………

…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………

3

3
3


Đồ án Robot công nghiệp
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
……………………………………………………………………………………….

4

4
4


Đồ án Robot công nghiệp
DANH MỤC HÌNH ẢNH, BẢNG BIỂU
STT
1

2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

32
33
34
35
36

5

Nội dung
Hình 1.1 Robot Puma 560
Hình 1.2 Robot Puma trong công nghiệp
Hình 2.1: Hệ tọa độ gắn trên các khâu của robot
Hình 2.2 Sơ đồ gắp vật của Robot Puma
Hình 2.3 Tọa độ của vật
Hình 2.4. Kết quả vị trí tay kẹp home
Hình 2.5. Kết quả vị trí tay kẹp thứ nhất
Hình 2.6. Kết quả vị trí tay kẹp thứ hai
Hình 2.7. Kết quả vị trí tay kẹp thứ ba
Hình 2.8. Kết quả vị trí tay kẹp thứ tư
Hình 2.9. Kết quả vị trí tay kẹp thứ 5
Hình 2.10 Kết quả bài toán
Hình 3.1 Cấu tạo chung của Robot Puma

Hình 3.2 Khâu và khớp động
Hình 3.3 Kết cấu tay máy
Hình 3.4 Động cơ
Hình 3.5 Kích thước và thông số kĩ thuật.
Hình 3.6 Cảm biến ngoại tín hiệu

Trang

12
12
14
21
22
24
25
25
26
26
27
27
29
30
31
32
33
34
34

Hình 3.7 Bộ truyền cơ khí
Hình 3.8 Khớp quay
Hình 3.9 Tay kẹp
Hình 3.10 Mô hình vít me kéo dây
Hình 3.11 Đế
Hình 4.1 Ảnh mô hình Robot Puma 560
Hình 4.2 Chức năng CAM
Hình 4.3 Cửa sổ khởi động chương trình
Hình 4.4 Chạy chương trình
Hình 4.5 Giao diện chính của chương trình

Hình 4.6 Bảng New Solidworks
Hình 4.7 Thân robot
Hình 4.8 Cánh tay 1
Hình 4.9 Cánh tay 2
Hình 4.10 Bàn tay robot
Hình 4.11 Chốt
Hình 4.12 Giáo diện lắp ghép robot
Hình 4.13 Thư mục lưu hình vẽ từng bộ phận.

5
5

35
35
36
37
38
41
42
43
43
44
44
45
45
46
46
47
47



Đồ án Robot công nghiệp
37

Hình 4.14 Các bộ phận lắp ghép

48

38
39

Hình 4.15 Lệnh mate
Hình 4.16 Kết quả lắp ghép xong

48
49

CHƯƠNG I: TỔNG QUAN VỀ ROBOT
1.1 Sơ lược về lịch sử phát triển
Thuật ngữ robot được sinh ra từ trên sân khấu, không phải trong phân
xưởng sản xuất. Những robot xuất hiện lần đầu tiên trên ở trên NewYork vào
ngày 09/10/1922 trong vở “Rossum’s Universal Robot” của nhà soạn kịch ngườiTiệp
Karen Kapek viết năm 1921, còn từ robot là cách gọi tắt của từ robota -theo tiếng Tiệp
có nghĩa là công việc lao dịch.
Những robot thực sự có ích được nghiên cứu để đưa vào những ứng dụng
trong công nghiệp thực sự lại là những tay máy. Vào năm 1948, nhà nghiên cứuGoertz
đã nghiên cứu chế tạo loại tay máy đôi điều khiển từ xa đầu tiên, và cùngnăm đó hãng
General Mills chế tạo tay máy gần tương tự sử dụng cơ cấu tácđộng là những động cơ
điện kết hợp với các cử hành trình. Đến năm 1954,Goertz tiếp tục chế tạo một dạng tay
máy đôi sử dụng động cơ servo và có thểnhận biết lực tác động lên khâu cuối. Sử dụng

những thành quả đó, vào năm1956 hãng General Mills cho ra đời tay máy hoạt động
trong công việc khảo sátđáy biển. Năm 1968 R.S. Mosher, thuộc hãng General
Electric, đã chế tạo mộtthiết bịbiết đi có bốn chân, có chiều dài hơn 3m, nặng 1.400kg,
sử dụng động cơđốt trong có công suất gắn 100 mã lực. Cũng trong lĩnh vực này, một
thành tựu khoa học công nghệ đáng kể đã đạtđược vào năm 1970 là xe tự hành thám
hiểm bề mặt của mặt trăng Lunokohod , được điều khiển từ trái đất Viện nghiên cứu
thuộc Trường Đại học Stanford vào năm 1969 đã thiết kế robot Shakey di động tinh vi
hơn để thực hiện những thí nghiệm về điều khiểnsử dụng hệ thống thu nhận hình ảnh

6

6
6


Đồ án Robot công nghiệp
để nhận dạng đối tượng. Robotnày được lập trình trước để nhận dạng đối tượng bằng
camera, xác định đườngđi đến đối tượng và thực hiện một số tác động trên đối tượng.
Năm 1952 máy điều khiển chương trình số đầu tiên ra đời tại Học Viện
Công nghệ Massachusetts (Hoa Kỳ). Trên cơ sở đó năm 1954, George Devol đãthiết
kế robot lập trình với điều khiển chương trình số đầu tiên nhờ một thiết bịdo ông phát
minh được gọi là thiết bị chuyển khớp được lập trình. JosephEngelberger, người mà
ngày nay thường được gọi là cha đẻ của robot côngnghiệp, đã thành lập hãng
Unimation sau khi mua bản quyền thiết bị của Devolvà sau đó đã phát triển những thế
hệ robot điều khiển theo chương trình. Năm1962, robot Unmation đầu tiên được đưa
vào sử dụng tại hãng General Motors, và năm 1976 cánh tay robot đầu tiên trong
không gian đã được sử dụng trên tàuthám hiểm Viking của cơ quan Không Gian
NASA của Hoa Kỳ để lấy mẫu đấttrên sao Hoả.
Trong hoạt động sản xuất, đa số những robot công nghiệp có hình dạng của“cánh
tay cơ khí”, cũng chính vì vậy mà đôi khi ta gặp thuật ngữ người máy -tay máy trong

những tài liệu tham khảo và giáo trình về robot.
1.2 Một số định nghĩa về robot
Robot công nghiệp là một tay máy tự động linh hoạt,được sử dụng để thaythế
từng phần hoặc toàn bộ các hoạt động cơ bắp và hoạt động trí tuệ của conngười với
nhiều khả năng thích ngi khác nhau.
Viện Nghiên cứu robot Hoa Kỳ đưa ra một định nghĩa về robot như sau:“Robot là
một tay máy nhiều chức năng, thay đổi được chương trình hoạt động,được dùng để di
chuyển vật liệu, chi tiết máy, dụng cụ hoặc dùng cho nhữngcông việc đặc biệt thông
qua những chuyển động khác nhau đã được lập trìnhnhằm mục đích hoàn nhiệmvụ đa
dạng” (Schlussel, 1985).thành nhữngđịnh nghĩa của M.P.Groover về robot không dừng
lại ở tay máy mà mở rộngra cho nhiều đối tượng khác có những đặc tính tương tự như
con người như làsuy nghĩ, có khả năng đưa ra quy định và có thể nhìn thấy hoặc cảm
nhận đượcđặc điểm của vật hay đối tượng mà nó phải thao tác hoặc xử lý.
TheoArtobolevski I.I., Vorobiov M.V. và các nhà nghiên cứu thuộc trường phái
khốiSEV trước đây thì phát biểu rằng: “Robot công nghiệp là những máy hoạt độngtự
động được điều khiển theo chương trình để thực hiện việc thay đổi vị trí củanhững đối

7

7
7


Đồ án Robot công nghiệp
tượng thao tác khác nhau với mục đích tự động hoácác quá trình sảnxuất”.
Robot công nghiệp là một tay máy vạn năng nhiều trục(số trục ít nhất là3), chuyển
động của cơ cấu chấp hành được điều khiển tự động (định nghĩa ttheo
tiêu chuẩn ISA TR 8373-1991)
Robot công nghiệp là một tay máy vạn năng hoạt động theo chươngtrìnhvà có
thể lập trình lại để hoàn thành và nâng cao hiệu quả hoàn thành các nhiệmvụ khác

nhau trong công nghiệp,như vận chuyển nguyên vật liệu,chi tiết dụng cụhoặc các thiết
bị chuyên dùng khác(định nghĩa theo đề xuất của viện nghiên cứurobot của Mỹ).
1.3 Phân loại robot
Robot công nghiệp rất phong phú đa dạng, có thể được phân loại theo các cách
sau:
-

Phân loại theo kết cấu
Phân loại theo hệ thống truyền động
Phân loại theo ứng dụng
Phân loại theo cách thức và đặc trưng của phương pháp điều khiển

1.4 Một số ứng dựng của Robot công nghiệp
Mục tiêu ứng dụng:
Mục tiêu ứng dụng của robot công nghiệp là thay thế từng phần hoặc toàn bộ lao động
nặng nhọc của con người hoặc thay thế con người khi phải làm việc trong điều kiện kỹ
thuật,môi trường đặc biệt, đồng thời mang lại năng suất chất lượng và hiệu quả
Robot được sử dụng trong nhiều lĩnh vực sản xuất và đời sống của côn người trong đó
robot công nghiệp đóng vai trò rất quan trọng và nó được ứng dụng trong nhiều lĩnh
vực công nghiệp như:
- Phục vụ máy NC và các hệ thống tự động linh hoạt .
- Đúc, phun phủ, lắp ráp và đóng gói.
- Tự động hàn.
- Đảm nhận thực hiện cấp phôi phục vụ các nguyên công trong các dây
truyền sản xuất tự động.
- Chế tạo máy.

8

8

8


Đồ án Robot công nghiệp
- Kiểm tra chất lượng và hiệu quả

- Phạm vi ứng dụng:
Tình hình ứng dụng robot công nghiệp trên thế giới:nhìn chung viêc sử dụng
robot công nghiệp ở các nước phát triển như:Nhật, Mỹ, Italy, Pháp, Anh là rất
phổ biến và tốc độ phát triển hàng năm rất nhanh, theo thống kê chư đầy đủ tỷ
lệ phân bố phạm vi ứng dụng robot trong các lĩnh vực như sau:ô tô 31%, thiết
bị điện 16%, dầu khí,hóa thực phẩm 12%, thiết bị viễn thông 8%, máy công cụ
6 %, kim khí 4%, máy tính 3% và các lĩnh vực khác chiếm khoảng 20%.
1.5 Tổng quan về Robot puma 560
1.5.1 Lịch sử phát triển
Robot PUMA là một cánh tay robot công nghiệp được phát triển bởi Victor
Scheinman người tiên phong trong việc phát triển của robot, Unimation. Ban đầu được
phát triển cho General Motors, PUMA được dựa trên các dự án Scheinman đầu tạo ra
khi tham dựĐại học Stanford .
Unimation sản xuất robot PUMA trong nhiều năm cho đến khi nó đã được mua
bởi Westinghouse (khoảng năm 1980), và sau đó là các công ty Thụy
Sĩ Staubli (1988). Nokia Robotics sản xuất khoảng 1500 PUMA robot trong những
năm 80; Puma-650 đã trở thành mô hình phổ biến nhất trong số các khách hàng. Một
số cũng được thiết kế bởi Nokia Robotics, như Nokia NS-16 công nghiệp Robot hoặc
NRS-15 . Nokia đã bán bộ phận robot của mình vào năm 1990.
Năm 2002 General Motors Controls, Robotics và tổ chức hàn (CRW) tặng các mẫu
thử nghiệm ban đầu của robot PUMA tại Bảo tàng Lịch sử Quốc gia Mỹ thuộc Viện
Smithsonian. Đã tham gia vào một bộ sưu tập riêng biệt của robot lịch sử quan trọng,
trong đó bao gồm một trong những người đầu tiên Unimate và Odetics Odex
Robot công nghiệp có những bước phát triển vượt bậc. Nhiều nước trên thế giớ

sớm áp dụng mạnh mẽ kỹ thuật robot và sản xuất và nó đã đem lại những hiệu quả to
lớn về kinh tế kỹ thuật, nâng cao năng suất lao động, tăng chất lượng và khả năng làm

9

9
9


Đồ án Robot công nghiệp
việc cạnh tranh của sản phẩm, cải thiện điều kiện làm việc của công nhân. Đối với
nước ta, kỹ thuật còn là vấn đề khá mới mẻ, nhất là việc nghiên cứu thiết kế, chế tạo
robot. Robot puma là loại robot hoạt động theo hệ tọa độ góc( hệ tọa độ phỏng sinh
học). Với ưu điểm là gọn nhẹ, vùng thao tác tương đối lớn so với khuôn khổ kích
thước của nó, tính linh hoạt rất cao có thể phối hợp làm việc với các robot khác, d dod
nod được ứng dụng ngày càng rộng rãi trong các lĩnh vực của kỹ thuật, trong công
nghiệp, y tế, khai thác…
Robot puma được thiết kế đầu tiên vào năm 1978 bởi công ty Unimation. Puma có
nghĩa là một máy móc vạn năng lập trình được sử dụng trong công việc lắp ráp, tay
máy robot Puma thường có 5 hoặc 6 khớp quay.

Hình 1.1 Robot Puma 560
1.5.2 Vai trò
Robot Puma được sử dụng trong nhiều lĩnh vực sản xuất và đời sống của con người
trong đó robot công nghiệp đóng vai trò rất quan trọng và nó được ứng dụng trong
nhiều lĩnh vực công nghiệp như: hàn điểm, hàn mạch, chế tạo và lắp ráp..vv..

Hình 1.2 Robot Puma trong công nghiệp

10


10
10


Đồ án Robot công nghiệp
1.5.3 Kết cấu
Robot PUMA là robot có 6 bậc tự do, cấu hình RRRRRRR, kích thước và số liệu
cụ thể như sau:
•

6 tay hướng trục có 3 trục tạo thành một cổ tay hình cầu.

•

kéo dài tối đa: 878mm từ trục trung tâm tại các trung tâm của cổ tay.

•

Payload có thể nặng từ 2.5 đến 4kg

•

cánh tay Builder: 83 kg (khoảng)

•

Độ lặp lại: ± 0.1mm

•


2,5 kg Tốc độ tối đa: 500mm / sec

•

4,0 kg Tốc độ tối đa: 470mm / sec

1.5.4 Kết luận
Qua việc nghiên cứu đề tài, cho chúng ta biết thêm về sự phát triển và
tầmquantrọng của robot công nghiệp. Cho chúng ta biết thêm về hệ thống sản
xuấttiên tiến Opencim đề tài đả khái quát hóa và xây dựng được mô hình
làmviệccho hệ thống OpenCIM phục vụ các chương trình đào tạo,nghiên cứukhoa
họcvà chuyển giao công nghệ.
- Thiết lập được mô hình làm việc cho hệ thống Cim
- Thiết kế-tích hợp mô hình làm việc cho hệ thống
- Quản lý, điều khiển và vận hành cho hệ thống
- Ứng dụng mô hình vào sản xuất thực tế

11

11
11


Đồ án Robot công nghiệp

CHƯƠNG II: TÍNH TOÁN ĐỘNG HỌC

2.1 Tính toán động học thuận robot
Trong bài toàn này khảo sát bài toán điều khiển động học robot, chủ yếu tập trung vào

hai bài toán
- Bài toán thuận: Xác định vị trí điểm cuối và hướng tay kẹp và tay máy đạt được khi
quy luật thay đổi theo thười gian của các thông số định vị của tay máy là hàm đã biết.
bài toán này nhắm phục vụ bài toán xác định phạm vi hoạt động của tay máy, bài toán
thuận trong động lực học tay máy.
- Bài toán ngược: Xác định quy luật thay đổi theo thời gian của tay máy để nó nắm bắt
được vị trí đã cho cảu đối tượng theo một hướng định trước của tay kẹp. Bài toán này
nhắm phục vụ các bài toán điều khiển quỹ đạo, các bài toán điều khiển tối ưu.
2.1.1 Sơ đồ động học và hệ tọa độ robot
Robot PUMA là robot có 6 bậc tự do, cấu hình RRRRRR.
Sơ đồ động và hệ tọa độ gắn trên các khâu của robot như sau:

12

12
12


Đồ án Robot công nghiệp

X3

Z4

X2

X6

X4 X5


Z2
Z3

O2 O3

Z5 O6

O4 O5

Z1
X1

O1

Z0

x

O0
X0

Hình 2.1: Hệ tọa độ gắn trên các khâu của robot
*BẢNG THÔNG SỐ DENAVIT - HARTENBERG (DH)
Khâu

θi

1

(θ1)


2

(θ2)

3

αi

ai

di

0

d1

a2

0

(θ3)

0

0

4

(θ4)


0

d4

5

(θ5)

0

0

6

(θ6)

0

d6

13

0

0

13
13


Z6


Đồ án Robot công nghiệp

+ Chọn a1 = 00 mm; a2 = 0 mm;
a3max = 0mm.
+ Giới hạn chuyển động cuả các khâu:
- Khâu 1:

-900≤θ1≤ +900

- Khâu 2:

-1200≤θ2≤ +1200

- Khâu 3:
- Khâu 4:

0 ≤ a3≤ 150 mm
-900≤θ4≤ +900

- Khâu 5:
- Khâu 6:
2.1.2 Phương trình động học Robot
+ Để mô tả mối quan hệ về hướng và vị trí của hệ tọa độ gắn trên hai khâu liền kề
nhau (Khâu thứ i và khâu i-1) ta dùng các ma trận A i ; được biểu diễn bởi các phép
biến đổi:
Ai = Rot(z, θ0). Trans(a,0,0). Trans(0,0,d). Rot(x, α)


14

14
14


Đồ án Robot công nghiệp
Hay
cosθ
 sin θ

 0

Ai =  0

− sin θ cos α
cos θ cos α
sin α
0

sin θ sin α
− cosθ sin α
cos α
0

a cosθ 
a sin θ 
d 

1 


Qui ước viết tắt các hàm lượng giác như sau:
Ci = cosθi;

Si = sinθi; Cij = cos(θi+θj); Sij = sin(θi+θj);.......

Ta có:
A1 =;

A2 =;

A3 =; A4 =;
A5 =; A6 =;
Tích các ma trận Ai được gọi là ma trận T:
5

T6 = A6; 4T6 = A5. A6 ;

3

T6 = A4.A5.A6;

2

T6= A3.A4.A5.A6 ; 1T6= A2.A3.A4.A5.A6 ;

T6= A1.A2.A3.A4.A5.A6 ;
Ta còn có ma trận trạng thái cuối:
 nx
n

 y
 nz

TE =  0

sx
sy
sz
0

ax
ay
az
0

px 
p y 
pz 

1

Ma trận TE mô tả hướng và vị trí của hệ toạ độ gắn trên khâu chấp hành cuối đối với hệ
r r r

tọa độ gốc. Trong đó: n , s, a là các véctơ chỉ phương của hệ tọa độ gắn trên khâu chấp


hành cuối, p là véctơ điểm chỉ vị trí của gốc hệ tọa độ gắn trên khâu chấp hành cuối.
a-vector có hướng tiếp cận (approach) đối tác
s-vector có hướng đường trượt (sliding) đóng mở bàn kẹp

hoặc phương nắm bắt(occupation) kí hiệu là o

15

15
15


Đồ án Robot công nghiệp
n- vector pháp tuyến (normal)
Vậy ta có ma trân:
T=
[sin(q6)*(cos(q4)*sin(q1) - sin(q4)*(cos(q1)*cos(q2)*cos(q3) –
cos(q1)*sin(q2)*sin(q3))) + cos(q6)*(cos(q5)*(sin(q1)*sin(q4) +
cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +
sin(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))),
cos(q6)*(cos(q4)*sin(q1) - sin(q4)*(cos(q1)*cos(q2)*cos(q3) –
cos(q1)*sin(q2)*sin(q3))) - sin(q6)*(cos(q5)*(sin(q1)*sin(q4) +
cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +
sin(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))),
cos(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) –
sin(q5)*(sin(q1)*sin(q4) + cos(q4)*(cos(q1)*cos(q2)*cos(q3) –
cos(q1)*sin(q2)*sin(q3))), d4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) –
d6*(sin(q5)*(sin(q1)*sin(q4) + cos(q4)*(cos(q1)*cos(q2)*cos(q3) –
cos(q1)*sin(q2)*sin(q3))) - cos(q5)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))) + a2*cos(q1)*cos(q2)]
[ - sin(q6)*(cos(q1)*cos(q4) - sin(q4)*(sin(q1)*sin(q2)*sin(q3) –
cos(q2)*cos(q3)*sin(q1))) - cos(q6)*(cos(q5)*(cos(q1)*sin(q4) +
cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) –
sin(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))),

sin(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q4)*(sin(q1)*sin(q2)*sin(q3) –
cos(q2)*cos(q3)*sin(q1))) - sin(q5)*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))) - cos(q6)*(cos(q1)*cos(q4)
– sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))),

16

16
16


Đồ án Robot công nghiệp
sin(q5)*(cos(q1)*sin(q4) + cos(q4)*(sin(q1)*sin(q2)*sin(q3)
– cos(q2)*cos(q3)*sin(q1))) + cos(q5)*(cos(q2)*sin(q1)*sin(q3)
+ cos(q3)*sin(q1)*sin(q2)), d4*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))
+ d6*(sin(q5)*(cos(q1)*sin(q4) + cos(q4)*(sin(q1)*sin(q2)*sin(q3)
– cos(q2)*cos(q3)*sin(q1))) + cos(q5)*(cos(q2)*sin(q1)*sin(q3)
+ cos(q3)*sin(q1)*sin(q2))) + a2*cos(q2)*sin(q1)]
[
-cos(q6)*(sin(q5)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))
–
cos(q4)*cos(q5)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)))
- sin(q4)*sin(q6)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)),
sin(q6)*(sin(q5)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))
- cos(q4)*cos(q5)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)))
-cos(q6)*sin(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)),-cos(q5)*(cos(q2)*cos(q3) sin(q2)*sin(q3)) - cos(q4)*sin(q5)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)),
d1 - d4*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) - d6*(cos(q5)*(cos(q2)*cos(q3) sin(q2)*sin(q3)) + cos(q4)*sin(q5)*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) + a2*sin(q2)]
[
0,
0,

0,
1]
Trên cơ sở đó, ta có hệ phương trình động học của Robot PUMA như sau:
nx =C1C24 - S1S24 = C124
ny =S1C24 + C1S24 = S124

17

17
17


Đồ án Robot công nghiệp
nz = 0
ox = -C1S24 - S1C24 = - S124
oy =- S1S24 + C1C24 = C124
oz = 0
ax = 0
ay = 0
az = -1
px = a2C12 + a1C1.
px = a2S12 + a1S1.
pz = d3 + d4.
2.1.3 Kết luận
- Mục đính bài toán thuận:
Xác định vị trí điểm cuối và hướng tay kẹp và tay máy đạt được khi quy luật thay đổi
theo thười gian của các thông số định vị của tay máy là hàm đã biết. bài toán này nhắm
phục vụ bài toán xác định phạm vi hoạt động của tay máy, bài toán thuận trong động
lực học tay máy.
- Sau khi giải bài toán động học thuận thu được các phương trình mô tả hướng và vị trí

tổng quát của tay kẹp.khi cho trước các giá trị của biến khớp sẽ thu được vị trí và
hướng của tay kẹp một cách cụ thể.

2.2 Tính toán động học ngược Robot
Trong thực tế, ta biết trước vị trí và hướng mà khâu chấp hành cuối của robot cần đạt
đến. Điều ta cần biết là giá trị của các biến khớp (góc quay) tại mỗi thời điểm đó. Giải
hệ phương trình (1), khi biết trước hướng và vị trí của hệ tọa độ gắn trên khâu chấp
hành cuối, ta sẽ xác định được tệp nghiệm (θ1, θ2, θ3, θ4,θ5,θ6) là giá trị của các biến

18

18
18


Đồ án Robot công nghiệp
khớp. Các phương trình xác định giá trị các biến khớp thông qua các véctơ được gọi
là hệ phương trình động học ngược của robot.
Bài toán động học ngược là rất khó, đối với tay máy 6 khớp có ba khớp cuối đồng quy
tại một điểm, ta có thể tách bài toán động học ngược thành hai bài toán đơn giản hơn
là động học ngược vị trí vã động học ngược hướng.
+ Tìm vị trí giao điểm các trục cổ tay ( tâm cổ tay)
+ Tìm hướng cưa cổ tay
- Trong thực tế với sự linh hoạt của 6 bậc tự do, robot Puma có
thể làm được rất nhiều công việc khác nhau, từ đơn giản đến phức tạp như gắp vật,
hàn, lắp ráp… Trong mô hình toán, để đơn giản hóa tính toán, cho robot stanford thực
hiện nhiệm vụ gắp vật từ khay phân loại sản phẩm đặt vào khu vực đóng gói sản
phẩm. Với các giá trị hướng và vị trí tay kẹp đã biết trước kết quả bài toán ngược trả
về giá trự của biến khớp tương ứng với mỗi vị trí và hướng của tay kẹp khi làm việc.
- Căn cứ vào kiểu dáng và kích thước vùng làm việc của robot, bố trí vị trí phôi trong

vùng làm việc theo sơ đồ sau:

19

19
19


Đồ án Robot công nghiệp

Z0

Y0

Z0

Y0

20

20
20


Đồ án Robot công nghiệp

Hình 2.2 Sơ đồ gắp vật của Robot Puma

0


1

Y6

3

X6
Z6

4

2
6
4
.5

9
6
.8

2
5

Z0

7
5
.2

Y0


H2.3 Tọa độ của vật
•

Độ dài các khâu

D
Độ dài

d1
264.5

d2
0

d3
0

d4
170

q1

q2

q3

q4

q5


q6

-160 :
160

-225 :45

-45 :225

-110 :170

-100 :100

-266 :266

Giới hạn(o)

21

21
21

d5
0

d6
170.5



Đồ án Robot công nghiệp

TE =

 nx
n
 y
 nz

0

sx
sy

ax
ay

sz
0

az
0

px 
py 

pz 

1 


Robot Puma 560 có 6 bậc tự do,cần thiết lập 6 phương trình để giải ra 6 ẩn tương
ứng..trong đó luôn có 3 phương trình vị trí và 3 phương trình về hướng.

TE =

−
−

−

0

sx
−

ax
ay

−
0

−
0

px 
py 

pz 

1 


Từ ma trận Tvà TE ta được hệ phương trình động học ngược :
(sx – a12)^2=0
(ay-a23)^2=0
(ax-a13)^2=0
(Px-a14)^2=0
(Py-a24)^2=0
(Pz-a34)^2=0
Ma trân hướng 3x3 lấy bất kỳ 3 pt như trên
Cách tìm:
Sx=cos(Yi, X0)

22

22
22


Đồ án Robot công nghiệp

Ax=cos(Zi, X0)
Ay=cos(Zi, Y0)
ứng với vị trí P0 ta có:
sx=cos(Y6, X0 )= Cos 36 = 0.48
ax= cos(Z6, X0)=0
ay=cos(Z6, Y0)=0
Tương tự ứng với P1 đến P6 :

23


23
23


Đồ án Robot công nghiệp

Ta có bảng sau:
P

P0

P1

P2

P3

P4

P5

Sx

0.48

0.94

0.94

0.94


-0.8

-0.8

Ax

0

0

0

0

0

0

Ay

0

0

0

0

0


0

P0: (Home) đây là điểm khởi xuất làm việc từ tư thế nghỉ mở kẹp cách hệ tọa độ gốc
đặt tại chân robot(64.3;138.8;210)
Vị trí 0 như hình vẽ
P0 =

24

24
24


Đồ án Robot công nghiệp
Hình 2.4. Kết quả vị trí tay kẹp home
P1 : Di chuyển tay kẹp đến vị trí 1 như hình vẽ.
P1 =

Hình 2.5. Kết quả vị trí tay kẹp thứ nhất
P2 Di chuyển tay kẹp xuống 35.6mm để gắp vật.
P2 =

Hình 2.6. Kết quả vị trí tay kẹp thứ hai

25

25
25