Robot 5dof utc Đại Học Giao Thông Vận Tải

TRƯỜNG ĐẠI HỌC GIAO THƠNG VẬN TẢI
KHOA CƠ KHÍ
BỘ MƠN CƠ ĐIỆN TỬ
----------------

BÀI TẬP LỚN ĐIỀU KHIỂN ROBOT
ĐỀ TÀI: ROBOT RV-M1 MITSUBISHI
GIÁO VIÊN HƯỚNG DẪN :PGS.TS ĐINH THỊ THANH HUYỀN
SINH VIÊN THỰC HIỆN

:

TRẦN MẠNH LINH

:

NGUYỄN VĂN TOÀN

:

NGUYỄN KHÁNH DUY

:

NGUYỄN XUÂN QUYẾT

LỚP

:

CƠ ĐIỆN TỬ 1

KHÓA

:

60

HÀ NỘI 2023


LỜI NÓI ĐẦU
Trong những năm gần đây , cùng với sự phát triển của khoa học công nghệ
và các hệ thống máy móc hiện đại, Robot đang khơng ngừng phát triển về số
lượng cũng như khả năng của chúng . Robot được áp dụng rộng rãi trong hầu
hết các lĩnh vực như sản xuất và công nghiệp , hàng không , giao thông vận
tải, ... Đặc biệt trong lĩnh vực sản xuất, cơng nghiệp thì Robot là sản phẩm giải
pháp cho việc nâng cao năng suất và chất lượng sản phẩm . Trong đó Robot RVM1 MITSUBISHI – một loại tay máy 5 bậc tự do có khả năng hoạt động linh
hoạt và được sử dụng nhiều trong công việc .
Trong bài tập lớn môn học “ Điều Khiển Robot ” cùng sự hướng dẫn của Cô
Đinh Thị Thu Huyền thì nhóm chúng mình có nghiên cứu , tìm hiểu về loại
Robot 5 bậc tự do RV-M1 MITSUBISHI . Trong bài báo cáo gồm có 3 chương :
Chương I : Động Lực Học Robot
Chương II : Chọn Động Cơ
Chương III : Thiết Kế Bộ Điều Khiển


MỤC LỤC
CHƯƠNG I : ĐỘNG LỰC HỌC ROBOT...........................................................1
1.1 Robot RV-M1 MITSUBISHI.....................................................................1
1.2 Cấu Tạo Robot RV-M1 MITSUBISHI.......................................................1

1.3 Động học Robot...........................................................................................5
1.3.1 Robot gắn hệ trục tọa độ 3D và lập bảng DH......................................5
1.3.2 Ma trận biến đổi thuần nhất giữa hai khâu liên tiếp.............................6
1.3.3 Phương trình lực động lực học Robot.................................................9
CHƯƠNG II CHỌN ĐỘNG CƠ.........................................................................33
2.1 Bảng thông số động cơ 110ST-IM02030..................................................33
2.2 Thông số động cơ từng khâu.....................................................................33
2.2.1 Động cơ khâu 1 : 110ST-IM0203......................................................33
2.2.2 Động cơ khâu 2 : 110ST-IM02030....................................................34
2.2.3 Động cơ khâu 3 : 110ST-IM02030....................................................34
2.2.4 Động cơ khâu 4 : 110ST-IM02030....................................................35
2.2.5 Động cơ khâu 5 : 110ST-IM02030....................................................35
2.3 Phương trình chuyển động của Robot.......................................................36
CHƯƠNG III THIẾT KẾ BỘ ĐIỀU KHIỂN....................................................42
3.1 Bộ điều khiển PI, PD,PID và PD bão hòa.................................................42
3.1.1 Xác định Kp , Kd và Ki trong bộ điều khiển.......................................42
3.1.2 Mô phỏng Matlab Simulink..............................................................56
3.2 Thiết kế bộ điều khiển gán điểm cực phản hồi trạng thái.........................82
PHỤ LỤC : Chương trình mơ phỏng Matlab....................................................92


CHƯƠNG I : ĐỘNG LỰC HỌC ROBOT
1.1 Robot RV-M1 MITSUBISHI

Hình 1.1 Mơ hình 3D Robot RV-M1 MITSUBISHI

1.2 Cấu Tạo Robot RV-M1 MITSUBISHI
Đế khâu :

Hình 1.2.1 khâu 0

Khâu 1:

Hình 1.2.2 khâu 1
Khâu 2:

Hình 1.2.3 khâu 2


Khâu 3:

Hình 1.2.4 khâu 3
Khâu 4:

Hình 1.2.5 khâu 4


Khâu 5:

Hình 1.2.6 khâu 5
Bàn tay nắm bắt

Hình 1.2.6 bàn tay nắm bắt


1.3 Động học Robot
1.3.1 Robot gắn hệ trục tọa độ 3D và lập bảng DH
 Tọa độ :

Hình 1.3.1 Gắn tọa độ 3D trên Robot

Hình 1.3.2 Gắn hệ trục tọa độ và góc quay


 Bảng DH:
i
1
2
3
4
5
6

ai-1

α i-1

0
-90

di
L1

0
0

o

0

0

L2
L3

0
0
0

90o

0
0

L4
L5

0

θi
θ1
θ2
θ3
θ 4 + 90o
θ 5 +180

-180

1.3.2 Ma trận biến đổi thuần nhất giữa hai khâu liên tiếp.
Do cách chọn trục sai so với quy tắc của bảng ĐH nên tính lại ta có:

0
1

T = Rot(z, θ 1) trans(0,0,L1)

c θ 1−s θ1 0 0
s θ1 c θ 1 0 0
0
1T =
0 0 1L1
0 0 0 1

[

c θ 3−s θ3 0 L2
s θ3 c θ 3 0 0
2
3T =
0 0 10
0 0 01

[

cθ2 −s θ2 0 0
0
0 10
; 12T = −s θ −cθ 0 0
2
2
0

0 01

]

[

−s θ 4 −c θ4 0 L3
c θ4 −s θ 4 0 0
; 34T =
0
0 10
0
0 01

] [

−c θ 5 s θ 5 0 0
0
0 −1 −L 4
4
5T =
−s θ5 −c θ5 0 0
0
0 0 1

[

0
6

T

HÀNG 1

]

]

−1 0 0 0
0 −1 0 0
; 56T = 0 0 1 L 5
0 0 0 1

] [ ]

=01T . 12T .23T .34T .45T .56T =


[
cos(t1)*cos(t5) + cos(t2)*cos(t3)*sin(t1)*sin(t4)*sin(t5) +
cos(t2)*cos(t4)*sin(t1)*sin(t3)*sin(t5) + cos(t3)*cos(t4)*sin(t1)*sin(t2)*sin(t5)
- sin(t1)*sin(t2)*sin(t3)*sin(t4)*sin(t5),
cos(t2)*cos(t3)*cos(t5)*sin(t1)*sin(t4) - cos(t1)*sin(t5) +
cos(t2)*cos(t4)*cos(t5)*sin(t1)*sin(t3) + cos(t3)*cos(t4)*cos(t5)*sin(t1)*sin(t2)
- cos(t5)*sin(t1)*sin(t2)*sin(t3)*sin(t4),
sin(t2 - t1 + t3 + t4)/2 - sin(t1 + t2 + t3 + t4)/2,
l3*sin(t1)*sin(t2)*sin(t3) - l2*cos(t2)*sin(t1) - l3*cos(t2)*cos(t3)*sin(t1) l4*cos(t1) - l5*cos(t2)*cos(t3)*cos(t4)*sin(t1) +
l5*cos(t2)*sin(t1)*sin(t3)*sin(t4) + l5*cos(t3)*sin(t1)*sin(t2)*sin(t4) +
l5*cos(t4)*sin(t1)*sin(t2)*sin(t3)
]

HÀNG 2
[
cos(t5)*sin(t1) - cos(t1)*cos(t2)*cos(t3)*sin(t4)*sin(t5) cos(t1)*cos(t2)*cos(t4)*sin(t3)*sin(t5) - cos(t1)*cos(t3)*cos(t4)*sin(t2)*sin(t5)
+ cos(t1)*sin(t2)*sin(t3)*sin(t4)*sin(t5),
cos(t1)*cos(t5)*sin(t2)*sin(t3)*sin(t4) - cos(t1)*cos(t2)*cos(t3)*cos(t5)*sin(t4)
- cos(t1)*cos(t2)*cos(t4)*cos(t5)*sin(t3) cos(t1)*cos(t3)*cos(t4)*cos(t5)*sin(t2) - sin(t1)*sin(t5),
cos(t1 + t2 + t3 + t4)/2 + cos(t2 - t1 + t3 + t4)/2,
l2*cos(t1)*cos(t2) - l4*sin(t1) + l3*cos(t1)*cos(t2)*cos(t3) l3*cos(t1)*sin(t2)*sin(t3) + l5*cos(t1)*cos(t2)*cos(t3)*cos(t4) l5*cos(t1)*cos(t2)*sin(t3)*sin(t4) - l5*cos(t1)*cos(t3)*sin(t2)*sin(t4) l5*cos(t1)*cos(t4)*sin(t2)*sin(t3) ]
HÀNG 3


[
sin(t2 + t3 + t4 - t5)/2 - sin(t2 + t3 + t4 + t5)/2,
- cos(t2 + t3 + t4 + t5)/2 - cos(t2 + t3 + t4 - t5)/2,
-sin(t2 + t3 + t4),
l1 - l3*sin(t2 + t3) - l2*sin(t2) - l5*sin(t2 + t3 + t4)
]

HÀNG 4
[ 0,

0,

0,

1]


1.3.3 Phương trình lực động lực học Robot
 Thơng số các khâu trong Robot :

khâu 0 :

Hình 1.3.3 Thơng số trong đế khâu
Khối lượng 13,776995kg
Momen quán tính khâu đế (khâu 0):
Ixx = 0,113818

Ixy = 0

Ixz = -0,022731

Iyx = 0

Iyy =0,149769

Iyz = 0,000001

Izx = -0,022731

Izy = 0,000001

Izz = 0,110865


Khâu 1:

Hình 1.3.4 Thơng số trong khâu 1
Khổi lượng 12,184665 kg
Momen quán tính khâu 1
Ixx = 0,145561 Ixy = 0,000015

Ixz = 0,006287

Iyx = 0,000015 Iyy = 0,165763

Iyz = 0,00005

Izx = 0,006287 Izy = 0,00005

Izz = 0,089568


Khâu 2:

Hình 1.3.5 Thơng số trong khâu 2
Khổi lượng 12,144184 kg
Momen quán tính khâu 2 :
Ixx = 0,031061

Ixy = 0

Ixz = -0,000046

Iyx = 0

Iyy = 0,360491

Iyz = 0,00009

Izx = -0,000046

Izy = 0,000090

Izz = 0,350351


Khâu 3:

Hình 1.3.6 Thơng số trong khâu 3
Khổi lượng 3,047721 kg
Momen quán tính khâu3 :
Ixx = 0,006256

Ixy = -0,000064 Ixz = 0

Iyx = -0,000064 Iyy = 0,046247

Iyz = 0

Izx = 0

Izz = 0,044095

Izy = 0


Khâu 4:

Hình 1.3.7 Thơng số trong khâu 4

Khối lượng 0,654841 kg
Momen quán tính khâu 4 :
Ixx = 0.000463 Ixy = 0

Ixz = 0

Iyx = 0

Iyy = 0.000263

Iyz = 0

Izx = 0

Izy = 0

Izz = 0,000595


Khâu 5

Hình 1.3.8 Thơng số trong khâu 5
Khối lượng 0,702798 kg
Momen quán tính khâu 5 :
Ixx = 0,001014

Ixy = 0

Ixz = 0

Iyx = 0

Iyy = 0,001003 Iyz = 0

Izx = 0

Izy = 0

Izz = 0,000658


 Vector biểu diễn khối tâm của từng khâu trong hệ quy chiếu của chính
nó:
rRG0 = [ -0,028935 ; 0 ; 0,060721 ; 1 ]
rRG1= [-0,025155 ; -0,00005 ; -0,082116 ; 1 ]
rRG2 = [0,108136 ; -0,003646 ; -0,003177 ; 1 ]
rRG3 = [ 0,097551 ; -0,000122 ; 0 ; 1 ]
rRG4 = [ 0 ; -0,010914 ; 0; 1 ]
rRG5 = [ 0 ; -0,000029 ; 0,024545 ; 1 ]
Vector gia tốc trọng trường:
Vg= [ 0 ; 0 ; -9.8 ; 1];

 Ma trận giả quán tính các khâu :
0,0549
0
0,0063
0
0
0,0347
0

0
J 1=
0,0063
0
0,1109
0
0
0
0
12,18

[
[
[

0,3399
0
0
0
0
0,0105
0,0001
0
J 2=
0
0,0001 0,0206
0
0
0
0

12,14

]
]

0,042 −0,0001
0
0
−0,0001
0,0021
0
0
J 3=
0
0
0,0042
0
0
0
0
3,05

]


0.0002
0
0
0
0.0004

0
0
J 4= 0
0
0
0,0001
0
0
0
0
0,6540

[

]

0,0003
0
0
0
0
0,0003
0
0
J5= 0
0
0,0007
0
0
0

0
0,7027

[

]

 Phương trình động lực học Robot
n j j
 T j T jT 
 2T j
T jT 
Fi  Tr 
Jj
Jj
 qk   Tr 
 q k q m 

q

q

q

q

q
j i k 1
j i k 1 m 1
 k

 k m
i 
i 
j

n



n

m g

T

j

j i

T j j
qi

rG j

∂T 1 ∂ T T1
∂T 2 ∂ T T2
∂T 3 ∂T T3
∂T 4 ∂T T4
J
q´ + Tr

J
q´ + Tr
J
q´ + Tr
J
q´ +
F1= Tr
∂ q1 1 ∂ q1 1
∂ q1 2 ∂ q 1 1
∂ q1 3 ∂ q 1 1
∂ q1 4 ∂ q 1 1

[

Tr

Tr

]

[

] [

]

[

]

[

∂T 5 ∂T T5
∂T 2 ∂ T T2
∂T 3 ∂T T3
∂T 4 ∂T T4
J5
q´1 + Tr
J2
q´2 + Tr
J3
q´2 + Tr
J
q´ +
∂ q1 ∂ q 1
∂ q2 ∂ q 1
∂ q2 ∂ q 1
∂ q2 4 ∂ q 1 2

[

∂T 5 ∂T T5
∂T 3 ∂T T3
∂T 4 ∂T T4
∂T 5 ∂T T5
J5
q´2 + Tr
J3
q´3 + Tr
J4

q´3 + Tr
J
q´
∂ q2 ∂ q 1
∂ q3 ∂ q 1
∂ q3
∂ q1
∂ q3 5 ∂ q 1 3

] [
]

]

[

[

]

]

[

[

] [
]

]

]

∂T 4 ∂T T4
∂T 5 ∂T T5
∂T 5 ∂T T5
J
q´ + Tr
J
q´ + Tr
J
q´ +
+ Tr
∂ q4 4 ∂ q 1 4
∂ q4 5 ∂ q1 4
∂ q5 5 ∂ q 1 5

[

Tr

+

[

∂2 T 1
∂2 q 1

]

J1

[

]

[

∂ T T1 2
∂2 T
∂ TT 2
∂2 T
∂T T3 2
∂2 T
∂T T4 2
∂2 T
∂T T5 2
q´1 + Tr 2 2 J 2 2 q´1 +Tr 2 3 J 3
q´1 +Tr 2 4 J 4
q´1 +Tr 2 5 J 5
q´1
∂ q1
∂q 1
∂ q1
∂ q 1 ∂ q1
∂ q1
∂ q1
∂ q1 ∂ q 1

] [

] [

] [

] [

]


Tr

[

∂2 T 2
∂2 q 2

[

∂2 T 4

∂ T T2 2
∂2 T
∂ TT 2
∂2T
∂ T T4 2
∂2 T
∂ TT 2
q´2 +Tr 2 3 J 3 3 q´2 + Tr 2 4 J 4
q´2 + Tr 2 5 J 5 5 q´2 +

∂ q1
∂ q1
∂ q 2 ∂ q1
∂ q2
∂ q 2 ∂ q1

] [

] [

] [

]

∂ T T3 2
∂2 T 4 ∂ T T4 2
∂2 T 5 ∂ T T5 2
Tr 2 J 3
q´3 +Tr 2 J 4
q´ +Tr 2 J 5
q´3 +
∂ q1 3
∂ q3 ∂ q1
∂ q3
∂ q 3 ∂ q1

Tr

[

∂2 T 3

J2

∂2 q 4

] [

J4

] [

]

∂ T T4
∂2 T
∂ TT 2
∂2 T
∂ T T5 2
2
q´ 4 ++ Tr 2 5 J 5 5 q´4 + Tr 2 5 J 5
q´5 +¿
∂ q1
∂ q1
∂ q4
∂ q5 ∂ q1

]

[

]

[
]
]
]

]

∂2 T 2
∂ T T2
∂2T 3
∂ T T3
∂2 T 4
∂T T4
2 Tr
J
q´ ´q +2 Tr
J
´q q´ +2 Tr
J
q´ q´ +
∂ q 1 ∂ q 2 2 ∂ q1 1 2
∂ q 1 ∂ q 2 3 ∂ q1 1 2
∂ q1∂ q2 4 ∂ q1 1 2

[

2 Tr

2 Tr

2 Tr

]

[

[

]

[

∂2T 5
∂ TT
∂2 T 3
∂ T T3
∂2 T 4
∂T T4
J 5 5 ´q1 q´ 2+2 Tr
J3
q´ 1 q´ 3 +2 Tr
J4
q´ q´ +
∂ q 1 ∂ q 2 ∂ q1
∂ q 1 ∂ q 3 ∂ q1
∂ q 1 ∂ q 3 ∂q 1 1 3

[

∂2 T 5
∂ T T5
∂2T 4
∂ T T4
∂2 T 5
∂T T5
J5
q´ 1 q´ 3 +2 Tr
J4
q´ 1 q´ 4 +2 Tr
J5
q´ q´ +
∂ q 1 ∂ q 3 ∂ q1
∂ q 1 ∂ q 4 ∂ q1
∂ q 1 ∂ q 4 ∂q 1 1 4

[

∂2 T 5
∂ T T5
∂2 T 3
∂ T T3
∂2 T 4
∂T T4
J5
q´ 1 q´ 5 + + 2 Tr
J3
q´ 2 q´ 3 +2 Tr

J4
q´ q´ +
∂ q 1 ∂ q 5 ∂ q1
∂ q 3 ∂q 2 ∂ q1
∂ q 3 ∂q 2 ∂q 1 2 3

]

[

]

[

]

[

[

]

[

]

]

[

]

∂2 T 5
∂ T T5
∂2 T 4
∂ T T4
∂2 T 5
∂T T5
2 Tr
J
q´ q´ +2 Tr
J
q´ q´ +2 Tr
J
q´ q´ +
∂ q 3 ∂q 2 5 ∂ q1 2 3
∂q 2 ∂ q4 4 ∂ q 1 2 4
∂ q 2 ∂ q 4 5 ∂q 1 2 4

[

]

[

]

[

]

∂2 T 5
∂ T T5
∂2 T 4
∂ T T4
∂2 T 5
∂ T T5
2 Tr
J
q´ q´ +2 Tr
J
q´ q´ +2 Tr
J
q´ q´ +
∂ q 2 ∂ q 5 5 ∂ q1 2 5
∂ q 3 ∂q 4 4 ∂ q1 3 4
∂ q 3 ∂q 4 5 ∂ q1 3 4

[

]

[

]

[

]

∂2 T 5
∂ T T5
∂2 T 5
∂ T T5
T ∂T1
T ∂T2
r g - m 2 ⃗g
⃗
r ⃗
2 Tr
J5
q´ 3 q´ 5 +2 Tr
J5
q´ 4 q´ 5 - m 1 ⃗g
∂ q1 1
∂ q 1 g2
∂ q 3 ∂q 5 ∂ q1
∂ q 4 ∂ q5 ∂ q1

[

m 3 ⃗g

T

]

[

∂T3

r
⃗
∂ q 1 g3
T

∂ T4

T

∂T5

r g -m 5 ⃗g
r
⃗
-m 4 ⃗g ∂ q ⃗
∂ q 1 g5
1
4

]