BÀI TẬP LỚN KỸ THUẬT ROBOT: TÌM HIỂU ROBOT MPL800II

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN ĐIỆN
BỘ MÔN TỰ DỘNG HĨA CƠNG NGHIỆP

----------

BÀI TẬP LỚN MƠN HỌC
KỸ THUẬT ROBOT
Đề tài:

TÌM HIỂU ROBOT MPL800II
GVHD: PGS.TS. Nguyễn Phạm Thục Anh
Nhóm: 1
Lớp: 129135

HÀ NỘI 1/2022


MỤC LỤC
PHÂN CƠNG NHIỆM VỤ ...........................................................................................3
DANH MỤC HÌNH VẼ.................................................................................................4
CHƯƠNG 1. GIỚI THIỆU ROBOT MPL800 II .......................................................5
1.1

Tổng quan về robot:.............................................................................................5

1.2

Giới thiệu .............................................................................................................6

1.3

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

1.4

Kết cấu cơ khí: .....................................................................................................8

1.5

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

CHƯƠNG 2. ĐỘNG HỌC THUẬN VỊ TRÍ CỦA ROBOT MPL800 II ...............12
2.1

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

2.2

Thiết kế giao diện Matlab ..................................................................................14

CHƯƠNG 3. MA TRẬN JACOBY ...........................................................................15
3.1

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

3.2

Lập trình tính tốn trên phần mềm Matlab ........................................................17

CHƯƠNG 4. ĐỘNG HỌC NGƯỢC ROBOT ..........................................................18

CHƯƠNG 5. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO CÁC KHỚP CỦA
ROBOT THEO QUY ĐẠO DẠNG BẬC 3 ...............................................................21
CHƯƠNG 6. THIẾT KẾ ĐIỀU KHIỂN CHUYỂN ĐỘNG CHO ROBOT MPL800
II THEO THUẬNT TỐN PID .................................................................................29
6.1

Tính tốn động lực học cho Robot ....................................................................29

6.2

Thuật tốn điều khiển Robot .............................................................................32

6.3

Mơ phỏng Simcape ............................................................................................34

2


PHÂN CƠNG NHIỆM VỤ
STT

Nhiệm vụ

Thành viên

MSSV

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.

Nguyễn Đắc Quang

20174132

2

Tính tốn động học thuận vị trí
Robot, chương trình trên MatLab
để nhập dữ liệu, hiển thị kết quả

Đồng Quang Vinh

20209507

3

Tính tốn ma trận Jacoby (thơng
qua JH), viêt chương trình trên
MatLab tính ma trận Jacobi

Nguyễn Hữu Thắng

20209506

4

Tính tốn động học đảo vị trí
Robot

Hà Quang Huy

20181523

Bùi Văn Long

20181587

Thái Văn Hóa

20181479

Nguyễn Văn Đức

20181410

Trương Văn Q

20181718

Vũ Đình Đức

20181416

5

6

Thiết kế quỹ đạo chuyển động cho
các khớp của Robot theo quỹ đạo
dạng bậc 3
Thiết kế điều khiển chuyển động
cho Robot theo thuật tốn PID,
xây dựng mơ hình động lực học
cho đối tượng

3


DANH MỤC HÌNH VẼ
Hình 1.1. Robot trong nhà máy .......................................................................................5
Hình 1.2: Một số loại Robot của hãng Yaskawa. ............................................................7
Hình 1.3: Robot MPL800 II. ...........................................................................................7
Hình 1.4: Ứng dụng của Robot MPL800 II trong cơng nghiệp. .....................................8
Hình 1.5. Robot MPL làm việc trong xưởng ...................................................................8
Hình 1.6: Robot MPL800 II khi nhìn từ trên xuống........................................................9
Hình 1.7: Robot MPL800 II khi nhìn từ đằng sau. ..........................................................9
Hình 1.8: Robot MPL800 II khi nhìn ngang. ................................................................10
Hình 2.1 Mơ hình robot MPL800 II ..............................................................................12
Hình 3.1 Chương trình matlab tính ma trận jacoby.......................................................17
Hình 5.1 Mơ phỏng khớp 1 ...........................................................................................22
Hình 5.2 Mơ phỏng khớp 2 ...........................................................................................23
Hình 5.3 Mơ phỏng khớp 3 ...........................................................................................24
Hình 5.4 Mơ phỏng khớp 4 ...........................................................................................25
Hình 6.1 Giao diện cài đặt thơng số ..............................................................................33
Hình 6.2 Kết quả mơ phỏng tọa độ tay máy theo các trục ............................................34

Hình 6.3 Mơ hình 3D chân đế ......................................................................................34
Hình 6.4 Khâu thứ nhất .................................................................................................35
Hình 6.5 Khâu thứ hai ...................................................................................................35
Hình 6.6 Khâu thứ ba ....................................................................................................36
Hình 6.7 Khâu tác động cuối .........................................................................................36
Hình 6.8 Mơ hình hồn chỉnh của robot ........................................................................37
Hình 6.9 Chuyển động khi mơ phỏng 1 ........................................................................37
Hình 6.10 Chuyển động khi mơ phỏng 2 ......................................................................38
Hình 6.11 Chuyển động khi mơ phỏng 3 ......................................................................38

4


CHƯƠNG 1. GIỚI THIỆU ROBOT MPL800 II
1.1 Tổng quan về robot:
Robot ( tiếng Anh: Robot ), "Người máy" là một loại máy có thể thực hiện những
cơng việc một cách tự động bằng sự điều khiển của máy tính hoặc các vi mạch điện tử
được lập trình. Robot là một tác nhân cơ khí, nhân tạo, ảo, thường là một hệ thống cơ
khí-điện tử.

Hình 1.1. Robot trong nhà máy
Robot cơng nghiệp chủ yếu được sử dụng trong các nhà máy với mục đích là sản
xuất các sản phẩm với yêu cầu chính xác cao, làm việc liên tục với năng suất không đổi.
Robot đem lại cho các doanh nghiệp những sản phẩm chất lượng cao, ít bị lỗi và
việc sản xuất liên tục cũng làm cho sản lượng sản phẩm sẽ tăng lên để đáp ứng nhu cầu
của người tiêu dùng.
Phân loại robot trong công nghiệp:
Bảng 1.1 Phân loại robot công nghiệp
Dấu hiệu phân loại

Tên gọi của tay máy

Theo số bậc chuyển động
( kể cả chuyển động rời của cả tay máy)

Có hai hoặc nhiều hơn ở dạng :
• Khơng di chuyển
• Tự chuyển dời

Theo số lượng tay máy

Một, hai hoặc nhiều tay máy được điều
khiển động thời
• Có nguồn dẫn động và điều
khiển riêng
• Có nguồn dẫn động riêng và
được điều khiển chung
• Có chung nguồn dẫn động
• Tự di chuyển

5


Theo tải trọng nâng của tay máy

•
•
•
•
•

Loại siêu nhẹ
Loại nhẹ
Loại trung
Loại nặng
Loại siêu nặng

Theo nguồn dẫn động của các cơ cấu
chấp hành

•
•
•
•

Khí nén
Thủy lực
Cơ điện
Hỗn hợp

Với điều khiển chương trình
• Theo chu kỳ
• Theo vị trí
• Theo chu vi
• Hỗn hợp
Với điều khiển theo cảm nhận
• Điều khiển khơng thích nghi
• Điều khiển thích nghi
Với trí tuệ nhân tạo

Theo hệ thống điều khiển theo nguyên
lý điều khiển

• Điều khiển riêng rẽ
• Điều khiển theo nhóm

Theo số robot được điều khiển đồng
thời

Theo độ chính xác

• Gồm các mức chính xác : 0, 1, 2,
3

Theo kiểu bảo hiểm

•
•
•
•

Kiểu thơng thường
Kiểu phịng bụi
Kiểu phòng nhiệt
Kiểu phòng nổ

1.2 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)…

6


Hình 1.2: Một số loại Robot của hãng Yaskawa.

Hình 1.3: Robot MPL800 II.
Robot thực hiện công đoạn sản xuất thay cho con người với tốc độ di chuyển nhanh
và chính xác. Ứng dụng trong nhiều lĩnh vực khác nhau như lắp ráp ơ tơ xe máy, đóng
tàu, sắt thép, chế biến thực phẩm, điện tử bán dẫn,..
Độ tin cậy, tốc độ nhanh và hiệu suất cao chỉ là một vài trong số những đặc điểm
đáng kinh ngạc của robot MPL800 II.
Đây là một robot 4 trục, hoàn hảo cho việc xếp dỡ hàng hóa hoặc chọn đơn hàng
với thiết kế hướng đến hiệu suất của nó, dễ dàng xử lý các tải khơng cân bằng vì nó có
mơmen qn tính cao cho phép.
MPL800 II có tầm với chiều dọc rộng 3.024 mm và tầm với về chiều ngang 3.159
mm mang lại cho robot này khả năng xây dựng tải trọng cao 102 inch trên pallet tiêu
chuẩn 48 inch x 40 inch. Ngồi ra, MPL800 II có cổ tay rỗng để cho phép thực hiện
nhiều chuyển động của cổ tay mà không gặp bất kỳ sự can thiệp nào. Cổ tay rỗng này
cũng cho phép các hãng hàng không và cáp được định tuyến từ đế đến EOAT, giúp nâng
cao độ an toàn và tuổi thọ của cáp.

7


MPL800 II được ghép nối với bộ điều khiển DX200.( DX200 là bộ điều khiển 4
trục đa chức năng, có thể kết hợp trong hệ thống nhiều băng tải và vị trí đặt pallet )

1.3 Ứ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, …).
Robot MPL800II 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.
Tuy nhiên thì robot MPL800II là dịng robot chun 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 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.

Hình 1.4: Ứng dụng của Robot MPL800 II trong công nghiệp.

1.4 Kết cấu cơ khí:

Hình 1.5. Robot MPL làm việc trong xưởng
8


Kết cấu cơ khí của robot thể hiện như trong hình vẽ:
- Kết cấu cơ khí của robot nhìn từ trên xuống:

Hình 1.6: 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:

Hình 1.7: Robot MPL800 II khi nhìn từ đằng sau.
Kết cấu cơ khí của robot khi nhìn ngang:

9

Hình 1.8: Robot MPL800 II khi nhìn ngang.
1.5 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.2: 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

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

Độ rung
Khác

10

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

Yêu cầu nguồn

11


CHƯƠNG 2. ĐỘNG HỌC THUẬN VỊ TRÍ 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

Hình 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

4

0

0

𝜃4

0

Ta xét các ma trận biến đổi:
𝑇10 = 𝐴1 = 𝑅𝑜𝑡𝑧 (𝜃1 ). 𝑡𝑟𝑎𝑛𝑧 (𝑑1 ). 𝑡𝑟𝑎𝑛𝑥 (𝑎1 ). 𝑅𝑜𝑡𝑥 (𝛼1 )
𝑐𝑜𝑠( 𝜃1 ) − 𝑠𝑖𝑛( 𝜃1 ) 0 0 1 0 0 0
1 0 0 𝑎1
0
1
0
0
0 1 0 0
𝑠𝑖𝑛( 𝜃1 ) 𝑐𝑜𝑠( 𝜃1 ) 0 0] [
].[
].
=[
.
0
0
1
𝑑
0 0 1 0
0
0
1 0
1
0 0 0 1
0
0

0 1 0 0 0 1
1
0
0
0
𝑐𝑜𝑠( 𝜃1 ) 0
𝑠𝑖𝑛( 𝜃1 )
𝑎1 𝑐𝑜𝑠( 𝜃1 )
0
0
0 𝑐𝑜𝑠( 90 ) − 𝑠𝑖𝑛( 90 ) 0
𝑠𝑖𝑛( 𝜃1 ) 0 − 𝑐𝑜𝑠( 𝜃1 ) 𝑎1 𝑠𝑖𝑛( 𝜃1 )
[
]=[
]
0
0
0 𝑠𝑖𝑛( 90 ) 𝑐𝑜𝑠( 90 ) 0
0
1
0
𝑑1
0
0
0
1
0
0
0
1

(2.1)

12


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

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

1
0
0
1

𝑇32 = 𝐴3 = 𝑅𝑜𝑡𝑧 (𝜃3 ). 𝑡𝑟𝑎𝑛𝑥 (𝑎3 ). 𝑅𝑜𝑡𝑥 (𝜃3 )
𝑐𝑜𝑠( 𝜃3 ) − 𝑠𝑖𝑛( 𝜃3 )
𝑠𝑖𝑛( 𝜃3 ) 𝑐𝑜𝑠( 𝜃3 )
=[
0
0
0
0

𝑐𝑜𝑠( 𝜃3 )
𝑠𝑖𝑛( 𝜃3 )
=[
0
0

0
0
1
0

0
0
1
0

0 1
0 0
].[
0 0
1 0

𝑠𝑖𝑛( 𝜃3 )
− 𝑐𝑜𝑠( 𝜃3 )
0
0

0
1
0
0

0
0
1
0

0
0
𝑎3 1
0
0 𝑐𝑜𝑠( 90 ) − 𝑠𝑖𝑛( 900 )
0
].[
0
0 𝑠𝑖𝑛( 900 ) 𝑐𝑜𝑠( 900 )

1
0
0
0

0
0
]
0
1

𝑎3 𝑐𝑜𝑠( 𝜃3 )
𝑎3 𝑠𝑖𝑛( 𝜃3 ) ]
(2.3)
0
1

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

0
0
1
0

0
0](2.4)
0
1

Vậy ta có ma trận:
T40 = A1. A2 . A3 . A4
cos(1 )
 sin( )
1
=
 0

 0
cos(3 )
 sin( )
3

 0

 0
 a 11
a
=  21
 a31

 a41

0 sin(1 ) a1 cos(1 )  cos( 2 ) − sin( 2 )
0 − cos(1 ) a1 sin(1 )   sin( 2 ) cos( 2 )

.
  0
1
0
d1
0
 
0
0
1
0
  0
0 sin(3 ) a3 cos(3 )  cos( 4 ) − sin( 4 )
0 − cos(3 ) a3 sin(3 )   sin( 4 ) cos( 4 )
.
  0
1
0
0
0
 
0
0
1
0
  0

a12
a22
a32

a42

a13
a23
a33
a43

a14 
a24 
a34 

a44 
13

0 a2 cos( 2 ) 
0 a2 sin( 2 ) 
.

1
0

0
1

0
0
1
0

0

0 
0

1


Trong đó:
a 11 = cos(1 ).cos( 4 ).cos( 2 +  3 ) + sin(1 ).sin( 4 )
a12 = − cos(1 ).cos( 2 + 3 ).sin( 4 ) + sin(1 ) cos( 4 )
a13 = cos(1 ).sin( 2 + 3 )
a14 = a3 cos(1 ).cos( 2 + 3 ) + a2 cos(1 ).cos( 2 ) + a1 cos(1 )
a21 = sin(1 ).cos( 2 + 3 ).cos( 4 ) − cos(1 ).sin( 4 )
a22 = − sin(1 ).cos( 2 + 3 ).sin( 4 ) − cos(1 ).cos( 4 )
a23 = sin(1 ).sin( 2 + 3 )
a24 = a3 sin(1 ).sin( 2 + 3 ) + a2 sin(1 ).cos( 2 ) + a1 sin(1 )
a31 = sin( 2 + 3 ).cos( 4 )
a32 = − sin( 2 + 3 ).sin( 4 )
a33 = − cos( 2 + 3 )
a34 = a3 sin( 2 + 3 ) + d1 + a2 sin( 2 )
a41 = 0
a42 = 0
a43 = 0
a44 = 1

2.2 Thiết kế giao diện Matlab

Hình 2.2 Chương trình matlab tính động học thuận

• Giới hạn góc ứng với thơng số kĩ thuật của Robot MPL800 II, nếu nhập sai sẽ
báo lỗi.

14


CHƯƠNG 3. MA TRẬN JACOBY
3.1 Tính tốn cơng thức
i

- Bước 1: xác định ma trận Tn :
 i nx
i
i
 ny
o Tn =  i
nz

 0
𝑇40 =
𝐶1 𝐶23 𝐶4 + 𝑆1 𝑆4
𝑆 𝐶 𝐶 − 𝐶1 𝑆4
[ -1 23 4
𝑆23 𝐶4
0

𝐶23 𝐶4
𝑆 𝐶
𝑇41 = [ 23 4
𝑆4
0
𝐶3 𝐶4

𝑆𝐶
𝑇42 = [ 3 4
𝑆4
0
𝐶4
𝑆
𝑇43 = [ 4
0
0

i

i

ox
i
oy
i
oz
0

ax
i
ay
i
az
0

px 


py 
i
pz 

1 

i

i

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

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

−𝑆4
𝐶4
0
0

0

0
1
0

𝑆23
−𝐶23
0
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.1)
𝑎3 𝑆23 + 𝑑1 + 𝑎2 𝑆2
1

𝑎3 𝐶23 + 𝑎2 𝐶2
𝑎3 𝑆23 + 𝑎2 𝑆2
]
0
1

𝑎3 𝐶3
𝑎3 𝑆3
]
0
1

0
0
]
0
1

(3.2)

(3.3)

(3.4)

- Bước 2: Xác định ma trận J H :

15


 H p x
 q
 1
 H p y

 q1
H

 p z
 H X  q1
JH =
=
Q  H x
o
 q1
H
  y

 q1
 H 
z

 q1

p x
q 2

H

H

p y

q 2

...
...

p z
q 2

...

x
q 2

...

H

H

H

 y

q 2
z
q 2

...

H

...

p x i i
= n y px − i n x i py

i +1

 x i
= nz
i +1

H

H

p y

i +1

p x 
q n 
H
p y 

q n 

H
p z 
q n 

H
x 
q n 

H

 y 

q n 
H
z 

q n 

H

H

H

= oy px − ox py
i

i

i

i

 y

i +1

p z i i
= a y px − i a x i py
i +1

= i oz

 z i
= az
i +1

H

H

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

𝑅𝑛0
Bước 3: Tính ma trận Jacoby: 𝐽 = [
0

0
0
−𝑎3
𝑆4
𝐶4
0

0
0
0
0
0
1]

(3.3)

0
𝐻
0] • 𝐽
𝑅𝑛

Vậy ma trận Jacoby là:
𝑅0 0
𝐻
𝐽=[ 4
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

Thay số a1 = 270, a2 =1250, a3 =1650 ta được:
16

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

(3.4)

𝐽
−𝑆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

0
0
0
𝐶1 𝑆23
𝑆1 𝑆23

−𝐶23 ]

(3.5)

3.2 Lập trình tính tốn trên phần mềm Matlab

Hình 3.1 Chương trình matlab tính ma trận jacoby

• Giới hạn góc ứng với thơng số kĩ thuật của Robot MPL800 II, nếu nhập sai sẽ
báo lỗi.

17


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 = 𝑉𝑃
Ta có:
cos(θ1 )
sin(θ1 )
𝑇10 = [
0
0

0
0
1
0

sin(θ1 )
−cos(θ1 )
0
0

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

(4.2)

(4.3)

Suy ra:
(T10 )−1

cos(θ1 )
0
= [
sin(θ1 )
0

sin(θ1 )
0
−cos(θ1 )
0

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(θ1 ). px + sin(θ1 ). py − a1
pz − d1
).
sin(θ1 px − cos(θ1 ). py
]
1

(4.5)
18


cos(θ2 + θ3 ). cos(θ4 ) −cos(θ2 + θ3 ). sin(θ4 ) sin(θ2 + θ3 ) a3 cos(θ2 + θ3 ) + a2 cos(θ2 )
).
).
)
VP1 = [ sin(θ2 + θ3 cos(θ4 ) −sin(θ2 + θ3 sin(θ4 ) −cos(θ2 + θ3 ) a3 sin(θ2 + θ3 + a2 sin(θ2 ) ]
sin(θ4 )
cos(θ4 )
0
0
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:
cos(θ3 ) =

m2 + n2 − a3 2 − a2 2
2a3 a2

sin(θ3 ) = ±√1 − (cos(θ3 ))2
⇒ θ3 = ATAN2(sin(θ3 ), cos(θ3 ))
Sử dụng cho Planar đảo vị trí ta có:
θ2 = 𝐴𝑇𝐴𝑁2(𝑝2 , 𝑝1 ) − 𝐴𝑇𝐴𝑁2(𝑎3 . sin(θ3 ) , 𝑎2 + 𝑎3 cos(θ3 ))
Trong đó:
𝑝1 = cos(θ1 ). px + sin(θ1 ). py − a1
19

(4.10)


𝑝2 = pz − d1
Điều kiện góc quay của từng khớp (theo thông số kĩ thuật) :

−180  1  180
0    135

2

−120  3  15.5
−360   4  360

Các 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:
−(𝑎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.

20


CHƯƠNG 5. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO
CÁC KHỚP CỦA ROBOT THEO QUY ĐẠO DẠNG BẬC 3
Bài toán : Thiết kế quỹ đạo chuyển động cho các khợp của robot theo quỹ đạo dạng
bậc 3
Quỹ đạo của từng khớp có dạng:

i (t ) = ai 0 + ai1t + ai 2t 2 + ai 3t 3

(5.1)

i (t ) = ai1 + 2ai 2t + 3ai 3t 2

(5.2)

Vận tốc tại mỗi khớp:

Với 𝑖 = 1,4 tương ứng với 4 khớp.
Ta được hệ các phương trình sau:

1 (t ) = a10 + a11t + a12t 2 + a13t 3
 2 (t ) = a20 + a21t + a22t 2 + a23t 3
3 (t ) = a30 + a31t + a32t 2 + a33t 3

(5.3)

 4 (t ) = a40 + a41t + a42t 2 + a43t 3
Giả sử Robot đi từ điểm A tới điểm B trong thời gian là t(s)=5s 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:

i (0) = ai 0 = i ( A)
i (t ) = ai 0 + ai1t + ai 2t 2 + ai 3t 3 = i ( B )
i (0) = ai1 = 0

(5.4)

i (t ) = ai1 + 2ai 2t + 3ai 3t 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ó:
Khi Robot di chuyển từ vị trí

A( px , p y , pz , nx , n y , ox , o y ) = (1720,0,3780,

3
− 3
, −0.5, −0.25,
) suy ra các góc theta
4
2

tương ứng là:

qA = [1A ,2 A ,3 A ,4 A ]T = [0,60,0,30]T

(5.5)

Vị trí điểm

B( px , p y , pz , nx , ny , ox , oy ) = (2083.3553, 2083.3553, 1077.9486, − 0.27087,
0.95388, 0.94506, − 0.22414) suy ra các góc theta tương ứng là:

qB = [1B ,2 B ,3B ,4 B ]T = [45,30,-45,-60]T
Từ đây ta có được:
− Phương trình quỹ đạo của khớp 1 với các hệ số như sau:
21

(5.6)


a10 =1 ( A) = 0
a11 = 0
3

3
(1 ( B) − 1 ( A)) = 2 (45 − 0) = 5.4
2
t
5
2
2
a13 = − 3 (1 ( B) − 1 ( A)) = − 3 (45 − 0) = −0.72
t
5
Ta có phương trình quỹ đạo khớp 1:
a12 =

1 (t ) = 5.4t 2 − 0.72t 3

(5.7)

(5.8)

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

1 (t ) = 10.8t − 2.16t 2

(5.9)

1 (t ) = 10.8 − 4.32t

(5.10)

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

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

Hình 5.1 Mơ phỏng khớp 1

− Phương trình quỹ đạo của khớp 2 với các hệ số như sau:

22


a20 = 2 ( A) = 60
a21 = 0
3
3
( 2 ( B) −  2 ( A)) = 2 (30 − 60) = −3.6
2
t
5
2
2
a23 = − 3 ( 2 ( B) −  2 ( A)) = − 3 (30 − 60) = 0.48
t
5
Ta có phương trình quỹ đạo khớp 2:
a22 =

2 (t ) = 60 − 3.6t 2 + 0.48t 3

(5.11)

(5.12)

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

 2 (t ) = −7.2t + 1.44t 2

(5.13)

 2 (t ) = −7.2 + 2.88t

(5.14)

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

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

Hình 5.2 Mơ phỏng khớp 2

23


− Phương trình quỹ đạo của khớp 3 với các hệ số như sau:

a30 =3 ( A) = 0
a31 = 0
3
3
a32 = 2 (3 ( B) − 3 ( A)) = 2 (−45 − 0) = −5.4
t
5

2
2
a33 = − 3 (3 ( B) − 3 ( A)) = − 3 ( −45 − 0) = 0.72
t
5
Ta có phương trình quỹ đạo khớp 3:

3 (t ) = −5.4t 2 + 0.72t 3

(5.15)

(5.16)

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

3 (t ) = −10.8t + 2.16t 2

(5.17)

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

3 (t ) = −10.8 + 4.32t
Kết quả mơ phỏng trên Matlab:

Hình 5.3 Mơ phỏng khớp 3
24

(5.18)

− Phương trình quỹ đạo của khớp 4 với các hệ số như sau:

a40 = 4 ( A) = 30
a41 = 0
3
3
a42 = 2 ( 4 ( B) −  4 ( A)) = 2 (−60 − 30) = −10.8
t
5
2
2
a43 = − 3 ( 4 ( B) −  4 ( A)) = − 3 (−60 − 30) = 1.44
t
5
Ta có phương trình quỹ đạo khớp 4:

4 (t ) = 30 −10.8t 2 + 1.44t 3

(5.19)

(5.20)

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

4 (t ) = −21.6t + 4.32t 2

(5.21)

 4 (t ) = −21.6 + 8.64t

(5.22)

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

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

Hình 5.4 Mơ phỏng khớp 4
25