TÌM HIỂU VỀ ROBOT PUMA 500

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

BÁO CÁO MƠN HỌC

KỸ THUẬT ROBOT

NHĨM 6
TÌM HIỂU VỀ ROBOT PUMA 500

GVHD:

PGS.TS.

Khoa:

Tự động hóa

Hà Nội, 7/2023


BẢNG PHÂN CÔNG NHIỆM VỤ


MỤC LỤC
PHẦN 1. GIỚI THIỆU VỀ PUMA 500.....................................................1
1.1

Giới thiệu về PUMA 500.........................................................................1

1.2

Thông số kỹ thuật....................................................................................2

1.3

Ứng dụng của robot PUMA 500..............................................................4

PHẦN 2. ĐỘNG HỌC THUẬN VỊ TRÍ...................................................5
2.1

Phương pháp Denavit – Hartenberg.........................................................5

2.2

Thiết lập bảng D-H cho robot PUMA......................................................8

PHẦN 3. TÍNH TỐN MA TRẬN JACOBY........................................10
3.1

Ma trận Jacoby và JH.............................................................................. 10

PHẦN 4. ĐỘNG HỌC NGƯỢC VỊ TRÍ.................................................16
4.1

Tổng quan phương pháp tính động học ngược robot PUMA.................16

4.2

Bài tốn tìm vị trí giao điểm trục cổ tay.................................................17

4.3

Tìm hướng của cổ tay............................................................................19

PHẦN 5. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT..............21
5.1

Giới thiệu về thiết kế quỹ đạo................................................................21

5.2

Thiết kế quỹ đạo trong không gian khớp................................................21

5.3

Điều khiển chuyển động cho Robot theo thuật tốn PID.......................23

5.3.1

Phương trình động lực học..............................................................23

5.3.2

Thiết kế bộ điều khiển PID..............................................................23

PHẦN

6.

XÂY DỰNG MƠ HÌNH ROBOT TRÊN SIMSCAPE/

MATLAB
24

6.1

Mơ hình robot trên SolidWork...............................................................24

6.2

Sơ đồ khối của Robot PUMA trên MATLAB/Simulink........................27

6.3

Bộ điều khiển PID..................................................................................28

6.4

Đáp ứng đầu ra trên các khớp................................................................31

TÀI LIỆU THAM KHẢO........................................................................35
PHỤ LỤC...................................................................................................36


PHẦN 1. GIỚI THIỆU VỀ PUMA 500
1.1 Giới thiệu về PUMA 500
PUMA( Programable Universal Machine for Assembly hoặc Programable
Universal for Arm) được phát triển bởi Giáo sư người Mĩ Victor David
Scheinman tại công ty robot tiên phong Unimation vào những năm 1980 dựa trên
những thiết kế mà ông phát minh ra khi còn học ở Đại học Stanford.
Puma 500 là dịng seri có thể đạt tới độ cao gần 2m sử dụng ngơn ngữ lập

trình val. Thiết kế của nó gồm 2 phần chính: cánh tay cơ và hệ thống điều khiển.
Chúng được kết nối với nhau bằng một hoặc hai dây cáp lớn nhiều dây dẫn. Khi
hai dây cáp được sử dụng, một dây dẫn truyền lực đến động cơ servo và phanh
trong khi dây thứ hai mang phản hồi vị trí cho từng khớp trở lại hệ Unimate 500
PUMA (1983), thiết bị điều thống điều khiển. Máy tính điều khiển và thiết bị đầu
cuối máy tính khiển dựa trên kiến trúc LSI-11 rất giống với máy tính PDP11.

Hình 1.1. Unimate 500 PUMA (1983)

Hệ thống có chương trình khởi động và công cụ gỡ lỗi cơ bản được tải trên
chip ROM. Hệ điều hành được tải từ bộ nhớ ngồi thơng qua một cổng nối tiếp,
thường là từ đĩa mềm. Bộ điều khiển cũng chứa bộ nguồn servo, bảng xử lý phản
hồi tương tự và kỹ thuật số, và hệ thống truyền động servo.

1


1.2 Thông số kỹ thuật
Bảng dưới đây là thông số kỹ thuật của Robot PUMA 500
AXES

6 revolute axes

Configuaration

6 degrees of freedom

Dive

Elictric DC servomotors

Power Requirement

110-130 VAC, 50-60Hz,1500 Watts

Repeatability

± 0.1 mm

Maximum static load

25 N

Maximum Straight Line Velocity

51cm/sec

Reach

86,6 cm to the wris
92.2 cm to the flange

Weight

54.4 kg

Hình 1.2. Thơng số (tính bằng inch) của robot PUMA 500


Hình 1.3. Hệ thống khớp của Robot PUMA 500

Hình 1.4. Giới hạn góc khớp


1.3 Ứng dụng của robot PUMA 500
Puma 500 được dùng trong các dây truyền sản suất gắp và di chuyển các
vật co khối lượng nhẹ như dây truyền sản xuất đồ uống, được dung trong các bản
vẽ kĩ thuật với tốc độ và độ chính xác cao và cịn nhiều ứng dụng khác.

Hình 1.5. Robot PUMA gắp vật

Tuy nhiên, ngày nay robot PUMA hiếm thấy được sử dụng trong công nghiệp.
Robot PUMA hiện chỉ được giới thiệu với mục đích giáo dục hoặc nghiên cứu.


PHẦN 2. ĐỘNG HỌC THUẬN VỊ TRÍ
2.1 Phương pháp Denavit – Hartenberg
Để đặt các hệ trục tọa độ lên Robot, phương pháp Denavit – Hartenberg sẽ
được sử dụng. Các bước thực hiện phương pháp được tóm tắt như sau:
Bước 1: Xác định số khớp và thanh nối của Robot. Robot n khớp (i = 1:n) sẽ
bao gồm khớp quay và khớp tịnh tiến
n+1 thanh nối: đánh số từ 0 đến n. Thanh nối 0 cố định.
Thanh nối n gắn với khâu tác động cuối
Khớp i nối giữa thanh nối i - 1 và i
Bước 2: Khai báo các hệ trục tọa độ trên mỗi thanh nối {OiXiYiZi} (i = 0:n)
Cách xác định trục Zi
Khớp (i+1) tịnh tiến: Zi là trục mà theo nó khớp (i+1) trượt
Khớp (i+1) quay: Zi là trục mà xung quanh nó khớp (i+1) quay
Cách xác định trục Xi
+ Trường hợp 1: Zi-1 và Zi chéo nhau: Xi là đường vng góc chung của Zi-1 và

Zi


+ Trường hợp 2: Zi-1 // Zi

+ Trường hợp 3: Zi-1 cắt Zi

Bước 3: Xác định các thông số động học của thanh nối Các tham số+ ai:
chiều dài thanh nối, ai>=0
+ αi: góc vặn của thanh nối ii: góc vặn của thanh nối i
+ di: độ lệch với thanh nối (i-1)
+ qi: góc quay của thanh nối i


Bước 4: Xác định ma trận Ai
cos (θθi)
Ai = [

sin(θθi)
0
0

− sin(θθi)cos(θαi)

sin(θθi)sin(θαi)

aicos(θθi)

cos(θθi)cos(θαi)
sin(θα )

− cos(θθi)sin(θαi)
cos(θα )

aisin(θθi)
d ]

0

i

0

i

i

1

Bước 5: Xác định ma trận 0T6 là kết quả của bài toán động học thuận Robot:
n0x
0
T6 0 = A1A2 … A6 = n y
nz0
[0

s0x
sy0
sz0
0

a0x
ay0
az0
0

p0x
py0
pz0
1 ]

Chú ý rằng, ma trận chuyển vị từ hệ i đến hệ i-1 là hàm của các biến khớp i
(nếu khớp thứ i là khớp quay) hoặc di (nếu khớp thứ i là khớp trượt).
Một cách tổng quát, quy tắc Denavit – Hartenberg cho phép tổ hợp các ma
trận chuyển vị riêng rẽ thành một mà trận chuyển vị thuần nhất, biểu diễn vị trí
và hướng của khâu n so với khâu cơ sở.


2.2 Thiết lập bảng D-H cho robot PUMA
Dựa vào phương pháp Denavit – Hartenberg, đặt các trục cho robot PUMA
như hình dưới:

Hình 2.1. Thiết lập các hệ trục tọa độ cho Robot PUMA

Các tham số của robot là xác định, ta có bảng D-H sau (đơn vị inch):
i

θi

αi

ai

di

1

q1

-90

0

0

2

q2

0

a2 = 17

d2 = 5.5

3

q3

90

0

0

4

q4

-90

0

d4 = 17.05

5

q5

90

0

0

6

q6

0

0

d6 = 2.2

Từ các giá trị góc khớp cho trước:
Q = [q1 q2 q3 q4 q5 q6]
Ma trận Ai được xác đinh:
cos (θθi)
Ai = [

sin(θθi)
0
0

− sin(θθi)cos(θαi)

sin(θθi)sin(θαi)

aicos(θθi)

cos(θθi)cos(θαi)
sin(θα )

− cos(θθi)sin(θαi)
cos(θα )

aisin(θθi)
d ]

0

i

0

i

i

1


Vị trí của điểm tác động cuối so với tọa độ gốc của Robot là kết quả của
bài toán động học thuận vị trí được xác định:
n0x
0
T6 0 = A1A2 … A6 = n y
nz0
[0

s0x
sy0
sz0
0

a0x
ay0
az0
0

p0x
py0
pz0
1 ]

Chương trình tính tốn trên MATLAB cho bài toán động học thuận Robot
PUMA được tổng hợp tại phần PHỤ LỤC.


PHẦN 3. TÍNH TỐN MA TRẬN JACOBY
3.1 Ma trận Jacoby và JH
Để tìm được mối quan hệ giữa tốc độ tay máy và tốc độ các khớp, ta sẽ tìm
thơng qua ma trận J
J=

∂X
∂Q

Có nhiều phương pháp để xác định ma trận J, một trong số đó là xác định thông
qua ma trận JH

i
n

Bước 1: Xác định ma trận T (i=0 -> n-1)


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

 H p
p
x
x
 
 q
q
H 1 H 2
p

y
p
y

q
 q
1
2
H
 H p
p

z
 q z q
H
 X
H
2
 H 1
J 

H
 
6n

Q
x
x
 
q
q

1
2
H
H


y

y


 q1
 H 
z

 q
 1

q

2
H

z
q
2

H
...

...



p
x


q 
n

H
p 
y 
q
n
H
p




z


...

q
n
H

x

...
q
n
H

...

...





y


q
n

H

z
q
n

 Khi (i+1) là khớp trượt, biến khớp ri+1
i
Sử dụng ma trận Tn
H

H
H
p
p
p
x
i
i
x
z  ia
 n;
 n;
r
z
z 
y r
i 1
i 1
i 1

H




r

H
x



r

H
y




z0

r
i 1












i
1

H

i 1

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

p

x


.i p  in .i p


in
y

i

1

H
p
z


x
y

x

.i p  io .i p ;


io


y

i

x

x

y

i

y

1

.i p  i a .i p ;


ia

1
H


;
y

p

x

x

;

y

H

H


i

i
y  o
z  a
z


;
;
x
z
z

i 1
i
i 1
1
Bước 3: Tính
J
 in

R0
J


.H J
0
R 
n
0

n

0


Với Robot Puma, có 6 DOFs và cả 6 khớp đều là khớp quay, ta có:
nx o x ax
ny oy ay
nz oz az
J= 0 0 0
0 0
[ 0 0

HJ
=

0
0
0
nx
ny
nz

0
0

0
0
0
ox

oy
oz

0
0
0
ax
ay
az]

nnx
y

ooyx

aayx

0

0

nz
0

oz az
0 0

0
nx
n

0
ox
o

JH11

JH12

JH13

JH21
JH31
0
ax . JH41
a

JH22
JH32
JH42

JH23
JH33
JH43

0
0

JH14
JH24

JH15
JH25

JH16 JH26

JH34
JH44

JH35
JH45

JH36
JH46


0 0
[ 0 0

0
0

nz

nx o x a x
n
0T =[ y oy ay
6 nz oz az
0 0 0

px
py
]
pz
1

y

y

oz

y

az]

JH51
[JH61

JH52
JH62

JH53
JH63

JH54
JH64

JH55
JH56

JH56
JH66 ]

0
Các hệ số của T được xác định ở phần động lực thuận vị trí
6
Tính tốn ma trận JH bằng việc tính các ma trận theo từng cột tương ứng
với từng khớp của robot.


H J  1n .1p 1n .1p
11
y x
x y
H
1 1
J  o . p
21
y x
1 1
H
 o . p
x y
1 1
1 1
J  a . p  a . p
31
y x
x y

H
1
J  n
41
z
H

1
J  o
51
z
H
1
J  a
61
z
HJ

 2n . 2 p  2n .2 p
12
y
x
x
y
2
2 . p  2o 2 p
o H

J 22
x.

y
y
x
p
32
y2 x 2 x
y
2 . p  a 2
H
HJ 2
.
J  n
42
z
H
2
J  o
52
z
H
2
J  a
62
z
a

HJ

o
y

 3n .3 p  3n .3 p
13
y
x
x
y
3
H
3 . p  3o 3 p
J 23 y
x.
ax
p
y 3 x 3 x
y
3 . p  a 3
H
.
HJ 
J

43
33

3

n



J

H
z
3

o

J

53
H

z

 3a
63
z


HJ

 4n . 4 p  4n . 4 p
14
y
x
x
y
4
4 . p  4o 4 p

o H
J 24 y
x.
y
ax
p
y4 x 4 x4 y
4
H
HJ 4 . p  a
J  n
44
z
H
4
J  o
54
z
H J  4a
34

64

z

HJ

 5n .5 p  5n .5 p
15
y

x
x
y
5
5 . p  5o 5 p
o H
J 25 y
x.
y
ax
p
35
y5 x 5 x
y
5 . p  a 5
H

HJ
5
.
J  n
45
z
H
5
J  o
55
z
H J  5a
65

z

HJ

o
y

 6n .6 p  6n . 6 p
16
y
x
x
y
6
H
6 . p  6o 6 p
J 26  y
x.
ax
p
36
y 6 x 6 x
y
6 . p  a 6
H

.
HJ
J


46
6

n


J

H
z
6

o

J

56
H

z

 6a
66
z


Sau khi tính được HJ ta tính được ma trận Jacobi của robot bằng công thức
sau:
R0

0 H
n
. J
J  
0
 0 Rn 
Ta thu được kết quả sau :
 J11 J12 J13 J14

 J21 J22 J23 J24

J
J
J
J
33
34
J   31 32
J
J
J
J
42
43
44
 41
J
J
J
J

52
53
54
 51
J
J
J
J
62
63
64
 61

J
J
J
J
J
J

15

25
35
45
55
65

J 
16


J 
26

J 
36

J 
46

J 
56

J 
66



PHẦN 4. ĐỘNG HỌC NGƯỢC VỊ TRÍ
4.1 Tổng quan phương pháp tính động học ngược robot PUMA
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.
Ta sẽ biểu diễn hướng và vị trí của cổ tay, cổ tay sẽ được biểu diễn đối với
hệ tọa độ cố định bên ngoài (world coordinate system – Hệ tọa độ O0). Ta phải
giải bài toán trên đối với các ẩn θ1, … , θ6
Phương pháp giải :Vị trí của tâm cổ tay RC xác định qua vị trí cơng cụ
(The given tool position – so với hệ tọa độ O0) và phương của Tool pointing
(Z6). Do đó vị trí của tâm cổ tay phụ thuộc vào 3 biến khớp đầu tiên.

Các biến khớp θ1 , θ2 , θ3 xác định từ ma trận định hướng cổ tay (The arm
orientation) T30 và ma trận định hướng công cụ (The given tool orientation) T60.
Vị trí C so với O0 sẽ bằng vị trí của C 0so với0 hệ3 tọa độ O3 nhân với ma trận biến
đổi đồng nhất của O3 trong tọa độ O0 (P = T . P ). Tương tự vị trí C so với O0
c

3

c

sẽ bằng vị trí C so 0với hệ0 tọa
độ O6 nhân với ma trận biến đổi đồng nhất của O6
trong tọa độ O0 ( P = T . P6 )
c

6

c

Các biến khớp θ4 , θ5 , θ6 xác định từ ma trận T 3 (θ61 , θ2 , θ3 ) và ma trận:
T 3 = T 0−1 (θθ , θ , θ ). T 0
6

3

1

2

3

6

Ta phân thành 2 cơng việc chính như sau:
• Thiết lập ma trận vị trí 𝑃𝑐= cột cuối cùng của T 0 (θθ1 , θ2 , θ3 ) và T 0 để tìm ra các
3

biến khớp θ1 , θ2 , θ3 . Tính

6

T3 = T0−1 T0
6

3

6

sau khi đã thay giá trị của các biến khớp vào θ1 , θ2 , θ3,vào T 0 (θθ13, θ2 , θ3 )
.• So sánh T 3 và T 3 (θθ4 , θ5 , θ6 ) để rút ra θ4 , θ5 , θ6 .
6

6

Ta giả sử C là điểm cổ tay máy