Trch t Lun Vn Thc S Tc gi: Nguyn Gia Minh Tho

. Khoa in-in T, H Bch Khoa TpHCM.

CHNG 3. THIT K B IU KHIN CHO ROBOT HAI BNH T

CN BNG
3.1.

M hnh ha robot hai bnh t cn bng. [1],[10],[11]

M hnh h thng ca robot hai bnh t cn bng c phn tch Hnh 3.1.

Hnh 3.1: M hnh phn tch ca robot hai bnh t cn bng

Bng 3.1: Bng k hiu v gi tr cc thng s ca robot hai bnh t cn bng
K hiu

Thng s

Gi tr
[n v]

xW , yW

V tr ca bnh xe theo hng trc x, trc y

[m]

xB , y B

V tr ca trng tm thn robot theo hng

[m]

trc x v hng trc y

Trch t Lun Vn Thc S Tc gi: Nguyn Gia Minh Tho

. Khoa in-in T, H Bch Khoa TpHCM.

M WL = M WR = M W Khi lng bnh xe, bnh xe tri v bnh xe

0.5 [kg]

phi c khi lng bng nhau

MB

Khi lng qui i ti trng tm ca thn

7 [kg]

robot
Gc lt thn robot.

[rad]

Gc quay

[rad]

RW

Bn knh bnh xe

0.075

= B

[m]
Khong cch t trng tm thn robot n

0.36 [m]

trc z ca hai bnh xe

D

Khong cch gia hai bnh xe

CL , CR

0.35 [m]

Moment tc dng ca ng c kt ni vi

[Nm]

bnh xe tri, bnh xe phi

Gia tc trng trng

9.8[ ms 2
]

H L , H R , VL ,VR

Lc, phn lc tng tc gia thn robot v

[Nm]

hai bnh xe tri, phi

VTL ,VTR

Phn lc ca mt t tng tc ln hai bnh

[Nm]

xe tri, phi

HTL , H TR

Lc ma st gia bnh xe tri, phi vi mt

[Nm]

ng

Xt bnh xe bn tri (bnh xe bn phi cng tng t)

S dng nh lut 2 Newton ln lt theo cc trc x, trc y v trc quay ca bnh xe.

M W
xWL = H TL H L

(3.1)

M W
yWL = VTL VL M W g

(3.2)

Trch t Lun Vn Thc S Tc gi: Nguyn Gia Minh Tho

. Khoa in-in T, H Bch Khoa TpHCM.

J WLWL = CL H TL R

(3.3)

V tr ca bnh xe v thn robot.

xWL = WL R

(3.4)

xWL = WL R

(3.5)

xWL = WL R

(3.6)

x + xWR
xB = L sin B + WL

(3.7)

x + xWR
xB = L ( cos B )B + WL
2

= L ( cos B ) B + xWM

( )

xB = L ( cos B )B L ( sin B ) B

(3.8)

+
xWM

(3.9)

y B = L (1 cos B )

(3.10)

y B = L ( sin B )B

(3.11)

( )

yB = L ( sin B ) B L ( cos B ) B

(3.12)

Xt thn robot
S dng nh lut 2 Newton ln lt theo cc trc x, trc y v trc quay ti im
trng tm ca thn robot.

M B
xB = H L + H R
M B
yB = VL + VR M B g + FC = VL + VR M B g +

(3.13)

( CL + CR )
L

J BB = (VL + VR ) L sin B ( H L + H R ) Lcos B ( C L + C R )

J = ( H L H R )

D
2

sin B

(3.14)
(3.15)
(3.16)

Trch t Lun Vn Thc S Tc gi: Nguyn Gia Minh Tho

. Khoa in-in T, H Bch Khoa TpHCM.

Th (3.15) , (3.14) vo (3.13), ta c:

( C + CR ) sin L sin M Lcos x C + C

J BB = M B
yB + M B g L
( L R ) (3.17)
B
B
B
B B
L

Rt gn (3.17) ta c :

J BB = M B L (
y B sin B ( cos B )
xB ) + M B gL sin B 1 + ( sin B )

) (C

+ CR ) (3.18)

T (3.9) v (3.12), ta c:

x +
xWR

yB sin B ( cos B )
xB = LB ( cos B ) WL
2

xW (3.19)
= L B ( cos B )

Th (3.19) vo (3.18) ta c:

2
J BB = M B L ( cos B )
xW M B L2B + M B gL sin B 1 + ( sin B ) ( CL + CR ) (3.20)

T (3.1) suy ra:

M W
xWL + M W
xWR = H TL H L + H TR H R = ( H L + H R ) + ( H TL + H TR ) (3.21)
T (3.3) suy ra : H TL =

C L J WLWL
CR J WRWR
H
=
;
TR
R
R

(3.21.b)

Th (3.13), (3.21.b) vo (3.21), ta c:

M W (
xWL +
xWR ) = M B
xB +

CL + CR J WLWL + J WRWR

)
(3.22)

xWL +
xWR = 2
xWM ; JWLWL = JWRWR = JWW ; t C = CL + CR
Ta c:
xWM = M B
xB +
Biu thc (3.22) s tr thnh: 2M W

C J W WL + WR

C 2 J WB
1

2
M
x
=

M
x
+

M: ( WL
W WM
B B
WR )
B , suy ra :
R
2

(3.23)

Th (3.9) v (3.23) ta c:

( )

2 M W
xWM = M B L ( cos B ) B + M B L ( sin B ) B

2 J WB
C
M B
xWM +
(3.24)
R
R

Trch t Lun Vn Thc S Tc gi: Nguyn Gia Minh Tho

. Khoa in-in T, H Bch Khoa TpHCM.

T phng trnh (3.20), (3.24) v thay : B bi , xWM bi x ; ta c h phng trnh

m t h thng nh sau:

2
J = M L ( cos )
x M B L2 + M B gL sin 1 + ( sin ) C
B
B

2
2 J W
C

2M W
x = M B L ( cos ) + M B L ( sin )
M B
x+

R
R

(3.25)

()

Xem moment qun tnh ca thn robot l mt thanh c chiu di L, khi lng M B ,
1
2
quay quanh trc z l trc ni gia hai bnh xe: J B = M B L
3

(3.26)

Xem moment qun tnh ca bnh xe robot l a trn xoay c bn knh R, khi
lng M W , quay quanh trc z trc ni gia hai bnh xe: J W =

1
M w R 2 (3.27)
2

Th (3.26) v (3.27) vo h phng trnh (3.25), ta c:

2
4
2

os
sin
1
sin
M
L

M
L
c

x
+
M
gL

C
(
)
(
)
B
B
B
3

2 ( M R + M L ( cos ) ) + C
( 2M + M )
sin
x
=
M
L

(
)
B
B
B
W
w

( )

(3.28)

Gii h phng trnh (3.28), ta c:

0,75( Mw R + MB L( cos ) ) ( cos )

1 =

2
M
+
M
L
(
)

W
B

0,75 1+ ( sin )2

2
0,75
M
L
sin
c
os
0,75
g
sin
0,75
c
os

(
)(
)
(
)
(
)
B

+
+

2

L
MB L
( 2MW + MB ) L
( 2MW + MB ) RL

2M + M 0,75( Mw R + MB L( cos ) ) ( cos ) x =

B
W

0,75( M R + M L( cos ) ) 1+ ( sin )2

B
2 0,75g ( Mw R + MB L( cos ) ) ( sin )
w

+
+ C
MBL( sin )
2

L
M
L
R
B

()

()

(3.29)

Trch t Lun Vn Thc S Tc gi: Nguyn Gia Minh Tho

. Khoa in-in T, H Bch Khoa TpHCM.

t bin trng thi nh sau: x1 = , x2 = , x3 = x, x4 = x . H phng trnh trng

thi m t robot (3.29) c vit li nh sau:

x1 = x2
x = f x + f x , x + g x C
2 1 ( 1) 2 ( 1 2 ) 1 ( 1)

x3 = x4
x4 = f3 ( x1 ) + f4 ( x1, x2 ) + g2 ( x1 ) C
Vi:

C = CL + CR
0.75 g ( sin x1 )

f1 ( x1 ) =
0.75 ( M w R + M B L ( cosx1 ) ) ( cosx1 )
1

M
M
L
2
+
(
)
B
W

0.75M B L ( sin x1 )( cosx1 )

2
( x2 )

( 2M W + M B ) L

f 2 ( x1 , x2 ) =
0.75 ( M w R + M B L ( cosx1 ) ) ( cosx1 )
1

M
M
L
2
+
(
)
W
B

0.75 1 + ( sin x )2

1
0.75 ( cosx1 )

M B L2
( 2 M W + M B ) RL

g1 ( x1 ) =
0.75 ( M w R + M B L ( cosx1 ) ) ( cosx1 )
1

2
M
+
M
L
(
)
W
B

0,75g ( Mw R + MB L ( cosx1 ) ) ( sin x1 )

f3 ( x1 ) =

0,75( Mw R + MB L( cosx1 ) ) ( cosx1 )

2MW + MB

(3.30)

Trch t Lun Vn Thc S Tc gi: Nguyn Gia Minh Tho

. Khoa in-in T, H Bch Khoa TpHCM.

( M L(sin x )( x ) )
2

f4 ( x1, x2 ) =

0,75( MwR + MBL( cosx1 ) ) ( cosx1 )

2MW + MB

0,75( M R + M L( cosx ) ) 1+ ( sin x )2

w
1
1
B
1

MB L2
R

g2 ( x1 ) =

0,75( Mw R + MB L( cosx1 ) ) ( cosx1 )

2MW + MB

T h phng trnh (3.30) ta thc hin m phng m hnh robot hai bnh t cn
bng trong Matlab/Simulink nh Hnh 3.2 .

Hnh 3.2: S m phng m hnh robot hai bnh trong MatLab/Simulink