Professional Documents
Culture Documents
(1.1)
Trong :
+ f : L s bc t do ca c cu.
+ : Bc t do ca mt vt rn khng chi lin kt trong khng gian lm vic ca
robot ( = 3 ng vi khng gian lm vic trong mt phng, = 6 ng vi khng
gian lm vic trong khng gian).
+ n : S khu ( k c gi c nh).
+
i
f : S bc t do ca khp th i.
+ g : Tng s khp ca c cu.
+
0
f : S bc t do tha
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 7
Thng thng robot 3 bc t do m bo c yu cu v khng gian thao tc.
Theo yu cu t ra ca bi ton, robot c hng kp vt ty nn ta tng thm
1 bc t do m bo linh hot v hng thao tc ca robot cho ph hp vi
c im lm vic ca robot vn chuyn.
c. Cc phng n thit k cu trc ph hp cho cc khu khp
Theo yu cu u bi, robot vn chuyn c hng thao tc ty nn ta chn 4
bc m bo linh hot khi hot ng.
Sau y l mt s cu trc robot ra
Hnh 1.1. Cu trc 1
Hnh 1.2. Cu trc 2
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 8
Hnh 1.3 Cu trc 3
Hnh 1.4. Cu trc 4
d. Phn tch, chn, thit k cu trc c chn
Ta la chn s dng cu trc 4 do cu trc ny c khu 3 chuyn ng tnh tin
m bo linh hot v phng chiu hot ng, khu 4 chuyn ng quay quanh
trc khu 3 nn tay kp robot c kinh hot v hng tc ng khi cm nm vn
chuyn.
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 9
Khu
Khu 1
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 10
Khu 2
Khu 3
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 11
Khu 4 v ty kp
Tng Th robot
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 12
2. Gii bi ton ng hc
a. Cho 1 quy lut chuyn ng ca cc khu robot
a1= 25cm, a2=20 cm, d3= 20cm
Cho
1
= ,
2
= ,
4
= , d
3
= 0.1t
Xc nh ma trn chuyn v thnh phn m t v tr ca khu th i so vi khu th
i-1
Dng tng qut:
os sin cos sin sin cos
sin os cos os sin sin
0 sin cos
0 0 0 1
i i i i i i i
i i i i i i i
i
i i i
c a
c c a
A
d
u u u u
u u u u
o o (
(
o o
(
=
( o o
(
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 13
Quy c vit tt :
( ) ( )
ij ij
os ; sin ; os ; sin
i i i i i j i j
C c S C c S u u u u u u = = = + = +
Ta c bng thng s Danevit- Hatenberg ca robot scara:
Bng h ta Danevit-Hatenberg
u d A o
1
*
1
u 0 a
1
0
2
*
2
u 0 a
2
t
3 0
*
3
d 0 0
4
*
4
u
4
d 0 0
Cc ma trn A
i
1 1 1 1
1 1 1 1
1
0
0 a
0 0 1 0
0 0 0 1
C S a C
S C S
D
(
(
(
=
(
(
2 2 2 2
2 1 2 2
2
0
0
0 0 1 0
0 0 0 1
C S a C
S C a S
D
(
(
(
=
(
(
3
3
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
D
d
(
(
(
=
(
(
4 4
4 4
4
4
0 0
0 0
0 0 1
0 0 0 1
C S
S C
D
d
(
(
(
=
(
(
Vi quy lut ca cc khu nh cho ta tnh c
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 14
Ma trn chuyn v thun nht m t hng v v tr ca cc khu:
Khu 1 so vi khu c s:
1 1 1 1
1 1 1 1
1 1
0
0
0 0 1 0
0 0 0 1
C S a C
S C a S
T D
(
(
(
= =
(
(
Khu 2 so vi khu c s:
12 12 1 1 2 12
12 12 1 1 2 12
2 1 2
0
0
.
0 0 1 0
0 0 0 1
C S a C a C
S C a S a S
T D D
+ (
(
+
(
= =
(
(
Khu 3 so vi khu c s
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 15
12 12 1 1 2 12
12 12 1 1 2 12
3 2 3
3
0
0
.
0 0 1
0 0 0 1
C S a C a C
S C a S a S
T T D
d
+ (
(
+
(
= =
(
(
Khu 4 so vi khu c s
12 4 12 4 12 4 12 4 1 1 2 12
12 4 12 4 12 4 12 4 1 1 2 12
4 3 4
3 4
0
0
.
0 0 1
0 0 0 1
C C S S S C C S a C a C
S C C S C C S S a S a S
T T D
d d
+ + (
(
+
(
= =
(
(
Vy ta im E c dng:
(0)
1 1 2 12 E
x a C a C = +
(0)
1 1 2 12 E
y a S a S = +
(0)
3 4 E
z d d =
Vi thng s quy lut cc khu cho ta tnh c:
Ta tnh c cc ma trn:
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 16
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 17
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 18
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 19
4(1,1) 4(1, 2) 4(1, 3) 4(1, 4)
4(2,1) 4(2, 2) 4(2, 3) 4(2, 4)
4
4(3,1) 4(3, 2) 4(3, 3) 4(3, 4)
4(4,1) 4(4, 2) 4(4, 3) 4(4, 4)
T T T T
T T T T
T
T T T T
T T T T
| |
|
|
=
|
|
\ .
dfadsfadfaf
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 20
1 1 1 1 1 1 1
cos sin( ) cos sin( )
6 6 3 6 6 3 3 1 1
4(1,1) cos sin( )
3 3 1 1 1 1 1 1 1
sin sin( ) sin sin( )
6 6 3 6 6 3 3
1 1 1 1 1 1
cos sin( ) sin sin( )
6 3 6 6 3 3
t t
T t
t t
t t
t t t t t t t
t t t
t t t t t t t
t t t t t t
( | | | |
+ + +
| | (
\ . \ . | |
(
= +
|
(
\ . | | | |
+ + +
( | |
\ . \ .
| | |
+ + +
|
\ .
+
1 1
sin sin( )
3 3 1 1 1 1 1 1 1
sin sin( ) s sin( )
6 6 3 6 6 3 3
t
t co t
t t t
t t t t t t t
( |
+
| (
\ . | |
(
+
|
(
\ . | | | |
+ + +
( | |
\ . \ .
4(1, 3) 0 T =
1 1 1 1 1 1 1
4(1, 4) 20cos sin( ) cos sin( )
6 6 3 6 6 3 3
1 1 1 1 1 1 1
20sin sin( ) sin sin( )
6 6 3 6 6 3 3
1 1 1 1
25cos sin( )
6 6 3 6
T t t
t t
t
t t t t t t t
t t t t t t t
t t t t
| | | |
= + + +
| |
\ . \ .
| | | |
+ + +
| |
\ . \ .
| |
+ + +
|
\ .
1 1 1 1 1 1 1
cos sin( ) sin sin( )
6 6 3 6 6 3 3 1 1
4(2, 2) sin sin( )
3 3 1 1 1 1 1 1 1
sin sin( ) cos sin( )
6 6 3 6 6 3 3
1 1 1 1 1 1 1
sin sin( ) sin sin(
6 6 3 6 6 3 3
t t
T t
t t
t
t t t t t t t
t t t
t t t t t t t
t t t t t t
( | | | |
+ + +
| | (
\ . \ . | |
(
= +
|
(
\ . | | | |
+ + + +
( | |
\ . \ .
| |
+ + +
|
\ .
+
)
1 1
cos sin( )
3 3 1 1 1 1 1 1 1
cos sin( ) s sin( )
6 6 3 6 6 3 3
t
t
t co t
t
t t t
t t t t t t t
( | |
| (
\ . | |
(
+
|
(
\ . | | | |
+ + +
( | |
\ . \ .
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 21
4(2, 3) 0 T =
1 1 1 1 1 1 1
4(2, 4) 20sin sin( ) cos sin( )
6 6 3 6 6 3 3
1 1 1 1 1 1 1
20cos sin( ) sin sin( )
6 6 3 6 6 3 3
1 1 1 1
25sin sin( )
6 6 3 6
T t t
t t
t
t t t t t t t
t t t t t t t
t t t t
| | | |
= + + +
| |
\ . \ .
| | | |
+ + + +
| |
\ . \ .
| |
+ + +
|
\ .
4(3,1) 0
4(3,1) 0
4(3, 2) 0
4(3, 3) 1
4(3, 4) 20 0.1*
4(4,1) 0
4(4, 2) 0
4(4, 3) 0
4(4, 4) 1
T
T
T
T
T t
T
T
T
T
=
=
=
=
=
=
=
=
=
V tr v vn tc im tc ng cui E:
1 1 1 1 1 1 1
20cos sin( ) cos sin( )
6 6 3 6 6 3 3
1 1 1 1 1 1 1
20sin sin( ) sin sin( )
6 6 3 6 6 3 3
1 1 1 1
25cos sin( )
6 6 3 6
xE t t
t t
t
t t t t t t t
t t t t t t t
t t t t
| | | |
= + + +
| |
\ . \ .
| | | |
+ + +
| |
\ . \ .
| |
+ + +
|
\ .
1 1 1 1 1 1 1
20sin sin( ) cos sin( )
6 6 3 6 6 3 3
1 1 1 1 1 1 1
20cos sin( ) sin sin( )
6 6 3 6 6 3 3
1 1 1 1
25sin sin( )
6 6 3 6
yE t t
t t
t
t t t t t t t
t t t t t t t
t t t t
| | | |
= + + +
| |
\ . \ .
| | | |
+ + + +
| |
\ . \ .
| |
+ + +
|
\ .
20 0.1* zE t =
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 22
2
2
2
10 1 1 1 1 1 1 1 1 1
sin sin( ) cos( ) cos sin( )
9 6 6 3 6 3 6 6 3 3
20 1 1 1 1 1 1 1 1
cos sin( ) sin sin( ) cos( )
9 6 6 3 6 6 3 3 3
10 1 1 1 1 1 1 1
cos sin( ) cos( ) sin
9 6 6 3 6 3 6 6
vEx t t t
t t t
t t
t t t t t t t t t t
t t t t t t t t t
t t t t t t t t
| | | |
= + + + +
| |
\ . \ .
| | | |
+ + +
| |
\ . \ .
| |
+ + +
|
\ .
2
2
1 1
sin( )
3 3
20 1 1 1 1 1 1 1 1
sin sin( ) cos sin( ) cos( )
9 6 6 3 6 6 3 3 3
25 1 1 1 1 1 1
sin sin( ) cos( )
18 6 6 3 6 3 6
t
t t t
t t
t t
t t t t t t t t t
t t t t t t t
| |
+
|
\ .
| | | |
+ + +
| |
\ . \ .
| |
+ + +
|
\ .
2
2
2
10 1 1 1 1 1 1 1 1 1
cos sin( ) cos( ) cos sin( )
9 6 6 3 6 3 6 6 3 3
20 1 1 1 1 1 1 1 1
sin sin( ) sin sin( ) cos( )
9 6 6 3 6 6 3 3 3
10 1 1 1 1 1 1 1
sin sin( ) cos( ) sin
9 6 6 3 6 3 6 6
vEy t t t
t t t
t t
t t t t t t t t t t
t t t t t t t t t
t t t t t t t t
| | | |
= + + + +
| |
\ . \ .
| | | |
+ + +
| |
\ . \ .
| |
+ + + +
|
\ .
2
2
1 1
sin( )
3 3
20 1 1 1 1 1 1 1 1
cos sin( ) cos sin( ) cos( )
9 6 6 3 6 6 3 3 3
25 1 1 1 1 1 1
cos sin( ) cos( )
18 6 6 3 6 3 6
t
t t t
t t
t t
t t t t t t t t t
t t t t t t t
| |
|
\ .
| | | |
+ + + +
| |
\ . \ .
| |
+ + + +
|
\ .
0.1 vEz =
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 23
c. th theo t
th ta im tc ng cui xE(t), yE(t),zE(t):
th xE(t):
th yE(t):
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 24
th zE(t):
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 25
d. Xy dng mt quy lut chuyn ng thao tc ca robot
Vn thit k qu o chuyn ng lin quan mt thit n bi ton iu
khin robot di chuyn t v tr ny n v tr khc trong khng gian lm vic. ng
i v qu o c thit k l cc lng t cho h thng iu khin v tr ca
robot. Do chnh xc ca qu o s nh hng n cht lng di chuyn ca
robot.
Thng thng, qu o dng a thc bc cao s p ng c cc yu
cu v v tr, tc , gia tc mi im gia 2 on di chuyn.
Yu cu ca thit k qu o l :
- Khu chp hnh phi m bo i qua ln lt cc im trong khng gian lm
vic hoc di chuyn theo mt qu o xc nh.
- Qu o ca robot phi l ng cong m bo tnh lin tc v v tr trong
mt khong nht nh.
- Khng c bc nhy v vn tc, gia tc.
- Qu o thng l ng cong thng thng.
Trn thc t hin nay c nhiu qu o l dng ng cong dng :
- a thc bc 2 :x(t)=a+bt+ct
2
- a thc bc 3 : x(t)=a+bt+ct
2
+dt
3
- a thc bc cao : x(t)=a+bt+..kt
n
Trong bi tp ln ny em s dng dng qu o l a thc bc 3 c dng:
X(t)=a+bt+ct
2
+dt
3
Thit k qu o a thc bc 3
T v tr ban u v hng ca tay robot, s dng phng trnh ng hc
ngc ta xc nh cc gi tr bin khp tng ng. Bi ton thit k qu o cho
khp l xc nh ng biu din ca v tr khp (gc quay ca khp quay hoc
khong tnh tin ca khp tnh tin) theo thi gian khi di chuyn t v tr ban u q
o
n v tr cui cng q
c
trong thi gian t
c
vi q l bin khp tng qut. Qu o di
chuyn ca khp gia 2 v tr s tha mn 4 iu kin: v tr ban u v v tr cui
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 26
cng, tc ti v tr ban u v v tr cui cng. Do a thc bc 3 s thch hp
cho qu o chuyn ng ca khp robot:
2 3
0 1 2 3
( )
i i i i i
q t a a t a t a t = + + +
(2.1)
Vi cc iu kin u, cui l:
0 0
( )
i i
q t q =
( )
i i
c c
q t q =
0 0
( )
i i
q t q
- -
=
( )
i i
c c
q t q
- -
=
(2.2a)
(Vi i l bin khp, i=1,2,3)
Trong thc t, tc ti v tr ban u v v tr cui cng ca khp bng khng:
0
( ) 0
( ) 0
i
i
c
q t
q t
-
-
=
=
(2.2b)
o hm phng trnh (3.1) ta c:
2
1 2 3
( ) 2 3
i i i i
q t a a t a t
-
= + + (2.3)
S dng cc iu kin u v cui (3.2) ta nhn c 4 phng trnh sau:
0 0 0
( )
i i i
q t a q = =
2 3
0 1 2 3
( )
i i i i i i
c c c c c
q t a a t a t a t q = + + + = (2.4)
0 1
( ) 0
i i
q t a
-
= =
2
1 2 3
( ) 2 3 0
-
= + + =
i i i i
c c c c
q t a t a t a t
Gii cc phng trnh trn ta c cc h s:
0 0
i i
a q =
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 27
1
0
i
a =
0
2 2
3
i i
i c
c
q q
a
t
=
0
3 3
2
i i
i c
c
q q
a
t
=
T 4 h s trn ta s c qu o dng a thc (3.1) ri t ta xc nh v tr ca
cc khp ti thi im bt k. Nhng gi tr l tn hiu t cho b iu khin v
tr truyn ng khp di chuyn n v tr tng ng.
V d:
Bi ton: Thit k qu o chuyn ng cho robot RRTR t v tr A n v tr
B trong khong thi gian 4s. Vi im: A (45, 0 ,-20) im :
B (
)
T phng trnh ng hc ngc ta tnh c gi tr cc bin khp ti cc im A,
B
Ti im A (45, 0 ,-20)
1
2
3
4
0
0
0
0
u
u
u
u
=
Ti im B (
)
suy ra
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 28
1 = 30
o
2 = 60
o
3 = 0
o
4 = 90
o
Theo (3.3) ta xc nh c cc h s ca cc khp:
- Khp 1
1
a
0
=
1
0
= 0
1
a
1
= 0,
1
a
2
= 3.6
1
a
3
= -0.32
Suy ra:
1
(t)= 3.6t
2
- 0.32t
3
0
= 0
2
a
1
= 0,
2
a
2
= 7.2
2
a
3
= -0.96
Ta c :
1
(t)= 7.2t
2
- 0.96t
3
1
0
= 0
4
a
1
= 0,
4
a
2
= 10.8
4
a
3
= -1.44
Ta c :
1
(t)= 2.4t
2
- 1.44t
3
1
]
0
r
c4
=
0
R
-
4
.
4
r
c4
=[
] [
]=[
]
0
r
4
=
0
R
4
.
4
r
4
=[
] [
]=[
]
0
c4
=[
]
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 33
0
4
=[
]
Thay vo h tng qut:
0 0 0
4,3 ,4 4
0 0
4 4
4,3 ,4 4,3 4
E
o o
c
E
F F P
M M r F r P
=
= +
F
3,4
= [
]= [
]
4,3
0
0
0
M
(
(
=
(
(
Xt khu 3:
Cc lc tc ng ln khu 3:
Lc
4,3 4,3
, F M nu trn.
Trng lc P
3
=[
]
=[
] [
]
Ma trn nh hng ca khu 3:
0
R
3
= [
]
Ta c:
3
r
3
=[
]
3
r
c3
=[
]
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 34
0
r
c3
=
0
R
3
.
3
r
c3
=[
] [
]=[
]
0
r
3
=
0
R
3
.
3
r
3
=[
] [
]=[
]
3
c3
=[
]
3
3
=[
]
Thay vo h tng qut:
0 0 0
3,2 4,3 3
0 0
3 3
3,2 4,3 3,2 3
o o
c
F F P
M M r F r P
=
= +
F
3,2
=[
]=[
]
3,2
0
0
0
M
(
(
=
(
(
Xt khu 2:
Cc lc tc ng ln khu 2:
Lc
3,2 3,2
, F M nu trn.
Trng lc
P
2
= [
]=[
] [
]
Ma trn nh hng ca khu 2:
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 35
0
R
2
=[
]
Ta c:
2
r
c2
=[
]
2
r
2
=[
]
0
r
c2
=
0
R
2
.
2
r
c2
[
] [
] [
]
0
r
2
=
0
R
2
.
2
r
2
[
] [
] [
]
0
c2
=[
]
0
2
=[
]
Thay vo h tng qut:
0 0 0
2,1 3,2 2
0 0
2 2
2,1 3,2 2,1 2
o o
c
F F P
M M r F r P
=
= +
F
2,1
=[
] [
]
M
2,1
=[
] [
] =
[
]
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 36
Xt khu 1:
Cc lc tc ng ln khu 1:
Lc
2,1 2,1
, F M nu trn..
Trng lc
P
1
= [
]=[
] [
]
Ma trn nh hng ca khu 1:
0
R
1
=[
]
Ta c:
0
r
c1
=[
]
0
r
1
=[
]
0
r
c1
=
0
R
1
.
0
r
c1
[
] [
] [
]
0
r
1
=
0
R
1
.
1
r
1
[
] [
] [
]
0
c1
=[
]
0
1
=[
]
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 37
Thay vo h tng qut:
0 0 0
1,0 2,1 1
0 0
1 1
1,0 2,1 1,0 1
o o
c
F F P
M M r F r P
=
= +
F
1,0
=[
] [
]
M
1,0
=[
]
Tnh ton lc tnh ln nht ti cc khp:
Qua tnh ton tnh hc trn nhn thy momen ti cc khp 3 v khp 4 u bng 0
Lc ti khp 3 (khp tnh tin) c gi tr c nh :
F
3,2
[
]
Kho st momen tnh hc ti khp 2 (khp quay):
M
2,1
= [
]
F
2,1
=[
]
Gi tr ca Momen ti khp 2:
M
2
=
=53.19676772
Kho st ti khp 1 (khp quay):
F
1,0
=[
] [
]
M
1,0
=[
]
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 38
Gi tr ca Momen ti khp 1:
M
1
=
=
Nhn thy gi tr ca momen ti khp 1 l mt hm s theo bin khp 1,2. D
dng thy c gi tr ln nht ca hm s trn t c khi 1=2. Hay ni cch
khc , cu hnh ca robot lc gc gia khu 1 v khu 2 ang bng 180 , cnh
tay ang vn ra xa nht. Gi tr momen lc s l :
M
1
= =365327,3985 (N/m)
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 39
4. Tnh ton ng lc hc
Chn ta suy rng l :
| | | |
1 2 3 4 1 2 3 4
T T
q q q q q d u u u = =
Bng m t v tr trng tm, khi lng, momen qun tnh khi ca tng khu ca
robot :
Khu V tr trng tm so
vi ta gn trn
khu
Khi
lng
Momen qun tnh khi tng khu
z2,z3,z4
x4
x3
x2
x1
z1
Zo
o
Xo
L1
L2 L3
L4
G1
G2
G3
G4
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 40
x
Gi
y
Gi
Z
Gi
I
xx
I
yy
I
zz
I
xy
I
xz
I
yz
1 -a
1
+L
1
0 0 M
1
I
1x
I
1y
I
1z
0 0 0
2 -a
1
+L
2
0 0 M
2
I
2x
I
2y
I
2z
0 0 0
3 0 0 -L
3
M
3
I
3x
I
3y
I
3z
0 0 0
4 0 0 -L
4
M
4
I
4x
I
4y
I
4z
0 0 0
Theo kt qu ca phn tnh ton ng hc thun ta c cc ma trn DH bin i cc
khu v khu c s nh sau :
1 1 1 1
1 1 1 1
1
0
0
0 0 1 0
0 0 0 1
C S a C
S C a S
T
(
(
(
=
(
(
12 12 1 1 2 12
12 12 1 1 2 12
2
0
0
0 0 1 0
0 0 0 1
C S a C a C
S C a S a S
T
+ (
(
+
(
=
(
(
12 12 1 1 2 12
12 12 1 1 2 12
3
3
0
0 a
0 0 1
0 0 0 1
C S a C a C
S C S a S
T
d
+ (
(
+
(
=
(
(
12 4 12 4 12 4 12 4 1 1 2 12
12 4 12 4 12 4 12 4 1 1 2 12
4
3 4
0
0
0 0 1
0 0 0 1
C C S S S C C S a C a C
S C C S C C S S a S a S
T
d d
+ + (
(
+
(
=
(
(
T cc ma trn T
i
(i=1,2,3,4), ta xc nh c ta khi tm ca cc khu i vi
h ta R
0
.
V tr khi tm ca khu th nht:
1 1 1 1 1 1 1 1
1 1 1 1 1 1 1
0 ( )
0 . 0
0 0 0 1 0 0
G
a C C S a L LC
r a S S C L S
( ( ( (
( ( ( (
= + =
( ( ( (
( ( ( (
(3.1)
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 41
o hm biu thc trn theo thi gian trong h quy chiu c nh ta c vn tc
khi tm
Suy ra vn tc khi tm khu th nht:
1 1 1
1 1 1 1 1
sin
os
0
G G
L q q
v r L q c q
(
(
= =
(
(
(3.2)
Ma trn Jacobi tnh tin ca khu th nht:
1 1
1 1 1
0 0 0
0 0 0
0 0 0 0
T
L S
J L C
(
(
=
(
(
(3.3)
V tr khi tm ca khu th 2:
1 1 2 12 12 12 2 2 1 1 2 12
2 1 1 2 12 12 12 1 1 2 12
0 ( )
0 . 0
0 0 0 1 0 0
G
a C a C C S a L a C L C
r a S a S S C a S L S
+ + ( ( ( (
( ( ( (
= + + = +
( ( ( (
( ( ( (
(3.4)
Vn tc khi tm ca khu th 2:
1 1 1 2 12 12
2 2 1 1 2 12 12
0
G G
a q S L q S
v r a q C L q C
(
(
= = +
(
(
(3.5)
Ma trn Jacobi tnh tin ca khu th 2:
1 1 2 12 2 12
2 1 1 2 12 2 12
0 0
0 0
0 0 0 0
T
a S L S L S
J a C L C L C
(
(
= +
(
(
(3.6)
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 42
V tr khi tm ca khu th 3:
1 1 2 12 12 12
3 1 1 2 12 12 12
3 3
1 1 2 12 1 1 2 12
3 1 1 2 12 1 1 2 12
3 3 3 3
0 0
0 . 0
0 0 1
G
G
a C a C C S
r a S a S S C
d L
a C a C a C a C
r a S a S a S a S
d L q L
+ ( ( (
( ( (
= + +
( ( (
( ( (
+ + ( (
( (
= + = +
( (
( ( + +
(3.7)
Vn tc khi tm ca khu th 3:
1 1 1 2 12 12
3 3 1 1 2 12 12
3
G G
a q S a q S
v r a q C a q C
q
(
(
= = +
(
(
(3.8)
Ma trn Jacobi tnh tin ca khu th 3:
1 1 2 12 2 12
3 1 1 2 12 2 12
0 0
0 0
0 0 1 0
T
a S L S L S
J a C L C L C
(
(
= +
(
(
(3.9)
V tr khi tm ca khu th 4:
1 1 2 12 12 4 12 4
4 1 1 2 12 12 4 12 4
3 4 4
1 1 2 12 1 1 2 12
4 1 1 2 12 1 1 2 12
3 4 4 3 4 4
0 0
0 . 0
0 0 1
G
G
a C a C C S
r a S a S S C
d d L
a C a C a C a C
r a S a S a S a S
d d L q d L
+ ( ( (
( ( (
= + +
( ( (
( ( (
+ + ( (
( (
= + = +
( (
( ( + +
(3.10)
Vn tc khi tm ca khu th 4:
1 1 1 2 12 12
4 4 1 1 2 12 12
3
G G
L q S a q S
v r L q C L q C
q
(
(
= = +
(
(
(3.11)
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 43
Ma trn Jacobi tnh tin ca khu th 4:
1 1 2 12 2 12
4 1 1 2 12 2 12
0 0
0 0
0 0 1 0
T
L S a S a S
J L C a C a C
(
(
= +
(
(
(3.12)
T cc ma trn T
i
ta suy ra c cc ma trn cosin ch hng ca cc khu l:
Ma trn cosin ch hng ca khu th nht:
1 1
1 1 1
0
0
0 0 1
C S
A S C
(
(
=
(
(
Ma trn cosin ch hng ca khu th 2:
12 12
2 12 12
0
0
0 0 1
C S
A S C
(
(
=
(
(
Ma trn cosin ch hng ca khu th 3:
12 12
3 12 12
0
0
0 0 1
C S
A S C
(
(
=
(
(
Ma trn cosin ch hng ca khu th 4:
12 4 12 4
4 12 12 4
0
4 0
0 0 1
C S
A S C
(
(
=
(
(
Ton t sng vc-t vn tc gc khu th nht:
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 44
1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1
0 0 0 0
. 0 . 0 0 0
0 0 1 0 0 1 0 0 0
T
S q C q C S q
A A C q S q S C q e
( ( (
( ( (
= = =
( ( (
( ( (
(3.13)
Suy ra vec-to vn tc gc trng h R
0
l:
| |
1 1
0 0
T
q e =
(3.14)
Ma trn Jacobi quay ca khu th nht:
1
0 0 0 0
0 0 0 0
1 0 0 0
J
e
(
(
=
(
(
(3.15)
Ton t sng vc-t vn tc gc khu th 2:
12 12 12 12 12 12
2 2 2 12 12 12 12 12 12
0 0
. 0 . 0
0 0 1 0 0 1
T
S q C q C S
A A C q S q S C e
( (
( (
= =
( (
( (
(3.16)
Suy ra:
(3.17)
Ma trn Jacobi quay ca khu th 2:
1 2
2 1 2
0 0
0 0
0 0 0
q q
q q e
(
(
= +
(
(
2
0 0 0 0
0 0 0 0
1 1 0 0
J
e
(
(
=
(
(
(3.18)
| |
2 1 2
0 0
T
q q e = +
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 45
Ton t sng ca vc-t vn tc gc khu th 3:
12 12 12 12 12 12
3 3 3 12 12 12 12 12 12
1 2
3 1 2
0 0
. 0 . 0
0 0 1 0 0 1
0 0
0 0
0 0 0
T
S q C q C S
A A C q S q S C
q q
q q
e
e
( (
( (
= =
( (
( (
(
(
= +
(
(
(3.19)
Suy ra:
| |
3 1 2
0 0
T
q q e = +
(3.20)
Ma trn Jacobi quay ca khu th 3:
3
0 0 0 0
0 0 0 0
1 1 0 0
J
e
(
(
=
(
(
(3.21)
Ton t sng ca vc-t vn tc gc khu th 4:
12 4 12 4 12 4 12 4 12 4 12 4
4 4 4 12 4 12 4 12 4 12 4 12 4 12 4
1 2 4
4 1 2 4
0 0
. 0 . 0
0 0 1 0 0 1
0 0
0 0
0 0 0
T
S q C q C S
A A C q S q S C
q q q
q q q
e
e
( (
( (
= =
( (
( (
+ (
(
= +
(
(
(3.22)
| |
4 1 2 4
0 0
T
q q q e = +
(3.23)
Ma trn Jacobi quay ca khu th 4:
2
0 0 0 0
0 0 0 0
1 1 0 1
J
e
(
(
=
(
(
(3.24)
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 46
Cc ma trn momen qun tnh ca cc khu, tnh i vi cc h ta i qua khi
tm tng ng v c cc trc song song vi cc trc ca h ta R
i
i vi khu th nht:
1
1 1
1
0 0
0 0
0 0
x
y
z
I
I I
I
(
(
=
(
(
(3.25)
i vi khu th 2:
2
2 2
2
0 0
0 0
0 0
x
y
z
I
I I
I
(
(
=
(
(
(3.26)
i vi khu th 3:
3
3 3
3
0 0
0 0
0 0
x
y
z
I
I I
I
(
(
=
(
(
(3.27)
i vi khu th 4:
4
4 4
4
0 0
0 0
0 0
x
y
z
I
I I
I
(
(
=
(
(
(3.28)
Ma trn khi lng xc nh
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 47
2 2 2 2 2 2 2
11 1 1 2 1 2 1 2 2 3 1 1 2 2 2 1 1 2 2 2
1z 2z 3z 4z
2 2 2
12 2 2 1 2 2 3 2 2 1 2 4 2 2 1 2 2z 3z 4z
13 31
14 41
( cos ) m (a 2a a cosq a ) m4(a 2a a cosq a )
I I I I
m m (L a cosq L ) m (a a a cosq ) m (a a a cosq ) I I I
m m 0
m m
m m L m a L a q L = + + + + + + + + +
+ + + +
= + + + + + + + +
= =
= =
4z
I
2 2 2
21 2 2 1 2 2 3 2 2 1 2 4 2 2 1 2 2z 3z 4z
2 2 2
22 2 2 3 2 4 2 2z 3z 4z
23 32
24 42 4z
33 3 4
34 43
44 4z
m m (L a cosq L ) m (a a a cosq ) m (a a a cosq ) I I I
m m L m a m a I I I
m m 0
m m I
m m m
m m 0
m I
= + + + + + + + +
= + + + + +
= =
= =
= +
= =
=
Ma trn C( , ) q q xc nh theo cng thc :
ij
1
( , ) .
n
ijk k
k
c q q h q
=
=
nn ta c :
11 2 2 1 2 3 2 1 2 4 2 1 2 2
12 2 2 1 2 3 2 1 2 4 2 1 2 1 2 2 1 2 3 2 1 2 4 2 1 2 2
13 14
21 2 2 1 2 3 2 1 2 4 2 1 2 1
c ( m L a sinq m a a sinq m a a sinq )
c ( m L a sinq m a a sinq m a a sinq )q ( m L a sinq m a a sinq m a a sinq )
c c 0
c (m L a sinq m a a sinq m a a sinq )
c
q
q
q
=
= +
= =
= + +
22 23 24
31 32 33 34
41 42 43 44
c c 0
c c c c 0
c c c c 0
= = =
= = = =
= = = =
Biu thc ng nng ca h c dng :
( )
11 12 13 14 1
21 22 23 24 2
1 2 3 4
31 32 33 34 3
41 42 43 44 4
1
. ( ).
2
1
2
T
T q M q q
m m m m q
m m m m q
T q q q q
m m m m q
m m m m q
=
| || |
| |
| |
=
| |
| |
\ . \ .
(3.29)
Biu thc th nng ca h c dng :
| |
1 C1 2 C2 3 C3 4 C4
2 1 3 1 3 3 4 1 3 4 4
0 0 g {m r m r m r m r }
m .d .g m .(d q L ).g m .(d q d L ).g
H = + + +
H = + + + +
(3.30)
o hm biu thc H theo q
i
ta s thu c cc lc suy rng c th nh sau :
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 48
3 4
( ) (0, 0, , 0)
T
g q m g m g =
Nu a vo thnh phn lc cn nht khp ng th hm hao tn c dng :
2 2 2 2
1 1 2 2 3 3 4 4
1
( )
2
b q b q b q b q = + + +
o hm biu thc
theo i
q
ta s thu c cc lc cn nht (l thnh phn lc
khng th) nh sau:
1 1
2 2
3 3
4 4
c
b q
b q
L
b q
b q
(
(
(
=
(
(
(3.31)
Gi i
t
l cc lc (nu t ti khp tnh tin ) hoc ngu lc (nu n t ti khp
quay) trc tip lm cho khp chuyn ng . Nh vy ta k hiu vc t lc ng c
to nn dch chuyn cc khp ng l :
1
2
3
4
L
t
t
t
t
t
(
(
(
=
(
(
(3.32)
Thay vo phng trnh :
( ). ( , ). ( ) ( ) M q q C q q q g q t t + + =
Ta thu c h phng trnh chuyn ng vi phn ca robot nh sau :
11 1 12 2 13 3 14 4 2 2 1 2 3 2 1 2 4 2 1 2 1 2
2
2 2 1 2 3 2 1 2 4 2 1 2 2 1 1 1
21 1 22 2 23 3 24 4 2 2 1 2 3 2 1 2 4 2 1
m q m q m q m q 2(m L a sinq m a a sinq m a a sinq ) q
(m L a sinq m a a sinq m a a sinq ) q
m q m q m q m q (m L a sinq m a a sinq m a a sin
q
q b t
+ + + + +
+ + + =
+ + + + + +
2
2 1
2 2 2
q )q
b q t + =
31 1 32 2 33 3 24 4 3 4 3 3 3
41 1 42 2 43 3 44 4 4 4 4
m q m q m q m q (m m )g b q
m q m q m q m q b q
t
t
+ + + + + =
+ + + + =
Nu vit di dng ma trn th thu c h phng trnh nh sau:
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 49
( ). ( , ). ( )
c
M q q C q q q g q L L
t
+ + + =
(3.33)
M phng mt s bi ton ng lc hc bng phn mm maple
Bi ton thun
Cho bit
( )
i
t t
, cc thng s ng hc ca robot bit trc, ta cn tm cc quy
lut chuyn ng ca cc khp tng ng.
Gii h phng trnh (*) pha trn bng Maple vi cc thng s u vo nh sau:
2sin( ) 2.5sin( ) 20 1.5sin( ) 4sin( )
12 4 6 3
T
t
L t t t t
t t t t
t t t t
(
= + + + + +
(
iu kin u :
| |
| |
0
0
0.1 0 0.2 0, 3
0 0 0 0
T
T
q
q
=
=
Cc thng s ng hc :
a
1
= 0,5m ; a
2
= 0,4m ; d
4
= 0,1m ; g =9,81m/s
2
L
1
= 0,25m ; L
2
= 0,2m ; L
4
= 0,05m
b
1
= 0,5; b
2
= 0,5; b
3
= 0,5; b
4
= 0,5;
m
1
= 10,238 kg m
2
= 3,606 kg
m
3
= 2,134 kg
m
4
= 0,511 kg
Cc thng s ng lc hc :
I
1x
= 1,980 ; I
1y
= 2,081 ; I
1z
= 0,238 (kg.m
2
)
I
2x
= 0,011 ; I
2y
= 0,104; I
2z
= 0,107;
I
3x
= 0,005 ; I
3y
= 0,040; I
3z
= 0,041;
I
4x
= 0,081 ; I
4y
= 0,081 ; I
4z
= 0,000;
Kt qu m phng trong khong thi gian t = [0,2.5] nh sau :
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 50
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 51
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 52
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 53
Bi ton ngc
Cho quy lut ca X(t) nh sau, xt trong khong thi gian T = 4s
3 2
.sin
2 3
5
0.4 0.15cos
6
5
0.35 0.05sin
6
( )
0.415 0.1sin
2
p
p
p
t
t
x
t
y
X t
z
t
e
t
t
t
t
| |
|
\ .
( | |
+
| (
\ .
(
(
(
| |
+ (
( |
\ .
(
( = =
(
(
| |
(
(
|
\ .
(
(
(
53auk hi gii bi ton ng lc hc ngc, ta s tm c q(t),do hm q(t) theo t ta
s tm c vn tc, gia tc tng ng ca bin khp. Thay vo phng trnh vi
phn chuyn ng (3.33) ta s tm c L
t
. Tin hnh m phng s ta c:
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 54
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 55
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 56
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 57
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 58
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 59
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 60
5. Thit k h dn ng cho khp th nht
Thng s u vo
M men ln nht Mmax = 365,32 Nm = 365320 Nmm
Vn tc gc khu 1 ln nht w
max
= 30 vng/ pht = (rad/s)
Tnh ton
Tnh s b cng sut
Cng sut ca khu 1
P1= T.w= 365,32. =1147.687 W = 1,148 kW
Cng sut s b ca trc ng c
q
1
P
P
dc
=
Vi: ... . . .
4 3 2 1
q q q q q =
Trong
i
q l hiu sut ca cc b truyn v cc cp ln trong h dn ng.
Tnh ton s b h dn ng khu 1 gm c 2 b truyn bnh rng tr (hiu sut
1
q ), 3 cp ln (hiu sut
2
q ), Ta tra bng 2.3[1], ta c:
ng c
Hp gim tc
Khu 1
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 61
1
= 0,97,
2
= 0.995
Ta c:
=
1
2
2
3
=0.956
Do cng sut s b ng c:
P
c
=
Kw
Tnh chn ng c
Theo ph lc bng P1.3 sch tnh ton thit k h dn ng c kh ta chn c
ng c
- Model : 4A100L8Y3
- Tc vng quay ng b: 750 vng/ pht
- Vn tc quay 698 vng / pht
- Cng sut ng c: 1.5 Kw
- Cos = 0.65
-
=1,7
-
Tnh ton v phn phi t s truyn
Vi ng c va chn c, ta c t s truyn tng
u
=25
Vi t s truyn thp nn ta s chn hp gim tc bnh rng tr rng thng 2 cp.
Trong qu trnh tnh ton ta s ch tnh ton cho 1 cp bnh rng do y l b
truyn 2 cp ging nhau c cng t s truyn gia cp nhanh v cp chm.
S
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 62
Thng s u vo
Cng sut trn trc ra (tay quay): P1=1,2 kW
Momen ca trc ra: T1 =365320 Nmm
Tc quay trc ra: n
1
=30 vng/pht
T s truyn u=u
1
.u
2
=5.5=25
Chn vt liu
Do khng c yu cu g c bit v theo quan im thng nht ha trong thit
k, y chn vt liu 2 cp bnh rng nh sau:
Bnh nh: Thp 45 ti ci thin t rn HB241285,
b1
=850 Mpa,
ch1
=
580 Mpa
Bnh ln: Thp 45 ti ci thin t rn HB192240,
b2
=750 Mpa,
ch2
=
450 Mpa
Xc nh ng sut cho php:
Theo bng 6.2 vi thp 45 ti ci thin t rn HB180350
o
Hlim
= 2HB + 70 , S
H
= 1,1,
o
Flim
= 1,8HB, S
F
= 1.75
Chn rn bnh rng nh HB
1
=245, bnh rng ln HB
2
=230 khi :
o
Hlim1
=2HB + 70= 2.245+70=560 Mpa
o
Flim
1=1,8HB=1,8.245 =441 Mpa
o
Hlim2
=2HB + 70= 2.230+70= 530 Mpa
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 63
o
Flim2
=1,8HB=1,8.230=414 Mpa
Theo cng thc (6.5) ti liu
N
Ho
= 30H
2,4
HB
do
N
Ho1
=30.245
2,4
= 1,6. 10
7
N
Ho2
=30.230
2,4
= 13.9.10
7
Theo cng thc (6.7) ti liu
N
HE
=60c(Ti/Tmax)
3
.n
i
t
i
N
HE2
=60c.n
1
/u
1
t
i
(Ti/Tmax)
3
.n
i
.t
i
Thay s vo ta tnh c
N
HE2
=6,05.10
7
> N
Ho2
do K
HL2
=1
Suy ra N
HE1
> N
Ho1
Do K
HL1
=1
Vy theo ct (6.1a) tnh c s b
[
H
] =
Hlim
.K
HL
/S
H
[
H
]
1
=560.1/1,1=509 Mpa
[
H
]
2
=530.1/1,1=481,8 Mpa
Tnh ton b truyn
Tnh s b khong cch trc:
Thay s ta tnh v chn c:
a
w1
= 85
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 64
Cc thng s n khp
m=(0,01...0.02)a
w
= (0.010.02).85=0.851.7
Chn m = 1,5
Z
1
= 2a
w
/[m(u+1)]=2.85/[1,5(5+1)]= 18,89
Chn Z
1
=19
Z
2
=u.Z
1
=5.19= 95
Do a
w
= m.(Z
1
+Z
2
)/2=1,5.(19+95)/2=85,5 mm
Ly a
w
= 90 mm do cn dch chnh tng khong cch trc t 85,5 ln 90
H s dch tm y= a
w
/m-0,5(Z
1
+Z
2
)= 90/1,5-0,5.(19+95)= 3 mm
k
y
=1000y/Zt = 1000.0,5/(19+95)= 4,386
Theo bng 6.10a ti liu ta tra c
k
x
= 0,05
H s gim nh rng:
y
= k
x
.Zt/1000 = 0,05.(19+95)/1000=0,00057
Tng h s dch chnh :
x = y+
y
=3 +0,00057 =3,00057
b
w
=88(
H
/[
H
])
2
= 35
Cc thng s v kch thc b truyn
Khong cch trc ln a
w
90
Khong cch trc
chia
a 85,5
ng knh chia
bnh nh
d
1
28,5
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 65
ng knh chia
bnh ln
d
2
142,5
Modul m 1,5
T s truyn u 5
S rng bnh rng
nh
Z
1
19
S rng bnh rng
ln
Z
2
95
H s dch chnh x 3,00057
Chiu rng vnh rng b
w
35
Chn s b ln
Da vo ng knh s b cc trc, v iu kin s dng bnh rng tr rng thng
ta chn loi ln: bi c trung
S dng phn mm solid work chng em chn c ln:
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 66
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 67
S dng phn mm solidwork chng em v c hp gim tc v ng c
ng c
Hp gim tc
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 68
6. Thit k h thng iu khin
a. Chn lut iu khin ph hp
Khi thit k thng iu khin ta c th b qua ng hc ca c cu chp hnh,qun
tnh ca ng c v b bin i.Nh vy t chc nng ca b iu khin l to ra
mt moomen cn thit truyn ng khp robot m bo khp robot lun bm
theo v tr t .
Ta chn thit k h theo h thng phn hi trong khng gian khp
Hnh 6.1 S tng qut ca h thng iu khin phn hi
Lut iu khin :
Hnh 6.2 S cu trc h thng iu khin robot vi b iu khin PD
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 69
Ta c phng trnh ng lc hc :
( ) ( , ) ( )
T
E
M q q C q q q G q U J F + + = +
, gi
H(q)=M(q)trnh nhm vi M l vector momen v ( , ) ( , ) V q q C q q q = , M U =
tip n ta coi robot khng chu tc dng ca ngoi lc v lut iu khin bm qu
o 0 F = nh vy phng trnh ng lc hc c rt gn nh sau:
( ). ( , ) ( ) M H q q V q q G q = + +
Lut iu khin : ( ) ( )
dk p d d d p d
M K q q K q q K K c c = + = +
(6.1)
Trong :
1 2
( , ,..., )
p p p pn
K diag K K K = - ma trn ng cho cc h s khuch
i ca tng khp ring bit.
1 2
( , ,..., )
d d d dn
K diag K K K =
-ma trn ng cho cc h s
khuch i o hm ca tng khp ring bit.
Vi lut iu khin ny gi thit thnh phn momen trng lc G(p) c
b hon ton.
H thng iu khin vi cu trc b iu khin nh trn, n nh tuyt i ton
cc. Thc vy chn hm Liapunov c dng nh sau:
1
.( . . . . )
2
L
T
T
Kp H q q V
c
c
=
+
(6.2)
Hm V
L
biu th tng nng lng ca h thng robot: Thnh phn cha Kp t
l vi nng lng u vo, thnh phn sau l ng nng ca robot m Kp v H l
cc ma trn c h s dng .Nn hm V
L
> 0 vi q khc qd.
Tnh o hm cp 1 ca V
L
ta nhn c:
1
.( . . . . . . . . . . )
2
L
T T T
T T
Kp Kp H q H q H q q q q
V
c c
c c
=
+ + + +
Do tnh cht i xng ca cc thnh phn . .
T
Kpc
c
, . .
T
H q
q
, ta rt gn c
1
. . . . . . .
2
L
T T
T
Kp H q H q q q
V
c
c
=
+ +
T phng trnh ng lc hc vi gi thit khng c thnh phn momen trng
lc G(q), nhn c phng trnh sau :
1
. . . . . .[ ( , )]
2
L
T T
T
Kp H q M V q q q q
V
c
c
=
+ +
S dng thuc tnh ca phng trnh ng lc hc v p dng lut iu khin
(6.1) ta c
1 1
. . . ( , ). . . ( ). . . .( . )
2 2
L
T T T T T
Kd C q q q H q q Kd H C q q q q q q
V
c c
= =
+ +
Trong :
( , ) ( , ). V q q C q q q =
.
Do ma trn
1
.
2
H C
l ma trn i xng ngc =>
1
.( . ) 0
2
T
H C q
q
=
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 70
. . 0
T
L
Kd q
q
V
s
=
(6.3)
T (6.2) ,(6.3) cho thy rng, mc dng ca V
L
ph thuc vo Kp; mc
m ca
L
V
ph thuc vo Kd . Do tng tc hi t bng tng gi tr Kd. Nng
cao chnh xc tinh ca h thng iu khin t c bng tng h s Kp ca
khu khuch i. Tuy nhin ,Kp v Kd qu ln s lm gim n nh v cht
lng qu trnh qu nh qu iu chnh , thi gian qu tng.
b. M phng bng Matlab
S dng th vin SimMechanis vi m hnh robot Robot c xut ra t
solidwork
Hnh 6.3 m hnh m phng
T m hnh bn v lp ca m hnh robot RR ny ta xut sang file *.xml ri
dng matlab c xut ra file di dng simulink, ri thit kdi dng cc khi ca
h thng iu khin ma trn Jacobien chuyn v.
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 71
Hnh 6.4 S khi iu khin ma trn jacobien chuyn v
Cc khi con nh nh sau :
Hnh 6.5 khi PD
Trong khi robot RR phn quan trng nht l phn xut t soid sang c s khi
nh sau:
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 72
Hnh 6.5 Khi con robot xut ra t Solidwork
Vi qu o ca cc khp thit lp trong phn thit lp qu o
Ta chn cc h s khuch i t l v o hm c la chn nh sau :
10000 0 0
0 10000 0
0 0 10000
P
K
| |
|
=
|
|
\ .
,
200 0 0
0 200 0
0 0 200
D
K
| |
|
=
|
|
\ .
Trong :
=(3.6*u(1)^2-0.48*u(1)^3)*pi/180
=(7.2*u(1)^2-0.96*u(1)^3)*pi/180
=(14.4*u(1)-2.88*u(1)^2)*pi/180
=(7.2*u(1)-1.44*u(1)^2)*pi/180
=(14.4*u(1)-2.88*u(1)^2)*pi/180
+ khi scope hin th tn hiu u ra ca qu trnh m phng theo thi gian.
+ khi thi gian t gi tr bng 8(s)
Hnh 6.6 S h thng
Kt qu m phng:
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 74
Hnh 6.7
Hnh 6.8 Sai s bin khp theo thi gian
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 75
Hnh 6.9 V tr cc khp theo thi gian
Hnh 6.10 Vn tc cc khp theo thi gian
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 76
Contents
MC LC ......................................................................................................................... 1
1. Phn tch la chn cu trc .......................................................................................... 2
a. Khi qut v robot ......................................................................................................... 2
Mt, mi, tai ...................................................................................................................... 2
b. Cu trc ng hc robot............................................................................................... 4
1.2 Bc t do ca robot..................................................................................................... 6
c. Cc phng n thit k cu trc ph hp cho cc khu khp ....................................... 7
d. Phn tch, chn, thit k cu trc c chn ................................................................ 8
2. Gii bi ton ng hc .................................................................................................12
3. Tnh ton lc ................................................................................................................31
4. Tnh ton ng lc hc ................................................................................................39
M phng mt s bi ton ng lc hc bng phn mm maple .....................................49
Bi ton thun..................................................................................................................49
5. Thit k h dn ng cho khp th nht .....................................................................60
6. Thit k h thng iu khin ........................................................................................68
Do tnh cht i xng ca cc thnh phn . .
T
Kpc
c
, . .
T
H q
q
, ta rt gn c .....................69
S dng th vin SimMechanis vi m hnh robot Robot c xut ra t solidwork ..........70
Hnh 6.11 Mmen cc khp theo thi gian
TNH TON THIT K ROBOT
GVHD: INH KHC TON Page 77