Bài tập Robot số 2

ROBOT
M
1
(t)
M
2
(t)

1
(t)

2
(t)
Bài tâp lớn robot Bài số 2
Bài tập robot số 2
(Đề số 5)
Đề bài:
Cho một Robot 2 thanh nối đợc truyền động bởi động cơ một chiều . Động
cơ một chiều đợc cấp điện từ 1bộ khuyếch đại điện áp.
1.Xây dựng mô hình Simulink để xác định các phản ứng của Robot với
các mô men đầu vào M
1
(t) và M
2
(t).



2.Thiết kế bộ điều khiển PID độc lập cho từng khớp.
3.Mô phỏng hệ thống với bộ điều khiển ở câu 2.
Số liệu của Robot:

Chiều dài thanh 1 (l
1
) :0.4 (m)
Chiều dàI thanh 2 (l
2
) : 0.2 (m).
Khối lợng thanh nối 1 ( m
1
) : 21.8 (Kg).
Khối lợng thanh nối 2 (m
2
) : 15 (Kg).
Hằng số mô men của khớp 1,2 ( K
M
) : 0.1 (Nm/A).
Điện trở phần ứng (r
1
,r
2
) : 0.3 (Om).
Tốc độ lớn nhất của động cơ khớp 1,2 (
max
) : 90 (rad/s).
Khối lợng lớn nhất ( m
1
) : 5 (Kg).
Tỉ số truyền cho cả 2 khớp ( i ) `: 30.
Sinh viên thực hiện Nguyễn mạnh Hà
1
θ

1
θ
2
Bµi t©p lín robot Bµi sè 2
C©u 1: X©y dùng m« h×nh simulink cña robot 2 thanh nèi
• Tríc hÕt ta t×m quan hÖ gi÷a gãc quay vµ tèc ®é ®Çu ra so víi momen
M
1
vµ M
2
ë ®Çu vµo ta theo ®Þnh lý lagrang
M
1
=
dt
d
(
.
1
L
θ∂
∂
)-
1
L
θ∂
∂
;
M
2

==
dt
d
(
.
2
L
θ∂
∂
)-
2
L
θ∂
∂
;
Trong ®ã L ®îc tÝnh bëi :
L=k-p=k
1
+k
2
+k
3
-p
1
-p
2
-p
3
;
K

1
=
2
v.m
2
11
+
2
.j
.
2
11
θ
;j
1
=
3
l.m
2
11
; v
2
1
=
4
l
.
2
1
2

1
θ
;
K
2
=
2
v.m
2
22
+
2
)(.j
2
.
2
1
.
2
θ+θ
;v
2
2
=
2
2
.
2
2
.

yx +
;
x
2
=l
1
.cos
1
θ
+
2
cos.l
212








θ+θ
;
=
2
.
x
- l
1
.sin

1
θ
.
1
.
θ
-








θ+θ








θ+θ
.
2
1
.
212

2
sin.l
;
Sinh viªn thùc hiÖn NguyÔn m¹nh Hµ
2
Bài tâp lớn robot Bài số 2
y
2
= l
1
.sin
1

+
2
sinl
212








+
;
=
2
.

y
l
1
.cos
1

.
1
.

+








+








+
.

2
1
.
212
2
cosl
;
v
2
2
=
.
x
2
2
+
.
2
2
y
=l
2
1
.
.
2
1

+
2

)(.l
2
.
2
1
.
2
2
+
+
.
1

.(
.
2
.
1
+
).l
1
.l
2
.cos
2

;
j
2
=

3
l.m
2
22
;
k
3
=
2
.
2
33
vm
thay l
2
ở v
2
bằng 2l
2
(chiều dàI thanh gấp đôi)
v
2
3
=l
2
1
.
2
1
.

+l
2
1
(
.
2
.
1

+
)
2
+2.l
1
.l
2
.
.
1

.(
.
2
.
1
+
)cos
2

;
p
1
=
2
sinl.g.m
111

;
p
2
=m
2
g(l
1
sin
1

+
2
sin
212






+

l
);
p
3
=m
3
g(l
1
sin
1

+l
2
sin(
21

+
));
L= k
1
+k
2
+k
3
-p
1
-p
2
-p
3

thay số vào ta có:
=
2
v.m
2
11
+
2
.j
.
2
11

+
2
v.m
2
22
+
2
)(.j
2
.
2
1
.
2
+
+
2

v.m
2
33
-
2
sinl.g.m
111

- m
2
g(l
1
sin
1

+
2
sinl
212






+
)m
3
g(l
1

sin
1

+l
2
sin(
21

+
))
thay các biểu thức vận tốc vào ta đợc:
Sinh viên thực hiện Nguyễn mạnh Hà
3
Bµi t©p lín robot Bµi sè 2
L=
2
.
.
2
1
θ
A
+
2
).(B
2
.
2
.
1

θ+θ
+C.
2
.
2
.
1
.
1
cos)( θθ+θθ
-D.sin
1
θ
-E.sin(
+
1
θ
2
θ
);
2
132
1
l)mm
12
m7
(A ++=
2
23
2

l)m
12
m7
(B +=
2132
ll)m
2
1
m(C +=
132
1
gl)mm
2
m
(D ++=
232
gl)m
2
1
m(E +=
A=5.23;B=0.55;C=1;D=121;E=24,5;
.
1
L
θ∂
∂
=
A
.
2

.
2
.
1
.
2
.
1
.
1
cos).2.(C)(B θθ+θ+θ+θ+θ
;
dt
d
(
.
1
L
θ∂
∂
)=
2
.
2
.
2
.
12
..
2

.
1
..
2
..
1
..
1
sin).2.(Ccos).2.(C)(B.A θθθ+θ−θθ+θ+θ+θ+θ
;
1
L
θ∂
∂
=-
)cos(.Ecos.D
211
θ+θ−θ
;
)cos(.cos.sinsin2)cos()cos2(
2112
.
2
22
.
2
.
1
..
22

..
121
θθθθθθθθθθθθ
+++−−++++= EDCCCBCBAM
;ghhHH1M
1
.
2
.
1112
.
2
2122
..
212
..
111
+θθ+θ+θ+θ=
Sinh viªn thùc hiÖn NguyÔn m¹nh Hµ
4
)cos2(
211
θ
CBAH ++=
)cos(
212
θ
CBH +=
2112
sin2

θ
Ch −=
2122
sin
θ
Ch −=
)(cos.cos
2111
θθθ
++= EDg
Bµi t©p lín robot Bµi sè 2
víi



.
2
L
θ∂
∂
=
2
.
1
.
2
.
1
cosC)(B θθ+θ+θ
;

dt
d
(
.
2
L
θ∂
∂
)=
2
.
2
.
12
..
1
..
2
..
1
sinCcosC)(B θθθ−θθ+θ+θ
;

2
L
θ∂
∂
=-
)cos(.Esin)(C

212
.
2
.
1
.
1
θ+θ−θθ+θθ
;
M
2
=
dt
d
(
.
2
L
θ∂
∂
)-
2
L
θ∂
∂
M
2
=
2
.

2
.
12
..
1
..
2
..
1
sinCcosC)(B θθθ−θθ+θ+θ
+
)cos(.Esin)(C
212
.
2
.
1
.
1
θ+θ+θθ+θθ
=
)cos(EsinCB)cosCB(
212
.
2
1
..
2
..
2

θ+θ+θθ+θ+θθ+
=
2
.
2
1211
..
222
..
121
ghHH +θ+θ+θ
víi
Sinh viªn thùc hiÖn NguyÔn m¹nh Hµ
5