bài tập robot số 2

ROBOT
M
1
(t)
M
2
(t)

1
(t)

2
(t)
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
232
gl)m

2
1
m(E
+=
θ
1
θ
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
θ∂
∂
)-
232
gl)m
2

1
m(E
+=
;
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
;
y
2
= l
1
.sin
1
θ
+
2
sinl
212








θ+θ

;
Sinh viªn thùc hiÖn NguyÔn m¹nh Hµ
2
=
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:
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
+=
Sinh viên thực hiện Nguyễn mạnh Hà
3
)cos2(
211
θ
CBAH
++=
)cos(
212
θ
CBH
+=
2112
sin2
θ
Ch
−=
2122
sin
θ

Ch
−=
)(cos.cos
2111
θθθ
++=
EDg
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
+θθ+θ+θ+θ=
víi

Sinh viªn thùc hiÖn NguyÔn m¹nh Hµ
4
.
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
221
cosCBH
θ+=
BH
22
=
2211
sinCh
θ=
)cos(Eg
212
θ+θ=
VËy
Sinh viªn thùc hiÖn NguyÔn m¹nh Hµ
5






+













θ
θθ+θ
+








θ
θ






=







2
1
.
2
1211
.
2
.
1112
2
2122
..
2
..
1
2221
1211
2
1
g
g
h
hh
HH
HH
M

M
Sinh viªn thùc hiÖn NguyÔn m¹nh Hµ
6