mô hình xe 2 bánh tự cân bằng cơ khí

Trích từ Luận Văn Thạc Sĩ – Tác giả: Nguyễn Gia Minh Thảo . Khoa Điện-Điện Tử, ĐH Bách Khoa TpHCM.



CHƯƠNG 3. THIẾT KẾ BỘ ĐIỀU KHIỂN CHO ROBOT HAI BÁNH TỰ
CÂN BẰNG

3.1. Mô hình hóa robot hai bánh tự cân bằng. [1],[10],[11]
Mô hình hệ thống của robot hai bánh tự cân bằng được phân tích ở Hình 3.1.

Hình 3.1: Mô hình phân tích của robot hai bánh tự cân bằng
Bảng 3.1: Bảng ký hiệu và giá trị các thông số của robot hai bánh tự cân bằng
Ký hiệu Thông số Giá trị
[đơn vị]
W
x
,
W
y

Vị trí của bánh xe theo hướng trục x, trục y [m]
B
x
,
B
y

Vị trí của trọng tâm thân robot theo hướng
trục x và hướng trục y
[m]
Trích t

ừ
Lu
ậ
n V
ă
n Th
ạ
c S
ĩ
– Tác gi
ả
:
Nguyễn Gia Minh Thảo
. Khoa
Đ
i
ệ
n-
Đ
i
ệ
n T
ử
,
Đ
H Bách Khoa TpHCM.



WL WR W

M M M
= =

Khối lượng bánh xe, bánh xe trái và bánh xe
phải có khối lượng bằng nhau
0.5 [kg]
B
M

Khối lượng qui đổi tại trọng tâm của thân
robot
7 [kg]
B
θ θ
=

Góc lật ở thân robot. [rad]
δ

Góc quay [rad]
W
R

Bán kính bánh xe 0.075
[m]
L Khoảng cách từ trọng tâm thân robot đến
trục z của hai bánh xe
0.36 [m]
D Khoảng cách giữa hai bánh xe 0.35 [m]
,

L R
C C

Moment tác dụng của động cơ kết nối với
bánh xe trái, bánh xe phải
[Nm]
g

Gia tốc trọng trường
9.8[
2
ms
−
]

,
, ,
L R L R
H H V V

Lực, phản lực tương tác giữa thân robot và
hai bánh xe trái, phải
[Nm]
,
TL TR
V V

Phản lực của mặt đất tương tác lên hai bánh
xe trái, phải
[Nm]

,
TL TR
H H

Lực ma sát giữa bánh xe trái, phải với mặt
đường
[Nm]

• Xét ở bánh xe bên trái (bánh xe bên phải cũng tương tự)
Sử dụng định luật 2 Newton lần lượt theo các trục x, trục y và trục quay của bánh xe.
W W
L TL L
M x H H
= −
ɺɺ
(3.1)
W W WL TL L
M y V V M g
= − −
ɺɺ
(3.2)
Trích t
ừ
Lu
ậ
n V
ă
n Th
ạ
c S

ĩ
– Tác gi
ả
:
Nguyễn Gia Minh Thảo
. Khoa
Đ
i
ệ
n-
Đ
i
ệ
n T
ử
,
Đ
H Bách Khoa TpHCM.



WL WL L TL
J C H R
θ
= −
ɺɺ
(3.3)
• Vị trí của bánh xe và thân robot.
WL WL
x R

θ
=
(3.4)
WL WL
x R
θ
→ =
ɺ
ɺ
(3.5)

WL WL
x R
θ
→ =
ɺɺ
ɺɺ
(3.6)

W WR
sin
2
L
B B
x x
x L
θ
+
 
= +

 
 
(3.7)

( ) ( )
W WR
WM
os os
2
L
B B B B B
x x
x L c L c x
θ θ θ θ
+
 
→ = + = +
 
 
ɺ ɺ
ɺ ɺ
ɺ ɺ
(3.8)

( ) ( )
(
)
2
WM
os sin

B B B B B
x L c L x
θ θ θ θ
→ = − +
ɺɺ ɺ
ɺɺ ɺɺ
(3.9)

(
)
1 os
B B
y L c
θ
= − −
(3.10)

(
)
sin
B B B
y L
θ θ
→ = −
ɺ
ɺ
(3.11)

( ) ( )
(

)
2
sin os
B B B B B
y L L c
θ θ θ θ
→ = − −
ɺɺ ɺ
ɺɺ
(3.12)

• Xét ở thân robot
Sử dụng định luật 2 Newton lần lượt theo các trục x, trục y và trục quay tại điểm
trọng tâm của thân robot.
B B L R
M x H H
= +
ɺɺ
(3.13)
(
)
sin
L R
B B L R B C L R B B
C C
M y V V M g F V V M g
L
θ
+
= + − + = + − +

ɺɺ
(3.14)
(
)
(
)
(
)
sin os
B B L R B L R B L R
J V V L H H Lc C C
θ θ θ
= + − + − +
ɺɺ
(3.15)
( )
2
L R
D
J H H
δ
δ
= −
ɺɺ
(3.16)

Trích t
ừ
Lu
ậ

n V
ă
n Th
ạ
c S
ĩ
– Tác gi
ả
:
Nguyễn Gia Minh Thảo
. Khoa
Đ
i
ệ
n-
Đ
i
ệ
n T
ử
,
Đ
H Bách Khoa TpHCM.



Thế (3.15) , (3.14) vào (3.13), ta có:
(
)
( )

sin sin os
L R
B B B B B B B B B B L R
C C
J M y M g L M Lc x C C
L
θ θ θ θ
 
+
= + − − − +
 
 
ɺɺ
ɺɺ ɺɺ
(3.17)
Rút gọn (3.17) ta có :
( )
(
)
( )
(
)
( )
2
sin os sin 1 sin
B B B B B B B B B B L R
J M L y c x M gL C C
θ θ θ θ θ
= − + − + +
ɺɺ

ɺɺ ɺɺ
(3.18)
T
ừ
(3.9) và (3.12), ta có:
( ) ( ) ( )
W WR
W
sin os os os
2
L
B B B B B B B B
x x
y c x L c L c x
θ θ θ θ θ θ
+
 
− = − − = − −
 
 
ɺɺ ɺɺ
ɺɺ ɺɺ
ɺɺ ɺɺ ɺɺ
(3.19)
Th
ế
(3.19) vào (3.18) ta có:
( ) ( )
(
)

( )
2
2
W
os sin 1 sin
B B B B B B B B B L R
J M L c x M L M gL C C
θ θ θ θ θ
= − − + − + +
ɺɺ ɺɺ
ɺɺ
(3.20)
T
ừ
(3.1) suy ra:
(
)
(
)
W W W WR
L TL L TR R L R TL TR
M x M x H H H H H H H H+ = − + − = − + + +
ɺɺ ɺɺ
(3.21)
T
ừ
(3.3) suy ra :
WL W
L L
TL

C J
H
R
θ
−
=
ɺɺ
;
WR WR
R
TR
C J
H
R
θ
−
=
ɺɺ
(3.21.b)
Th
ế
(3.13), (3.21.b) vào (3.21), ta có:
( )
(
)
WL W WR WR
W W WR
L R L
L B B
C C J J

M x x M x
R
θ θ
+ − +
+ = − +
ɺɺ ɺɺ
ɺɺ ɺɺ ɺɺ
(3.22)
Ta có:
2
WL WR WM
x x x
+ =
ɺɺ ɺɺ ɺɺ
;
WL W WR WR W W
L
J J J
θ θ θ
= =
ɺɺ ɺɺ ɺɺ
;
đặ
t
L R
C C C
= +

Bi
ể

u th
ứ
c (3.22) s
ẽ
tr
ở
thành:
(
)
W WL WR
W WM
2
B B
C J
M x M x
R
θ θ
− +
= − +
ɺɺ ɺɺ
ɺɺ ɺɺ

Mà:
( )
WL WR
1
2
B
θ θ θ
+ =

, suy ra :
W
W WM
2
2
B
B B
C J
M x M x
R
θ
−
= − +
ɺɺ
ɺɺ ɺɺ
(3.23)
Th
ế
(3.9) và (3.23) ta có:
( ) ( )
( )
2
W
W WM WM
2
2 os sin
B
B B B B B B B
J
C

M x M L c M L M x
R R
θ
θ θ θ θ
= − + − − +
ɺɺ
ɺɺ ɺ
ɺɺ ɺɺ
(3.24)
Trích t
ừ
Lu
ậ
n V
ă
n Th
ạ
c S
ĩ
– Tác gi
ả
:
Nguyễn Gia Minh Thảo
. Khoa
Đ
i
ệ
n-
Đ
i

ệ
n T
ử
,
Đ
H Bách Khoa TpHCM.



T
ừ
ph
ươ
ng trình (3.20), (3.24) và thay :
B
θ
b
ở
i
θ
,
W
M
x
b
ở
i
x
; ta có h
ệ

ph
ươ
ng trình
mô t
ả
h
ệ
th
ố
ng nh
ư
sau:
( ) ( )
(
)
( ) ( )
( )
2
2
2
W
W
os sin 1 sin
2
2 os sin
B B B B
B B B
J M L c x M L M gL C
J
C

M x M L c M L M x
R R
θ θ θ θ θ
θ
θ θ θ θ

= − − + − +



= − + − − +

ɺɺ ɺɺ
ɺɺ
ɺɺ
ɺɺ ɺ
ɺɺ ɺɺ
(3.25)

•

Xem moment quán tính c
ủ
a thân robot là m
ộ
t thanh có chi
ề
u dài L, kh
ố
i l

ượ
ng
B
M
,
quay quanh tr
ụ
c z – là tr
ụ
c n
ố
i gi
ữ
a hai bánh xe:
2
1
3
B B
J M L
=
(3.26)
•

Xem moment quán tính c
ủ
a bánh xe robot là
đĩ
a tròn xoay có bán kính R, kh
ố
i

l
ượ
ng
W
M
, quay quanh tr
ụ
c z –tr
ụ
c n
ố
i gi
ữ
a hai bánh xe:
2
W w
1
2
J M R
=
(3.27)
Th
ế
(3.26) và (3.27) vào h
ệ
ph
ươ
ng trình (3.25), ta có:
( ) ( )
(

)
( ) ( )
( )
( )
( )
2
2
2
W w
4
os sin 1 sin
3
2 sin os
B B B
B B B
M L M L c x M gL C
C
M M x M L M R M L c
R
θ θ θ θ
θ θ θ θ

= − + − +




+ = − + +



ɺɺ
ɺɺ
ɺ ɺɺ
ɺɺ
(3.28)
Gi
ả
i h
ệ
ph
ươ
ng trình (3.28), ta có:
( )
(
)
( )
( )
( )( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )

( )
( )
( )
w
W
2
2
2
W W
w
W
2 w
w
0,75 os os
1
2
0,75 1 sin
0,75 sin os 0,75 sin 0,75 os
2 2
0,75 os os
2
0,75
0,75 os sin
sin
B
B
B
B B B
B
B

B
B
M R M L c c
M M L
M L c g c
C
M M L L M L M M RL
M R M L c c
M M x
L
M R
g M R M L c
M L
L
θ θ
θ
θ
θ θ θ θ
θ
θ θ
θ θ
θ θ
 
+
− =
 
 
+
 
 

+
 
− + +
 
+ +
 
 
 
+
+ − =
 
 
 
+
+
− +
ɺɺ
ɺ
ɺɺ
ɺ
( )
( )
( )
( )
2
2
os 1 sin
1
B
B

M L c
C
M L R
θ θ















 
+

 
+

 
 

 


(3.29)
Trích t
ừ
Lu
ậ
n V
ă
n Th
ạ
c S
ĩ
– Tác gi
ả
:
Nguyễn Gia Minh Thảo
. Khoa
Đ
i
ệ
n-
Đ
i
ệ
n T
ử
,
Đ
H Bách Khoa TpHCM.

Đặ
t bi
ế
n tr
ạ
ng thái nh
ư
sau:
1 2 3 4
, , ,
x x x x x x
θ θ
= = = =
ɺ
ɺ
. H
ệ
ph
ươ
ng trình tr
ạ
ng
thái mô t
ả
robot (3.29)
đượ
c vi

ế
t l
ạ
i nh
ư
sau:
( ) ( ) ( )
( )
1 2
2 1 1 2 1 2 1 1
3 4
4 3 1 4


, C


x x
x f x f x x g x
x x
x f x f x
=
= + +
=
= +
ɺ
ɺ
ɺ
ɺ
( ) ( )

1 2 2 1
, C
x g x






+

(3.30)
V
ớ
i:

C C C
L R
= +



( )
(
)
( )
( )
( )
( )
1

1 1
w 1 1
W
0.75 sin
0.75 os os
1
2
B
B
g x
L
f x
M R M L c x c x
M M L
 
−
 
 
=
 
+
−
 
 
+
 



( )

(
)
(
)
( )
( )
( )
( )
( )
( )
2
1 1
2
W
2 1 2
w 1 1
W
0.75 sin os
2
,
0.75 os os
1
2
B
B
B
B
M L x c x
x
M M L

f x x
M R M L c x c x
M M L
 
 
 
+
 
=
 
+
−
 
 
+
 



( )
( )
(
)
( )
( )
( )
( )
( )
( )
2

1
1
2
W
1 1
w 1 1
W
0.75 1 sin
0.75 os
2
0.75 os os
1
2
B B
B
B
x
c x
M L M M RL
g x
M R M L c x c x
M M L
 
+
 
+
 
+
 
 

=
 
+
−
 
 
+
 



( )
(
)
(
)
(
)
( )
( )
( )
w 1 1
3 1
w 1 1
W
0,75 os sin
0,75 os os
2
B
B

B
g M R M L c x x
L
f x
M R M L c x c x
M M
L
 
− +
 
 
 
=
 
+
+ −
 
 
 


Trích t
ừ
Lu
ậ
n V
ă
n Th
ạ
c S

ĩ
– Tác gi
ả
:
Nguyễn Gia Minh Thảo
. Khoa
Đ
i
ệ
n-
Đ
i
ệ
n T
ử
,
Đ
H Bách Khoa TpHCM.




( )
( )( )
(
)
( )
( )
( )
2

1 2
4 1 2
w 1 1
W
sin
,
0,75 os os
2
B
B
B
M L x x
f x x
M R M L c x c x
M M
L
=
 
+
+ −
 
 
 



( )
( )
(
)

( )
(
)
( )
( )
( )
2
w 1 1
2
2 1
w 1 1
W
0,75 os 1 sin
1
0,75 os os
2
B
B
B
B
M R M L c x x
M L R
g x
M R M L c x c x
M M
L
 
+ +
 
+

 
 
 
=
 
+
+ −
 
 
 


•

T
ừ
h
ệ
ph
ươ
ng trình (3.30) ta th
ự
c hi
ệ
n mô ph
ỏ
ng mô hình robot hai bánh t
ự
cân
b

ằ
ng trong Matlab/Simulink nh
ư
Hình 3.2 .


Hình 3.2: S
ơ

đồ
mô ph
ỏ
ng mô hình robot hai bánh trong MatLab/Simulink