Bài tập lớn Kĩ thuật Robot: Xây dựng mô hình Robot Scara

Đại học Bách Khoa Hà Nội
Trường Điện- Điện tử

Kĩ thuật Robot
Xây dựng mơ hình Robot Sacra
Nhóm sinh viên thực hiện

MSSV

Nguyễn Đăng Lai

20181564

Nguyễn Đức Long

20181608

Trần Thanh Tùng

20181529

Nguyễn Văn Hùng

20181509

Đào Đức Thắng

20181752

Vũ Thị Thu

20181774

Lê Duy Long

20181597

Vũ Gia Lộc

20181586

Nguyễn Hữu Thắng

20181755

Lê Quang Hiếu

20141508

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

Hà Nội, 1-2022


Mục lục
Phần 1. Giới thiệu về Robot Scara...................................................................... 2
I. Giới thiệu về robot. ........................................................................................ 2
1. Giới thiệu chung. ........................................................................................ 2
2. Robot Scara YASKAWA Model MYS450F ................................................ 5
II. Các ứng dụng trong cơng nghiệp................................................................ 6
III. Kết cấu cơ khí ............................................................................................. 7

1. Cấu tạo cơ khí ............................................................................................. 7
2. Thơng số kỹ thuật cơ bản ........................................................................... 8
Phần 2. Động học Robot Scara............................................................................ 9
I. Động học thuận .............................................................................................. 9
1. Gắn các hệ tọa độ ........................................................................................ 9
2. Lập bảng D- H và tính tốn ...................................................................... 10
3. Tính tốn trên matlab ............................................................................... 12
II. Động học đảo .............................................................................................. 12
1.Tính tốn động học đảo ............................................................................. 12
1.1. Phép đảo vị trí .................................................................................... 12
1.2. Phép phân ly ....................................................................................... 14
2. Tính toán trên Matlab ............................................................................... 15
Phần 3. Ma Trận Jacobi .................................................................................... 16
I. Tính tốn ma trận Jacobi............................................................................ 16
II. Chương trình code matlab ....................................................................... 17
Phần 4. Thiết kế quỹ đạo chuyển động ............................................................ 18
I. Thiết kế quỹ đạo .......................................................................................... 18
II. Xét bài tốn cụ thể ..................................................................................... 18
Phần 5. Xây dựng mơ hình động lực học cho robot Scara ............................. 22
I. Xây dựng mơ hình động lực học ................................................................ 22
Phần 6. Điều khiển Robot Scara ....................................................................... 27
1


Phần 1. Giới thiệu về robot Scara
I. Giới thiệu về robot.
1. Giới thiệu chung.
• Scara Robot được phát minh lần đầu tiên bởi Giáo sư Hiroshi Makino tại Đại
học Yamanashi, Nhật Bản vào năm 1978. Nó được thiết kế ra để đưa vào các
ứng dụng lắp ráp và từ năm 1981 thì nó được ứng dụng trong các dây chuyền

lắp ráp cơng nghiệp. Đó là tay máy scara đặc biệt gồm hai khớp quay và một
khớp trượt, nhưng cả ba khớp đều có trục song song với nhau. Kết cấu này làm
cho tay máy cứng, vững hơn theo phương thẳng đứng Z nhưng kém cứng vững
hơn theo phương ngang XY. Loại này chuyên dùng trong công việc lắp ráp với
tải trọng nhỏ theo phương thẳng đứng.

Hình 1. Robot Scara thực tế
2


• Scara có thể hiểu theo hai định nghĩa như sau:
o Thứ nhất, Scara - Selective Compliance Assembly Robot Arm có nghĩa
là cánh tay Robot lắp ráp tuân thủ có chọn lọc. Đây là định nghĩa về nó
được đưa ra ngay từ đây và ý nghĩa này vẫn dùng phổ biến nhất hiện
nay.
o Thứ hai, Scara - Selective Compliance Articulated Robot Arm có nghĩa
là cánh tay Robot khớp nối tuân thủ có chọn lọc. Đây là định nghĩa sau
này của nó. Định nghĩa này được đưa ra khi nó được đưa vào áp ứng cho
các nhiệm vụ khác nằm ngoài nhiệm vụ lắp ráp trước đó mà nó được ứng
dụng.
• Scara Robot luôn là chủ đề được quan tâm nhiều trong lĩnh vực tự động hóa
bởi tính ứng dụng thực tiễn của nó trong xã hội hiện nay nói chung và trong
ngành cơng nghiệp nói riêng. Mỗi tay máy đều có một cơ cấu khác nhau, do đó
có các phương trình động học, động lực học cũng khác nhau cho nên việc tính
tốn và điều khiển mỗi loại tay máy là rất phức tạp. Có thể thấy tay máy là một
đề tài khơng mới nhưng nó mang tính chất khó, tính nghiên cứu và ứng dụng
cao.
• Nhờ bố trí khớp trục song song của SCARA, cánh tay hơi tuân theo trục XY
và được cố định theo hướng Z . Điều này là thuận lợi cho nhiều loại hoạt động
lắp ráp.

• Thuộc tính thứ hai của SCARA là bố trí cánh tay hai liên kết tương tự như cánh
tay của con người chúng ta, do đó thuật ngữ thường được sử dụng, khớp nối.
Tính năng này cho phép cánh tay mở rộng vào các khu vực hạn chế và sau đó
rút lại hoặc "gập lại" ra khỏi đường đi. Điều này thuận lợi cho việc chuyển các
bộ phận từ ô này sang ô khác hoặc để tải / dỡ tải các trạm xử lý.

3


• SCARA thường nhanh hơn các hệ thống robot Cartesian tương đương. SCARA
có thể đắt hơn các hệ thống Cartesian tương đương và phần mềm điều khiển
yêu cầu động học ngược cho các di chuyển nội suy tuyến tính.
• Robot Scara của một số hãng sản xuất:

Hình 2. Một số hãng sản xuất robot Scara

4


2. Robot Scara YASKAWA Model MYS450F
Robot SCARA có các đặc điểm nổi bật như:
• Robot SCARA có 4 trục lý tưởng cho các hệ thống xử lý và lắp ráp tốc độ
cao.
• Cung cấp hiệu suất vượt trội trong các ứng dụng như lắp ráp, pha chế, xử lý
phần nhỏ, đóng gói vỏ, tự động hóa phịng thí nghiệm, đóng gói, xử lý năng
lượng mặt trời và bán dẫn.
• Lý tưởng cho các hệ thống đa quy trình địi hỏi khả năng chọn-và-đặt.
• Dễ dàng tích hợp với các ứng dụng robot hiện có để mở rộng các quy trình
tự động hiện tại.
• Là dịng sản phẩm Yaskawa, robot 4-15 trục với trọng tải 1-800 kg.

• Sử dụng bộ điều khiển giống như các robot Yaskawa khác. Vùng làm việc
có thể thay đổi tùy thuộc vào kích thước và tải trọng của robot.
• Có tốc độ cao, hình thức nhỏ gọn,tối thiểu khơng gian lắp đặt.
• Có tầm với xun tâm 450 mm, hành trình trục U có tải trọng tối đa 180 mm
và 6 kg.
• Các trục bên ngồi có sẵn, các khớp linh hoạt.
• Độ lặp lại cao, có sự tuân thủ trong tất cả các trục, làm cho các robot này lý
tưởng cho các nhiệm vụ lắp ráp.
• Có sẵn nhiều loại kết nối bus trường.
• Tải trọng tối đa 6 kg.
• Tầm xuyên tâm 450 mm.
• Hành trình trục U 180 mm.
• Điều khiển: FS100, MXL200.
• Sử dụng bộ điều khiển giống như những robot Yaskawa lớn hơn.

5


• Tích hợp tầm nhìn dễ dàng với MotoSight 2D, hệ thống quan sát dựa trên
tầm nhìn Cognex.
• Mang lại hiệu quả cao, trong khi yêu cầu đầu tư vốn tối thiểu.
• Khớp nối linh hoạt, dễ dàng triển khải, lắp đặt lại khi nhu cầu sản xuất thay
đổi.
II. Các ứng dụng trong công nghiệp
Mục tiêu ứng dụng robot công nghiệp nhằm góp phần nâng cao năng suất dây
chuyền cơng nghệ, giảm giá thành sản phẩm, nâng cao chất lượng và khả năng cạnh
tranh của sản phẩm, đồng thời cải thiện điều kiện lao động, Điều đó xuất phát từ
những ưu điểm cơ bản của robot, đó là:
•

Robot cơng nghiệp có thể thực hiện được một qui trình thao tác hợp lí bằng
hoặc hơn người thợ lành nghề một cách ổn định trong suốt thời gian làm việc.
Vì thế robot cơng nghiệp có thể góp phần nâng cao chất lượng và khả năng
cạnh tranh của sản phẩm. Hơn thế robot cịn có thể nhanh chóng thay đổi cơng
việc để thích nghi với sự thay đổi mẫu mã, kích cỡ sản phẩm theo yêu cầu của
thị trường cạnh tranh.

• Khả năng giảm giá thành sản phẩm do ứng dụng robot làm giảm được đáng kể
chi phí cho người lao động.
• Việc áp dụng robot có thể làm tăng năng suất dây chuyền cơng nghệ. Sở dĩ như
vậy vì nếu tăng nhịp độ khẩn trương của dây chuyền sản xuất, nếu không thay
thế con người bằng robot thì người thợ khơng thể theo kịp hoặc rất chóng mệt
mỏi.
• Robot có thể cải thiện điều kiện lao động. Đó là ưu điểm nổi bật nhất mà chúng
ta cần lưu tâm. Vì trong thực tế sản xuất có rất nhiều nơi người lao động phải
làm việc trong mơi trường có hại cho sức khoẻ hoặc dễ xảy ra tai nạn lao động.

6


• Robot công nghiệp được ứng dụng nhiều trong các lĩnh vực: đúc, gia công áp
lực, hàn và nhiệt luyện, gia cơng và lắp ráp…
III. Kết cấu cơ khí
1. Cấu tạo cơ khí

Hình 3. Robot Scara YASKAWA Model MYS450F
Tất cả kích thước đều có đơn vị là milimet và chỉ để tham khảo.
Yêu cầu bản vẽ chi tiết cho tất cả các yêu cầu thiết kế / kỹ thuật.
Thông số kỹ thuật
Hệ (trục)

Phạm vi

Tốc độ cực đại

Mômen

Sai số

Số trục điều khiển

4

tọa độ

chuyển động

[mm/s]

quán tinh

[mm]

Khối hàng [kg]

3/6
450

cực đại

cho phép

Phạm vi tiếp cận

[mm]

[𝑘𝑔 ⋅ 𝑚2 ]

xuyên tâm [mm]

S+L

450 mm

-

-

±0.015

Trục chữ U [mm]

180

S

+242/-62

420

-

-

Khối lượng [kg]

27

L

±147.5

720

-

-

Công suất [kVA]

1.25

U

180 mm

1,083 mm/s

-

±0.01

R

±360

2400

0.01/0.12

±0.005

7


Tốc độ tối đa và mơmen qn tính có thể thay đổi tùy thuộc vào tải.
Robot được kết cấu theo dạng Scara là phương án thích hợp để vận chuyển các
vật có khối lượng lớn. Các khớp quay được truyền động bằng động cơ điện.
Tốc độ quay của các động cơ điện điều khiển được nhờ bộ biến tần công suất
1,5 kw.
Robot được điều khiển nhờ bộ lập trình PLC. Các động cơ điện có phản hồi vị
trí và điều khiển tốc độ quay nhờ bộ biến tần. Hệ thống điều khiển yêu cầu phản
hồi đảm bảo hoạt động ổn định và an tồn cho robot.
2. Thơng số kỹ thuật cơ bản
Tải trọng lớn nhất ở khâu cuối
Các giới hạn

100 kg

𝜃1

3000

𝜃2

3200

𝜃̇1

1000

𝜃̇2

1250 /𝑠

chuyển động
của các khâu

Tốc độ lớn nhất

Độ chinh xác lặp lại vị trí

±1 𝑚𝑚

Khối lượng

200 kg

Phạm vi ứng dụng chủ yếu

Vận chuyển các đồ vật
giữa các vị trí trong dây
chuyền sản xuất

8


Phần 2. Động học Robot Scara
I. Động học thuận
1. Gắn các hệ tọa độ
+ Hệ tọa độ {𝑂0 } cố định gắn với thân Robot
+ Quan hệ giữa hệ tọa độ {𝑂1 } với {𝑂0 }: quay góc 𝜃1 quanh trục 𝑧0 tịnh tiến
đoạn 𝑎1 trên trục 𝑥0 .
+ Quan hệ giữa hệ tọa độ {𝑂2 } với {𝑂1 }: quay góc 𝜃2 quanh trục 𝑧1 , tịnh tiến
đoạn 𝑎2 trên trục 𝑥1 và quay góc 180 độ trên quanh trục 𝑥2 .
+ Quan hệ giữa hệ tọa độ {𝑂3 } với {𝑂2 }: tịnh tiến đoạn 𝑟3 trên trục 𝑧2 .
+ Quan hệ giữa hệ tọa độ {𝑂4 } với {𝑂3 }: quay góc 𝜃4 quanh trục 𝑧2 . Đây là hệ
tọa độ gắn tay kẹp, khâu tác động cuối.

Hình 4. Chọn các hệ tọa độ gắn với robot Scara

9


2. Lập bảng D- H và tính tốn
Bài tốn động học thuận cho phép xác định vị trí của tay kẹp robot trong không
gian làm việc từ các biến khớp.
Bảng D - H:
𝜃
𝜃1

𝜃2
0
𝜃4

Thanh nối
1
2
3
4

𝛼
0
180
0
0

a
a1
a2
0
0

d
0
0
r3
0

Ma trận biểu diễn quan hệ giữa khung tọa độ (i) với (i - 1):
𝐴𝑖−1

𝑖

𝑐𝑜𝑠 𝜃𝑖
𝑠𝑖𝑛 𝜃𝑖
=[
0
0

− 𝑠𝑖𝑛 𝜃𝑖 𝑐𝑜𝑠 𝛼𝑖
𝑐𝑜𝑠 𝜃𝑖 𝑐𝑜𝑠 𝛼𝑖
𝑠𝑖𝑛 𝛼𝑖
0

𝑠𝑖𝑛 𝜃𝑖 𝑠𝑖𝑛 𝛼𝑖
− 𝑐𝑜𝑠 𝜃𝑖 𝑠𝑖𝑛 𝛼𝑖
𝑐𝑜𝑠 𝛼𝑖
0

𝑎𝑖 𝑐𝑜𝑠 𝜃𝑖
𝑎𝑖 𝑠𝑖𝑛 𝜃𝑖
]
𝑑𝑖
1

Lần lượt thay các thông số, ta được:
𝑐1
𝑠
𝐴10 = [ 1
0
0

−𝑠1
𝑐1
0
0

0
0
𝐴23 = [
0
0

0 𝑎1 𝑐1
0 𝑎1 𝑠1
]
1
0
0
0

𝑎2
𝑎
; 𝐴12 = [ 2
0
0

𝑎2
−𝑎2
0
0

0
0
0
0

𝑐4
𝑠
; 𝐴34 = [ 4
0
0

𝑎2
𝑐4
0
0

1 0
0 0
]
1 𝑟3
0 1

0
0
−1
0
0
0
1

0

0
0
]
0
1

Ma trận 𝑇40 biểu diễn tọa độ tay máy trong hệ trục tọa độ gốc:
𝑇40 = 𝐴10 . 𝐴12 . 𝐴23 . 𝐴34 ;
Ta lần lượt tính:
𝑐4
𝑠
𝑇43 = 𝐴34 ; 𝑇42 = 𝐴23 . 𝐴34 = [ 4
0
0

10

−𝑠2
𝑐4
0
0

0 0
0 0
]
1 𝑟3
0 1

𝑎2 𝑐2
𝑎2 𝑠2
]
0
0


𝑐2 𝑐4 + 𝑠2 𝑠4
𝑠 𝑐 − 𝑐2 𝑠4
𝑇41 = 𝐴12 . 𝐴24 = [ 2 4
0
0

−𝑐2 𝑠4 + 𝑠2 𝑐4
−𝑠2 𝑠4 − 𝑐2 𝑐4
0
0

0 𝑎2 𝑐2
0 𝑎2 𝑠2
]
1
𝑟3
0
1

Vậy:
𝑇40 = 𝐴10 . 𝐴14 =
𝑐1 (𝑐2 𝑐4 + 𝑠2 𝑠4 ) − 𝑠1 (−𝑐2 𝑠4 + 𝑠2 𝑐4 ) 𝑐1 (−𝑐2 𝑠4 + 𝑠2 𝑐4 ) − 𝑠1 (−𝑠2 𝑠4 − 𝑐2 𝑐4 ) 0 𝑎1 𝑐1 + 𝑎2 (𝑐1 𝑐2 − 𝑠1 𝑠2 )
𝑠2 𝑐4 − 𝑐2 𝑠4

−𝑠2 𝑠4 − 𝑐2 𝑐4
0 𝑎1 𝑐1 + 𝑎2 (𝑐1 𝑐2 − 𝑠1 𝑠2 )
[
]
0
0
−1
−𝑟3
0
0
0
1

Rút gọn:
𝑐1+2−4
𝑠
𝑇40 = [ 1+2−4
0
0

𝑠1+2−4
−𝑐1+2−4
0
0

0 𝑎1 𝑐1 + 𝑎2 𝑐1+2
0 𝑎1 𝑠1 + 𝑎2 𝑠1+2
]
−1
−𝑟3

0
1

Trong đó: c~cos; s~sin; 𝑐1+2−4 = 𝑐𝑜𝑠(𝜃1 + 𝜃2 − 𝜃4 )
Theo ký hiệu hiệu tổng quát:
𝑛𝑥
𝑛𝑦
𝑇40 = [ 𝑛
𝑧
0

𝑜𝑥
𝑜𝑦
𝑜𝑧
0

𝑎𝑥
𝑎𝑦
𝑎𝑦
0

𝑝𝑥
𝑝𝑦
𝑝𝑦 ] ,
1

Vậy ta có hệ phương trình động học thuận là:
𝑛𝑥 = 𝑐1+2−4

;

𝑜𝑥 = 𝑠1+2−4

𝑛𝑦 = 𝑠1+2−4

;

𝑜𝑦 = −𝑠1+2−4 ; 𝑎𝑦 = 0

𝑛𝑧 = 0

;

; 𝑎𝑥 = 0

𝑜𝑧 = 0

𝑝𝑥 = 𝑙1 𝑐1 + 𝑙2 𝑐12

;
;

𝑝𝑦 = 𝑙1 𝑠1 + 𝑙2 𝑠12

; 𝑎𝑧 = −1 ;

11

𝑝𝑧 = −𝑟3

3. Tính tốn trên matlab

II. Động học đảo

1.Tính tốn động học đảo
Ma trận 𝑇40 cho ta biết hướng và vị trí của khâu tác động cuối của Robot. Yêu
cầu của bài tốn động học ngược vị trí là xác định giá trị của các biến khớp
𝑄 = [𝜃1

𝜃2

𝑟3

𝜃4 ]𝑇 để khâu tác động cuối có vị trí và hướng mong muốn.

1.1. Phép đảo vị trí
Ta có:
𝑛𝑥
𝑛
𝑇40 = [ 𝑦
𝑛𝑧
0

𝑜𝑥
𝑜𝑦
𝑜𝑧
0

𝑎𝑥

𝑎𝑦
𝑎𝑧
0

𝑝𝑥
𝑐12−4 𝑠12−4 0 𝑎1 𝑐1 + 𝑎2 𝑐12
𝑝𝑦
𝑠12−4 −𝑐12−4 0 𝑎1 𝑠1 + 𝑎2 𝑠12
]
𝑝𝑧 ] = [ 0
−𝑟3
0 −1
0
0
1
0
1

+ Tính tốn 𝜃1
Ta có:
12


𝑝𝑥 = 𝑎1 𝑐1 + 𝑎2 𝑐12

(1)

𝑝𝑦 = 𝑎1 𝑠1 + 𝑎2 𝑠12
Chuyển vế rồi bình phương (1) và (2):

(2)

(𝑝𝑥 − 𝑎1 𝑐1 )2 = (𝑎2 𝑐12 )2
2
(𝑝𝑦 − 𝑎1 𝑠1 ) = (𝑎2 𝑠12 )2
Cộng cả 2 vế của 2 phương trình với nhau:

(3)
(4)

𝑎22 = 𝑝𝑥2 + 𝑝𝑦2 + 𝑎12 − 2𝑎1 (𝑝𝑥 𝑐1 + 𝑝𝑦 𝑠1 )
𝑝𝑥2 + 𝑝𝑦2 + 𝑎12 − 𝑎22
⟺ 𝑝𝑥 𝑐1 + 𝑝𝑦 𝑠1 =
2𝑎1

Đặt

2 +𝑎 2 −𝑎 2
𝑝𝑥2 +𝑝𝑦
1
2

2𝑎1

(5)

=𝐴

Khi đó 𝑝𝑥 𝑐1 + 𝑝𝑦 𝑠1 = 𝐴 ⇒ 𝜃1 = 𝐴𝑇𝐴𝑁2(𝑝𝑦 , 𝑝𝑥 ) +
𝐴𝑇𝐴𝑁2(±√𝑝𝑥2 + 𝑝𝑦2 − 𝐴2 , 𝐴)

+ Tính tốn 𝜃2
Sử dụng lại phương trình (1) và (2)
Khi đó có:
𝑝𝑥 − 𝑎1 𝑐1
𝑎2
𝑝𝑦 − 𝑎1 𝑠1
𝑠12 =
𝑎2
𝑝𝑥 −𝑎1 𝑐1 𝑝𝑦 −𝑎1 𝑠1
= 𝐴𝑇𝐴𝑁2 (
,
)
𝑎
𝑎
𝑐12 =

Khi đó ⇒ 𝜃12

2

𝑝𝑥 −𝑎1 𝑐1 𝑝𝑦 −𝑎1 𝑠1

⇒ 𝜃2 = 𝐴𝑇𝐴𝑁2 (

𝑎2

,

𝑎2

(6)
(7)

2

) − 𝐴𝑇𝐴𝑁2(𝑝𝑦 , 𝑝𝑥 ) +

𝐴𝑇𝐴𝑁2(±√𝑝𝑥2 + 𝑝𝑦2 − 𝐴2 , 𝐴)
+ Tính tốn 𝜃4

Khi đó ⇒ 𝜃12−4

𝑛𝑥 = 𝑐12−4
𝑝𝑦 = 𝑠12−4
= 𝐴𝑇𝐴𝑁2(𝑛𝑥 , 𝑛𝑦 )

⇒ 𝜃4 = 𝐴𝑇𝐴𝑁2 (

𝑝𝑥 − 𝑎1 𝑐1 𝑝𝑦 − 𝑎1 𝑠1
,
) − 𝐴𝑇𝐴𝑁2(𝑛𝑥 , 𝑛𝑦 )
𝑎2
𝑎2
13

(8)
(9)


+ Tính tốn 𝑟3

Dễ dàng nhận thấy 𝑟3 = −𝑝𝑧
1.2. Phép phân ly
Bước 1: Tính 𝜃1
0
Nhân vào bên trái 2 vế phương trình T4 = A1(𝜃1 )A2(𝜃2 )A3(𝜃3 )A4(𝜃4 ) với ma

trận 𝐴1−1 ta được:
𝐴1−1

1
𝑠1 /𝑐1
−𝑠1 /𝑐1
1
=[
0
0
−𝑠1 𝑎1 /𝑐1 0

0
0
1
0

0
0]
1
1

𝐴1−1 . 0𝑇4 = 𝐴2 (𝜃2 )𝐴3 (𝜃3 )𝐴4 (𝜃4 ) (*)
𝑉𝑇1 (𝜃1 ) =

−1

𝐴1 . 0𝑇4 =

𝑛𝑥 + 𝑛𝑦 𝑠1 /𝑐1 𝑜𝑥 + 𝑜𝑦 𝑠1 /𝑐1 𝑎𝑥 + 𝑎𝑦 𝑠1 /𝑐1
𝑝𝑥 + 𝑝𝑦 𝑠1 /𝑐1
−𝑠 𝑛 /𝑐 + 𝑛𝑦 −𝑠1 𝑜𝑥 /𝑐1 + 𝑜𝑦 −𝑠1 𝑎𝑥 /𝑐1 + 𝑎𝑦 −𝑠1 𝑝𝑥 /𝑐1 + 𝑝𝑦
[ 1 𝑥 1
]
𝑛𝑥
𝑎𝑥
𝑜𝑥
𝑝𝑥
−𝑠1 𝑎1 𝑛𝑥 /𝑐1 −𝑠1 𝑎1 𝑛𝑥 /𝑐1
−𝑠1 𝑎1 𝑛𝑥 /𝑐1 −𝑠1 𝑎1 𝑛𝑥 /𝑐1 + 1
𝑉𝐹1 (𝜃1 ) = 𝐴2 (𝜃2 )𝐴3 (𝜃3 )𝐴4 (𝜃4 )
𝑐12−4 − 𝑠12−4 𝑠1 /𝑐1 − 𝑠1 𝑎1 /𝑐1 (𝑎1 𝑐1 + 𝑎2 𝑐12 ) 𝑐12−4 𝑠1 /𝑐1 + 𝑠12−4 0 𝑎1 𝑐1 + 𝑎2 𝑐12
𝑠
+ 𝑐12−4 𝑠1 /𝑐1 − 𝑠1 𝑎1 /𝑐1 (𝑎1 𝑐1 + 𝑎2 𝑐12 𝑠12−4 𝑠1 /𝑐1 − 𝑐12−4 0 𝑎1 𝑠1 + 𝑎2 𝑠12
= [ 12−4
]
−𝑟3
𝑠1 𝑎1 𝑟3 /𝑐1 − 1
−1
0
0
0
1
−𝑠1 𝑎1 /𝑐1

Cân bằng với phần tử tương ứng trong hai ma trận
Tìm 𝜃1
Bước 2 :Tính 𝜃2
Nhân tiếp phương trình (*) với 𝐴−1
2 ta được
𝐴−1
2 . 𝑉𝑇1 (𝜃1 ) = A3(𝜃3 ) A4(𝜃4 )
Cân bằng với phần tử tương ứng trong hai ma trận
Tìm 𝜃2
Tương tự tìm 𝜃4
14


2. Tính tốn trên Matlab

15


Phần 3. Ma Trận Jacobi
I. Tính tốn ma trận Jacobi
Xác định ma trận 𝐽𝐻 :
𝑠1+2−4 (𝑎1 𝑐1 + 𝑎2 𝑐1+2 ) − 𝑐1+2−4 (𝑎1 𝑠1 + 𝑎2 𝑠1+2 )
𝑎2 𝑐2 (𝑠2 𝑐4 − 𝑐2 𝑠4 ) − 𝑎2 𝑠2 (𝑐2 𝑐4 − 𝑠2 𝑐4 )
−𝑐1+2−4 (𝑎1 𝑐1 + 𝑎2 𝑐1+2 ) − 𝑠1+2−4 (𝑎1 𝑠1 + 𝑎2 𝑠1+2 ) 𝑎2 𝑐2 (−𝑠2 𝑠4 − 𝑐2 𝑐4 ) − 𝑎2 𝑠2 (−𝑐2 𝑠4 + 𝑠2 𝑐4 )
0
0
=
0
0
0

0
[
−1
1

𝐴 𝐵
𝐶 𝐷
0 0
=
0 0
0 0
[−1 0

0
0
1
0
0
0

0
0
0
0
0
1]

Với
A= 𝑠1+2−4 (𝑎1 𝑐1 + 𝑎2 𝑐1+2 ) − 𝑐1+2−4 (𝑎1 𝑠1 + 𝑎2 𝑠1+2 )
B= 𝑎2 𝑐2 (𝑐2 𝑐4 − 𝑐2 𝑠4 ) − 𝑎2 𝑠2 (𝑐2 𝑐4 − 𝑐2 𝑠4 )

C=−𝑐1+2−4 (𝑎1 𝑐1 + 𝑎2 𝑐1+2 ) − 𝑠1+2−4 (𝑎1 𝑠1 + 𝑎2 𝑠1+2 )
D=−𝑠1+2−4 (𝑎1 𝑐1 + 𝑎2 𝑐1+2 + 𝑎1 𝑠1 + 𝑎2 𝑠1+2 )
Ta đi xác định ma trận quay 𝑅𝑛0 và ta có:
𝑅0
[ 𝑛
0

0
]=
𝑅𝑛0
𝑐1+2−4
𝑠1+2−4
0
0
0
[ 0

𝑠1+2−4
−𝑐1+2−4
0
0
0
0

0
0
0
0
−1
0

0 𝑐1+2−4
0 𝑠1+2−4
0
0

0
0
0
𝑠1+2−4
−𝑐1+2−4
0

Với:
16

0
𝐸
0
𝐺
0
0
=
0
0
0
0
−1] [ 0

𝐹
𝐻

0
0
0
0

0 0
0 0
−1 0
0 𝐸
0 𝐺
0 0

0
0
0
𝐹
𝐻
0

0
0
0
0
0
−1]

0
0
1
0

0
0

0
0
0
0
0
1]


E= 𝑐1+2−4 , F=𝑠1+2−4 , G=𝑠1+2−4 , H=−𝑐1+2−4
Ma trận Jacobi:
𝑅0
𝐽=[ 𝑛
0

𝐸
𝐺
0
𝐻 0
0] . 𝐽 = 0
𝑅𝑛
0
[0

𝐹
𝐻
0
0

0
0

0 0
0 0
−1 0
0 𝐸
0 𝐺
0 0

(𝐸𝐴 + 𝐹𝐶)
𝐺𝐴 + 𝐻𝐶
0
=
0
0
[
1

II. Chương trình code matlab

17

0
0
0
𝐹
𝐻
0

0
𝐴 𝐵
0
𝐶 𝐷
0
0 0
∗
0
0 0
0
0 0
−1] [−1 0

(𝐸𝐵 + 𝐹𝐷)
𝐺𝐵 + 𝐻𝐷
0
0
0
1

0 0
0 0
1 0
0 0
0 0
0 −1]

0
0
1

0
0
0

0
0
0
0
0
1]


Phần 4. Thiết kế quỹ đạo chuyển động
I. Thiết kế quỹ đạo
- Thiết kế quỹ đạo chuyển động là xác định đường biểu diễn của vị trí góc
khớp (góc quay của khớp với khớp quay & độ di chuyển của khớp đối với
khớp tịnh tiến) theo thời gian khi di chuyển từ vị trí ban đầu 𝑞0 đến vị trí
cuối 𝑞𝑐 trong khoảng thời gian 𝑡𝑐 với 𝑞 là biến khớp tổng quát.
- Quỹ đạo chuyển động của khớp giữa hai vị trí phải thỏa mãn 4 điều kiện: vị
trí đầu, vị trí cuối, tốc độ tại vị trí đầu & tốc độ tại vị trí cuối. Do đó, đa thức
bậc 3 sẽ phù hợp với quỹ đạo của khớp robot:
𝑞(𝑡) = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 2 + 𝑎3 𝑡 3
- Giả sử tại thời điểm đầu & cuối, vận tốc bằng 0, ta có hệ phương trình cho
mỗi khớp:
𝑞(𝑡0 ) = 𝑞0 = 𝑎0
𝑞(𝑡𝑐 ) = 𝑞𝑐 = 𝑎0 + 𝑎1 𝑡𝑐 + 𝑎2 𝑡𝑐2 + 𝑎3 𝑡𝑐3
𝑞̇ (𝑡0 ) = 𝑞̇ 0 = 𝑎1
{𝑞̇ (𝑡𝑐 ) = 𝑞̇ 𝑐 = 𝑎1 𝑡𝑐 + 2𝑎2 𝑡𝑐 + 3𝑎3 𝑡𝑐2 = 0
Giải hệ phương trình trên ta được phương trình quỹ đạo:
𝑎0 = 𝑞0

𝑎1 = 0
𝑎2 =

3(𝑞𝑐 −𝑞0 )

{ 𝑎3 = −

𝑡𝑐2
2(𝑞𝑐 −𝑞0 )
𝑡𝑐3

II. Xét bài toán cụ thể
Thiết kế quỹ đạo chuyển động với thông số:
𝑞10
𝜋/6

𝑞1𝑐
𝜋/3

𝑞20
0

𝑞2𝑐
𝜋/3

- Lập phương trình cho khớp 1:

18

𝑞40

−𝜋/12

𝑞4𝑐
3𝜋/4

𝑡
5


𝜋

𝑎10 = 𝑞10 =
𝑎11 = 0
𝑎12 =

𝜋

6

3(𝑞1𝑐 −𝑞10 )

=

𝑡𝑐2

2(𝑞1𝑐 −𝑞10 )

{ 𝑎13 = −

𝜋 𝜋

3 6
52

3( − )

50

𝜋 𝜋
2( − )
3 6
53

=−

𝑡𝑐3

𝑎10 =
6
𝑎11 = 0
𝜋
⇔
𝑎12 =

𝜋

{ 𝑎13 = − 375

Ta có phương trình động học của khâu 1:
𝜋

𝜋

6
𝜋

50

𝑞1 (𝑡) = +
𝑞̇ 1 (𝑡) =

25
𝜋

𝑡2 −
𝜋

𝑡−

125
2𝜋

𝑡

𝜋

𝑡3

375
2

𝑞̈ (𝑡) = −
𝑡
25
125
{ 1
Tương tự cho khớp 2:
𝑎20 = 𝑞20 = 0
𝑎21 = 0
𝑎22 =

3(𝑞2𝑐 −𝑞20 )

=

𝑡𝑐2

{ 𝑎23 = −

2(𝑞2𝑐 −𝑞20 )
𝑡𝑐3

𝜋
3

3( −0)
52

=−

𝑎10 = 0

𝑎11 = 0
⇔ 𝑎12 = 𝜋

25

𝜋
2( −0)
3
53

2𝜋

{𝑎13 = − 375

Phương trình động học của khâu 2:
𝑞2 (𝑡) =

𝜋
25

𝑞̇ 2 (𝑡) = −
{

𝑞̈ 2 (𝑡) = −

𝜋

𝑡2 −
𝜋
125

2𝜋
125

𝑡

375
2

𝑡3

𝑡

• Khâu 4:
𝜋

𝑎40 = 𝑞40 = −
12
𝑎41 = 0
𝑎42 =

3(𝑞4𝑐 −𝑞40 )

{ 𝑎43 = −

𝑡𝑐2

=

2(𝑞4𝑐 −𝑞40)
𝑡𝑐3

𝜋

3(

3𝜋 𝜋
+ )
4 12
52

=−

3𝜋 𝜋
2( + )
4 12
53

19

𝑎40 = −
12
𝑎41 = 0
𝜋
⇔
𝑎42 =
10

𝜋

{ 𝑎43 = − 75

Phương trình động học của khâu 4:
𝑞4 (𝑡) = −
𝜋

𝜋
12

𝑞̇ 4 (𝑡) = 𝑡
5
𝜋

{

𝜋

+

𝑡2 −

10
𝜋 2
− 𝑡
25
2𝜋

𝑞̈ 4 (𝑡) = −
5

25

𝜋
75

𝑡3

𝑡

Thiết kế quỹ đạo trên matlab
Code:
%khớp1
syms t
>> y1=pi/6+t^2*pi/50-t^3*pi/375
>> ezplot(y1,[0 5])
>> xlabel('t(s)')
>> ylabel('y1')
>> grid
%khớp2
syms t
>> y2=t^2*pi/25-t^3*2*pi/375;
>> ezplot(y2,[0 5])
>> xlabel('t(s)')
>> ylabel('y2')
>> grid
%khớp4
>> syms t
>> y4=-pi/12+t^2*pi/10-t^3*pi/75;
>> ezplot(y4,[0 5])
>> xlabel('t(s)')

20


>> ylabel('y4')
>> grid
Ta có kết quả mơ phỏng như sau:

Hình 5. Hình dạng quay đạo của các khớp 1, 2 và 4

21


Phần 5. Xây dựng mơ hình động lực học cho robot Scara
I. Xây dựng mơ hình động lực học
Hàm Lagrange: L = K – P
Trong đó K là tổng động năng, P là tổng thế năng hệ thống.
Phương trình động lực học:
𝐹𝑖 (𝑇𝑖 ) =

𝑑 𝜕𝐿
𝜕𝐿
( )−
𝑑𝑡 𝜕𝑞̇ 𝑖
𝜕𝑞𝑖

Với
• 𝑞𝑖 và biến khớp:
𝑞𝑖 = 𝜃𝑖 với khớp quay và 𝑞𝑖 = 𝑟𝑖 với khớp tịnh tiến.
• 𝐹𝑖 lực đặt lên khớp tịnh tiến.
• 𝑇𝑖 mơmen khớp quay

1

1

2

2

• 𝐾𝑖 = 𝑚𝑖 𝑣𝑖2 + 𝐽𝑖 𝜔𝑖2 động năng thanh nối i
• 𝑃𝑖 = 𝑚𝑖 𝑔ℎ𝑖 thế năng thanh nối i

Hình 6. Mơ hình robot Scara được gắn các hệ trục tọa độ

22

(10)


Thanh nối thứ nhất:
Tọa độ của khối tâm được xác định:
𝑎1
𝑃𝑐1 = [ 𝑐𝜃1
2

𝑎1
𝑠𝜃
2 1

𝑇

0]

Tốc độ khối tâm thanh nối 1 là:
𝑎
̇ = [− 1 𝑠𝜃1 𝜃̇1
𝑃𝑐1
2

𝑎1
𝑐𝜃 𝜃̇
2 1 1

𝑇

0]

Tốc độ quay của thanh nối 1 là:
𝜔𝑐1 = [0

0 𝜃̇1 ]𝑇

Động năng khối tâm thanh nối 1 là:
1 𝑇
1 𝑇
1 1
̇ 𝑚1 𝑃𝑐1
̇ + 𝜔𝑐1
𝐾1 = 𝑃𝑐1
𝐼𝑧𝑧1 𝜔𝑐1 = ( 𝑚1 𝑎12 + 𝐼𝑧𝑧1 ) 𝜃̇12
2

2
2 4
Thế năng:
𝑎1
𝑐𝜃
2 1
𝑃1 = −𝑚1 𝐺 𝑇 𝑃𝑐1 = −𝑚1 [0 0 −𝑔] 𝑎1
=0
𝑠𝜃1
2
[ 0 ]
Thanh nối thứ 2:
Tọa độ của khối tâm được xác định:
𝑃𝑐2 = [𝑎1 𝑐𝜃1 +

𝑎2
𝑐𝜃
2 12

𝑎1 𝑠𝜃1 +

𝑎2
𝑠𝜃
2 12

𝑇

0]

Tốc độ khối tâm thanh nối 2:

̇ = [−𝑎1 𝑠𝜃1 𝜃̇1 −
𝑃𝑐2

𝑎2
𝑠𝜃12 𝜃̇12
2

𝑎1 𝑐𝜃1 𝜃̇1 +

𝑎2
𝑐𝜃12 𝜃̇12
2

Tốc độ quay của thanh nối 2 là:
𝜔𝑐2 = [0 0

𝜃̇12 ]𝑇

Động năng của khối tâm thanh nối 2:
1 𝑇
1 𝑇
̇ 𝑚2 𝑃𝑐2
̇ + 𝜔𝑐2
𝐾2 = 𝑃𝑐2
𝐼𝑧𝑧2 𝜔2
2
2
23

𝑇

0]


1
𝑎22 2
1
2 ̇2
2
= 𝑚2 (𝑎1 𝜃1 + 𝜃̇12 + 𝑎1 𝑎2 𝜃̇1 𝜃̇12 𝑐𝜃2 ) + 𝐼𝑧𝑧2 𝜃̇12
2
4
2
Thế năng:

a2


 a1c1 + 2 c12 


a2

T
P2 = − m2G Pc2 = − m2  0 0 − g   a1s1 + s12  = 0
2







0
Thanh nối thứ 3:
Tọa độ của khối tâm thanh nối thứ 3 được xác định:
𝑃𝑐3 = [𝑎1 𝑐𝜃1 + 𝑎2 𝑐𝜃12

𝑎1 𝑠𝜃1 + 𝑎2 𝑠𝜃12

𝑇
𝑎3
+ 𝑎4 − 𝑟3 ]
2

Tốc độ khối tâm thanh nối thứ 3 là:
̇ = [−𝑎1 𝑠𝜃1 𝜃̇1 − 𝑎2 𝑠𝜃12 𝜃̇12
𝑃𝑐3

𝑎1 𝑐𝜃1 𝜃̇1 + 𝑎2 𝑐𝜃12 𝜃̇12

𝑇
𝑎3
+ 𝑎4 − 𝑟3 ]
2

Tốc độ quay thanh nối 3 là:
𝜔𝑐3 = [0 0

𝜃̇12 ]𝑇

Động năng của khối tâm thanh nối 3:

K3 =

(

)

1 T
1
1
1
P c3 m3 Pc3 + cT3 I zz3 3 = m3 a1212 + a22122 + 2a1a2112c 2 + r32 + I zz3122
2
2
2
2

Thế năng:


 a1c1 + a2 c12

P3 = −m3GT Pc3 = −m3  0 0 − g   a1s1 + a2 s12
 a
 3 + a4 − r3
 2
24





a3
 = m3 g ( + a4 − r3 )
2


