BÀI TẬP LỚN KỸ THUẬT ROBOT: Tìm hiểu về robot Puma 500

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

------

BÁO CÁO BÀI TẬP LỚN
Đề tài: Tìm hiểu về robot Puma 500
Giảng viên hướng dẫn: PGS. TS Nguyễn Phạm Thục Anh
Nhóm sinh viên thực hiện: 6
Lớp:129135

Năm học 2021 – 2022


Mục lục
Nội dung báo cáo

1. Giới thiệu về Robot Puma 500,
các ứng dụng trong cơng nghiệp, kết
cấu cơ khí, các thơng số kĩ thuật cơ
bản

Sinh viên thực hiện

Trang

1-3

Nguyễn Hồng long 20181611

2. Tính tốn động học thuận vị trí Hồng Minh Đức 20181402

Robot. Xây dựng chương trình phần
mềm trên matlab để nhập liệu và hiện Nguyễn Hương Ly 20181637
thị kết quả
Phạm Yến linh 20181581

3-9

3. Tính tốn ma trận Jacoby (thơng qua Hồng Minh Đức 20181402
jH) và viết chương trình trên matlab

9-15

Nguyễn Hương Ly 20181637
Phạm Yến linh 20181581

4. Tính tốn động học đảo vị trí Robot Tống Xuân Sơn 20186313

15-22

Vi Hồng Nam 20181671
Nguyễn Văn Sang 20181725

5. Thiết kế quỹ đạo chuyển động cho Tống Xuân Sơn 20186313
các khớp của robot theo quỹ đạo bậc 3

23

Vi Hồng Nam 20181671

6. Xây dựng mơ hình động lực học

cho đối tượng trên Toolbox
Simscape/Matlab

Nguyễn Văn Sang 20181725
Đoàn Diễm Quỳnh 20181390
Đỗ Quang Cường 20160534

24-34


1 Giới thiệu về Robot Puma 500, các ứng dụng trong cơng
nghiệp, kết cấu cơ khí, các thơng số kĩ thuật cơ bản
1.1 Giới thiệu về robot Puma 500
PUMA( P rogramable U niversal M achine for A ssembly hoặc P
rogramable U niversal for A rm)
Đượ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ệ 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.2 Thông số kĩ thuật
AXES

6 revolute axes

Configuaration

6 degrees of freedom

Dive

Elictric DC servomotors
1


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 wrist
92.2 cm to the flange

Weight

54.4 kg

Các thơng số cơ khí tính bằng inch
2


Joint

Axis of Maximum angle

rotation of rotation

Joint 1Waist

Z

320°

Joint 2- X
Shoulder

250°

Joint 3Elbow

X

270°

Joint4Y
Wrist
Rotation

300°

1.3 Ứng dụng 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
y2mate.com - Food

y2mate.com application with Puma 500 robot_480p.mp4Drawing with the PUMA Robot Arm_480p.mp4

Video ứng dụng robot puma 500

2. Tính tốn động học thuận vị trí Robot. Xây dựng chương trình
phần mềm trên matlab để nhập liệu và hiện thị kết quả
2.1 Phương pháp Danevit – Hartenberg (D-H)

3


Bước 1: Xác định số khớp và thanh nối của Robot
- n khớp (i=1->n): khớp quay + tịnh tiến
- (n+1) thanh nối: 0->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)
➢
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
➢

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

Hình1: Zi-1 và Zi chéo nhau

4


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

Hình 2 Zi-1 // Zi

Trường hợp 3: Zi-1 X Zi

Hình 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 i
+ di: độ lệch với thanh nối (i-1)
+ qi: góc quay của thanh nối i
5


Bước 4: Xác định ma trận Ai
T ii −1 = Ai = Rot(Z,θi)Trans(Z,di)Trans(Xi,ai)Rot(X,ai)
cos i − sin  i 0 0 1 0 0 0  1 0 0 ai 
 sin 
cos i 0 0 0 1 0 0  0 1 0 0 
i

=

 0


 0

0
1
0 cos
i

0 sin  i

0
0

0
0

1 0  0 0 1 d i  0 0 1


0 1  0 0 0 1  0 0 0

0
0
0

1
− sin  i cos i
cos i cos i
sin  i
0

0

1

0
− sin  i
cos i
0

cos i
 sin 
i
= 
 0

 0

sin  i sin  i
− cos i sin  i
cos i
0

ai cos i 
ai sin  i 
di 

1 

Bước 5: Tìm T 0n
T 0n = A 10 .A 12 …A nn −1

6

n x0
 0
ny
=  0
n z

 0

s x0
s 0y
s z0
0

a x0
a 0y
a z0
0

p x0 

p 0y 
p z0 

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 d i (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 Phân tích động học của Robot PUMA
2.2.1 Động học thuận vị trí

Hình 2.2 Gắn hệ trục tọa độ cơ bản và các hệ tọa độ trung gian cho Robot
Áp dụng quy tắc Denavit – Hartenberg ta thu được bảng D-H, biểu diễn dưới
bảng:
Bảng 2.1 Bảng thơng số D-H
Khâu tác động

𝜶𝒊

𝒂𝒊

𝒅𝒊

𝜽𝒊

1

-90

0

0

t1

2

0

a2

-d2

t2

3

90

a3

0

t3

4

-90

0

d4

t4

5

90

0

0

t5

6

0

0

d6

t6

Các ma trận Ai được tính tốn dựa theo cơng thức:
7


Ai = Rot ( Z ,i )Trans ( Z , d i )Trans ( X i , ai ) Rot ( X ,  i )
ci
 s
Ai =  i

0

0
Ta thu được kết quả:
c
 s
A =
0

0

1

1

1

c
 s
A =
0

0
4

4

4

−c s

c c
s
0
1

1

1

1

1

−c s
c c
s
0
4

3

4

4

4

s s
− s c
c

0
1

1

1

1

1

s s
− s c
c
0
4

4

a c
a s
d
1








a c 
a s 

d 

1 
1

1

1

1

1

4

4

−c i s i
c i c i
s i
0

4

4

4

4

4

4

c
 s
A =
0

0

s i s i
− s i c i
c i
0
2

2

2

c
 s
A =
0

0

5

−c s
c c
s
0
2

2

ai c i 
ai s i 
di 

1 
2

2

5

−c s
c c
s
0
5

5

5

5

Từ 6 ma trận cơ sở ta tính tốn ma trận vị trí 𝑇06

2

2

2

2

5

s s
− s c
c
0

2

2

5

s s
− s c
c
0

𝑛𝑥
𝑛
𝑦
𝑇60 = 𝐴1 𝐴2 𝐴3 𝐴4 𝐴5 𝐴6 = [
𝑛𝑧
0

5

5

𝑜𝑥
𝑜𝑦
𝑜𝑧
0

2

2

2

2

2

5

5

a c
a s
d
1
a c
a s
d
1
5

5

5

5

𝑎𝑥
𝑎𝑦
𝑎𝑧
0

5

5















c
 s
A =
0

0

3

3

3

c
 s
A =
0

0
6

𝑝𝑥
𝑝𝑦
]
𝑝𝑧
1

6

6

−c s
c c
s
0
3

3

3

3

6

6

6

6

3

3

3

3

−c s
c c
s
0

s s
−s c
c
0
3

6

s s
−s c
c
0
6

6

6

3

3

3

3

3

6

Trong đó:
nx = − 𝑠𝑖𝑛(𝑡6) ∗ (𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡1) + 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4))
− 𝑐𝑜𝑠(𝑡6) ∗ (𝑐𝑜𝑠(𝑡5) ∗ (𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) − 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1)
∗ 𝑐𝑜𝑠(𝑡4)) + 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡5))
ny = 𝑠𝑖𝑛(𝑡6) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡4) − 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4)) + 𝑐𝑜𝑠(𝑡6)
∗ (𝑐𝑜𝑠(𝑡5) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) + 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡1))
− 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡5))
ny = 𝑠𝑖𝑛(𝑡6) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡4) − 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4)) + 𝑐𝑜𝑠(𝑡6)
∗ (𝑐𝑜𝑠(𝑡5) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) + 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡1))
− 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡5))
nz = 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡4) ∗ 𝑠𝑖𝑛(𝑡6) − 𝑐𝑜𝑠(𝑡6) ∗ (𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡5)
+ 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑐𝑜𝑠(𝑡5))
ox = 𝑠𝑖𝑛(𝑡6) ∗ (𝑐𝑜𝑠(𝑡5) ∗ (𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) − 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡4))
+ 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡5)) − 𝑐𝑜𝑠(𝑡6) ∗ (𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡1)
+ 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4))
oy = 𝑐𝑜𝑠(𝑡6) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡4) − 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4)) − 𝑠𝑖𝑛(𝑡6)
∗ (𝑐𝑜𝑠(𝑡5) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) + 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡1))

− 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡5))
oz = 𝑠𝑖𝑛(𝑡6) ∗ (𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡5) + 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑐𝑜𝑠(𝑡5))
+ 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡6) ∗ 𝑠𝑖𝑛(𝑡4)

8

3

a c
a s
d
1

a c
a s
d
1
6

6

6

6

6

6
















ax = 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡5) − 𝑠𝑖𝑛(𝑡5) ∗ (𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) − 𝑐𝑜𝑠(𝑡2
+ 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡4))
ay = 𝑠𝑖𝑛(𝑡5) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) + 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡1)) + 𝑠𝑖𝑛(𝑡2
+ 𝑡3) ∗ 𝑐𝑜𝑠(𝑡5) ∗ 𝑠𝑖𝑛(𝑡1)
az = 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡5) − 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡5)
px = 𝑑2 ∗ 𝑠𝑖𝑛(𝑡1) − 𝑑6 ∗ (𝑠𝑖𝑛(𝑡5) ∗ (𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) − 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1)
∗ 𝑐𝑜𝑠(𝑡4)) − 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡5)) + 𝑑4 ∗ 𝑠𝑖𝑛(𝑡2
+ 𝑡3) ∗ 𝑐𝑜𝑠(𝑡1) + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎3 ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑐𝑜𝑠(𝑡2)
∗ 𝑐𝑜𝑠(𝑡3) − 𝑎3 ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3)
py = 𝑑6 ∗ (𝑠𝑖𝑛(𝑡5) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) + 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡1))
+ 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡5) ∗ 𝑠𝑖𝑛(𝑡1)) − 𝑑2 ∗ 𝑐𝑜𝑠(𝑡1) + 𝑑4 ∗ 𝑠𝑖𝑛(𝑡2
+ 𝑡3) ∗ 𝑠𝑖𝑛(𝑡1) + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡1) + 𝑎3 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑐𝑜𝑠(𝑡3)
∗ 𝑠𝑖𝑛(𝑡1) − 𝑎3 ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3)
pz = 𝑑4 ∗ 𝑐𝑜𝑠(𝑡2 + 𝑡3) − 𝑎3 ∗ 𝑠𝑖𝑛(𝑡2 + 𝑡3) − 𝑎2 ∗ 𝑠𝑖𝑛(𝑡2) + 𝑑6 ∗ (𝑐𝑜𝑠(𝑡2
+ 𝑡3) ∗ 𝑐𝑜𝑠(𝑡5) − 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡5))

3.Tính tốn ma trận jacoby thơng qua ma trận Jh và viết

chương trình trên matlab
3.1 Ma trận Jacoby
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 Tin (i=0->n-1)

9


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

➢

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

Sử dụng ma trận Tni

➢

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

Bước 3: Tính J

10


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

.
0 0 0 nx ox ax
0 0 0 ny oy ay
[ 0 0 0 nz oz az ]
JH11
nx ox ax 0 0 0
JH21
ny oy ay 0 0 0
JH31
n oz az 0 0 0
H
J= z
.
JH41
0 0 0 nx ox ax
0 0 0 ny oy ay
JH51
[ 0 0 0 nz oz az ] [JH61
n x ox a x p x
n y oy a y p y
0
]
T6 = [
n z oz a z p z
0 0 0 1
-

JH12
JH22
JH32

JH42
JH52
JH62

JH13
JH23
JH33
JH43
JH53
JH63

JH14
JH24
JH34
JH44
JH54
JH64

JH15
JH25
JH35
JH45
JH55
JH65

JH16
JH26
JH36
JH46
JH56

JH66 ]

Các hệ số của 0T6 được xác định ở phần động lực thuận vị trí .
Tiếp theo ta sẽ 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.

J11 = 1ny. 1px – 1nx. 1py
H
J21 = 1oy. 1px – 1ox. 1py
H
J31 = 1ay. 1px – 1ax. 1py
H
J41= 1nz
H
J51= 1oz
H
J61= 1az
H

J12 = 2ny. 2px – 2nx. 2py
H
J22 = 2oy. 2px – 2ox. 2py
H
J32 = 2ay. 2px – 2ax. 2py
H
J42= 2nz
H
J52= 2oz
H
J62= 2az

H

J13 = 3ny. 3px – 3nx. 3py
H
J23 = 3oy. 3px – 3ox. 3py
H
J33 = 3ay. 3px – 3ax. 3py
H
J43= 3nz
H
J53= 3oz
H
J63= 3az
H

11


J14 = 4ny. 4px – 4nx. 4py
H
J24 = 4oy. 4px – 4ox. 4py
H
J34 = 4ay. 4px – 4ax. 4py
H
J44= 4nz
H
J54= 4oz
H
J64= 4az
H

J15 = 5ny. 5px – 5nx. 5py
J25 = 5oy. 5px – 5ox. 5py
H
J35 = 5ay. 5px – 5ax. 5py
H
J45= 5nz
H
J55= 5oz
H
J65= 5az
H
H

J16 = 6ny. 6px – 6nx. 6py
J26 = 6oy. 6px – 6ox. 6py
H
J36 = 6ay. 6px – 6ax. 6py
H
J46= 6nz
H
J56= 6oz
H
J66= 6az
Sau khi tính được HJ ta tính được ma trận Jacobi của Robot bằng công thức:
H
H

J=[

𝑅0𝑛
0

0 H
]x J
𝑅0𝑛

Ta được kết quả như sau:
J11
J21
J
J = 31
J41
J51
[J61

J12
J22
J32
J42
J52
J62

J13
J23
J33
J43
J53
J63

J14
J24
J34
J44
J54
J64

J15
J25
J35
J45
J55
J65

J16
J26
J36
J46
J56
J66 ]

3.2 Viết chương trình Matlab tính ma trận Jacoby thơng qua ma trận JH
3.2.1 Chương trình matlab tính tốn động học thuận và ma trận JH
function[J_so]=kinematic(value_t1, value _t2, value _t3, value _t4, value _t5, value _6,
value _a2, value _a3, value _d2, value _d4, value _d6)
syms theta d alph a
A = [cos(theta) -cos(alph)*sin(theta) sin(alph)*sin(theta) a*cos(theta)
sin(theta) cos(alph)*cos(theta) -sin(alph)*cos(theta) a*sin(theta)
12

0
sin(alph)
cos(alph)
d
0
0
0
1
];
% tinh A1
syms t1 a1;
A1= subs(A,{a,alph,d,theta},{0,-pi/2,0,t1});
% tinh A2
syms t2 a2 d2;
A2= subs(A,{a,alph,d,theta},{a2,0,-d2,t2});
% tinh A3
syms t3 a3;
A3= subs(A,{a,alph,d,theta},{a3,pi/2,0,t3});
% tinh A4
syms t4 d4;
A4= subs(A,{a,alph,d,theta},{0,-pi/2,d4,t4});
% tinh A5
syms t5 d5;
A5= subs(A,{a,alph,d,theta},{0,pi/2,0,t5});
% tinh A6
syms t6 d6;
A6= subs(A,{a,alph,d,theta},{0,0,d6,t6});
T56=A6;
T46=simplify(A5*A6);

T36=simplify(A4*A5*A6);
T26=simplify(A3*A4*A5*A6);
T16=simplify(A2*A3*A4*A5*A6);
T06=simplify(A1*A2*A3*A4*A5*A6)
%Tính JH
J11=simplify(T06(2,1)*T06(1,4)-T06(1,1)*T06(2,4))
J21=simplify(T06(2,2)*T06(1,4)-T06(1,2)*T06(2,4));
J31=simplify(T06(2,3)*T06(1,4)-T06(1,3)*T06(2,4));
J41=simplify(T06(3,1));
J51=simplify(T06(3,2));
J61=simplify(T06(3,3));
J12=simplify(T16(2,1)*T16(1,4)-T16(1,1)*T16(2,4));
J22=simplify(T16(2,2)*T16(1,4)-T16(1,2)*T16(2,4));
J32=simplify(T16(2,3)*T16(1,4)-T16(1,3)*T16(2,4));
J42=simplify(T16(3,1));
J52=simplify(T16(3,2));
J62=simplify(T16(3,3));
J13=simplify(T26(2,1)*T26(1,4)-T26(1,1)*T26(2,4));
J23=simplify(T26(2,2)*T26(1,4)-T26(1,2)*T26(2,4));
J33=simplify(T26(2,3)*T26(1,4)-T26(1,3)*T26(2,4));
J43=simplify(T26(3,1));
J53=simplify(T26(3,2));
13


J63=simplify(T26(3,3));
J14=simplify(T36(2,1)*T36(1,4)-T36(1,1)*T36(2,4));
J24=simplify(T36(2,2)*T36(1,4)-T36(1,2)*T36(2,4));
J34=simplify(T36(2,3)*T36(1,4)-T36(1,3)*T36(2,4));
J44=simplify(T36(3,1));

J54=simplify(T36(3,2));
J64=simplify(T36(3,3));
J15=simplify(T46(2,1)*T46(1,4)-T46(1,1)*T46(2,4));
J25=simplify(T46(2,2)*T46(1,4)-T46(1,2)*T46(2,4));
J35=simplify(T46(2,3)*T46(1,4)-T46(1,3)*T46(2,4));
J45=simplify(T46(3,1));
J55=simplify(T46(3,2));
J65=simplify(T46(3,3));
J16=simplify(T56(2,1)*T56(1,4)-T56(1,1)*T56(2,4));
J26=simplify(T56(2,2)*T56(1,4)-T56(1,2)*T56(2,4));
J32=simplify(T56(2,3)*T56(1,4)-T56(1,3)*T56(2,4));
J46=simplify(T56(3,1));
J56=simplify(T56(3,2));
J66=simplify(T56(3,3));
JH=[J11 J12 J13 J14 J15 J16
J21 J22 J23 J24 J25 J26
J31 J32 J33 J34 J35 J36
J41 J42 J43 J44 J45 J46
J51 J52 J53 J54 J55 J56
J61 J62 J63 J64 J65 J66];
% tinh ma tran jacobi J
K=[T06(1,1) T06(1,2) T06(1,3) 0
0
0
T06(2,1) T06(2,2) T06(2,3) 0
0
0
T06(3,1) T06(3,2) T06(3,3) 0
0
0

0
0
0
T06(1,1) T06(1,2) T06(1,3)
0
0
0
T06(2,1) T06(2,2) T06(2,3)
0
0
0
T06(3,1) T06(3,2) T06(3,3)];
J=simplify(K*JH);
J_so=subs(J,{t1,t2,t3,t4,t5,t6,a2,a3,d2,d4,d6},{value_t1,value_t2,value_t3,
value_t4,value_t5,value_t6,value_a2,value_a3,value_d2,value_d4,value_d6})
end

14


3.2 Giao diện chương trình động học thuận và ma trận jacoby

Hình 3.2 Giao diện chương trình tính tốn động học thuận và ma trận Jacoby

4. ĐỘNG HỌC NGƯỢC VỊ TRÍ ROBOT PUMA
Ta có thể tách bài tố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 độ 𝑂0 ). 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 độ 𝑂0 ) và phương của Tool pointing ( Z 6 ). 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) 𝑇30 và ma trận định hướng công cụ (The given tool orientation) 𝑇60 . Vị
trí C so với 𝑂0 sẽ bằng vị trí của C so với hệ tọa độ 𝑂3 nhân với ma trận biến đổi
đồng nhất của 𝑂3 trong tọa độ 𝑂0 (𝑃𝑐0 = 𝑇30 𝑃𝑐3 ). Tương tự vị trí C so với 𝑂0 sẽ bằng
vị trí C so với hệ tọa độ 𝑂6 nhân với ma trận biến đổi đồng nhất của 𝑂6 trong tọa độ
𝑂0 (𝑃𝑐0 = 𝑇60 𝑃𝑐6 )
Các biến khớp 𝜃4 , 𝜃5 , 𝜃6 xác định từ ma trận 𝑇63 (𝜃1 , 𝜃2 , 𝜃3 ) và ma trận
−1
𝑇63 = 𝑇30 (𝜃1 , 𝜃2 , 𝜃3 ) ∗ 𝑇60
15


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 𝑇30 (1 , 2 ,3 ) và 𝑇60 để tìm ra các
−1

biến khớp 1 ,2 ,3 . Tính 𝑇63 = 𝑇30 𝑇60 sau khi đã thay giá trị của các biến khớp vào
 , , vào 𝑇 0 (1 , 2 ,3 ) .
1

2

3

3

• So sánh 𝑇63 và 𝑇63 (𝜃4 , 𝜃5 , 𝜃6 ) để rút ra  4 ,  5 ,  6 .
Ta giả sử C là điếm cổ tay máy.

Ta chia bài toán thành 2 phần:
• Tìm vị trí giao điểm trục cổ tay.
• Tìm hướng cổ tay.
𝑇60 = ⏟
𝐴1 𝐴2 𝐴3 ⏟
𝐴4 𝐴5 𝐴6
𝑇30

1. Bài tốn tìm vị trí giao điểm trục cổ tay
Tọa độ C trong hệ tọa độ O6 là:

𝑇63

0
0
𝑃𝑐6 = [
]
−𝑑6
1
16


Tọa độ C trong hệ tọa độ O0 là:
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥
−𝑎𝑥 𝑑6 + 𝑝𝑥

0
𝑎
𝑝
𝑦
𝑦
𝑛
𝑜
−𝑎
𝑑 + 𝑝𝑦
0
𝑦
𝑃𝑐0 = 𝑇60 . 𝑃𝑐6 = [ 𝑦
].[
]=[ 𝑦 6
]
𝑎
𝑝
𝑛𝑧 𝑜𝑧
−𝑑6
𝑧
𝑧
−𝑎𝑧 𝑑6 + 𝑝𝑧
1
0 0
0 1
1

(4.1)

Ma trận biến đổi đồng nhất của O3 trong O0 là:

𝑐1 𝑐23
𝑇30 = 𝐴1 𝐴2 𝐴3 = [𝑠1 𝑐23
−𝑠23
0
Trong đó :
𝑐1 = cos (𝜃1 )
𝑐2 = cos (𝜃2 )
𝑐3 = cos(𝜃3 )
𝑐4 = cos (𝜃4 )
𝑐5 = cos (𝜃5 )
{𝑐6 = cos (𝜃6 )

−𝑠1
𝑐1
0
0

𝑐1 𝑠23 𝑑2 𝑠1 + 𝑎2 𝑐1 𝑐2 + 𝑎3 𝑐1 𝑐2 𝑐3 − 𝑎3 𝑐1 𝑠2 𝑠3
𝑠1 𝑠23 −𝑑2 𝑐1 + 𝑎2 𝑠1 𝑐2 − 𝑎3 𝑠1 𝑠2 𝑠3 + 𝑎3 𝑠1 𝑐2 𝑐3
]
𝑐23
−𝑎3 𝑠23 − 𝑎2 𝑠2
0
1

𝑠1 = sin (𝜃1 )
𝑠2 = sin (𝜃2 )
𝑠3 = sin(𝜃3 )
𝑠4 = sin (𝜃4 )
𝑠5 = sin (𝜃5 )

{𝑠6 = sin (𝜃6 )

𝑠12 = sin (𝜃1 + 𝜃2 )
𝑠23 = sin (𝜃2 + 𝜃3 )
𝑠34 = sin(𝜃3 + 𝜃4 )
𝑐12 = cos (𝜃1 + 𝜃2 )
𝑐23 = cos (𝜃2 + 𝜃3 )
{ 𝑐34 = cos(𝜃3 + 𝜃4 )

Tọa độ C trong hệ tọa độ O0 là:

0
0
𝑃𝑐6 = [ ]
𝑑4
1
Mặt khác ta có tọa độ của C trong tọa độ O0 là:
𝑐1 𝑐23 −𝑠1 𝑐1 𝑠23 𝑑2 𝑠1 + 𝑎2 𝑐1 𝑐2 + 𝑎3 𝑐1 𝑐2 𝑐3 − 𝑎3 𝑐1 𝑠2 𝑠3
0
0
𝑠1 𝑠23 −𝑑2 𝑐1 + 𝑎2 𝑠1 𝑐2 − 𝑎3 𝑠1 𝑠2 𝑠3 + 𝑎3 𝑠1 𝑐2 𝑐3
𝑃𝑐0 = 𝑇30 𝑃𝑐3 = [𝑠1 𝑐23 𝑐1
].[ ]
−𝑠23
𝑐23
−𝑎3 𝑠23 − 𝑎2 𝑠2
𝑑4
0
0
0

1
1
0
𝑑4 𝑐1 𝑠23 + 𝑑2 𝑠1 + 𝑎2 𝑐1 𝑐2 + 𝑎3 𝑐1 𝑐2 𝑐3 − 𝑎3 𝑐1 𝑠2 𝑠3
𝑑 𝑠 𝑠 − 𝑑2 𝑐1 + 𝑎2 𝑠1 𝑐2 − 𝑎3 𝑠1 𝑠2 𝑠3 + 𝑎3 𝑠1 𝑐2 𝑐3
 𝑃𝑐0 = [ 4 1 23
]
(4.2)
𝑑4 𝑐23 − 𝑎3 𝑠23 − 𝑎2 𝑠2
1
Đồng nhất 2 vế của (4.1) và (4.2) ta được hệ phương trình sau:
−𝑎𝑥 𝑑6 + 𝑝𝑥 = 𝑑4 𝑐1 𝑠23 + 𝑑2 𝑠1 + 𝑎2 𝑐1 𝑐2 + 𝑎3 𝑐1 𝑐2 𝑐3 − 𝑎3 𝑐1 𝑠2 𝑠3
(1)
−𝑎
𝑑
+
𝑝
=
𝑑
𝑠
𝑠
−
𝑑
𝑐
+
𝑎
𝑠
𝑐
−
𝑎

𝑠
𝑠
𝑠
+
𝑎
𝑠
𝑐
𝑐
(2)
{ 𝑦 6
𝑦
4 1 23
2 1
2 1 2
3 1 2 3
3 1 2 3
−𝑎𝑧 𝑑6 + 𝑝𝑧 = 𝑑4 𝑐23 − 𝑎3 𝑠23 − 𝑎2 𝑠2
(3)
Ta lấy (1)𝑠1 − (2)𝑐1 ta thu được:
𝑠1 (−𝑎𝑥 𝑑6 + 𝑝𝑥 ) − 𝑐1 (−𝑎𝑦 𝑑6 + 𝑝𝑦 ) = 𝑠1 𝑐1 (𝑑4 𝑠23 +𝑎2 𝑐2 + 𝑎3 𝑐2 𝑐3 − 𝑎3 𝑠2 𝑠3 ) +
𝑑2 𝑠12 − 𝑐1 𝑠1 (𝑑4 𝑠23 + 𝑎2 𝑐2 + 𝑎3 𝑐2 𝑐3 − 𝑎3 𝑠2 𝑠3 ) + 𝑑2 𝑐12
 𝑑2 = 𝑠1 (−𝑎𝑥 𝑑6 + 𝑝𝑥 ) − 𝑐1 (−𝑎𝑦 𝑑6 + 𝑝𝑦 )

17


 𝜃1 = 𝐴𝑇𝐴𝑁2(−𝑎𝑥 𝑑6 + 𝑝𝑥 , 𝑎𝑦 𝑑6 − 𝑝𝑦 ) +
2

𝐴𝑇𝐴𝑁2 (±√(−𝑎𝑥 𝑑6 + 𝑝𝑥 )2 + (𝑎𝑦 𝑑6 − 𝑝𝑦 ) − 𝑑22 , 𝑑2 )

Ta lấy (1)𝑐1 + (2)𝑠1 và kết hợp phương trình 3 ta có hệ:
𝑐 (−𝑎𝑥 𝑑6 + 𝑝𝑥 ) + 𝑠1 (−𝑎𝑦 𝑑6 + 𝑝𝑦 ) = 𝑑4 𝑠23 +𝑎2 𝑐2 + 𝑎3 𝑐2 𝑐3 − 𝑎3 𝑠2 𝑠3 = 𝑀
{ 1
−𝑎𝑧 𝑑6 + 𝑝𝑧 = 𝑑4 𝑐23 − 𝑎3 𝑠23 − 𝑎2 𝑠2
=𝑁
𝑐1 (−𝑎𝑥 𝑑6 + 𝑝𝑥 ) + 𝑠1 (−𝑎𝑦 𝑑6 + 𝑝𝑦 ) = 𝑑4 𝑠23 +𝑎2 𝑐2 + 𝑎3 𝑐23 = 𝑀
−𝑎𝑧 𝑑6 + 𝑝𝑧 = 𝑑4 𝑐23 − 𝑎3 𝑠23 − 𝑎2 𝑠2
=𝑁
Ta có :
𝑀2 + 𝑁 2 = (𝑑4 𝑠23 +𝑎2 𝑐2 + 𝑎3 𝑐23 )2 + (𝑑4 𝑐23 − 𝑎3 𝑠23 − 𝑎2 𝑠2 )2
{

 𝑀2 + 𝑁 2 = 𝑑42 + 𝑎22 + 𝑎32 + 2𝑑4 𝑎2 𝑠3 + 2𝑎3 𝑎2 𝑐3
 𝑀2 + 𝑁 2 −𝑑42 − 𝑎22 − 𝑎32 = 2𝑑4 𝑎2 𝑠3 + 2𝑎3 𝑎2 𝑐3
Đặt 𝑋 = 𝑀2 + 𝑁 2 −𝑑42 − 𝑎22 − 𝑎32
𝜃3 = 𝐴𝑇𝐴𝑁2(2𝑑4 𝑎2 , 2𝑎2 𝑎3 ) + 𝐴𝑇𝐴𝑁2(±√(2𝑎3 𝑎2 )2 + (2𝑑4 𝑎2 )2 − 𝑋 2 , 𝑋)
Ta khai triển M và N ta được hệ
𝑀 = 𝑑4 𝑠2 𝑐3 + 𝑑4 𝑠3 𝑐2 + 𝑎2 𝑐2 + 𝑎3 𝑐3 𝑐2 − 𝑎3 𝑠2 𝑠3
{
𝑁 = 𝑑4 𝑐2 𝑐3 − 𝑑4 𝑠2 𝑠3 − 𝑎2 𝑠2 − 𝑎3 𝑠2 𝑐3 − 𝑎3 𝑠3 𝑐2
𝑘 = 𝑑4 𝑐3 − 𝑎3 𝑠3
𝑀 = 𝑠2 (𝑑4 𝑐3 − 𝑎3 𝑠3 ) + 𝑐2 (𝑑4 𝑠3 + 𝑎2 + 𝑎3 𝑐3 )
{
đặt: { 1
𝑘2 = 𝑑4 𝑠3 + 𝑎2 + 𝑎3 𝑐3
𝑁 = 𝑐2 (𝑑4 𝑐3 − 𝑎3 𝑠3 ) − 𝑠2 (𝑑4 𝑠3 + 𝑎2 + 𝑎3 𝑐3 )
𝑀 = 𝑐2 𝑘2 +𝑠2 𝑘1
{
𝑁 = 𝑐2 𝑘1 − 𝑠2 𝑘2

 𝑛𝑔ℎ𝑖ệ𝑚 𝑑𝑢𝑦 𝑛ℎấ𝑡: 𝜃2 = 𝐴𝑇𝐴𝑁2(𝑘1 𝑀 − 𝑘2 𝑁, 𝑘1 𝑁 + 𝑘2 𝑀)

2. Bài tốn tìm hướng của cổ tay.
Sau khi giải xong bài tốn tìm vị trí giao điểm của các trục cổ tay ta đi đến bài
tốn tìm hướng sử dụng ma trận 𝑇63 để tính tốn.
Ta có:
𝑐4 𝑐5 𝑐6 − 𝑠4 𝑠6 −𝑐4 𝑐5 𝑠6 − 𝑠4 𝑐6 𝑐4 𝑠5 𝑑6 𝑐4 𝑠5
𝑠4 𝑠5 𝑑6 𝑠4 𝑠5
𝑇63 = 𝐴4 𝐴5 𝐴6 = [𝑠4 𝑐5 𝑐6 + 𝑐4 𝑠6 −𝑠4 𝑐5 𝑠6 + 𝑐4 𝑐6
] (4.3)
−𝑠5 𝑐6
𝑠5 𝑠6
𝑐5 𝑑4 + 𝑑6 𝑐5
0
0
0
1
−1
0
0 3
Mặt khác
𝐴1 𝐴2 𝐴3 𝐴4 𝐴5 𝐴6 = 𝑇6 ↔ 𝑇3 𝑇6 = 𝑇60 ↔ 𝑇63 = 𝑇30 𝑇60
Mà

18


−1

𝑇30 𝑇60 =

𝑛𝑥 𝑐1 𝑐23 − 𝑛𝑧 𝑠23 + 𝑛𝑦 𝑠2 𝑐23 𝑜𝑥 𝑐1 𝑐23 − 𝑜𝑧 𝑠23 + 𝑜𝑦 𝑠1 𝑐23
𝑛𝑦 𝑐1 − 𝑛𝑧 𝑠1
𝑜𝑦 𝑐1 − 𝑜𝑥 𝑠1
𝑛𝑧 𝑐23 + 𝑛𝑥 𝑐1 𝑠23 + 𝑛𝑦 𝑠1 𝑠23 𝑜𝑧 𝑐23 + 𝑜𝑥 𝑠23 𝑐1 + 𝑜𝑦 𝑠1 𝑠23
[
0
0
Đồng nhất 2 vế của 2 ma trận (4.3) và (4.4) ta thu được
𝑐5 = 𝑎𝑧 𝑐23 + 𝑎𝑥 𝑠23 𝑐1 + 𝑎𝑦 𝑠1 𝑠23
Đặt: 𝑘3 = 𝑎𝑧 𝑐23 + 𝑎𝑥 𝑠23 𝑐1 + 𝑎𝑦 𝑠1𝑠23

𝑎𝑥 𝑐1 𝑐23 − 𝑎𝑧 𝑠23 + 𝑎𝑦 𝑠1 𝑐23 𝑓𝑥
𝑎𝑦 𝑐1 − 𝑎𝑥 𝑠1
𝑓𝑦
(4.4)
𝑎𝑧 𝑐23 + 𝑎𝑥 𝑠23 𝑐1 + 𝑎𝑦 𝑠1 𝑠23 𝑓𝑧
0
1]

Ta có: 𝜃5 = 𝐴𝑇𝐴𝑁2(±√1 − 𝑘32 , 𝑘3 )
Nếu 𝑠𝑖𝑛𝜃5 ≠ 0 có:
𝑠4 𝑠5 = 𝑎𝑦 𝑐1 − 𝑎𝑥 𝑠1
−𝑠5 𝑐6 = 𝑛𝑧 𝑐23 + 𝑛𝑥 𝑐1 𝑠23 + 𝑛𝑦 𝑠1 𝑠23
{𝑐 𝑠 = 𝑎 𝑐 𝑐 − 𝑎 𝑠 + 𝑎 𝑠 𝑐
{ 𝑠 𝑠 =𝑜 𝑐 +𝑜 𝑠 𝑐 +𝑜 𝑠 𝑠
4 5
𝑥 1 23
𝑧 23
𝑦 1 23
5 6

𝑧 23
𝑥 23 1
𝑦 1 23
𝑎𝑦 𝑐1 − 𝑎𝑥 𝑠1
𝑠4 =
𝑠5
↔
𝑎𝑥 𝑐1 𝑐23 − 𝑎𝑧 𝑠23 + 𝑎𝑦 𝑠1 𝑐23
𝑐4 =
{
𝑠5
𝑜𝑧 𝑐23 + 𝑜𝑥 𝑠23 𝑐1 + 𝑜𝑦 𝑠1 𝑠23
𝑠6 =
𝑠5
↔
𝑛𝑧 𝑐23 + 𝑛𝑥 𝑐1 𝑠23 + 𝑛𝑦 𝑠1 𝑠23
𝑐6 = −
{
𝑠5
→ 𝜃4 = 𝐴𝑇𝐴𝑁2(𝑠4 , 𝑐4 )
→ 𝜃6 = 𝐴𝑇𝐴𝑁2(𝑠6 , 𝑐6 )
Nếu 𝑠𝑖𝑛𝜃5 = 0 có:
Với cos 𝜃5 = 1 ta có 𝜃5 = 0
𝑐46 −𝑠46 0
0
𝑠
𝑐
0
0
46

𝑇63 = [ 46
]
1 𝑑4 + 𝑑6
0
0
0
0
0
1
 Ta tính được:
𝜃4 + 𝜃6 = 𝐴𝑇𝐴𝑁2(𝑛𝑦 𝑐1 − 𝑛𝑥 𝑠1 , 𝑛𝑥 𝑐1 𝑐23 − 𝑛𝑧 𝑠23 + 𝑛𝑦 𝑠1 𝑐23 )
Với cos 𝜃5 = −1 ta có 𝜃5 = 𝜋

−𝑠𝑡6 −𝑡4
𝑠
𝑇63 = [ 𝑡6−𝑡4
0
0

𝑠𝑡6 −𝑡4
𝑠𝑡6 −𝑡4
0
0

0
0
0
0
]
−1 𝑑4 −𝑑6

0
1

 Ta tính được:
𝜃6 − 𝜃4 = 𝐴𝑇𝐴𝑁2(𝑜𝑋 𝑐1 𝑐23 − 𝑜𝑧 𝑠23 + 𝑜𝑦 𝑠1 𝑐23 , 𝑜𝑦 𝑐1 − 𝑜𝑥 𝑠1 )

Giải xong 2 bài toán trong động học ngược ta tìm ra các kết quả của
𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 , 𝜃5 , 𝜃6 như sau:

19


2

𝜃1 = 𝐴𝑇𝐴𝑁2(−𝑎𝑥 𝑑6 + 𝑝𝑥 , 𝑎𝑦 𝑑6 − 𝑝𝑦 ) + 𝐴𝑇𝐴𝑁2(±√(−𝑎𝑥 𝑑6 + 𝑝𝑥 )2 + (𝑎𝑦 𝑑6 − 𝑝𝑦 ) − 𝑑22 , 𝑑2 )
𝜃2 = 𝐴𝑇𝐴𝑁2(𝑘1 𝑀 − 𝑘2 𝑁, 𝑘1 𝑁 + 𝑘2 𝑀)
𝜃3 = 𝐴𝑇𝐴𝑁2(2𝑑4 𝑎2 , 2𝑎2 𝑎3 ) + 𝐴𝑇𝐴𝑁2(±√(2𝑎3 𝑎2 )2 + (2𝑑4 𝑎2 )2 − 𝑋 2 , 𝑋)
𝜃4 = 𝐴𝑇𝐴𝑁2(𝑠4 , 𝑐4 )
𝜃5 = 𝐴𝑇𝐴𝑁2 (±√1 − 𝑘32 , 𝑘3 )
{ 𝜃6 = 𝐴𝑇𝐴𝑁2(𝑠6 , 𝑐6 )

Từ những kết quả trên ta thay thông số thực tế của robot PUMA vào ta được bảng
kết quả như sau:
Góc
𝜃1
𝜃2
𝜃3
𝜃4
𝜃5
𝜃6

TH1
156.65
88.06
174.21
-2.74
85.47
-179.87

TH2
156.65
88.06
174.21
177.35
-85.47
0.22

TH3
156.65
89.52
0.59
-2.73
88.48
-180.02

TH4
156.65
89.52
0.59
177.36

-88.48
0.08

TH5
0.52
87.76
173.56
-0.0090
85.48
-180.09

TH6
0.52
87.76
173.56
180.08
-85.48
0.0007

TH7
TH8
0.52
0.52
89.2
89.2
1.24
1.24
-0.0090 180.08
88.47
-88.47

-180.09 0.00025

II. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT
1. Giới thiệu và cơ sở thiết kế quỹ đạo
Thiết kế quỹ đạo chuyển động của robot có liên quan mật thiết đến bài tốn điều
khiển robot di chuyển từ vị trí này sang 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 là:
− 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 các
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:
𝑥 (𝑡 ) = 𝑎 + 𝑏𝑡 + 𝑐𝑡 2
− Đa thức bậc 3:
𝑥 (𝑡 ) = 𝑎 + 𝑏𝑡 + 𝑐𝑡 2 + 𝑑𝑡 3
− Đa thức bậc cao: 𝑥 (𝑡 ) = 𝑎 + 𝑏𝑡 + 𝑐𝑡 2 + ⋯ + 𝑘𝑡 𝑛
Ở đây ta sẽ sử dụng quỹ đạo dạng đa thức bậc 3 để thiết kế cho robot PUMA.
20


2.Thiết kế quỹ đạo trong không gian khớp
Chọn hai điểm A, B bất kỳ trong không gian làm việc, biết tọa độ (Px,P y, Pz)
và hướng của các khâu thao tác. Thiết kế quỹ đạo chuyển động bất kỳ từ A đến B.
Theo bài toán động học ta xác định được các biến khớp 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 , 𝜃5, 𝜃6 tại A
và B.

Chọn quỹ đạo thiết kế là hàm đa thức bậc 3 theo thời gian như sau:
𝜃𝑖 (𝑡) = 𝑎𝑖 + 𝑏𝑖 𝑡 + 𝑐𝑖 𝑡 2 + 𝑑𝑖 𝑡 3

Với: i =1÷6 tương ứng với 6 biến khớp
Ta được hệ phương trình sau:
𝜃1 (𝑡) = 𝑎1 + 𝑏1 𝑡 + 𝑐1 𝑡 2 + 𝑑1 𝑡 3
𝜃2 (𝑡) = 𝑎2 + 𝑏2 𝑡 + 𝑐2 𝑡 2 + 𝑑2 𝑡 3
𝜃3 (𝑡) = 𝑎3 + 𝑏3 𝑡 + 𝑐3 𝑡 2 + 𝑑3 𝑡 3
𝜃4 (𝑡) = 𝑎4 + 𝑏4 𝑡 + 𝑐4 𝑡 2 + 𝑑4 𝑡 3
𝜃5 (𝑡 ) = 𝑎5 + 𝑏5 𝑡 + 𝑐5 𝑡 2 + 𝑑5 𝑡 3
{𝜃6 (𝑡 ) = 𝑎6 + 𝑏6 𝑡 + 𝑐6 𝑡 2 + 𝑑6 𝑡 3
Giả sử thời gian robot đi từ điểm A đến điểm B là trong t(s) và vân tốc tại 2 điểm
đó là bằng 0.
Từ đó ta có hệ phương trình:
𝜃𝑖 (𝐴) = 𝑎𝑖
𝜃𝑖 (𝐵) = 𝑎𝑖 + 𝑏𝑖 𝑡 + 𝑐𝑖 𝑡 2 + 𝑑𝑖 𝑡 3
.

𝜃𝑖 (𝐴) = 𝑏𝑖 = 0
.

2

{𝜃𝑖 (𝐵) = 𝑏𝑖 + 2𝑐𝑖 𝑡 + 3𝑑𝑖 𝑡 = 0
𝑎𝑖 = 𝜃𝑖 (𝐴)
𝑏𝑖 = 0
3[𝜃𝑖 (𝐵) − 𝜃𝑖 (𝐴)]
𝑐𝑖 =
𝑡2
2[𝜃𝑖 (𝐵) − 𝜃𝑖 (𝐴)]

𝑑
=
{ 𝑖
𝑡3
Giả sử robot PUMA cần dịch chuyển từ vị trí A) đến điểm B là 6s tương ứng
với tọa độ khớp là A(0,0,0,0,0) đến B(90,30,0,0,90). (góc ở đây là 𝜃𝑖 – góc quay
của khớp, đơn vị ‘độ’).
Ta suy ra hệ số phương trình của quỹ đạo cho khâu 1 là:

21


𝑎1 = 𝜃1 (𝐴) = 0
𝑏1 = 0
3[𝜃1 (𝐵) − 𝜃1 (𝐴)] 3[90 − 0]
𝑐1 =
=
= 7.5
𝑡2
62
2[𝜃1 (𝐵) − 𝜃1 (𝐴)] −2[90 − 0]
𝑑
=
=
= −0.833
{ 1
𝑡3
63
Khi đó ta có phương trình quỹ đạo vị trí, vận tốc, gia tốc theo thời gian :
𝜃1 (𝑡) = 7,5. 𝑡 2 − 0,833. 𝑡 3

𝜃̇1 (𝑡) = 7,5.2. 𝑡 − 0,833.3. 𝑡 2
𝜃̈1 (𝑡) = 7,5.2 − 0,833.3.2. 𝑡
Đồ thị quỹ đạo chuyển động bậc 3 của khớp 1:

22


5.Điều khiển chuyển động cho robot theo thuật toán PID
5.1 Phương trình động lực học.

M = K P E − K DQ + K I  Edt

Hình 1. Sơ đồ cấu trúc bộ điều khiển PID

5.2 Thiết kế bộ điều khiển PID
Với các tham số Kp=450, Ki=600, Kd=20.
Dựa vào đặc điểm ảnh hưởng của các tham số tới đáp ứng đầu ra của
mơ hình ta có thể chỉnh định các tham số sao cho đàu ra bám giá trị đặt và
đạt một số các tiêu chí về độ quá độ và thời gian đáp ứng cũng như sai lệch
tĩnh.

23