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ị]

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

xB , y B Vị trí của trọng tâm thân robot theo hướng [m]

trục x và hướng trục y
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.

M WL = M WR = M W Khối lượng bánh xe, bánh xe trái và bánh xe 0.5 [kg]

phải có khối lượng bằng nhau

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

δ Góc quay [rad]

RW Bán kính bánh xe 0.075

[m]
L Khoảng cách từ trọng tâm thân robot đến 0.36 [m]
trục z của hai bánh xe
D Khoảng cách giữa hai bánh xe 0.35 [m]
CL , CR Moment tác dụng của động cơ kết nối với [Nm]
bánh xe trái, bánh xe phải
g Gia tốc trọng trường 9.8[ ms −2
]

H L , H R , VL ,VR Lực, phản lực tương tác giữa thân robot và [Nm]
hai bánh xe trái, phải
VTL ,VTR Phản lực của mặt đất tương tác lên hai bánh [Nm]
xe trái, phải
HTL , H TR Lực ma sát giữa bánh xe trái, phải với mặt [Nm]
đường

• 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.
xWL = H TL − H L
M W ɺɺ (3.1)

yWL = VTL − VL − M W g
M W ɺɺ (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.

J WLθɺɺWL = CL − H TL R (3.3)

• Vị trí của bánh xe và thân robot.

xWL = θ WL R (3.4)

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

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

 x + xWR 
xB = L sin θ B +  WL  (3.7)
 2 
 xɺ + xɺWR 
→ xɺB = L ( cosθ B )θɺB +  WL  = L ( cosθ B ) θ B + xɺWM
ɺ (3.8)
 2 
2
( )
xB = L ( cosθ B )θɺɺB − L ( sin θ B ) θɺB
→ ɺɺ + ɺɺ
xWM (3.9)

y B = − L (1 − cosθ B ) (3.10)

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

2
→ ɺɺ ( )
yB = − L ( sin θ B ) θɺɺB − L ( cosθ B ) θɺB (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.
xB = H L + H R
M B ɺɺ (3.13)

( CL + CR )
yB = VL + VR − M B g + FC = VL + VR − M B g +
M B ɺɺ sin θ B (3.14)
L
J BθɺɺB = (VL + VR ) L sin θ B − ( H L + H R ) Lcosθ B − ( C L + C R ) (3.15)

D
J δ δɺɺ = ( H L − H R ) (3.16)
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.

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

 ( C + CR ) sin θ  L sin θ − M Lcosθ ɺɺx − C + C
J BθɺɺB =  M B ɺɺ
yB + M B g − L B  B B B B ( L R ) (3.17)
 L 
Rút gọn (3.17) ta có :

J BθɺɺB = M B L ( ɺɺ (
xB ) + M B gL sin θ B − 1 + ( sin θ B )
y B sin θ B − ( cosθ B ) ɺɺ
2
) (C L + CR ) (3.18)

Từ (3.9) và (3.12), ta có:

 ɺɺ
x + ɺɺ
xWR 
yB sin θ B − ( cosθ B ) ɺɺ
ɺɺ xB = − LθɺɺB − ( cosθ B )  WL  = − Lθ B − ( cosθ B ) ɺɺ
ɺɺ xW (3.19)
 2 
Thế (3.19) vào (3.18) ta có:

J BθɺɺB = − M B L ( cosθ B ) ɺɺ ( 2
)
xW − M B L2θɺɺB + M B gL sin θ B − 1 + ( sin θ B ) ( CL + CR ) (3.20)

Từ (3.1) suy ra:

xWR = H TL − H L + H TR − H R = − ( H L + H R ) + ( H TL + H TR ) (3.21)
xWL + M W ɺɺ
M W ɺɺ

C L − J WLθɺɺWL CR − J WRθɺɺWR
Từ (3.3) suy ra : H TL = ; H TR = (3.21.b)
R R
Thế (3.13), (3.21.b) vào (3.21), ta có:

M W ( ɺɺ xWR ) = − M B ɺɺ
xWL + ɺɺ xB +
(
CL + CR − J WLθɺɺWL + J WRθɺɺWR )
(3.22)
R
xWL + ɺɺ
Ta có: ɺɺ xWM ; JWLθɺɺWL = JWRθɺɺWR = JWθɺɺW ; đặt C = CL + CR
xWR = 2ɺɺ

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

1 C − 2 J WθɺɺB
Mà: ( WL
θ + θ WR ) = θ B , suy ra :
2 M x
ɺɺ
W WM = − M x
ɺɺ
B B + (3.23)
2 R
Thế (3.9) và (3.23) ta có:
2 2 J WθɺɺB C
xWM = − M B L ( cosθ B ) θɺɺB + M B L ( sin θ B ) θɺB
2 M W ɺɺ ( ) −
R
− M B ɺɺ
xWM +
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 θ , xWM bởi x ; ta có hệ phương trình
mô tả hệ thống như sau:

 J θɺɺ = −M L ( cosθ ) ɺɺ
 B B (
2
x − M B L2θɺɺ + M B gL sin θ − 1 + ( sin θ ) C )
 2 2 J Wθɺɺ C (3.25)
2M W ɺɺ ɺɺ ɺ
x = −M B L ( cosθ ) θ + M B L ( sin θ ) θ − () − M B ɺɺ
x+
 R R
• 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 M B ,
1 2
quay quanh trục z – là trục nối giữa hai bánh xe: J B = M B L (3.26)
3
• 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
1
lượng M W , quay quanh trục z –trục nối giữa hai bánh xe: J W = M w R 2 (3.27)
2
Thế (3.26) và (3.27) vào hệ phương trình (3.25), ta có:

 4
 3 M B L2 ɺɺ
θ = − M B L ( c os θ ) x
ɺɺ + M B gL sin θ − 1 + ((sin θ )
2
C )
 (3.28)
( 2M + M ) ɺɺ x = M L ( sin θ ) θɺ 2 − ( M R + M L ( cosθ ) ) θɺɺ + C
( )
 W B B w B
R
Giải hệ phương trình (3.28), ta có:
  0,75( Mw R + MB L( cosθ ) ) ( cosθ ) 
  −1θɺɺ =
  ( 2M + M ) L 
 W B 


 0,75M B L ( sinθ )( cosθ
()) 2
θɺ −
0,75g ( sinθ ) +  ( )
 0,75 1+ ( sinθ )2
+
0,75( cosθ ) 
C
 ( 2MW + MB ) L L 
 MB L 2
( 2MW + MB ) RL 
  



 2M + M − 0,75( Mw R + MB L( cosθ ) ) ( cosθ )  ɺɺx =
 W B
L


 


()
ɺ 2 0,75g ( Mw R + MB L( cosθ ) ) ( sinθ )
 w B ( )
 0,75( M R + M L( cosθ ) ) 1+ ( sinθ )2
1

MBL( sinθ ) θ − +

+ C
 L M B L2
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: x1 = θ , x2 = θɺ, x3 = x, x4 = xɺ . Hệ phương trình trạng
thái mô tả robot (3.29) được viết lại như sau:

 xɺ1 = x2
 xɺ = f x + f x , x + g x C
 2 1 ( 1) 2 ( 1 2 ) 1 ( 1)
ɺ (3.30)
x3 = x4
 xɺ4 = f3 ( x1 ) + f4 ( x1, x2 ) + g2 ( x1 ) C
Với:
C = CL + CR
 −0.75 g ( sin x1 ) 
 
 L 
f1 ( x1 ) =
 0.75 ( M w R + M B L ( cosx1 ) ) ( cosx1 ) 
 − 1
 ( 2 M + M ) L 
 W B 
 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
 ( 2 M + M ) L 
 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 ) 
 
 L 
f3 ( x1 ) =  
 0,75( Mw R + MB L( cosx1 ) ) ( cosx1 ) 
 2MW + MB − 
 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.

f4 ( x1, x2 ) =
( M L(sin x )( x ) )
B 1 2
2

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

 2MW + MB − 
 L 
 
 0,75( M R + M L( cosx ) ) 1+ ( sin x )2 
 w B 1 (1
+
1 )
 MB L2 R 
g2 ( x1 ) =  
 0,75( Mw R + MB L( cosx1 ) ) ( cosx1 ) 
 2MW + MB − 
 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