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

BÁO CÁO BÀI TẬP LỚN

ROBOT MPK2F

NHÓM
06
S Thành viên tham MSSV
T gia
T
1 Vũ Đình Cần 2017366
9
2 Nguyễn Duy Tân 20174187
3 Nguyễn Văn Hảo 20173841
4 Đỗ Như Thể 20174230
5 Phạm Quốc Huy 20173961
6 Nguyễn Văn Thắng 20174205
7 Phan Mạnh Quyền 20174149
8 Đại Tuấn Hải 20161278
9 Nguyễn Tiến Điệp 20173739
1
0

Ngành Kỹ thuật điều khiển và tự động hóa

Chuyên ngành Tự động hóa

Giảng viên hướng PGS. TS. Nguyễn Phạm Thục Anh

dẫn:
Bộ môn: Tự động hóa công nghiệp của
Chữ ký
Viện: Viện Điện D
 HÀ NỘI, 06/2021
Yêu cầu đề tài
Tìm hiểu về ROBOT MPK2F:
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. Yêu cầu có hình ảnh hoặc clip hoạt động.
2. Tính toá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 toán ma trận Jacoby (thông qua J^H) và viêt chương trình trên MatLab
4. Tính toá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. Thiết kế điều khiển chuyển động cho Robot theo thuật toán PID
7. Xây dựng mô hình động lực học cho đối tượng trên ToolBox Simscape/MatLab.
 LỜI NÓI ĐẦU
Với tốc độ công nghiệp hoá, hiện đại hoá đất nước như ngày nay thì vấn đề
tự động hoá sản xuất có vai trò đặc biệt quan trọng. Vì vậy trong những năm gần
đây các lĩnh vực thuộc tự động hóa ngày càng được quan tâm nghiên cứu và phát
triển, trong đó không thểkhông nói đến những ứng dụng quan trọng của kỹ thuật
Robot trong công nghiệp. Ứng dụng kỹ thuật Robot trong công nghiệp nhằm
nâng cao năng suất dâychuyền công nghệ, nâng cao chất lượng và khả năng cạnh
tranh của sản phầm, đồng thờicải thiện điều kiện lao đông. Sự cạnh tranh hàng
hóa đặt ra một vấn đề thời sự làm sao để hệ thống tự động hóa sản xuất phải có
tính linh hoạt nhằm đáp ứng với sự biến động thường xuyên của thị trường hàng
hóa. Robot công nghiệp là bộ phận cấu thành không thể thiếu trong hệ thống sản
xuất tự động linh hoạt đó.
Ở nước ta, từ những năm 1990 trở lại đây, Robot và kỹ thuật Robot đã được
ứng dụng vào sản xuất khá rộng rãi. Trong những Robot được ứng dụng vào sản
xuất thì Motoman là một Robot công nghiệp đóng góp đáng kể trog lĩnh vực tự
động hóa công nghiệp ở nước ta. Đây là một robot được ứng dụng trong khá
nhiều lĩnh vực: nhà tự động, các dây chuyền sản xuất công nghiệp tự động, phun
sơn… Nhận thấy tầm quan trọng của kỹ thuật Robot nói chung và ứng dụng của
Robot Motoman nói riêng, với kiến thức học hỏi được trong quá trình học môn
Kỹ thuật Robot do cô giáo TS. Nguyễn Phạm Thục Anh nhóm sinh viên của
chúng em đã làm BTL tìm hiểu về Robot MKP2F-5 với mục đích tìm hiểu, xây
dựng, mô phỏng mô hình điều khiển robot nhằm ứng dụng lý thuyết vào thực tế.
Dù được sự hướng dẫn tận tình của cô giáo nhưng do quá trình tích lũy kiến thức
va kinh nghiệm còn hạn chế nên bài làm của nhóm còn nhiều thiếu sót. Chúng e
mong cô góp ý thêm để bài làm của chúng em được hoàn thiện hơn.
Em xin chân thành cảm ơn!
 MỤC LỤC

CHƯƠNG 1. TỔNG QUAN VỀ ROBOT MPK2F-5........................................1

1.1 Lịch sử hình thành và phát triển của RoBot..............................................1
1.2 Robot MKP2F/ MKP2F-5.........................................................................1
Giới thiệu chung.........................................................................1
Ứng dụng của Robot MKP2F/ MKP2F-5...................................2
Một số thông số kỹ thuật Robot..................................................2
Một số ưu điểm Robot................................................................4
Bộ điều khiển FS100 của Robot.................................................4
CHƯƠNG 2. TÍNH TOÁN ĐỘNG HỌC THUẬN VỊ TRÍ ROBOT..............6
2.1 Bài toán động học thuận Robot MKP2F....................................................6
2.2 Xây dựng chương trình trên Matlab để nhập dữ liệu và hiển thị kết quả...7
CHƯƠNG 3. TÍNH TOÁN MA TRẬN JACOBY............................................9
3.1 Tính toán ma trận Jacoby..........................................................................9
3.2 Xây dựng chương trình tính toán trên Matlab.........................................12
CHƯƠNG 4. BÀI TOÁN TÍNH TOÁN ĐỘNG HỌC ĐẢO VỊ TRÍ ROBOT
.............................................................................................................................. 13
4.1 Tính toán động học đảo vị trí robot.........................................................13
4.2 Code tính toán cho bài toán động học ngược...........................................14
CHƯƠNG 5. THIẾT KẾ QUỸ ĐẠO TRONG KHÔNG GIAN KHỚP.......16
5.1 Tổng quan................................................................................................16
5.2 Tính toán thiết kế quỹ đạo chuyển động..................................................16
5.3 Code thiết kế quỹ đạo chuyển động và đồ thị quỹ đạo của Robot...........17
CHƯƠNG 6. THIẾT KẾ THUẬT TOÁN ĐIỀU KHIỂN CHUYỂN ĐỘNG
CHO ROBOT THEO THUẬT TOÁN PID.....................................................19
6.1 Vẽ hình dạng robot trên SolidWorks.......................................................19
Phần đế.....................................................................................19
Link 1.......................................................................................19
Link 2.......................................................................................20
Link 3.......................................................................................20
Link 4.......................................................................................20
Link 5.......................................................................................21
 Mô hình hoàn chỉnh..................................................................21
6.2 Điều khiển PID........................................................................................21
CHƯƠNG 7. XÂY DỰNG MÔ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI
TƯỢNG TRÊN PHẦN MỀM MATLAB..........................................................23
7.1 Cài đặt và sử dụng...................................................................................23
7.2 Thiết kế và cài đặt bộ điều khiển cho robot.............................................23
7.3 Mô phỏng................................................................................................25
TÀI LIỆU THAM KHẢO................................................................................26
 DANH MỤC HÌNH VẼ
Hình 1.1 Robot MKP2F/MKP2F-5......................................................................1
Hình 1.2 Không gian làm việc của Robot MKP2F/MKP2F-5..............................2
Hình 1.3 Một số mặt cắt của Robot MKP2F/MKP2F-5.......................................3
Hình 1.4 Thông số kỹ thuật của Robot MKP2F/MKP2F-5..................................3
Hình 1.5 Robot MKP2F/MKP2F-5 được treo trên trần nhà.................................4
Hình 1.6 Đặc điểm chính của bộ điều khiển FS100.............................................5
Hình 2.1 Các hệ trục tọa độ của mỗi khớp...........................................................6
Hình 2.2 Code tính toán động học thuận..............................................................8
Hình 2.3 Xây dựng Guide, nhập thông số và hiển thị kết quả..............................8
Hình 3.1 Code tính toán ma trận Jacoby.............................................................12
Hình 3.2 Xây dựng Guide, nhập thông số và hiển thị kết quả............................12
Hình 5.1 Đồ thị quỹ đạo chuyển động của Robot...............................................18
Hình 6.1 Phần đế Robot.....................................................................................19
Hình 6.2 Link 1 của Robot.................................................................................19
Hình 6.3 Link 2 của Robot.................................................................................20
Hình 6.4 Link 3 của Robot.................................................................................20
Hình 6.5 Link 4 của Robot.................................................................................20
Hình 6.6 Link 5 của Robot.................................................................................21
Hình 6.7 Mô hình hoàn chỉnh của Robot............................................................21
Hình 6.8 Mô hình Robot khi có thêm PID cho từng khớp..................................21
Hình 6.9 Đồ thị khớp thứ nhất của Robot...........................................................22
Hình 6.10 Thông số bộ điều khiển PID..............................................................22
Hình 7.1 Mô hình mô phỏng trên Matlab&Simulink.........................................23
Hình 7.2 Cài đặt cho khối Slider Gain................................................................24
Hình 7.3 Khối điều khiển PID cho các khớp......................................................24
Hình 7.4 Đồ thị giữa tín hiệu đầu ra và tín hiệu đặt của khớp 1.........................25
Hình 7.5 Mô hình điều khiển chung cho 5 khớp................................................25
 CHƯƠNG 1. TỔNG QUAN VỀ ROBOT MPK2F-5

1.1 Lịch sử hình thành và phát triển của RoBot

Kể từ khi Robot đầu tiên được chế tạo với sản phẩm đầu tiên có tên gọi người
máy công nghiệp là verstran của công ty Mỹ vào năm 1960 các sản phẩm liên
quan đến tay máy công nghiệp bắt đầu được quan tâm nghiên và nhờ đó kỹ thuật
Robot bắt đầu có những bước phát triển đầu tiên.
Cùng theo bản quyền của Mỹ các nước trên thế giới đă chạy đua sản xuất robot
công nghiệp,các công ty về Robot được thành lập để nghiên cứu và phát triển
,ứng dụng Robot trong tự động hoá công nghiệp.
Yaskawa Motoman là một công ty chuyên cung cấp các giải pháp tự động hóa
sáng tạo cho hầu như tất cả các ngành công nghiệp và ứng dụng robot như: hàn,
lắp ráp, sơn, pha chế, cắt gọt vật liệu,..với hơn 175 mô hình robot khác biệt và
hơn 40 giải pháp công nghệ để giải quyết các công việc một cách hoàn chỉnh như
các hệ thống vận hành , thiết bị an toàn .
1.2 Robot MKP2F/ MKP2F-5
Giới thiệu chung
Robot MKP2F/MKP2F-5là Robot tác động nhanh, linh hoạt, nhỏ gọn và đáng tin
cậy. Đây là một loại Robot hoạt động với nhiều cài đặt. Nó cũng cung cấp rất
nhiều ứng dụng, với hiệu suất sử dụng cao, đảm bảo những yêu cầu về chất
lượng, thời gian hoàn vốn ngắn. MPK2F-5 là một robot tác động tốc độ cao với 5
trục, mang lại hiệu suất cao và độ tin cậy cho việc xử lý thực phẩm, chọn, đóng
gói và các ứng dụng xử lý vật liệu tốc độ cao khác. MKP2F-5 được thiết kế cứng
và thẳng, điều này dẫn đến độ ồn làm việc thấp, thời gian bảo trì lâu. Ngoài ra nó
còn được thiết kế nhỏ gọn, cổ tay mỏng, hiệu suất hoạt động cao ngay cả trong
những vị trí khó.

Hình 1.1 Robot MKP2F/MKP2F-5

Thiết kế cánh tay nhỏ gọn, mỏng của nó cho phép tiếp cận vào các không gian
kín và cánh tay rỗng cho phép các dây cáp và ống được chạy trong nội bộ toàn
bộ robot. MPK2F-5 có dung tích xử lý 5 kg và được điều khiển bằng bộ điều
1
khiển robot nhỏ FS100. MPK2F-5 được thiết kế (cổ tay và thân máy) cho các
ứng dụng mà sự sạch sẽ là quan trọng.

Ứng dụng của Robot MKP2F/ MKP2F-5

Robot MKP2F-5 được ứng dụng cho các dây chuyền sản xuất tự động, hiện nay
các lĩnh vực phổ biến nhất là:
- Các quá trình hàn và nhiệt luyện.
- Công nghệ gia công lắp ráp
- Phun sơn, vận chuyển hàng hóa.
Một số thông số kỹ thuật Robot
- Số bậc tự do: 5
- Kiểu khớp: khớp quay
- Vùng không gian làm việc:

Hình 1.2 Không gian làm việc của Robot MKP2F/MKP2F-5

MPK2F-5 là những robot 5 trục hiệu suất cao được thiết kế cho các ứng dụng
chọn nhanh và đóng gói bao bì. FS100 là một bộ điều khiển mạnh mẽ với cấu
trúc phần mềm mở chưa từng có. Lý tưởng cho việc chọn nhanh, lắp đặt và đóng
gói hàng hóa thực phẩm và hàng tiêu dùng.
Một số mặt cắt của Robot:

Hình 1.3 Một số mặt cắt của Robot MKP2F/MKP2F-5

Hình 1.4 Thông số kỹ thuật của Robot MKP2F/MKP2F-5

 Một số ưu điểm Robot
 Robot tốc độ cao với tốc độ trục tốt nhất
 Thiết kế tiết kiệm năng lượng không
 Robot dùng cho việc làm sạch thực phẩm và các ứng dụng khác
trong đó vấn đề sạch sẽ là điều quan trọng.
 Cổ tay rỗng thông qua cánh tay cáp làm giảm nhiễu và cải thiện
độ tin cậy của hệ thống.
 Nhanh chóng, linh hoạt và nhỏ gọn.
 Lý tưởng cho thu gom, sắp xếp và đóng thùng hàng hóa thực
phẩm tốc độ cao và hàng hóa tiêu thụ.
 Khối lượng tải cho MPK2F là 2 kg, MPK2F-5 là 5 kg. Cả hai
mô hình có độ chính xác ± 0,5 mm.
 Nhỏ gọn, thiết kế cánh tay mỏng cho phép tiếp cận vào không
gian hạn chế, nâng cao năng suất của hệ thống.
 Thiết kế cánh tay rỗng cho phép cáp và ống mềm được chạy bên
trong toàn bộ robot từ đế đến cổ tay nhằm cải thiện độ tin cậy.
 Có thể dùng cho ứng dụng thực phẩm được Tổ chức Vệ sinh
Quốc gia (NSF-H1) chứng nhận an toàn dùng trong thực phẩm.
 MPK2F: có thể để dưới sàn, hoặc treo tường hoặc trần nhà;
 MPK2F-5: có thể đặt dưới sàn hoặc treo lên trần nhà.

Hình 1.5 Robot MKP2F/MKP2F-5 được treo trên trần nhà

Bộ điều khiển FS100 của Robot

Bộ điều khiển FS100 nhỏ, mạnh mẽ và mở. Được thiết kế để đóng gói, xử
lý các bộ phận nhỏ và các ứng dụng lắp ráp, nó có hiệu suất cần thiết cho tốc độ
cao, hoạt động riêng biệt. Nó cũng có kiến trúc phần mềm mở để cho phép nhà
chế tạo máy và nhà tích hợp hệ thống phát triển các giải pháp phần mềm tùy
chỉnh của riêng họ. Được thiết kế cho robot Motoman có tải trọng từ 20 kg trở
xuống.
Các đặc điểm chính:
Hình 1.6 Đặc điểm chính của bộ điều khiển FS100
 CHƯƠNG 2. TÍNH TOÁN ĐỘNG HỌC THUẬN VỊ TRÍ ROBOT

2.1 Bài toán động học thuận Robot MKP2F

Áp dụng phương pháp Denavit- Hartenberg cho mô hình toán học của
Robot, ta đặt các hệ trục tọa độ

Hình 2.1 Các hệ trục tọa độ của mỗi khớp

Từ mô hình Robot ta xác định được bảng D-H

i 𝛼𝑖 𝑎𝑖 𝜃𝑖 ⅆ
𝑖
1 9 0 𝜃1 ⅆ
0 1
°
2 0 𝑎2 𝜃2 0
°
3 0 𝑎3 𝜃3 0
°
4 9 0 𝜃4 + 0
0 90o
°
5 0 0 𝜃5 0
°
Bảng 2.1 Bảng D-H

Với a2= 350mm; a3=550mm; d1= 420mm

Các ma trận A được xác định bằng ma trận biến đổi thuần nhất tổng quát.
cosi
sin  cos  i .sin i sin a .c
cos  .cos  i.sini
sin 
Ai   i i i
 0 sin 

 
Dựa vào bảng D-H ta thay các giá trị và ma trận Ai ta được các ma trận
sau
C1 0 S1 0
C2 0 a2.C2 C3 0 a3.C3 
 
S2 S3
    
S 0 C 0 S 0 a .S S 0 a .S

C C
;A
A1   0
1
1 0
1
d 2
0
2
0
2
1

22 
0 ; A 3  0
3 3
0 1

33
0

 1
   

0 0 0 1 0 0 0 1 0 0 0 1
 
 S4 0 C4 0  C5 0 0

S5
   
C 0 S 0 S 0 0

C
A4   4 4 ; A  5 5 
 0 1 0 0 5
0 0 1 0
0 0 0 1 0 0 0 1
   

Nhân các ma trận A1, A2, A3, A4, A5 ta được ma trận biểu diễn vị trí và
hướng của khâu tác động cuối của Robot như sau:
T5 0  A10.A21.A32.A43.A54
T5 0  A1 .A2 .A3 .A4.A5
𝑛 � � 𝑝
� � 𝑥
𝑇50 = [𝑥 ]
� �
� �
𝑛 �
Sử dụng công cụ matlab để nhập � các
và nhân 𝑝 ma trận. Dùng hàm simplify
để rút gọn ta được ma trận T 50 .
𝑆1. 𝑆5 − 𝑆234. 𝐶1. 𝐶5 𝐶5. 𝑆1 + 𝑆234. 𝐶1. 𝑆5 𝐶234. 𝐶1 𝑎3. 𝐶1. 𝐶23 + 𝑎2. 𝐶1. 𝐶2
𝑇0
5
= [ 1. 𝑆5 − 𝑆234. 𝐶5. 𝑆1𝑆234. 𝑆1.234
−𝐶 𝑆5 − 𝐶1. 𝐶5 𝐶234. 𝑆1 𝑎3. 𝑆1. 𝐶23 + 𝑎2. 𝑆1. 𝐶2
]
𝐶 2345
.𝐶 −𝐶 .5 𝑆 𝑆234 ⅆ + 𝑎13. 𝑆+
232 𝑎2 . 𝑆
0 0
0 1
2.2 Xây dựng chương trình trên Matlab để nhập dữ liệu và hiển thị kết quả
 Hình 2.2 Code tính toán động học thuận

Hình 2.3 Xây dựng Guide, nhập thông số và hiển thị kết quả
 CHƯƠNG 3. TÍNH TOÁN MA TRẬN JACOBY

3.1 Tính toán ma trận Jacoby

Ta xác định các ma trận Tin theo các hệ tọa độ lần lượt từ khâu cuối trở về
gốc:
C5 S5 0 0
S C 0 0
T5   
4 5 5
0 0 1 0
 
0 0 0 1
 C5.S4 S4.S5 C4 0
C .C C .S S 0
T3 4 5 4 5 4 

5
 S5 0 0

C5

 0 0 0 1
 S34. S34 . C34 a3.C3 
C5 S5
 
C .C C .S S a .S
T2 34 5 34 5 34 33

5
 S5 0 0

C5

 0 0 0 1
 S234. S234 . C234 a3.C23  a2.C2 
C5 S5
 
C .C C .S S a  a .S
.S
T 1  234 5 234 5 234 3 23 22 

5
 S5 0 0 

C5

 0 0 0 1 
 S1.S5  C5 .S1  S234 C234 . a3.C1.C23  a2.C1.C2 
S234.C1.C5 .C1.S5 C1
 
C .S  S .C .S C  a .S .C
.S
.S C .C .S
.C a .S
S
T0 1 5 234 5 1 234 1 5 1 5 234 1 3 1 23 2 1 2 

5
 C .C C .S S a .  a .S 
d S
234 5 234 5 234 1 3 23 2 2
 
 0 0 0 1 
nxi oxi axi pxi 
 
n o a p
T i  yi yi yi yi 

n
n o a p 
zi zi zi zi
 
 0 0 0 1 
Tiếp theo ta sẽ tính toá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
- Với khớp 𝜃1 là khớp quay, sử dụng ma trận T50 ta được cột 1 của ma trận
JH.
 J H  n p  n . p  S  a C  a C 
11 y1 x1 x1 y1 5 3 23 2 2
J H  o p  o . p  C  a C aC 
21 y1 x1 x1 y1 5 3 23 2 2
JH a p a .p 0
31 y1 x1 x1 y1

J H41  n z1  S234C5
J H  o  S S
51 z1 234 5

J  a  C
H
61 z1 234

- Với khớp 𝜃2 là khớp quay, sử dụng ma trận T51 ta được cột 2 của ma trận
JH.
J H  n p  n . p  C a S  a S 
12 y2 x2 x2 y2 5 2 34 3 4
J H  o p  o . p  S  a S  a S 
22 y2 x2 x2 y2 5 2 34 3 4
J H
a  a . p  a C  a C
p
32 y2x2 x2 y2 2 34 3 4
JHn 
S
42 z2 5

J  o z 2  C5
H
52

JHa 0
62 z

- Với khớp 𝜃3 là khớp quay, sử dụng ma trận

T52 ta được cột 3 của ma trận
JH
JH n p n .p aCS
13 y3 x3 x3 y3 3 5 4
J H
 o p  o . p  a S S
23 y3 x3 x3 y3 34 5
J H
a p a.p  a C
33 y3 x3 x3 y3 3 4
J n 
H
S
43 z3 5

J  o z 3  C5
H
53

JHa 0
63 z
3
- Với khớp 𝜃4 là khớp quay, sử dụng ma trận
T53 ta được cột 4 của ma trận
JH
JH n p n .p 0
14 y4 x4 x4 y4
J H
o p o .p 0
24 y4 x4 x4 y4
J H
a p a .p 0
34 y4 x4 x4 y4
JHn S
44 z4 5

J  o z 4  C5
H
54

JHa 0
64 z
4
- Với khớp 𝜃5 là khớp quay sử dụng ma trận T JH
5
4
ta được cột 5 của ma trận
 JH n p n .p 0
15 y5 x5 x5 y5
JHo p o .p 0
25 y5 x5 x5 y5
JH a p a .p 0
35 y5 x5 x5 y5

J 45H  nz5 
0
JHo 
0
55 z5

J 65H  az  1
5
 Như vậy ta đã xác định được ma trận JH 6x5 như trên. Tiếp theo để xác định
được ma trận Jacoby ta áp dụng công thức sau:
nx o a 0 0 0 
n 
nzy xy x y 
J o a 0 0 0  .J H
 o a 0 0 0 
 0 x x x
 z z
0 0 0 n ay 
 o 
 y y a z 
0 0 0 n o
n  H J JH J JH JH
x H
 
 ny J
11 12 13 14 15
J H
H
JH
H J JH
  21 22 23 24 25 

nz JH JH
H J JH J H
J  . 31 32 33 34 35
 0 0 0 n
H o a J 
H H
J J J H
J H
 x x x   41 42 43 44 45
 J JH H
H 0 0 0 n oy JH H J J
 y
 51 52 53 54
 55
 ay H H H H
J
H
J J J J 

z   61 62 63 64 65 
+Với nx, ny, nz, ox, oy, oz, ax, ay, az được xác định từ ma trận hàm truyền khi tính
toán động lực học thuận vị trí
 S1.S5  C5 .S1  C234 S234 . a3.C1.C23  a2.C1.C2 
S234.C1.C5 .C1.S5 C1
 
C .C .  C . C .C  C .S .S .S a .S  a .S .C
S S .C
S
T0 234 5 1 1 5 1 5 234 1 5 234 1 3 1 23 2 1 2 

5
 S .C S . C a .  a .S 
S S

d
 234 5 234 5 234 1 3 23 2 2

 0 0 0 1 
Suy ra:
 nx ax   S1.S5  S234 .C1 
C5 .S1  C234
ox S234.C1.C5 .C1.S5
 
n o  C C .C  C .S
a C
.C
.S S . .S .S
S
 y y y  234 5 1 1 5 1 5 234 1 5 234 1 

 nz az  S234 .C5 S234 . S234 .S5 

S5
oz 

Sử dụng Matlab để nhập và nhân ma trận. Sau khi dùng hàm simplify ta sẽ được
ma trận Jacoby thông qua ma trận JH như sau:
−𝑆1(𝑎3𝐶23 + 𝑎2𝐶2) −𝐶1(𝑎3𝑆23 + 𝑎2𝑆2)−𝑎3𝑆23𝐶1 0 0
𝐶 (𝑎 𝐶+ 𝑎 𝐶 ) 0 0
13 232 2 −𝑆13(𝑎232𝑆+
2 𝑎 𝑆 ) −𝑎 𝑆 𝑆
3 23 1
0 𝑎3𝐶23 + 𝑎2𝐶2 𝑎 3 𝐶 23 0 0
𝐽=
0 𝑆1 𝑆1 𝑆1
𝑆234𝐶1
[ 0 −𝐶1 0 −𝐶1 0 −𝐶1 0 𝑆234𝑆1
1 −𝐶234 ]
3.2 Xây dựng chương trình tính toán trên Matlab

Hình 3.1 Code tính toán ma trận Jacoby

Hình 3.2 Xây dựng Guide, nhập thông số và hiển thị kết quả
CHƯƠNG 4. BÀI TOÁN TÍNH TOÁN ĐỘNG HỌC ĐẢO VỊ TRÍ ROBOT

4.1 Tính toán động học đảo vị trí robot

 Tính (φ1)
Ta có phương trình động học thuận của Robot có dạng :
𝑇0 = 𝐴0(φ1). 𝐴1(φ2). 𝐴2(φ3). 𝐴3(φ4). 𝐴4(φ5)(∗)
5 1 2 3 4 5
Với :

Ta nhân 2 vế của phương trình (*) với ma trận nghịch đảo của 𝐴0 ta
1 được
phương trình sau
𝑇1 = (𝐴0)−1. 𝑇 0 = 𝐴1. 𝐴2. 𝐴3. 𝐴4 = 𝐵1 (1)
5 1 5 2 3 4 5
Từ đó ta tính được :
... ... c12 ...
... ... 3 ...
s
𝑇1  123 
5 ... ... 0 ...
 
0 0 0 1

Biết Ma trận 𝐵1 tính theo công thức (1) :

... ... a c  a ...
 x 1 y
... ... 
𝐵1   s1 a z ...

 ... ... axs1  ayc1 ...
 
0 0 0 1
Ta có : 𝑎𝑥𝑠1 − 𝑎𝑦𝑐1 = 0
Suy ra : φ1 = 𝑎𝑡𝑎𝑛2(𝑎𝑥, 𝑎𝑦)

Tính φ3

𝑝𝑥𝑐1 + 𝑝𝑦𝑠1 = 𝑎3𝑐23 + 𝑎2𝑐2
Ta lại có :
𝑝𝑧 − ⅆ = 𝑎3 𝑠23 − 𝑎2 𝑠2
1
{
Bình phương 2 về của 2 phương trình rồi cộng vế theo vế
=> 𝑎22 + 𝑎32 + 2𝑎2𝑎3𝑐3 = (𝑝𝑥𝑐1 + 𝑝𝑦𝑠1)2 + (𝑝𝑧 − ⅆ1)2

(𝑝𝑥𝑐1+ 𝑝𝑦𝑠1)2+(𝑝𝑧−𝑑1)2− 𝑎 22+ 𝑎32

𝑐3 =
2𝑎2 𝑎3
Từ đó ta có :
 2
(𝑝 𝑐 + 𝑝 𝑠𝑥 )1𝑦
+(𝑝 −𝑑 )2− 𝑎 2+ 𝑎 2
1𝑧12 (𝑝 𝑐 + 𝑝 𝑠 )2𝑥+(𝑝
1𝑦 −𝑑 )2− 𝑎 2+ 𝑎 2
1𝑧123 )
φ3 = 𝑎𝑡𝑎𝑛2(±√1 − ( 2𝑎2𝑎3
3 )2; 2𝑎2𝑎3
  Tính φ2

Ta nhân 2 vế của phương trình (1) với ma trận nghịch đảo của 𝐴12 ta được
phương trình sau
𝑇 2 = (𝐴1)−1. 𝑇1 = 𝐴2. 𝐴3. 𝐴4 = 𝐵2 (4)
5 2 5 3 4 5
Ma trận 𝑇5 được tính theo phương trình (4):
2


 a3c3  
... ... ... a3s
𝑇52  ... ...
3

... 0 
0 0 0 1
 
Ma trận 𝐵2 được tính theo công thức (4):
... ... ... pzs2  d1s2  a2  pxc1c2  pyc2s1 
 
... ... ... pc dc pcs p ss
𝐵2   z 2 1 2 x 1 2 y 1 2 
 ... ... ... pxs1  pyc1 
0 0 0 1
 
(𝑝𝑧 − ⅆ1)𝑐2 − 𝑠2(𝑝𝑥𝑐1 + 𝑝𝑦𝑠1) = 𝑎3𝑠3
Ta có
{ 𝑧 − ⅆ1)𝑠2 + 𝑐2(𝑝𝑥𝑐1 + 𝑝𝑦𝑠1) = 𝑎3𝑐3 + 𝑎2
(𝑝

Từ đó ta có:
φ2 = 𝑎𝑡𝑎𝑛2[(𝑝𝑧 − ⅆ1)(𝑎3𝑐3 + 𝑎2) − (𝑝𝑥𝑐1 + 𝑝𝑦𝑠1)𝑎3𝑠3;(𝑝𝑧 − ⅆ1)(𝑎3𝑐3 +
𝑎2) + (𝑝𝑥𝑐1 + 𝑝𝑦𝑠1)𝑎3𝑠3]
4.2 Code tính toán cho bài toán động học ngược
clear all; clc
syms d1 d2 d3 d4 a2 a3 t1 t2 t3 t4 t5 nx ny nz ox oy oz ax
ay az px py pz
T = [nx,ox,ax,px;ny,oy,ay,py;nz,oz,az,pz;0,0,0,1]
A1 = [cos(t1),0,sin(t1),0 ; sin(t1),0,-cos(t1),0 ; 0,1,0,d1
; 0,0,0,1]

A2 = [cos(t2),-sin(t2),0,a2*cos(t2) ;
sin(t2),cos(t2),0,a2*sin(t2) ; 0,0,1,0 ; 0,0,0,1]

A3 = [cos(t3),-sin(t3),0,a3*cos(t3) ;
sin(t3),cos(t3),0,a3*sin(t3) ; 0,0,1,0 ; 0,0,0,1]

A4 = [-sin(t4),0,cos(t4),0 ; cos(t4),0,sin(t4),0 ; 0,1,0,0

; 0,0,0,1]

A5 = [cos(t5),-sin(t5),0,0 ; sin(t5),cos(t5),0,0 ; 0,0,1,0

; 0,0,0,1]
T50=simplify(A1*A2*A3*A4*A5)
T51=simplify(A2*A3*A4*A5)
B1=simplify((A1^-1*T)
T52=simplify(A3*A4*A5)
B2=simplify((A2^-1*B1)
T53=simplify(A4*A5)
B3=simplify((A3^-1*B2)
T54=A5
B4=simplify((A4^-1*B3)
 CHƯƠNG 5. THIẾT KẾ QUỸ ĐẠO TRONG KHÔNG GIAN KHỚP

5.1 Tổng quan

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:
o Đa thức bậc 2 : x(t)= a + bt + ct2
o Đa thức bậc 3 : x(t)= a + bt + ct2 + dt3
o Đa thực bậc n : x(t)= a + bt + ct2 +..........+ ndtn
5.2 Tính toán thiết kế quỹ đạo chuyển động
Thiết kế quỹ đạo chuyển động trong không gian khớp tiến hành như sau
- Chọn 2 điểm A, B bất kỳ trong không gian làm việc, xây dựng quỹ đạo
cho điểm tác động cuối của Robot đi từ A đến B
- Từ bài toán động học ngược, xác định các biến khớp Ɵ 1, Ɵ2, Ɵ3, Ɵ4, Ɵ5 tại
A và B là Ɵ1(A), Ɵ2(A), Ɵ3( ), Ɵ4(A), Ɵ5(A), Ɵ1(B), Ɵ2(B), Ɵ3(B), Ɵ4(B),
Ɵ5(B).
- Chọn quỹ đạo thiết kế là hàm đa thức bậc 3 theo thời gian như sau
-
Ɵi = ai + bit + cit2 + dit3
Với i = 1,2,3,4,5
- Nếu thời gian robot đi từ A đến B là trong t(s) và vận tốc tại 2 điểm A và
B của bằng 0. Ta có
ai = Ɵi(A)
bi = Ɵ̇ i̇(A)

- Tính toán các giá trị ai , bi , ci , di ( i = 1,2,3,4,5) và thay vào hệ phương

trình

Ɵ1= a1 + b1t + c1 t2 + d1 t3
Ɵ2 = a2 + b2t + c2 t2+ d2t3
Ɵ3= a3 + b3t + c3 t2 + d3 t3
Ɵ 4= a4 + b4t + c4 t2 + d4t3
 Ɵ 5= a5 + b5t + c5 t2+d5 t3
Từ đó thu được quỹ đạo chuyển động của mỗi khớp.
Giả sử thời gian bắt đầu chuyển động đến hết chuyển động của các khớp là
như nhau: t1  t 2  t3 1s. Từ đó ta xác định được các phương trình về quỹ đạo, vận
tốc, gia tốc cho 3 khớp đầu của robot như sau:
Khớp thứ nhất:

- Quỹ đạo : 𝑄1(𝑡) = 228,2𝑡2 − 192,2𝑡3

- Vận tốc : 𝑄1̇ (𝑡 ) = −576𝑡 + 576𝑡 2
- Gia tốc : 𝑄1̈ (𝑡 ) = 576 − 1153,2𝑡 2
Khớp thứ 2:
- Quỹ đạo : 𝑄2(𝑡) = −58,2𝑡2 + 38,8𝑡3
- Vận tốc : 𝑄2̇ (𝑡 ) = −116,4𝑡 + 116,4𝑡 2
- Gia tốc : 𝑄2̈ (𝑡 ) = −116,4 + 232,8𝑡 2
Khớp thứ 3:
- Quỹ đạo : 𝑄3(𝑡) = −130,5𝑡2 + 87𝑡 3
- Vận tốc : 𝑄3̇ (𝑡 ) = −261 + 261𝑡 2
- Gia tốc : 𝑄3̈ (𝑡 ) = −261𝑡 2 + 522𝑡 2
Trên đây là ví dụ cho robot với 3 khớp đầu tiên theo động học ngược vị
trí.
Ta có thể sử dụng động học thuận để nhập giá trị các góc rồi từ đó thiết kế
quỹ đạo nhưng như thế nó không đúng so với thực tế. Ví dụ như ở đây là bài toán
đi qua 2 điểm đơn có thể dùng được nhưng khi bài toán đòi hỏi đi qua các điểm
trung gian thì phương pháp này không dùng được!
5.3 Code thiết kế quỹ đạo chuyển động và đồ thị quỹ đạo của Robot
clear all
clc
syms thetaS thetaSf tf t a0 a1 a2 a3 x
% don vi la degrees
thetaS=0;
thetaSf=96;
tf=1;
ts=[0:0.1:tf];
% van toc tai diem dau va diem cuoi la 0
a0= thetaS;
a1=diff(thetaS,x);
a2=(3/tf^2)*(thetaSf-thetaS)-(2/tf)*diff(thetaS,x)-
(1/tf)*diff(thetaSf,x);
a3=(-2/tf^3)*(thetaSf- thetaS)+(1/tf^2)*(diff(thetaSf,x)
+diff(thetaS,x));
% Quy dao vi tri khop quay S:
Quydao(t)=a0+a1*t+a2*t^2+a3*t^3
y0=a0+a1*ts+a2*ts.^2+a3*ts.^3;
subplot(221);
plot(ts,y0)
title('Do thi quy dao');
xlabel('seconds');
ylabel('degrees');
grid on
% Quy dao toc do khop quay S:
Vantoc(t)=diff(Quydao(t),t)
y1=a1+2*a2*ts+3*a3*ts.^2;
subplot(222);
plot(ts,y1)
title('Do thi toc do');
xlabel('seconds');
ylabel('Degree/sec');
grid on
% Quy dao gia toc:
Giatoc(t)=diff(Vantoc(t),t)
y2=2*a2+6*a3*ts;
subplot(223)
plot(ts,y2)
title('Do thi gia toc');
xlabel('seconds');
ylabel('Degree/sec^2');
grid on

Hình 5.1 Đồ thị quỹ đạo chuyển động của Robot

 CHƯƠNG 6. THIẾT KẾ THUẬT TOÁN ĐIỀU KHIỂN CHUYỂN ĐỘNG
CHO ROBOT THEO THUẬT TOÁN PID

Để xây dựng thuật toán điều khiển PID cho robot, ta sẽ dung matlab/Simulink để
mô phỏng. Để có thể mô phỏng trên Matlab thì ta sẽ vẽ hình dạng của Robot trên
SolidWorks sau đó chuyển qua Matlab để mô phỏng.
6.1 Vẽ hình dạng robot trên SolidWorks
Phần đế

Hình 6.1 Phần đế Robot

Trên đây là hình vẽ phần đế của robot, bao gòm cả hệ tọa độ xyz mà nhóm đã
chọn.
Link 1

Hình 6.2 Link 1 của Robot

Link 2

Hình 6.3 Link 2 của Robot

Link 3

Hình 6.4 Link 3 của Robot

Link 4

Hình 6.5 Link 4 của Robot

 Link 5

Hình 6.6 Link 5 của Robot

Mô hình hoàn chỉnh

Hình 6.7 Mô hình hoàn chỉnh của Robot

6.2 Điều khiển PID

Sau khi đã vẽ xong trên SolidWorks. Ta dùng Simspace Multibody để chuyển
mô hình qua Matlab. Sau đó ta sẽ thiết kế bộ điều khiển PID cho từng khớp quay
của Robot.

Hình 6.8 Mô hình Robot khi có thêm PID cho từng khớp

Trên đây là mô hình sau khi đã thêm PID cho từng khớp
Sau đó ta sẽ lựa chọn các thông số PID để điều khiển cho từng khớp
Ví dụ, ta xét đến khớp thứ nhất:
Hình 6.9 Đồ thị khớp thứ nhất của Robot

Hình 6.10 Thông số bộ điều khiển PID

 CHƯƠNG 7. XÂY DỰNG MÔ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI
TƯỢNG TRÊN PHẦN MỀM MATLAB

7.1 Cài đặt và sử dụng

- Để tiến hành mô phỏng Robot sau khi đã vẽ bằng SolidWorks dựa trên
Toolbox Simscape của Matlab, chúng ta cần cài đặt “Simscape Multibody
Link”.
- Sau khi đã xuất file Soildworks dưới dạng file .xml → Gõ lệnh
“mech_import” để chuyển đổi file .xml sang file simulink.

Sau khi tiến hành chuyển đổi xong chúng ta có mô hình robot trên phần mềm
Matlab&simulnk ở hình dưới.

Hình 7.1 Mô hình mô phỏng trên Matlab&Simulink

7.2 Thiết kế và cài đặt bộ điều khiển cho robot

- Cài đặt góc quay cho các khớp ở khối Slider Gain
 Hình 7.2 Cài đặt cho khối Slider Gain

- Về cơ bản khối điều khiển của các khớp sẽ được cài đặt như sau.

Hình 7.3 Khối điều khiển PID cho các khớp

- Trong đó bộ PID lấy sẵn từ thư viện Simulink, tín hiệu đưa vào điều khiển
cho khớp và tín hiệu phản hồi về bộ điều khiển PID, Scope giúp hiển thị
sự so sánh giữa tín hiệu đặt và tín hiệu đầu ra của các khớp.
- Sau khi cài đặt tín hiệu đặt tại khối slider thì chúng ta có đồi thị hiển thị
nơi Scope như sau.
 Hình 7.4 Đồ thị giữa tín hiệu đầu ra và tín hiệu đặt của khớp 1

7.3 Mô phỏng

Hình 7.5 Mô hình điều khiển chung cho 5 khớp

Nhận xét:
+ Các biến khớp đạt giá trị mong muốn trong khoảng thời gian ngắn.
+ Độ quá điều chỉnh nhỏ.
 TÀI LIỆU THAM KHẢO

[1] N. P. T. Anh, Bài giảng Kỹ thuật Robot, 2021.

[2] "www.motoman.com," 6 2019. [Online].
Available: https://www.motoman.com/en-
us/products/robots/industrial/packaging- palletizing/mpk-series/mpk2f.
[Accessed 6 2021].