BÀI TẬP LỚN KT ROBOT: Tìm hiểu robot PlateCrane EX Microplate Handler

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

----------

BÀI TẬP LỚN
Tính tốn Robot hình trụ PlateCrane EX Microplate Handler
GV.Hướng Dẫn: PGS.TS. Nguyễn Phạm Thục Anh
Mã Học Phần: EE4344 Mã Lớp: 129135
STT
1
2
3
4
5
6
7
8
9
10

Họ & Tên
Lê Duy Huy
Lê Đức Thọ
Nguyễn Duy Phương
Vũ Minh Hiếu
Trần Việt Hưng
Nguyễn Đức Hiền
Vũ Mạnh Trường
Phạm Hữu Thanh
Nguyễn Văn Trụ

Trịnh Văn Hùng

Hà Nội, 12/2021

MSSV
20173957
20174244
20174122
20173854
20173938
20173845
20174304
20181759
20181792
20173932


MỤC LỤC
DANH MỤC BẢNG BIỂU
DANH MỤC HÌNH VẼ
ĐỀ TÀI................................................................................................................................. 4
1. Giới thiệu Robot...............................................................................................................5
Robot PlateCrane EX ................................................................................................ 5
Ứng dụng trong công nghiệp..................................................................................... 5
Thơng số kỹ thuật ...................................................................................................... 6
Hình ảnh vận hành thực tế......................................................................................... 7
2. Động học thuận vị trí .......................................................................................................9
Xác định các hệ trục .................................................................................................. 9
Xác định bảng D-H ................................................................................................. 10
Xác định các ma trận ............................................................................................... 10

Chương trình tính tốn sử dụng Matlab .................................................................. 11
3. Ma Trận Jacoby..............................................................................................................13
Ma trận Jacoby ........................................................................................................ 13
Chương trình tính tốn sử dụng Matlab .................................................................. 15
4. Động học đảo vị trí ........................................................................................................17
Lý thuyết ................................................................................................................. 17
Động học đảo của Robot PlateCrane EX ................................................................ 17
5. Thiết kế quỹ đạo chuyển động .......................................................................................19
Cơ sở lý thuyết ........................................................................................................ 19
Thiết kế quỹ đạo cho Robot PlateCrane EX ........................................................... 19
6. Xây dựng mô hình động lực học....................................................................................25
Thiết kế robot PlateCrane EX trên phần mềm Solid Work..................................... 25
Kết nối Solid Work và Matlab/Tool box simscape ................................................. 28
Thiết kế bộ điều khiển ............................................................................................. 31
Kết nối bộ điều khiển và mơ hình động lực học ..................................................... 33
7. Kết Luận.........................................................................................................................38
8. Phụ lục ...........................................................................................................................39
Động học thuận-Ma trận Jacoby ............................................................................. 39
Động học đảo .......................................................................................................... 45
Thiết kế quỹ đạo (Chương 5) .................................................................................. 45


ĐHBKHN

Kỹ thuật Robot- 129135

DANH MỤC BẢNG BIỂU
Bảng 1-1 Thông số kỹ thuật của Robot ...............................................................................7
Bảng 2-1 Bảng D-H của robot PlateCrane EX ..................................................................10
Bảng 6-1 Thơng số của bộ PID ..........................................................................................32

Nhóm 7

2


ĐHBKHN

Kỹ thuật Robot- 129135

DANH MỤC HÌNH VẼ
Hình 1.1 Robot PlateCrane EX ngồi thực tế ......................................................................5
Hình 1.2 Khơng gian làm việc của robot PlateCrane EX ....................................................7
Hình 1.3 Hình ảnh của robot PlateCrane EX .......................................................................8
Hình 2.1 Cách xác định trục Xi trong 3 trường hợp ............................................................9
Hình 2.2 Gán các hệ trục tọa độ cho robot PlateCrane EX ...............................................10
Hình 2.3 Giao diện tính tốn ban đầu ................................................................................12
Hình 2.4 Kết quả tính tốn ma trận T04 ............................................................................12
Hình 3.1 Giao diện tính tốn ban đầu ................................................................................15
Hình 3.2 Kết quả tính tốn ma trận Jacoby........................................................................16
Hình 5.1 Đồ thị quỹ đạo, vận tốc và gia tốc của khớp 2 ....................................................21
Hình 5.2 Đồ thị quỹ đạo, vận tốc và gia tốc của khớp 2 ....................................................22
Hình 5.3 Đồ thị quỹ đạo, vận tốc và gia tốc của khớp 3 ....................................................23
Hình 5.4 Đồ thị quỹ đạo, vận tốc và gia tốc của khớp 4 ....................................................24
Hình 6.1 Phần đế Robot .....................................................................................................25
Hình 6.2 Thanh nối 1 .........................................................................................................25
Hình 6.3 Thanh nối 2 .........................................................................................................26
Hình 6.4 Thanh nối 3 .........................................................................................................26
Hình 6.5 Khâu tác động cuối .............................................................................................27
Hình 6.6 Robot PlateCrane EX sau khi ráp hồn chỉnh ....................................................27

Hình 6.7 Mơ hình động lực học của Robot trong Simulink ..............................................29
Hình 6.8 Mơ hình robot trên Matlab ..................................................................................31
Hình 6.9 Mơ hình điều khiển PID theo khơng gian khớp ..................................................32
Hình 6.10 Bộ PID cho khớp 1 ...........................................................................................32
Hình 6.11 Bộ PID cho khớp 2 ...........................................................................................32
Hình 6.12 Bộ PID cho khớp 3 ...........................................................................................33
Hình 6.13 Bộ PID cho khớp 4 ...........................................................................................33
Hình 6.14 Sơ đồ sau khi kết nối hồn chỉnh ......................................................................34
Hình 6.15 Đáp ứng vị trí của khớp 1 .................................................................................35
Hình 6.15 Đáp ứng vị trí của khớp 2 .................................................................................35
Hình 6.15 Đáp ứng vị trí của khớp 3 .................................................................................36
Hình 6.15 Đáp ứng vị trí của khớp 4 .................................................................................36

Nhóm 7

3


ĐHBKHN

Kỹ thuật Robot- 129135

ĐỀ TÀI
Tính tốn thiết kế Robot hình trụ PlateCrane EX Microplate Handler.
Yêu cầu:
1) Giới thiệu về Robot nhóm nghiên cứu, 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. u cầu có hình ảnh hoặc clip hoạt động.
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 dữ liệu, hiển thị kết quả.
3) Tính tốn ma trận Jacoby (thơng qua JH) và viết chương trình trên MatLab.

4) Tính tốn động học đảo vị trí Robot.
5) Thiết kế quỹ đạo chuyển động cho các khớp của Robot theo quỹ đạo dạng bậc 3.
6) Xây dựng mơ hình động lực học cho đối tượng trên ToolBox Simscape/MatLab.

Nhóm 7

4


ĐHBKHN

Kỹ thuật Robot- 129135

1. Giới thiệu Robot
Robot PlateCrane EX
Robot trong báo cáo này là robot PlateCrane EX của hãng Hudsonrobotics. Robot
PlateCrane EX là robot linh hoạt, thích hợp cho việc tự động hóa vận chuyển các tấm vật
liệu.
Nhờ trang bị bộ phần mềm Hudson’s SoftLinx, PlateCrane EX có thể được định cấu
hình để phục vụ một hoặc nhiều thiết bị tương thích với nhiều loại tấm khác nhau.
PlateCrane EX là một robot tác động tốc độ cao với 4 trục. Có thể được cấu hình
với tối đa 15 tấm xếp chồng lên nhau hoặc sử dụng 10 ngăn xếp thay thế. Được thiết kế để
dễ sử dụng trong khi duy trì độ tin cậy và tính linh hoạt được u cầu bởi các chuyên gia
tự động hóa.
Tất cả các hệ thống PlateCrane đều đi kèm với một cơ sở sáng tạo cho phép nhiều
thiết bị gắn trực tiếp vào cánh tay robot. Hệ thống khóa lại với nhau tạo ra sự ổn định khi
làm việc trên hầu hết mọi bề mặt.

Hình 1.1 Robot PlateCrane EX ngồi thực tế

Ứng dụng trong công nghiệp
PlateCrane EX là giải pháp lý tưởng để tự động hóa việc xử lý tấm trong phịng thí
nghiệm:
▪ Xếp chồng và vận chuyển các vi mẫu tiêu chuẩn.
▪ Bao gồm phiên bản đầy đủ của phần mềm lập trình SoftLinx V.

Nhóm 7

5


ĐHBKHN

Kỹ thuật Robot- 129135

▪ Tấm Crane có sẵn với một bộ kẹp tiêu chuẩn hoặc một bộ kẹp bên cho các ứng dụng
truy cập ngẫu nhiên. Cả hai bộ kẹp đều quay để làm việc với các vị trí lồng ngang
hoặc dọc.
▪ Tất cả các hệ thống PlateCrane đều hoạt động với các tấm có nắp đậy.
▪ TấmCrane tiêu chuẩn với 2 ngăn xếp, mỗi ngăn xếp chứa 25 tấm có nắp, 30 tấm
khơng có nắp.
▪ Số lượng tấm có thể được mở rộng lên 15 ngăn xếp hoặc tối đa ba băng chuyền 10
ngăn xếp.
▪ Có sẵn các ngăn xếp được kiểm sốt nhiệt độ tùy chọn, có thể mở rộng cũng như dễ
vận hành và giá cả phải chăng.

Thông số kỹ thuật
▪ Số bậc tự do: 4
▪ Gồm 2 khớp quay, 2 khớp tịnh tiến
▪ Vùng không gian làm việc:

Nhóm 7

6


ĐHBKHN

Kỹ thuật Robot- 129135

Hình 1.2 Khơng gian làm việc của robot PlateCrane EX

Ta có bảng thơng số kĩ thuật của robot:
Bảng 1-1 Thông số kỹ thuật của Robot

Chuyển động quay

345°

Chuyển động chiều ngang cánh tay
(khớp 3)

12 - 18 inches từ trung tâm

Chuyển động chiều dọc (khớp 2)

Từ 4 - 22 inches

Chiều cao và cân nặng

29 inches; 20.5kg

Nhiệt độ hoạt động

15°C to 40°C (59° to 104°F)

Truyền thơng

RS232

Nguồn

115/220 VAC, 50/60 Hz

Hình ảnh vận hành thực tế
Một vài video vận hành thực tế của Robot PlateCrane EX.
/> /> />
Nhóm 7

7


ĐHBKHN

Kỹ thuật Robot- 129135

Hình 1.3 Hình ảnh của robot PlateCrane EX

Nhóm 7

8


ĐHBKHN

Kỹ thuật Robot- 129135

2. Động học thuận vị trí
Xác định các hệ trục
Theo tài liệu của hãng, robot PlateCrane EX có 4 bậc tự do. Trước khi xác định bảng
Denavit – Hartenberg, ta cần xác định các hệ trục tọa độ cho các khớp:
▪ Xác định trục Zi: vì tất cả các khớp của robot đều là khớp quay nên Zi là trục mà
khớp (i+1) quay xung quanh nó.
▪ Xác định trục Xi:
+ Nếu Zi-1 và Zi chéo nhau thì Xi là đường vng góc chung của Zi-1 và Zi.
+ Nếu Zi-1 và Zi song song thì Xi nằm trên đường vng góc Zi và đi qua 2 tâm Oi
và Oi-1.
+ Nếu Zi-1 cắt Zi, chọn Oi là giao điểm giữa Zi-1 và Zi, Xi là đường vng góc mặt
phẳng chứa Zi-1 và Zi tại điểm Oi.
Zi-1

Zi

Zi

Zi-1

Xi

Xi-1

Xi

Oi-1

Oi

Oi

Xi

Zi-1

Zi

Hình 2.1 Cách xác định trục Xi trong 3 trường hợp

Nhóm 7

9


ĐHBKHN

Kỹ thuật Robot- 129135

d3

O3

Z2

O2

mm)

mm)

Z3
X3

X2

O4

X4

Z4

Z0, Z1

d2

d4=139(mm)

O0 O1
X0

X1
Hình 2.2 Gán các hệ trục tọa độ cho robot PlateCrane EX

Xác định bảng D-H
Các thông số của bảng Denavit – Hartenberg được xác định theo các quy tắc như sau:
▪

ai : khoảng cách giữa 2 trục Z i −1 và Z i theo trục X i .

▪  i : góc quay từ thanh nối Z i −1 đến Z i quanh trục X i theo chiều dương.
▪  i : góc quay từ thanh nối X i −1 đến trục X i quanh trục Z i −1 theo chiều dương.
▪

d i : khoảng cách giữa 2 trục X i −1 và X i theo trục Z i −1 .

➢ Ta được bảng D-H như sau:
Bảng 2-1 Bảng D-H của robot PlateCrane EX

KTĐ

ai

i

i

1

0

0

1 +90°

2

0

-90°

0

3

0

-90°

0

4

0

0

4

di

Khớp

0

Quay

d2
d3

Tịnh tiến

d4

Quay

Tịnh tiến

.

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

Nhóm 7

10


ĐHBKHN

Kỹ thuật Robot- 129135

Trước khi tiến hành xác định các ma trận của robot từ bảng D-H, để thuận tiện cho
việc trình bày, ta quy ước các kí hiệu sau:

ci = cos(i )
( i = 1;2;3;4 )

s
=
sin
(

)
i
i
Các ma trận A được xác định bởi công thức ma trận biến đổi thuần nhất tổng quát
như sau:
sin  i .sin i
ai .cos i 
cosi − cos  i .sin i


sin i
cos  i .cos i
− sin  i .cos i
ai .sin i 
i-1

Ai =
(*)
0

sin  i

cos  i
di


0
0
1
0

Từ ma trận tổng quát (*), ta thu được các ma trận A như sau:
 −s1
c
0
A1 =  1
0

0
1
0
2
A3 = 
0

0

−c1

0

−s1

0

0

1

0

0

0

0

0

1

−1

0

0

0

0
0 
0

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

;

;

1
0
1
A2 = 
0

0
c4
s
3
A4 =  4
0

0

0

0

0

1

−1

0

0

0

−s 4

0

c4

0

0

1

0

0

0 

0 
d2 

1 
0 
0 
d4 

1 

Ma trận T40 có dạng (d4 = 139):

ox
ax
p x  c1s 4 − c 4s1
c1c 4 + s1s 4
n x
n


oy
ay
p y  c1c 4 + s1s 4
c 4s1 − c1s 4
y
T40 = A10 .A12 .A 23 .A 43 = 
=
n z
0
oz

az
p z  0

 
0
0
0
1  0
0
(Chương trình tính tốn bằng Matlab được ghi ở phần Phụ lục 8.1).

0
0
−1
0

−d 3c1 
−d 3s1 
d2 − d4 

1


Chương trình tính tốn sử dụng Matlab
Phần giao diện nhóm thiết kế bao gồm các phần cơ bản như sau:
o Nhập các thơng số góc Theta 1(độ), chuyển động dọc d2(mm), chuyển động
ngang d3 (mm) và góc Theta 4(độ).
Các giá trị này được nhập vào Text Box, và được xử lý nếu nhập giá trị lớn
hơn giá trị cho phép thì giá trị sẽ bằng giá trị lớn nhất, tương tự đối với giá trị
nhỏ nhất.

o Nút “Tính”: để tính tốn ra ma trận T04 kết quả được hiển thị ở bảng “Ma
trận T04” và ma trận Jacoby được hiển thị ở bảng “Ma trận Jacoby”.
Nhóm 7

11


ĐHBKHN

Kỹ thuật Robot- 129135

o Nút “Xóa”: để đóng chương trình.

Hình 2.3 Giao diện tính tốn ban đầu

Hình 2.4 Kết quả tính tốn ma trận T04

(Chương trình giao diện được ghi ở phần Phụ lục 8.1).

Nhóm 7

12


ĐHBKHN

Kỹ thuật Robot- 129135

3. Ma Trận Jacoby
Ma trận Jacoby

Với Robot có nhiều khớp thành phần thì góc quay của bàn tay máy khơng tường
minh. Từ đó ta đưa ra giải pháp sử dụng thuật toán ma trận JH, thuật toán gồm 3 bước:
▪ Bước 1: Xác định các ma trận: T4i (i = 0 ÷ 3) . Tính theo đúng luật D-H.
c4
s
T43 = A 43 =  4
0

0

-s 4

0

c4

0

0

1

0

0

c4
0
2
2

3
T4 = A 3 .A 4 = 
-s 4

0

0 
0 
d4 

1 

-s 4

0

0

1

-c 4

0

0

0

c4
-s

1
1
2
3
T4 = A 2 .A 3 .A 4 =  4
0

0

0 
d 4 
d3 

1 

-s 4

0

-c 4

0

-1

-1

0

0

c1s 4 − c4s1
c c + s s
0
0
1
2
3
T4 = A1 .A 2 .A 3 .A4 =  1 4 1 4
0

0



d3

d2 - d4 

1

0

c1c4 + s1s 4

0

c4s1 − c1s 4

0

0

−1

0

0

−d 3c1 
−d3s1 
d2 − d4 

1


▪ Bước 2: Xác định ma trận JH
Khớp 1 là khớp quay, sử dụng ma trận T40 ta được cột 1 của ma trận JH
H
J 11
=
 H
J 21 =
 H
J 31 =
 H
J 41 =
 H
J 51 =
J H =

 61

0

n y 0 p x - 0 n x 0 p y = d 3s1 (c1s 4 - s1c 4 ) - c1d 3 (c1c 4 + s1s 4 )

0

o y 0 p x - 0 o x 0 p y = d 3s1 (c1c 4 + s1s 4 ) + c1d 3 (c1s 4 - c 4s1 )

0

ay 0 px - 0 ax 0 py = 0

0

nz = 0

0

oz = 0

0

a z = -1

Khớp 2 là khớp tịnh tiến, sử dụng ma trận T41 ta được cột 2 của ma trận JH

Nhóm 7

13


ĐHBKHN

Kỹ thuật Robot- 129135

H
J 12
 H
J 22
J H
 32
 H
J 42
J H
 52
H
J 62

= 1nz = 0
= 1 oz = 0
= 1 a z = -1
=0
=0
=0

Khớp 3 là khớp tịnh tiến, sử dụng ma trận T42 ta được cột 3 của ma trận JH
H
J 13

= 2 n z = -s 4
 H 2
J 23 = o z = -c 4
J H = 2 a = 0
 33
z
 H
J 43 = 0
J H = 0
 53
H
J 63
=0

Khớp 4 là khớp quay, sử dụng ma trận T43 ta được cột 4 của ma trận JH
H
 J 14
 H
 J 24
 H
 J 34
 H
 J 44
 H
 J 54
J H
 64

= 3 ny 3 px - 3 nx 3 py = 0
= 3 oy 3 px - 3 ox 3 py = 0

= 3 ay 3 px - 3 ax 3 py = 0
= 3 nz = 0
= 3 oz = 0
= 3 az = 1

d 3s1 (c1s 4 - s1c 4 ) - c1d 3 (c1c 4 + s1s 4 )
d s (c c + s s ) + c d (c s - c s )
1 3 1 4
4 1
 31 1 4 1 4
0
→ JH = 
0
0

-1

0

-s 4

0

-c 4

-1

0

0

0

0

0

0

0

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

▪ Bước 3: Tính ma trận Jacoby

Nhóm 7

14


ĐHBKHN

Kỹ thuật Robot- 129135

 R 04
J=
0

0 
x JH
0
R 4 

n x

0
Trong đó: R 4 = n y
n
 z

ox
oy
oz

(**)
a x  c1s 4 − c 4s1

a y  = c1c 4 + s1s 4
a z  0

c1c 4 + s1s 4
c 4s1 − c1s 4
0

0 
0 
−1

Thay vào công thức (**), ta tìm được ma trận Jacoby:

-d 3s1
-d c
 3 1
0
→J=
0
0

1

0

-c1

0

-s1

1

0

0

0

0

0

0

0

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

(Chương trình tính tốn bằng Matlab được ghi ở phần Phụ lục 8.18.1).

Chương trình tính tốn sử dụng Matlab
Chương trình tính ma trận Jacoby được tích hợp với chương trình ma trận tính T04
như đã trình bày ở 2.4.

Hình 3.1 Giao diện tính tốn ban đầu

Nhóm 7

15


ĐHBKHN

Kỹ thuật Robot- 129135

Hình 3.2 Kết quả tính tốn ma trận Jacoby

Nhóm 7

16


ĐHBKHN

Kỹ thuật Robot- 129135

4. Động học đảo vị trí
Lý thuyết
Trong bài toán động học đảo, ngược lại với bài toán động học thuận ở chương 2, ta cần
xác định giá trị các biến khớp của Robot khi đã biết vị trí và hướng của điểm tác động cuối.
Ma trận T40 tính tốn từ bài tốn động học thuận ở chương 2 là ma trận bao gồm vị trí và
hướng của khâu tác động cuối, được xác định từ quỹ đạo chuyển động mong muốn của
robot:

n x
n
y
0

T4 = 
n z

0

ox

ax

oy

ay

oz

az

0

0

px 
p y 
= A1 .A 2 .A 3 .A 4
pz 

1 

(1)

Động học đảo của Robot PlateCrane EX
Vì Robot PlateCrane EX có 4 bậc tự do nên ta dùng phương pháp phân ly biến để
giải bài toán động học đảo:
 −s1
 −c
-1
Ta có: A1 =  1
0

0

c1

0

−s1

0

0

1

0

0

0
0 
0


1

➢ Tính góc θ1
Nhân 2 vế của (1) với A1-1 ta được:
-s1
-c
-1
0
VT1 = A1 .T4 =  1
0

0
c4
-s
VF1 = A 2 .A 3 .A 4 =  4
0

0

c1

0

-s1

0

0

1

0

0
-s 4

0

-c 4

0

0

-1

0

0

0 n x

0   n y
.
0 n z
 
1  0
0

ox

ax

oy

ay

oz

az

0

0

px 
p y 
pz 

1 



d3

d2 - d4 

1


Cân bằng thành phần cột 3 hàng 2 của 2 ma trận ta được phương trình:

-a x c1 - a ys1 = 0  a x cos(θ1 ) + a ysin(θ1 ) = 0

→ θ1 = ATAN2(-a x ,ay )
Hoặc θ1 = ATAN2(a x ,-a y )

Nhóm 7

17


ĐHBKHN

Kỹ thuật Robot- 129135

➢ Tính góc θ4
Cân bằng thành phần cột 1 hàng 1 và cột 1 hàng 2 của 2 ma trận ta được hệ phương
trình:


-n x s1 + n y c1 = c 4  -n x sin(θ1 ) + n y cos(θ1 ) = cos(θ 4 )


-n x c1 - n ys1 = -s 4  n x cos(θ1 ) + n y sin(θ1 ) = sin(θ 4 )

→ θ4 = ATAN2 ( n x cos(θ1 ) + n y sin(θ1 ), -n x sin(θ1 ) + n y cos(θ1 ) )
➢ Tính d2, d3
Cân bằng thành phần cột 4 hàng 2 và cột 4 hàng 3 của 2 ma trận ta được hệ phương

trình:

-p x c1 - p ys1 = d 3
d 3 = -p x c1 - p ys1
d 3 = -p x cos(θ1 ) - p y sin(θ1 )



p z = d 2 - d 4
d 2 = p z + d 4
d 2 = p z + 139
(Chương trình tính tốn A1-1 và VF1 bằng Matlab được ghi ở phần Phụ lục 8.28.1).

Nhóm 7

18


ĐHBKHN

Kỹ thuật Robot- 129135

5. Thiết kế quỹ đạo chuyển động
Cơ sở lý thuyết
Thiết kế quỹ đạo chuyển động của Robot có liên quan đến bài tố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:
o Đa thức bậc 2: q(t) = a 0 + a1t + a 2 t 2
o Đa thức bậc 3: q(t) = a 0 + a1t + a 2 t 2 + a 3t 3
o Đa thức bậc 5: q(t) = a 0 + a1t + a 2 t 2 + a 3t 3 + a 4 t 4 + a 5 t 5
o Đa thức bậc n: q(t) = a 0 + a1t + a 2 t 2 + a 3t 3 + ... + a n t n

Thiết kế quỹ đạo cho Robot PlateCrane EX
Thiết kế quỹ đạo cho không gian khớp ta tiến hành theo các bước:
▪ Từ hai điểm A, B trong không gian làm việc, sử dụng các phương trình động học
ngược vị trí xác định được các biến khớp tương ứng. Bài toán thiết kế quỹ đạo cho
khớp là xác định đường biểu diễn của vị trí khớp (góc quay của khớp quay hoặc độ
di chuyển của khớp tịnh tiến) theo thời gian khi di chuyển từ vị trí ban đầu q 0 đến
vị trí cuối qc trong thời gian tc với q là biến khớp tổng quát.
▪ Từ bài toán động học ngược, xác định các biến khớp 1 , r2 , r3 ,  4 tại A là:

1 ( A), r2 ( A), r3 ( A), 4 ( A) và tại B là 1 ( B), r2 ( B), r3 ( B), 4 ( B) .
▪ Với quỹ đạo đa thức bậc 3 có dạng: q(t) = a 0 + a1t + a 2 t 2 + a 3t 3
▪ Giả sử robot đi từ vị trí ban đầu q0 đến vị trí cuối qc trong t(s) và vận tốc tại 2 điểm
đó bằng 0, ta có hệ phương trình như sau:

q (t0 ) = a0 = q0

q (tc ) = a0 + a1tc + a2tc2 + a3tc3 = qc

•

q (t0 ) = a1 = 0

•
2

q (tc ) = a1 + 2a2tc + 3a3tc = 0
Nhóm 7

19


ĐHBKHN

Kỹ thuật Robot- 129135

▪ Giải hệ phương trình, ta được bộ số tương ứng với mỗi biến khớp

 a0 = q0
a = 0
 1

3( qc − q0 )
 a2 =
tc2


2( qc − q0 )
 a3 = −
tc3

▪ Sau khi tính toán các giá trị a0i , a1i , a2i , a3i (i = 1, 2,3, 4) của mỗi khớp ta được các
phương trình quỹ đạo các khớp tương ứng:

q1 (t ) = a01 + a11t + a21t 2 + a31t 3

2
3
q2 (t ) = a02 + a12t + a22t + a32t

2
3
q3 (t ) = a03 + a13t + a23t + a33t
q (t ) = a + a t + a t 2 + a t 3
04
14
24
34
 4
▪ Từ đó ta thu được quỹ đạo chuyển động của các khớp
Ví dụ: Giả sử thời gian bắt đầu chuyển động đến khi hết chuyển động của từng
khớp là như nhau t1 = t2 = t3 = t4 = 2s, trong thời gian đó, các khớp của Robot

1 , r2 , r3 ,4 

T

di chuyển từ  40, 250,350,120 → 80,150, 400, 20
T

T

Ta có phương trình quỹ đạo, vận tốc và gia tốc của từng khớp là:

1)
Khớp 1

a01 = 40
a = 0
q1 (t ) = 40 + 30t 2 − 10t 3
11



•
3(80 − 40)
→ q1 (t ) = 60t − 30t 2
a21 =
= 30
2
2

••

q1 (t ) = 60 − 60t
2(80 − 40)
=
−
10
a31 = −

23

Nhóm 7

20


ĐHBKHN

Kỹ thuật Robot- 129135

Hình 5.1 Đồ thị quỹ đạo, vận tốc và gia tốc của khớp 2

2)

Khớp 2

a02 = 250
a = 0
q2 (t ) = 250 − 75t 2 + 25t 3
12



•
2
3(150 − 250)
a22 =
= −75 → q 2 (t ) = −150t + 75t
2
2

••


 q 2 (t ) = −150 + 150t
2(150 − 250)
a
=
−
=
25
 32

23

Nhóm 7

21


ĐHBKHN

Kỹ thuật Robot- 129135

Hình 5.2 Đồ thị quỹ đạo, vận tốc và gia tốc của khớp 2

3)

Khớp 3

a03 = 350
a = 0
q3 (t ) = 350 + 37.5t 2 − 12.5t 3

13



•
3(400 − 350)
→ q 3 (t ) = 75t − 37.5t 2
a23 =
=
37.5
22

••

q 3 (t ) = 75 − 75t
2(400 − 350)
=
−
12.5
a33 = −

23

Nhóm 7

22


ĐHBKHN

Kỹ thuật Robot- 129135

Hình 5.3 Đồ thị quỹ đạo, vận tốc và gia tốc của khớp 3

4)

Khớp 4

a04 = 120
a = 0
q4 (t ) = 120 − 75t 2 + 25t 3
14

•


3(20 − 120)
→ q 4 (t ) = −150t + 75t 2
a24 =
=
−
75
22

••

2(20 − 120)
q 4 (t ) = −150 + 150t
=
25

a34 = −

23

Nhóm 7

23


ĐHBKHN

Kỹ thuật Robot- 129135

Hình 5.4 Đồ thị quỹ đạo, vận tốc và gia tốc của khớp 4

(Chương trình xây dựng đồ thị bằng Matlab được ghi ở phần Phụ lục 8.38.1).

Nhóm 7

24