BÀI tập lớn kĩ THUẬT ROBOT tìm HIỂU ROBOT MPL800II

5/1/2018

BÀI TẬP
LỚN KĨ
THUẬT

TÌM HIỂU ROBOT MPL800II

ROBOT

Mã lớp: 103134 | Nhóm 1


LỜI NĨI ĐẦU
Ngày nay nước ta đang tiến hành cơng nghiệp hóa hiện đại hóa ngày một mạnh mẽ,
dần dần lao động chân tay sẽ được thay thế bằng tự động. Và robot là một lực lượng sinh ra
để giảm lao động cho con người, nó càng ngày càng đóng vai trị quan trọng trong cơng
nghiệp.
Ở trong bài tập lớn này chúng em xin trình bày tìm hiểu của mình về Robot MPL800II
do hãng Yaskawa sản xuất. Tuy vậy, trong lúc tìm hiểu chúng em khơng tránh khỏi những
thiếu sót, mong cô bổ sung và sửa chữa để bài tập lớn của chúng em thêm hoàn thiện.
Cuối cùng, chúng em xin cảm ơn cô Nguyễn Phạm Thục Anh đã tận tình giảng dạy,
giúp đỡ bọn em khi thực hiện bài tập lớn này ạ.

Những người thực hiện
Nhóm 1

1


MỤC LỤC

CHƯƠNG 1:

GIỚI THIỆU ROBOT MPL800 II ........................................................... 3

1.1.

Giới thiệu. ........................................................................................................... 3

1.2.

Ứng dụng của Robot MPL800 II trong công nghiệp. ........................................ 4

1.3.

Kết cấu cơ khí. .................................................................................................... 5

1.4.

Thơng số kỹ thuật. .............................................................................................. 7

CHƯƠNG 2:

ĐỘNG HỌC THUẬN CỦA ROBOT MPL800 II ................................... 8

2.1.

Tính tốn cơng thức. ........................................................................................... 8

2.2.

Giao diện lập trình trên Matlab ........................................................................ 10

CHƯƠNG 3:

MA TRẬN JACOBY ............................................................................. 11

3.1.

Tính tốn cơng thức. ......................................................................................... 11

3.2.

Giao diện lập trình Matlab. .............................................................................. 12

CHƯƠNG 4:

ĐỘNG HỌC NGƯỢC ROBOT ............................................................. 13

CHƯƠNG 5:

THIẾT KẾ QUỸ ĐẠO CỦA CÁC KHỚP DẠNG BẬC 3 ................... 16

CHƯƠNG 6:

XÂY DỰNG MÔ HÌNH ĐỘNG LỰC HỌC TRÊN TOOLBOX

SIMMECHANICS/MATLAB .......................................................................................... 21
6.1.

Xây dựng phương trình động lực học cho Robot MPL800 II (3 góc đầu tiên):

………………………………………………………………………………..21

6.2.

Mơ hình 3D của tay máy Robot ....................................................................... 23

6.3.

Mơ hình Robot trên Simmechanics/Matlab. .................................................... 26

PHỤ LỤC:…….. .......................................................................................................... ….28
Danh mục hình ảnh:........................................................................................................... 28
Danh mục bảng:.. ............................................................................................................... 28
TÀI LIỆU THAM KHẢO ................................................................................................. 29

2


CHƯƠNG 1: GIỚI THIỆU ROBOT MPL800 II
1.1. Giới thiệu.
Robot MPL800 II được sản xuất bởi hãng Robot nổi tiếng Yaskawa. Yaskawa là tập
đoàn hàng đầu thế giới trong sản xuất và cung cấp các sản phẩm trong lĩnh vực robot công
nghiệp, biến tần, truyền động điện…Trong lĩnh vực robot công nghiệp, hãng đã sản xuất tất
cả các loại robot như: robot gắp (Handling), robot nâng bốc, đóng gói (Picking/packing,
palletizing) Robot hàn, hàn điểm (Arc handling, spot welding), Robot sơn (Painting), Robot
lắp ráp ( Assembly/distributing)…

Hinh 1.1: Một số loại Robot của hãng Yaskawa.
Robot cơng nghiệp MPL800 II là loại robot có tốc độ cao, tính linh hoạt và hiệu suất
làm việc lớn, đảm bảo độ tin cậy chính xác.

Hinh 1.2: Robot MPL800 II.
3


Robot MPL800 II có các đặc điểm chính là:
 Số bậc tự do: 4 bậc.
 Có cơ cấu, khung thiết kế vững chắc, có khả năng mang các tải từ 80kg đến 800kg ở tốc
độ cao. Những robot này cho phép đạt được cân bằng quán tính cao nhất cho ứng dụng
bốc xếp hàng.
 Chiều ngang 3.1 m và chiều dọc 3 m cùng khả năng xoay 360 độ, cho phép chúng có thể
tích hợp làm việc cùng lúc với nhiều băng tải và các vị trí xếp pallet khác nhau.
 Ống dẫn khí nén và cáp điều khiển các trục cũng như cáp tín hiệu fieldbus được tích hợp
đi ngầm bên trong tay máy, thiết kế này giúp nâng cao sự an tồn và duy trì sự bền bỉ,
giảm thiểu tối đa khả năng va chạm với các thiết bị ngoại vi.
 Robot MPL tương thích với bộ điều khiển DX200 hoặc nền tảng MLX200 tích hợp PLC.
1.2. Ứng dụng của Robot MPL800 II trong công nghiệp.
Đây là loại robot phù hợp với các ứng dụng về đóng gói (điều khiển dỡ và đóng gói vào
khay, hộp, thùng carton, túi, …).
Các hoạt chất bôi trơn ở trong hộp số robot dịng MPL được chứng nhận tiêu chuẩn an
tồn NSF-H1. Đây là tiêu chuẩn đặc biệt quan trọng trong ngành thực phẩm và đồ uống. Do
vậy Robot MPL có thể áp dụng rộng rãi trong nhiều ngành công nghiệp như: thực phẩm và
nước giải khát, nhà kho và các sản phẩm công nghiệp khác.

Hinh 1.3: Ứng dụng của Robot MPL800 II trong cơng nghiệp.
Tuy nhiên thì robot MPL là dịng robot chuyên dụng, thích hợp và được ưu tiên dùng
cho các ứng dụng nâng bốc, di chuyển hàng có khối lượng lớn lên đến 800kg, sử dụng trong

4

rất nhiều ngành như: nước giải khát, thực phẩm, gạch, xi măng…do các đặc điểm chuyên
dụng của robot MPL.
1.3. Kết cấu cơ khí.
Kết cấu cơ khí của robot thể hiện như trong hình vẽ:

Hinh 1.4: Robot MPL800 II khi nhìn từ trên xuống.
Kết cấu cơ khí của Rotbot MPL800 II nhìn từ đằng sau:

Hinh 1.5: Robot MPL800 II khi nhìn từ đằng sau.
5


Kết cấu cơ khí của robot khi nhìn ngang:

Hinh 1.6: Robot MPL800 II khi nhìn ngang.

6


1.4. Thơng số kỹ thuật.
Các thơng số kỹ thuật chính của Robot MPL800 II được trình bày trong bảng sau:
Bảng 1.1: Bảng thơng số kỹ thuật chính của Robot MPL800 II.
Mẫu
Kiểu
Trục điều khiển
Tải trọng nâng
Khả năng lặp lại
Phạm vi chuyển động

Tốc độ tối đa
Quán tính cho phép
Khối lượng

Trục S – quay
Trục L – cánh tay dưới
Trục U – cánh tay trên
Trục T – cổ tay
Trục S – quay
Trục L – cánh tay dưới
Trục U – cánh tay trên
Trục T – cổ tay
Trục T – cổ tay
Nhiệt độ
Độ ẩm
Độ rung

Điều kiện môi trường
Khác

Yêu cầu nguồn

7

MOTOMAN-MPL800 II
YR-MPL0800-J00
4 khớp nối theo thiều dọc
800kg
±0.5mm
-180˚ – +180˚

-45˚ – +90˚
-120˚ – +15˚
-360˚ – +360˚
1.13 rad/s, 65˚/s
1.13 rad/s, 65˚/s
1.13 rad/s, 65˚/s
2.18 rad/s, 125˚/s
500kg৹m2
2550kg
0˚C đến +45˚C
20 - 80% RH (khơng ngưng tụ)
4.9m/𝑠2 hoặc ít hơn
Khơng có khí hoặc chất lỏng
ăn mịn, hoặc các khí gây nổ
Khơng có nước, dầu hoặc bụi
Khơng có nhiễu điện q mức
(plasma)
10kVA


CHƯƠNG 2: ĐỘNG HỌC THUẬN CỦA ROBOT
MPL800 II
2.1. Tính tốn cơng thức.
Y1

Y2

a1=270

a2=1250

a3=1650

Z1

X1

X2

X3,4

Z2
Y3,4

d1= 880
Z0

Z3,4

Y0
X0

Hinh 2.1: Mơ hình Robot MPL800 II
Bảng 2.1: Bảng Denavit-Hartenberg.
i

𝑎𝑖

𝛼𝑖

𝜃𝑖

𝑑𝑖

1

𝑎1 = 270

90°

𝜃1

𝑑1 = 880

2

𝑎2 = 1250

0

𝜃2

0

3

𝑎3 = 1650

90°

𝜃3

0

0

𝜃4

0

4

0

Ta xét:
𝑇10 = 𝑅𝑜𝑡𝑧 (𝜃1 ).𝑡𝑟𝑎𝑛𝑧 (𝑑1 ). 𝑡𝑟𝑎𝑛𝑥 (𝑎1 ).𝑅𝑜𝑡𝑥 (𝛼1 )

1
0
⁡[
0
0

cos⁡(𝜃1 ) −sin⁡(𝜃1 ) 0
sin⁡(𝜃1 ) cos⁡(𝜃1 ) 0
=[
0
0
1
0

0
0
0
0
0
cos⁡(90°) −sin⁡(90°) 0
]
sin⁡(90°) cos⁡(90°) 0
0
0
1

0 1
0].⁡[0
0 0
1 0

0
1
0
0

0
0
1
0

0
1
0

0
].⁡[
𝑑1 0
1
0

0
1
0
0

0
0
1
0

𝑎1
0
].
0
1
(2.1)

8


𝑇21 = 𝑅𝑜𝑡𝑧 (𝜃2 .𝑡𝑟𝑎𝑛𝑥 (𝑎2 )
cos⁡(𝜃2 )
sin⁡(𝜃2 )
=[

0
0
cos⁡(𝜃2 )
sin⁡(𝜃2 )
=[
0
0
cos⁡(𝜃2 )
sin⁡(𝜃2 )
=[
0
0

−sin⁡(𝜃2 )
cos⁡(𝜃2 )
0
0
−sin⁡(𝜃2 )
cos⁡(𝜃2 )
0
0
−sin⁡(𝜃2 )
cos⁡(𝜃2 )
0
0

0
0
1
0

0
0
1
0
0
0
1
0

0 1 0 0
0 0 1 0
].⁡[
0 0 0 1
1 0 0 0
𝑎2 . cos⁡(𝜃2 )
𝑎2 ⁡. sin⁡(𝜃2 )
]
0
1
1250. cos⁡(𝜃2 )
1250. sin⁡(𝜃2 )
]
0
1

𝑇32 ⁡ = ⁡ 𝑅𝑜𝑡𝑧 (𝜃3 ). 𝑡𝑟𝑎𝑛𝑥 (𝑎3 ). 𝑅𝑜𝑡𝑥 (𝛼3 )
cos⁡(𝜃3 ) −sin⁡(𝜃3 ) 0 0 1 0 0 𝑎3 ⁡ 1
0
sin⁡(𝜃3 ) cos⁡(𝜃3 ) 0 0 0 1 0 0
=⁡[

][
].[
0
0
0
1 0 0 0 1 0
0
0
0
0 1 0 0 0 1
cos⁡(𝜃3 ) 0 sin⁡(𝜃3 ) 𝑎3 . cos⁡(𝜃3 )
sin⁡(𝜃3 ) 0 −cos⁡(𝜃3 ) 𝑎3 . sin⁡(𝜃3 )
=[
]
0
1
0
0
0
0
0
1

cos⁡(𝜃4 )
sin⁡(𝜃4 )
𝑇43 = 𝑅𝑜𝑡𝑧 (𝜃4 ) = [
0
0

−sin⁡(𝜃4 )

cos⁡(𝜃4 )
0
0

0
0
1
0

𝑎2 ⁡
0
]
0
1

0
0
cos⁡(90°) −sin⁡(90°)
sin⁡(90°) cos⁡(90°)
0
0

(2.2)

0
0
]
0
1

0
0]
0
1

(2.3)

(2.4)

Vậy ta có ma trận:
𝑇40 =𝑇10 .⁡𝑇21 .⁡𝑇32 . 𝑇43 =
cos⁡(𝜃1 ) 0 sin⁡(𝜃1 ) 𝑎1 . cos⁡(𝜃1 ) cos⁡(𝜃2 ) −sin⁡(𝜃2 ) 0 𝑎2 . cos⁡(𝜃2 )
sin⁡(𝜃1 ) 0 −cos⁡(𝜃1 ) 𝑎1 . sin⁡(𝜃1 ) sin⁡(𝜃2 ) cos⁡(𝜃2 ) 0 𝑎2 . sin⁡(𝜃2 )
[
].⁡[
].
0
1
0
𝑑1
0
0
1
0
0
0
0
1
0
0

0
1
cos⁡(𝜃3 ) 0 sin⁡(𝜃3 ) 𝑎3 . cos⁡(𝜃3 ) cos⁡(𝜃4 ) −sin⁡(𝜃4 ) 0 0
sin⁡(𝜃3 ) 0 −cos⁡(𝜃3 ) 𝑎3 . sin⁡(𝜃3 ) sin⁡(𝜃4 ) cos⁡(𝜃4 ) 0 0
⁡[
].⁡[
]=
0
1
0
0
0
0
1 0
0
0
0
1
0
0
0 1

𝑎11
𝑎
[𝑎21
31
𝑎41

𝑎12
𝑎23

𝑎32
𝑎42

𝑎13
𝑎23
𝑎33
𝑎43

𝑎14
𝑎24
𝑎34 ]
𝑎44

9

(2.5)


Trong đó:
𝑎11 = cos⁡(𝜃1 ).⁡cos⁡(𝜃4 ).⁡cos⁡(𝜃2 + 𝜃3 ) + sin⁡(𝜃1 ).⁡sin⁡(𝜃4 )
𝑎12 = - cos⁡(𝜃1 ).⁡cos⁡(𝜃2 + 𝜃3 ).⁡sin⁡(𝜃4 ) + sin⁡(𝜃1 ).⁡cos⁡(𝜃4 )
𝑎13 = cos⁡(𝜃1 ).⁡sin⁡(𝜃2 + 𝜃3 )
𝑎14 = 𝑎3 .cos⁡(𝜃1 ).⁡cos⁡(𝜃2 + 𝜃3 ) + 𝑎2 .cos⁡(𝜃1 ).⁡cos⁡(𝜃2 ) + 𝑎1 . cos⁡(𝜃1 )
= 1650 cos⁡(𝜃1 ).⁡cos⁡(𝜃2 + 𝜃3 ) + 1250⁡cos⁡(𝜃1 ).⁡cos⁡(𝜃2 ) + 270⁡cos⁡(𝜃1 )
𝑎21 = sin⁡(𝜃1 ).cos(𝜃2 + 𝜃3 ) . cos⁡(𝜃4 ) - cos⁡(𝜃1 ).⁡sin⁡(𝜃4 )
𝑎22 = - sin⁡(𝜃1 ).cos(𝜃2 + 𝜃3 ) . sin⁡(𝜃4 ) - cos⁡(𝜃1 ).⁡cos⁡(𝜃4 )
𝑎23 = sin⁡(𝜃1 ).⁡sin⁡(𝜃2 + 𝜃3 )
𝑎24 = 𝑎3 . sin⁡(𝜃1 ).⁡cos⁡(𝜃2 + 𝜃3 ) + 𝑎2 . sin⁡(𝜃1 ).⁡cos⁡(𝜃2 ) + 𝑎1 . sin⁡(𝜃1 )
= 1650 sin⁡(𝜃1 ).⁡cos⁡(𝜃2 + 𝜃3 ) + 1250⁡sin⁡(𝜃1 ).⁡cos⁡(𝜃2 ) + 270⁡sin⁡(𝜃1 )
𝑎31 = sin(𝜃2 + 𝜃3 ) . cos⁡(𝜃4 )

𝑎32 = - sin(𝜃2 + 𝜃3 ) . sin⁡(𝜃4 )
𝑎33 = - cos(𝜃2 + 𝜃3 )
𝑎34 = 𝑎3 .sin(𝜃2 + 𝜃3 ) + 𝑑1 + 𝑎2 . sin⁡(𝜃2 )
= 1650 sin(𝜃2 + 𝜃3 ) + 880 + 1250⁡sin⁡(𝜃2 )
𝑎41 = 0
𝑎42 = 0
𝑎43 = 0
𝑎44 = 1
2.2. Giao diện lập trình trên Matlab

Hinh 2.2: Giao diện lập trình Matlab tính động học thuận.
10


CHƯƠNG 3: MA TRẬN JACOBY
3.1. Tính tốn cơng thức.
Tìm các ma trận 𝑇40 , 𝑇41 , 𝑇42 , 𝑇43 .
𝑇40 =
𝐶1 𝐶23 𝐶4 + 𝑆1 𝑆4
𝑆 𝐶 𝐶 − 𝐶1 𝑆4
[ 1 23 4
𝑆23 𝐶4
0

−𝐶1 𝐶23 𝑆4 + 𝑆1 𝐶4
−𝑆1 𝐶23 𝑆4 − 𝐶1 𝐶4
−𝑆23 𝑆4
0

𝑪𝟐𝟑 𝑪𝟒

𝑺 𝑪
𝑻𝟏𝟒 = [ 𝟐𝟑 𝟒
𝑺𝟒
𝟎
𝐶3 𝐶4
𝑆 𝐶
𝑇42 = [ 3 4
𝑆4
0
𝐶4
𝑆
𝑇43 = [ 4
0
0

−𝑪𝟐𝟑 𝑺𝟒
−𝑺𝟐𝟑 𝑺𝟒
𝑪𝟒
𝟎

−𝐶3 𝑆4
−𝑆3 𝑆4
𝐶4
0
−𝑆4
𝐶4
0
0

0

0
1
0

𝑆3
−𝐶3
0
0

𝑺𝟐𝟑
−𝑪𝟐𝟑
𝟎
𝟎

𝐶1 𝑆23
𝑆1 𝑆23
−𝐶23
0

𝐶1 (𝑎3 𝐶23 + 𝑎2 𝐶2 + 𝑎1 𝐶1 )
𝑆1 (𝑎3 𝐶23 + 𝑎2 𝐶2 + 𝑎1 𝐶1 )
]
𝑎3 𝑆23 + 𝑑1 + 𝑎2 𝑆2
1

𝒂𝟑 𝑪𝟐𝟑 + 𝒂𝟐 𝑪𝟐
𝒂𝟑 𝑺𝟐𝟑 + 𝒂𝟐 𝑺𝟐
]
𝟎
𝟏

(3.1)

(3.2)

𝑎3 𝐶3
𝑎3 𝑆3
]
0
1

(3.3)

0
0
]
0
1

(3.4)

Vậy ma trận 𝐽𝐻 là:
−𝑆4 (𝑎3 𝐶23 + 𝑎2 𝐶2 + 𝑎1 𝐶1 )
−𝐶4 (𝑎3 𝐶23 + 𝑎2 𝐶2 + 𝑎1 𝐶1 )
0
𝐽𝐻 =
𝑆23 𝐶4
−𝑆23 𝑆4
[
−𝐶23

𝑎 2 𝑆3 𝐶 4
−𝑎2 𝑆3 𝑆4 ⁡⁡⁡
−𝑎3 − 𝑎2 𝐶3 ⁡⁡
𝑆4
𝐶4
0

11

0
0
−𝑎3 ⁡⁡
𝑆4
𝐶4
0

0
0
0
0
0
1]

(3.5)


Vậy ma trận Jacoby là:
𝑅40
𝐽=[

0

0
𝐻
0 ⁡] . 𝐽
𝑅4

−𝑆1 (𝑎3 𝐶23 + 𝑎2 𝐶2 + 𝑎1 𝐶1 ) 𝐶1 (−𝑎3 𝑆23 −𝑎2 𝐶2 ) −𝐶1 𝑎3 𝑆23
𝐶1 (𝑎3 𝐶23 + 𝑎2 𝐶2 + 𝑎1 𝐶1 ) 𝑆1 (−𝑎3 𝑆23 −𝑎2 𝑆2 ) −𝑆1 𝑎3 𝑆23
0
𝑎3 𝐶23 +𝑎2 𝐶2 ⁡⁡
𝑎3 𝐶23 ⁡⁡
=
0
𝑆1
𝑆1
0
−𝐶1
−𝐶1
[
1
0
0

0
0
0
𝐶1 𝑆23
𝑆1 𝑆23
−𝐶23 ]

(3.3)

Thay số a1 = 270, a2 =1250, a3 =1650 ta được:
𝐽
−𝑆1 (1650𝐶23 + 1250𝐶2 + 270𝐶1 )
𝐶1 (1650𝐶23 + 1250𝐶2 + 270𝐶1 )
0
=
0
0
[
1

𝐶1 (−1650𝑆23 − 1250𝐶2 )
𝑆1 (−1650𝑆23 − 1250𝑆2 )
1650𝐶23 + 1250𝐶2 ⁡⁡
𝑆1
−𝐶1
0

−𝐶1 1650𝑆23
−𝑆1 1650𝑆23
1650𝐶23 ⁡⁡
𝑆1
−𝐶1
0

3.2. Giao diện lập trình Matlab.

Hinh 3.1: Giao diện lập trình Matlab tính ma trận Jacoby.

12

0
0
0
𝐶1 𝑆23
𝑆1 𝑆23
−𝐶23 ]

(3.4)


CHƯƠNG 4: ĐỘNG HỌC NGƯỢC ROBOT
𝑛𝑥
𝑛
T40 = T10 . T21 . T32 . T43 = [ 𝑦
𝑛𝑧
0

𝑜𝑥
𝑜𝑦
𝑜𝑧
0

𝑎𝑥
𝑎𝑦
𝑎𝑧
0

𝑝𝑥
𝑝𝑦
]
𝑝𝑧
0

(4.1)

Với:
𝑛𝑥 = cos⁡(θ1 ).⁡cos⁡(θ4 ).⁡cos⁡(θ2 + θ3 ) + sin⁡(θ1 ).⁡sin⁡(θ4 )
𝑜𝑥 = - cos⁡(θ1 ).⁡cos⁡(θ2 + θ3 ).⁡sin⁡(θ4 ) + sin⁡(θ1 ).⁡cos⁡(θ4 )
𝑎𝑥 = cos⁡(θ1 ).⁡sin⁡(θ2 + θ3 )
𝑝𝑥 = a3. cos⁡(θ1 ).⁡cos⁡(θ2 + θ3 ) + a2.⁡cos⁡(θ1 ).⁡cos⁡(θ2 ) + a1.⁡cos⁡(θ1 )
𝑛𝑦 = sin⁡(θ1 ).cos(θ2 + θ3 ) . cos⁡(θ4 ) - cos⁡(θ1 ).⁡sin⁡(θ4 )
𝑜𝑦 = - sin⁡(θ1 ).cos(θ2 + θ3 ) . sin⁡(θ4 ) - cos⁡(θ1 ).⁡cos⁡(θ4 )
𝑎𝑦 = sin⁡(θ1 ).⁡sin⁡(θ2 + θ3 )
𝑝𝑦 = a3. sin⁡(θ1 ).⁡cos⁡(θ2 + θ3 ) + a2.⁡sin⁡(θ1 ).⁡cos⁡(θ2 ) + a1.⁡sin⁡(θ1 )
𝑛𝑧 = sin(θ2 + θ3 ) . cos⁡(θ4 )
𝑜𝑧 = - sin(θ2 + θ3 ) . sin⁡(θ4 )
𝑎𝑧 = - cos(θ2 + θ3 )
𝑝𝑧 = a3. sin(θ2 + θ3 ) + d1 + a2.⁡sin⁡(θ2 )
Áp dụng phương pháp phân ly biến:
𝑉𝑇 = (𝑇10 )−1 𝑇40 = 𝑇10 . 𝑇21 . 𝑇32 . 𝑇43 = 𝑉𝑃⁡⁡

(4.2)

Ta có:
cos(θ1 )
sin(θ1 )

𝑇10 = ⁡⁡ [
0
0

0 sin(θ1 ) 𝑎1 . cos(θ1 )
0 −cos(θ1 ) 𝑎1 . sin(θ1 )
]
1
0
𝑑1
0
0
1

(4.3)

Suy ra:
cos(θ1 ) sin(θ1 )
0
0
(T10 )−1 = ⁡ [
sin(θ1 ) −cos(θ1 )
0
0

13

0
1
0

0

−𝑎1
−𝑑1
]
0
1

(4.4)


VT1
cos(θ1 ). nx⁡ + sin(θ1 ). ny⁡
nz⁡
=⁡
sin(θ1 ). nx⁡ − cos(θ1 ). ny⁡
[
0

cos(θ1 ). ox⁡ + sin(θ1 ). oy⁡
oz⁡
sin(θ1 ). ox⁡ − cos(θ1 ). oy⁡
0

cos(θ1 ). ax⁡ + sin(θ1 ). ay⁡
az⁡
sin(θ1 ). ax⁡ − cos(θ1 ). ay⁡
0

cos(θ2 + θ3 ). cos(θ4 ) −cos(θ2 + θ3 ). sin(θ4 )

sin(θ2 + θ3 ). cos(θ4 ) −sin(θ2 + θ3 ). sin(θ4 )
VP1 = ⁡ [
sin(θ4 )
cos(θ4 )
0
0

cos(θ1 ). px⁡ + sin(θ1 ). py⁡ − ⁡ a1
pz⁡ − d1
sin(θ1 ). px⁡ − cos(θ1 ). py⁡
]
1

(4.5)

sin(θ2 + θ3 ) a3 cos(θ2 + θ3 ) + ⁡ a2 cos(θ2 )
−cos(θ2 + θ3 ) a3 sin(θ2 + θ3 ) + ⁡ a2 sin(θ2 )
]
0
0
0
1

(4.6)

Cân bằng 2 vế ta có các phương trình sau:
sin(θ1 ). px⁡ − cos(θ1 ). py⁡ = 0⁡

(4.7)

Suy ra:
θ1 = ATAN2(py⁡ , px )
θ1 = ATAN2(−py⁡ , −px )

{

sin(θ4 ) = sin(θ1 ). nx⁡ − cos(θ1 ). ny⁡ ⁡
cos(θ4 ) = ⁡sin(θ1 ). ox⁡ − cos(θ1 ). oy⁡

(4.8)

Suy ra:
θ4 = ATAN2(sin(θ1 ). nx⁡ − cos(θ1 ), sin(θ1 ). ox⁡ − cos(θ1 ). oy⁡ )
{

cos(θ1 ). px⁡ + sin(θ1 ). py⁡ − ⁡ a1 = a3 cos(θ2 + θ3 ) + ⁡ a2 cos(θ2 )⁡
pz⁡ − d1 = ⁡ a3 sin(θ2 + θ3 ) + ⁡ a2 sin(θ2 )

(4.9)

Đặt:
{

m = 𝑎3 cos(θ2 + θ3 ) + ⁡ 𝑎2 cos(θ2 )⁡
n = ⁡ 𝑎3 sin(θ2 + θ3 ) + ⁡ 𝑎2 sin(θ2 )

Bình phương cả 2 vế ta được:
m2 = ⁡ (a3 cos(θ2 + θ3 ))2 + ⁡ 2. a3 cos(θ2 + θ3 ). a2 cos(θ2 ) + ⁡ (a2 cos(θ2 ))2 ⁡
{ 2
n = ⁡ (a3 sin(θ2 + θ3 ))2 + ⁡ 2. a3 sin(θ2 + θ3 ). a2 sin(θ2 ) + ⁡ (a2 sin(θ2 ))2

Cộng hai phương trình ta được:
m2 + n2 = ⁡ a3 2 + ⁡ a2 2 + 2a3 a2 ⁡cos(θ3 )
Suy ra:
m2 + n2 − a3 2 − ⁡ a2 2
cos(θ3 ) = ⁡
2a3 a2
sin(θ3 ) = ±√1 − (cos(θ3 ))2
⇒ θ3 = ATAN2(sin(θ3 ), cos(θ3 ))
14

(4.10)


Sử dụng phép đảo vị trị Robot Planar ta có:
θ2 = 𝐴𝑇𝐴𝑁2(𝑝2 , 𝑝1 ) − 𝐴𝑇𝐴𝑁2(𝑎3 . sin(θ3 ) , 𝑎2 + 𝑎3 cos(θ3 ))
Trong đó:
𝑝1 = ⁡cos(θ1 ). px⁡ + sin(θ1 ). py⁡ − ⁡ a1
𝑝2 = ⁡ pz⁡ − d1
Điều kiện góc quay của từng khớp:
180  1  180
0    135

2

120  3  15.5
360   4  360

Kết luận giá trị góc 1,2,3,4 chỉ phụ thuộc vào px ,⁡py ,⁡pz ,⁡nx ,⁡ny , ox ,⁡oy ,
Điều kiện các thông số nhập vào phải thỏa mãn các điều kiện sau:
−(𝑎1 + 𝑎2 + 𝑎3 ) ≤ 𝑝𝑥 , 𝑝𝑦 ≤ 𝑎1 + 𝑎2 + 𝑎3

𝑑1 ≤ 𝑝𝑧 ≤ 𝑎1 + 𝑎2 + 𝑑1
−1 ≤ 𝑛𝑥 , 𝑛𝑦 , 𝑜𝑥 , 𝑜𝑦 ≤ 1
Và các giá trị vecto n, o, a tương ứng tạo thành một tam diện thuận.

15


CHƯƠNG 5: THIẾT KẾ QUỸ ĐẠO CỦA CÁC
KHỚP DẠNG BẬC 3
Quỹ đạo của từng khớp có dạng:
𝜃𝑖 (𝑡) = 𝑎𝑖0 + ⁡ 𝑎𝑖1 𝑡 + 𝑎𝑖2 𝑡 2 + 𝑎𝑖3 𝑡 3

(5.1)

Vận tốc tại mỗi khớp:
𝜃̇𝑖 (𝑡 ) = 𝑎𝑖1 + 2𝑎𝑖2 𝑡 + 3𝑎𝑖3 𝑡 2

(5.2)

̅̅̅̅ tương ứng với 4 khớp.
Với 𝑖 = 1,4
Ta được hệ các phương trình sau:
𝜃1 (𝑡) = 𝑎10 + ⁡ 𝑎11 𝑡 + 𝑎12 𝑡 2 + 𝑎13 𝑡 3
𝜃2 (𝑡) = 𝑎20 + ⁡ 𝑎21 𝑡 + 𝑎22 𝑡 2 + 𝑎23 𝑡 3
2

𝜃3 (𝑡) = 𝑎30 + ⁡ 𝑎31 𝑡 + 𝑎32 𝑡 + 𝑎33 𝑡

3

(5.3)

𝜃4 (𝑡) = 𝑎40 + ⁡ 𝑎41 𝑡 + 𝑎42 𝑡 2 + 𝑎43 𝑡 3
Giả sử Robot đi từ điểm A tới điểm B trong thời gian là t(s) với điều kiện là vận tốc tại
2 thời điểm đầu và cuối bằng 0.
Từ đó ta có hệ phương trình sau:
𝜃𝑖 (0) = 𝑎𝑖0 = 𝜃𝑖 (𝐴)⁡
𝜃𝑖 (𝑡 ) = 𝑎𝑖0 + ⁡ 𝑎𝑖1 𝑡 + 𝑎𝑖2 𝑡 2 + 𝑎𝑖3 𝑡 3 = 𝜃𝑖 (𝐵)
𝜃̇𝑖 (0) = 𝑎𝑖1 = 0;

(5.4)

𝜃̇𝑖 (𝑡 ) = 𝑎𝑖1 + 2𝑎𝑖2 𝑡 + 3𝑎𝑖3 𝑡 2 = 0
Từ đây ta sẽ xây dựng được quỹ đạo dạng bậc 3 cho từng khớp. Sử dụng động học đảo
ta có:
√2

Khi Robot di chuyển từ vị trí A(𝑝𝑥 , ⁡𝑝𝑦 , 𝑝𝑧 , 𝑛𝑥 , 𝑛𝑦, ⁡𝑜𝑥 , 𝑜𝑦⁡ ) = (270, 0, 3780, 0, - , 0,
2

−

√2
2

) suy ra các góc theta tương ứng là:
𝑞𝐴 = [𝜃1𝐴 , 𝜃2𝐴 , 𝜃3𝐴 , 𝜃4𝐴 ]𝑇 = [⁡0, 90, 0, 45]𝑇

(5.5)

Vị trí điểm B(𝑝𝑥 , ⁡𝑝𝑦 , 𝑝𝑧 , 𝑛𝑥 , 𝑛𝑦, ⁡𝑜𝑥 , 𝑜𝑦⁡ ) = (2009.7069, 1160.3048, 597.1573,
0.28033, 0.7392, 0.7392, -0.57322) suy ra các góc theta tương ứng là:
𝑞𝐵 = [𝜃1𝐵 , 𝜃2𝐵 , 𝜃3𝐵 , 𝛳𝜃4𝐵 ]𝑇 = [30, 45, −90, −30]𝑇
trong thời gian t = 5s.

16

(5.6)


Từ đây ta có được:
 Phương trình quỹ đạo của khớp 1 với các hệ số như sau:
𝑎10 = 𝜃1 (𝐴) = 0
𝑎11 = 0
3
3
(
(
)
)
(30 − 0) = 3.6
𝜃
𝐵
−
⁡
𝜃
(𝐴)
=
⁡
1

1
𝑡2
52
2
2
= ⁡ − 3 (𝜃1 (𝐵) − ⁡ 𝜃1 (𝐴)) = ⁡⁡ − 3 (30 − 0) = −0.48
𝑡
5
𝑎12 = ⁡

𝑎13

(5.7)

Ta có phương trình quỹ đạo khớp 1:
𝜃1 (𝑡 ) = 3.6𝑡 2 − 0.48𝑡 3

(5.8)

𝜃̇1 (𝑡 ) = ⁡7.2𝑡 − 1.44𝑡 2

(5.9)

𝜃̈1 (𝑡 ) = 7.2 − 2.88𝑡

(5.10)

Phương trình vận tốc:

Phương trình gia tốc:

Kết quả mơ phỏng trên Matlab:

Hinh 5.1: Đồ thị vị trí, vận tốc, gia tốc khớp thứ nhất.

17


 Phương trình quỹ đạo của khớp 2 với các hệ số như sau:
𝑎20 = 𝛳2 (𝐴) = 90;
𝑎21 = 0;
𝑎22 = ⁡

3
3
(𝛳2 (𝐵) − ⁡ 𝛳1 (𝐴)) = ⁡ 2 ∗ (45 − 90) = −5.4;
2
𝑡
5

𝑎23 = ⁡ −

(5.11)

2
2
(
)
(
)

(45 − 90) = 0.72;
(𝛳
)
𝐵
−
⁡
𝛳
𝐴
=
⁡⁡
−
2
2
𝑡3
53

Ta có phương trình quỹ đạo khớp 2:
𝛳2 (𝑡 ) = 90 − 5.4𝑡 2 + 0.72𝑡 3

(5.12)

Phương trình vận tốc:
𝛳̇2 (𝑡 ) = ⁡ −10.8𝑡 + 2.16𝑡 2

(5.13
)

Phương trình gia tốc:
𝛳̈2 (𝑡 ) = −10.8 + 4.32𝑡
Kết quả mô phỏng trên Matlab:

Hinh 5.2: Đồ thị vị trí, vận tốc, gia tốc khớp thứ hai.

18

(5.14)


 Phương trình quỹ đạo của khớp 3 với các hệ số như sau:
𝑎30 = 𝜃3 (𝐴) = 0
𝑎31 = 0
3
3
(
(
)
)
(−90 − 0) = −10.8
𝜃
𝐵
−
⁡
𝜃
(𝐴)
=
⁡
3
3
𝑡2
52

2
2
= ⁡ − 3 (𝜃3 (𝐵) − ⁡ 𝜃3 (𝐴)) = ⁡⁡ − 3 (−90 − 0) = 1.44
𝑡
5

𝑎32 = ⁡
𝑎33

(5.15)

Ta có phương trình quỹ đạo khớp 3:
𝜃3 (𝑡 ) = −10.8𝑡 2 + 1.44𝑡 3

(5.16)

Phương trình vận tốc:
𝜃̇3 (𝑡 ) = ⁡ −21.6𝑡 + 4.32𝑡 2

(5.17)

Phương trình gia tốc:
𝜃̈3 (𝑡 ) = −21.6 + 8.64𝑡
Kết quả mơ phỏng trên Matlab:

Hinh 5.3: Đồ thị vị trí, vận tốc, gia tốc khớp thứ ba.

19

(5.1

8)


 Phương trình quỹ đạo của khớp 4 với các hệ số như sau:
𝑎40 = 𝜃4 (𝐴) = 45
𝑎41 = 0
3
3
(
(
)
)
(−30 − 45) = −9
𝜃
𝐵
−
⁡
𝜃
(𝐴)
=
⁡
4
4
𝑡2
52
2
2
= ⁡ − 3 (𝜃4 (𝐵) − ⁡ 𝜃4 (𝐴)) = ⁡⁡ − 3 (−30 − 45) = 1.2
𝑡
5

𝑎42 = ⁡
𝑎43

(5.19)

Ta có phương trình quỹ đạo khớp 4:
𝜃4 (𝑡 ) = 45 − 9𝑡 2 + 1.2𝑡 3

(5.20)

𝜃̇4 (𝑡) = ⁡ −18𝑡 + 3.6𝑡 2

(5.21)

Phương trình vận tốc:

Phương trình gia tốc:
𝜃̈4 (𝑡) = −18 + 7.2𝑡
Kết quả mơ phỏng trên Matlab:

Hinh 5.4: Đồ thị vị trí, vận tốc, gia tốc khớp thứ tư.

20

(5.22
)


CHƯƠNG 6: XÂY DỰNG MƠ HÌNH ĐỘNG LỰC

HỌC TRÊN TOOLBOX
SIMMECHANICS/MATLAB
Toolbox Simmechanics/Matlab là một phần mềm cho phép người dùng mô phỏng được
các chi tiết cơ khí, từ đó xây dựng được mơ hình các bộ phận máy, các máy móc cơ học của
robot.
6.1. Xây dựng phương trình động lực học cho Robot MPL800 II (3 góc đầu tiên):
Cơng thức tính ma trận quán tính là:
3
𝑇
𝑇
𝑀 (𝑞) = ∑(𝑚𝑖 𝐽𝑇𝑖
𝐽𝑇𝑖 + 𝐽𝑅𝑖
𝐴𝑖 𝐼𝑖 𝐴𝑇𝑖 𝐽𝑅𝑖 )

(6.1)

𝑖=1

Với:
 𝑚𝑖 là khối lượng của khâu thứ 𝑖.
 𝐽𝑇𝑖 là ma trận Jacoby tịnh tiến tương ứng với tọa độ suy rộng thứ 𝑖.
 𝐽𝑅𝑖 là ma trận Jacoby quay tương ứng với tọa độ suy rộng thứ 𝑖.
 𝐴𝑖 là ma trận cosin chỉ hướng của hệ trục tọa độ thứ 𝑖 so với hệ trục tọa độ cố định.
 𝐼𝑖 là ma trận momen quán tính khối đi qua tâm của khâu thứ 𝑖.
𝑚11
𝑚
𝑀 (𝑞) = [ 21
𝑚31

𝑚12

𝑚22
𝑚32

𝑚13
𝑚23 ]
𝑚33

(
6.2)

Với:
2
2
2 2
 𝑚11 = 𝐼1𝑧 + 𝐼2𝑦 𝑐22 + 𝐼2𝑥 𝑠22 + 𝐼3𝑥 𝑠23
+ 𝐼3𝑦 𝑐23
+ 𝑚2 𝑙𝐶2
𝑐2 + 𝑚2 (2𝑎2 𝑙𝐶3 𝑐2 𝑐23 + 𝑎22 𝑐22 +
2 2 )
𝑙𝐶3
𝑐23

 𝑚12 = 𝑚13 = 𝑚21 = 𝑚31 = 0
2
2
 𝑚22 = 𝐼2𝑧 + 𝐼3𝑧 + 𝑚2 𝑙𝐶2
+ 𝑚3 (𝑙𝐶3
+ 𝑎22 + 2𝑎2 𝑙𝐶3 𝑐3 )
2
 𝑚23 = 𝑚32 = 𝐼3𝑧 + 𝑚3 (𝑙𝐶3

+ 𝑎2 𝑙𝐶3 𝑐3 )
2
 𝑚33 = 𝐼3𝑧 + 𝑚3 𝑙𝐶3

Cơng thức tính ma trận ly tâm Coriolis:
𝑇

𝐶 (𝑞, 𝑞̇ ) =

𝜕𝑀 (𝑞)
𝜕𝑞

1 𝜕𝑀 (𝑞)
(𝐼𝑛 ⊗ 𝑞̇ ) − (
(𝑞̇ ⊗ 𝐼𝑛 ))
2
𝜕𝑞
21

(
6.3)


𝑐11
𝐶 (𝑞, 𝑞̇ ) = [𝑐21
𝑐31

𝑐12
𝑐22
𝑐32

𝑐13
𝑐23 ]
𝑐33

(6.4
)

Với:
2
2
 𝑐11 = −2𝑚2 𝑙𝐶2
𝑠2 𝑐2 𝑞̇ 2 − 2𝑚3 (𝑙𝐶3
𝑠23 𝑐23 + 𝑎22 𝑠2 𝑐2 + 𝑎2 𝑙𝐶3 (2𝑠2 𝑐23 − 𝑠3 ))𝑞̇ 2 −
2
2𝑚3 (𝑙𝐶3
𝑠23 𝑐23 + 𝑎2 𝑙𝐶3 𝑐2 𝑠23 )𝑞̇ 3 + 2(𝐼2𝑥 − 𝐼2𝑦 )𝑠2 𝑐2 𝑞̇ 2 + 2(𝐼3𝑥 − 𝐼3𝑦 )𝑠23 𝑐23 (𝑞̇ 2 + 𝑞̇ 3 )

 𝑐12 = 𝑐13 = 0
2
2
 𝑐21 = (𝑚2 𝑙𝐶2
𝑠2 𝑐2 + 𝑚3 (𝑙𝐶3
𝑠23 𝑐23 + 𝑎22 𝑠2 𝑐2 + 𝑎2 𝑙𝐶3 (2𝑐2 𝑠23 − 𝑠3 ))) 𝑞̇ 1 − (2(𝐼2𝑥 −

𝐼2𝑦 )𝑠2 𝑐2 + (I3𝑥 − 𝐼3𝑦 )𝑠23 𝑐23 )𝑞̇ 1
 𝑐22 = −2𝑚3 𝑙𝐶3 𝑎2 𝑠3 𝑞̇ 3
 𝑐23 = −𝑚3 𝑎2 𝑙𝐶3 𝑠3 𝑞̇ 3
 𝑐31 = (𝑚3 𝑙𝐶3 𝑠23 (𝑎2 𝑐2 + 𝑙𝐶3 𝑐23 ) − (𝐼3𝑥 − 𝐼3𝑦 )𝑠23 𝑐23 )𝑞̇ 1
1

 𝑐32 = 𝑚3 𝑎2 𝑙𝐶3 𝑠3 (2𝑞̇ 2 − 𝑞̇ 3 )
2
1

 𝑐33 = 𝑚3 𝑎2 𝑙𝐶3 𝑠3 𝑞̇ 2
2

Biểu thức thế năng của tay máy (chỉ xét đối với 3 tọa độ suy rộng đầu tiên) là:
Π = 𝑚1 𝑔(𝑑1 − ℎ𝐶1 ) + 𝑚2 𝑔𝑙𝐶2 𝑠2 + 𝑚3 𝑔(𝑎2 𝑠2 + 𝑙𝐶3 𝑠23 )

(
6.5)

Ma trận trọng trường:
𝑇
0
𝜕Π
𝑔 (𝑞) = ( ) = [𝑚2 𝑔𝑙𝐶2 𝑐2 + 𝑚3 𝑔𝑎2 𝑐2 + 𝑚3 𝑔𝑙𝐶3 𝑐23 ]
𝜕𝑞
𝑚 𝑔𝑙 𝑐
3

𝐶3 23

(6.
6)

Phương trình Langrange loại II của tay máy (chỉ xét đối với 3 tọa độ suy rộng đầu tiên)
là:

𝑀 (𝑞) 𝑞̈ + 𝐶 (𝑞, 𝑞̇ ) 𝑞̇ + 𝑔 (𝑞) = 𝜏

22

(
6.7)


6.2. Mơ hình 3D của tay máy Robot

Hinh 6.1: Chân đế tay máy Robot.

Hinh 6.2: Khâu thứ nhất.
23


Hinh 6.3: Khâu thứ hai.

Hinh 6.4: Khâu thứ tư.

24